diff --git a/jsk_apc2016_common/launch/object_segmentation_3d.launch b/jsk_apc2016_common/launch/object_segmentation_3d.launch
index 38042f42d..84cc0176b 100644
--- a/jsk_apc2016_common/launch/object_segmentation_3d.launch
+++ b/jsk_apc2016_common/launch/object_segmentation_3d.launch
@@ -8,12 +8,12 @@
+ pkg="jsk_perception" type="fcn_object_segmentation.py" output="screen">
gpu: 0
model_name: fcn32s
- model_h5: $(find jsk_apc2016_common)/trained_data/fcn32s_v2_148000.chainermodel
+ model_file: $(find jsk_apc2016_common)/trained_data/fcn32s_v2_148000.chainermodel
diff --git a/jsk_apc2016_common/node_scripts/bounding_box_array_to_collision_object.py b/jsk_apc2016_common/node_scripts/bounding_box_array_to_collision_object.py
new file mode 100755
index 000000000..2928c5f99
--- /dev/null
+++ b/jsk_apc2016_common/node_scripts/bounding_box_array_to_collision_object.py
@@ -0,0 +1,52 @@
+#!/usr/bin/env python
+
+import time
+
+import tf
+from std_msgs.msg import String
+from geometry_msgs.msg import PointStamped
+from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import Quaternion
+from geometry_msgs.msg import TransformStamped
+from geometry_msgs.msg import Vector3
+from jsk_recognition_msgs.msg import PolygonArray
+from jsk_recognition_msgs.msg import BoundingBoxArray
+import moveit_commander
+import rospy
+import tf2_geometry_msgs.tf2_geometry_msgs as tf_utils
+from jsk_topic_tools import ConnectionBasedTransport
+
+import numpy as np
+
+
+def callback(box_arr_msg):
+ for name in scene.get_known_object_names():
+ if name.startswith('polygon_'):
+ scene.remove_world_object(name)
+
+ for i, box_msg in enumerate(box_arr_msg.boxes):
+ pose = PoseStamped()
+ pose.header.frame_id = box_arr_msg.header.frame_id
+ pose.header.stamp = box_arr_msg.header.stamp
+ pose.pose.position.x = box_msg.pose.position.x
+ pose.pose.position.y = box_msg.pose.position.y
+ pose.pose.position.z = box_msg.pose.position.z
+ pose.pose.orientation.w = box_msg.pose.orientation.w
+ size = (box_msg.dimensions.x, box_msg.dimensions.y, box_msg.dimensions.z)
+ rospy.loginfo('Adding polygon_%04d' % i)
+ scene.add_box(
+ name='polygon_%04d' % i,
+ pose=pose,
+ size=size,
+ )
+
+
+if __name__ == '__main__':
+ rospy.init_node('bounding_box_array_to_collision_object')
+
+ scene = moveit_commander.PlanningSceneInterface()
+ time.sleep(1)
+
+ sub = rospy.Subscriber('~input', BoundingBoxArray, callback)
+
+ rospy.spin()
diff --git a/jsk_apc2016_common/node_scripts/laserscan_to_pointcloud2.py b/jsk_apc2016_common/node_scripts/laserscan_to_pointcloud2.py
new file mode 100755
index 000000000..9a4d88626
--- /dev/null
+++ b/jsk_apc2016_common/node_scripts/laserscan_to_pointcloud2.py
@@ -0,0 +1,18 @@
+#!/usr/bin/env python
+
+import rospy
+from sensor_msgs.msg import PointCloud2
+from sensor_msgs.msg import LaserScan
+from laser_geometry import LaserProjection
+
+def callback(laserscan_msg):
+ laserscan_point = LaserProjection()
+ pointcloud2_point = laserscan_point.projectLaser(laserscan_msg)
+ pub = rospy.Publisher('~output', PointCloud2, queue_size=100)
+ pub.publish(pointcloud2_point)
+
+if __name__ == '__main__':
+ rospy.init_node('laserscan_to_pointcloud2')
+ sub = rospy.Subscriber('~input', LaserScan, callback)
+
+ rospy.spin()
diff --git a/jsk_apc2016_common/node_scripts/polygon_array_to_box_array_on_ground.py b/jsk_apc2016_common/node_scripts/polygon_array_to_box_array_on_ground.py
new file mode 100755
index 000000000..c6d5b332f
--- /dev/null
+++ b/jsk_apc2016_common/node_scripts/polygon_array_to_box_array_on_ground.py
@@ -0,0 +1,113 @@
+#!/usr/bin/env python
+
+import time
+
+import tf
+from std_msgs.msg import String
+from geometry_msgs.msg import PointStamped
+from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import Quaternion
+from geometry_msgs.msg import TransformStamped
+from geometry_msgs.msg import Vector3
+from jsk_recognition_msgs.msg import PolygonArray
+from jsk_recognition_msgs.msg import BoundingBox
+from jsk_recognition_msgs.msg import BoundingBoxArray
+import moveit_commander
+import rospy
+import tf2_geometry_msgs.tf2_geometry_msgs as tf_utils
+from jsk_topic_tools import ConnectionBasedTransport
+
+import numpy as np
+
+
+class StopMoveitTransport(ConnectionBasedTransport):
+ def __init__(self):
+ super(StopMoveitTransport, self).__init__()
+ self.flag = 1
+
+ def subscribe(self):
+ self.sub_img = rospy.Subscriber('stop_moveit', String, self._process)
+
+ def _process(self, msg):
+ if msg.data == 'stop':
+ self.flag = 0
+ if msg.data == 'start':
+ self.flag = 1
+
+
+def callback(ply_arr_msg):
+ if stop_moveit_sub.flag == 1:
+ dst_frame = ply_arr_msg.header.frame_id
+ stamp = ply_arr_msg.header.stamp
+ try:
+ listener.waitForTransform(
+ src_frame,
+ dst_frame,
+ stamp,
+ timeout=rospy.Duration(1),
+ )
+ except Exception, e:
+ rospy.logerr(e)
+ return
+ dst_pose = listener.lookupTransform(src_frame, dst_frame, stamp)
+ transform = TransformStamped()
+ transform.header.frame_id = src_frame
+ transform.header.stamp = stamp
+ transform.child_frame_id = dst_frame
+ transform.transform.translation = Vector3(*dst_pose[0])
+ transform.transform.rotation = Quaternion(*dst_pose[1])
+
+ box_array = []
+ for ply_msg in ply_arr_msg.polygons:
+ # polygon -> points
+ points = np.zeros((len(ply_msg.polygon.points), 3),
+ dtype=np.float64)
+ for i, pt in enumerate(ply_msg.polygon.points):
+ pt_stamped = PointStamped(header=ply_arr_msg.header, point=pt)
+ pt = tf_utils.do_transform_point(pt_stamped, transform).point
+ points[i, 0] = pt.x
+ points[i, 1] = pt.y
+ points[i, 2] = pt.z
+
+ # make bounding box
+ box = BoundingBox()
+ # polygon center
+ x_max, y_max, z_max = np.max(points, axis=0)
+ x_min, y_min, z_min = np.min(points, axis=0)
+ x = (x_max + x_min) / 2.0
+ y = (y_max + y_min) / 2.0
+ z = z_max / 2.0
+ box.pose.position.x = x
+ box.pose.position.y = y
+ box.pose.position.z = z
+ box.pose.orientation.w = 1
+ # polygon size
+ box.dimensions.x = x_max - x_min
+ box.dimensions.y = y_max - y_min
+ box.dimensions.z = z_max
+
+ box_array = box_array + [box]
+
+ bounding_box_array = BoundingBoxArray()
+ bounding_box_array.header.stamp = stamp
+ bounding_box_array.header.frame_id = 'base_link'
+ bounding_box_array.boxes = box_array
+ pub = rospy.Publisher('~output', BoundingBoxArray, queue_size=10)
+ pub.publish(bounding_box_array)
+
+
+if __name__ == '__main__':
+ rospy.init_node('polygon_array_to_box_array_on_ground')
+
+ time.sleep(1)
+
+ listener = tf.listener.TransformListener()
+
+ src_frame = rospy.get_param('~fixed_frame', 'base_link')
+
+ stop_moveit_sub = StopMoveitTransport()
+ stop_moveit_sub.subscribe()
+
+ sub = rospy.Subscriber('~input', PolygonArray, callback)
+
+ rospy.spin()
diff --git a/jsk_apc2016_common/package.xml b/jsk_apc2016_common/package.xml
index 705363288..c891e8cf8 100644
--- a/jsk_apc2016_common/package.xml
+++ b/jsk_apc2016_common/package.xml
@@ -45,6 +45,7 @@
tf2_ros
tf2_eigen
pcl_ros
+ moveit_commander
roslint
diff --git a/jsk_apc2016_common/samples/FA_I_sensor2_precise_from_table1.dat b/jsk_apc2016_common/samples/FA_I_sensor2_precise_from_table1.dat
new file mode 100644
index 000000000..e4904fd83
--- /dev/null
+++ b/jsk_apc2016_common/samples/FA_I_sensor2_precise_from_table1.dat
@@ -0,0 +1,50 @@
+0 32798.3
+1 29099.2
+2 28003.9
+3 28558.7
+4 28425.0
+5 28386.0
+6 29374.7
+7 28621.1
+8 28975.1
+9 29200.5
+10 28402.6
+11 26594.9
+12 21142.1
+13 20291.3
+14 17679.5
+15 16799.5
+16 15560.8
+17 14769.3
+18 14258.7
+19 13749.4
+20 13324.3
+21 13023.7
+22 12618.2
+23 12255.5
+24 12036.4
+25 11850.8
+26 11616.0
+27 11575.5
+28 11429.5
+29 11283.3
+30 11161.1
+31 11061.2
+32 10978.5
+33 10897.7
+34 10848.0
+35 10788.3
+36 10723.4
+37 10680.6
+38 10632.9
+39 10601.6
+40 10583.3
+41 10542.8
+42 10513.0
+43 10471.7
+44 10443.2
+45 10416.8
+46 10408.1
+47 10371.0
+48 10347.5
+49 10342.7
diff --git a/jsk_apc2016_common/samples/FA_I_sensor2_precise_from_table2.dat b/jsk_apc2016_common/samples/FA_I_sensor2_precise_from_table2.dat
new file mode 100644
index 000000000..eed1a365b
--- /dev/null
+++ b/jsk_apc2016_common/samples/FA_I_sensor2_precise_from_table2.dat
@@ -0,0 +1,50 @@
+0 23452.5
+1 24788.5
+2 25087.9
+3 25240.3
+4 24833.9
+5 25081.0
+6 25142.7
+7 25176.2
+8 25288.1
+9 25611.8
+10 26967.9
+11 26687.7
+12 21898.9
+13 20342.2
+14 18929.1
+15 17834.1
+16 16391.4
+17 16139.2
+18 14968.8
+19 14037.6
+20 13499.8
+21 13425.1
+22 13118.2
+23 12496.2
+24 12447.6
+25 12143.4
+26 11996.9
+27 11796.6
+28 11713.7
+29 11602.0
+30 11509.9
+31 11293.6
+32 11231.9
+33 11168.6
+34 11064.1
+35 11013.8
+36 11015.9
+37 10917.5
+38 10833.9
+39 10800.4
+40 10738.3
+41 10708.5
+42 10657.9
+43 10629.3
+44 10608.1
+45 10565.5
+46 10549.2
+47 10525.4
+48 10489.6
+49 10456.8
diff --git a/jsk_apc2016_common/samples/FA_I_sensor2_precise_from_table3.dat b/jsk_apc2016_common/samples/FA_I_sensor2_precise_from_table3.dat
new file mode 100644
index 000000000..5d99c7775
--- /dev/null
+++ b/jsk_apc2016_common/samples/FA_I_sensor2_precise_from_table3.dat
@@ -0,0 +1,50 @@
+0 29424.3
+1 28870.1
+2 28522.0
+3 28274.2
+4 28249.2
+5 28476.1
+6 28141.0
+7 28152.9
+8 28980.4
+9 27424.7
+10 27194.8
+11 26087.5
+12 21836.1
+13 20479.7
+14 18927.2
+15 17782.0
+16 16055.9
+17 14995.5
+18 14467.3
+19 14122.2
+20 13441.4
+21 13157.5
+22 12900.9
+23 12486.7
+24 12316.2
+25 12178.5
+26 11916.8
+27 11696.8
+28 11701.7
+29 11510.7
+30 11355.4
+31 11284.8
+32 11223.2
+33 11113.9
+34 11034.9
+35 10989.5
+36 10926.2
+37 10851.5
+38 10810.2
+39 10758.3
+40 10717.4
+41 10676.5
+42 10667.4
+43 10626.2
+44 10576.0
+45 10552.7
+46 10532.6
+47 10504.3
+48 10479.7
+49 10468.1
diff --git a/jsk_apc2016_common/samples/FA_I_sensor2_precise_from_table4.dat b/jsk_apc2016_common/samples/FA_I_sensor2_precise_from_table4.dat
new file mode 100644
index 000000000..ac42b0e40
--- /dev/null
+++ b/jsk_apc2016_common/samples/FA_I_sensor2_precise_from_table4.dat
@@ -0,0 +1,50 @@
+0 24166.4
+1 26369.7
+2 26441.3
+3 26555.0
+4 26419.1
+5 26514.1
+6 27252.5
+7 27225.3
+8 27184.5
+9 26533.5
+10 26129.7
+11 25159.4
+12 23073.5
+13 21159.6
+14 19632.9
+15 17363.7
+16 15840.7
+17 15059.5
+18 14549.3
+19 13847.2
+20 13466.6
+21 13179.4
+22 12916.4
+23 12404.5
+24 12242.3
+25 12047.0
+26 11817.6
+27 11746.3
+28 11650.3
+29 11472.1
+30 11363.5
+31 11299.1
+32 11194.7
+33 11124.6
+34 11042.6
+35 10997.6
+36 10937.2
+37 10863.7
+38 10821.1
+39 10769.8
+40 10736.5
+41 10692.8
+42 10676.6
+43 10636.1
+44 10593.5
+45 10567.0
+46 10545.7
+47 10521.3
+48 10492.4
+49 10485.8
diff --git a/jsk_apc2016_common/samples/FA_I_sensor2_precise_toward_table1.dat b/jsk_apc2016_common/samples/FA_I_sensor2_precise_toward_table1.dat
new file mode 100644
index 000000000..838e17b2d
--- /dev/null
+++ b/jsk_apc2016_common/samples/FA_I_sensor2_precise_toward_table1.dat
@@ -0,0 +1,50 @@
+50 10493.2
+49 10499.5
+48 10497.4
+47 10499.6
+46 10500.5
+45 10499.4
+44 10496.1
+43 10495.1
+42 10497.7
+41 10491.8
+40 10492.9
+39 10492.2
+38 10491.9
+37 10496.2
+36 10524.3
+35 10545.6
+34 10589.2
+33 10616.3
+32 10639.7
+31 10672.9
+30 10700.8
+29 10748.0
+28 10774.2
+27 10822.4
+26 10862.1
+25 10909.4
+24 10964.6
+23 11005.4
+22 11090.4
+21 11139.3
+20 11228.8
+19 11326.8
+18 11420.9
+17 11505.5
+16 11576.0
+15 11717.0
+14 11861.8
+13 12025.5
+12 12250.5
+11 12447.0
+10 12815.1
+9 13070.1
+8 13433.5
+7 13849.9
+6 14301.9
+5 15268.0
+4 15848.9
+3 17370.0
+2 19171.4
+1 21327.5
diff --git a/jsk_apc2016_common/samples/FA_I_sensor2_precise_toward_table2.dat b/jsk_apc2016_common/samples/FA_I_sensor2_precise_toward_table2.dat
new file mode 100644
index 000000000..f30a03825
--- /dev/null
+++ b/jsk_apc2016_common/samples/FA_I_sensor2_precise_toward_table2.dat
@@ -0,0 +1,50 @@
+50 10521.6
+49 10507.6
+48 10508.7
+47 10511.6
+46 10508.4
+45 10510.5
+44 10508.8
+43 10510.1
+42 10509.3
+41 10510.7
+40 10511.7
+39 10514.4
+38 10516.5
+37 10539.3
+36 10554.4
+35 10584.8
+34 10612.7
+33 10635.8
+32 10660.0
+31 10690.6
+30 10703.4
+29 10749.9
+28 10780.9
+27 10826.8
+26 10887.8
+25 10946.0
+24 10996.6
+23 11046.1
+22 11132.6
+21 11145.3
+20 11232.4
+19 11311.0
+18 11413.8
+17 11530.5
+16 11628.8
+15 11775.2
+14 11916.0
+13 12174.7
+12 12381.9
+11 12621.9
+10 12958.8
+9 13240.4
+8 13692.3
+7 14093.7
+6 14553.2
+5 15841.9
+4 16426.6
+3 18385.9
+2 19748.9
+1 22070.5
diff --git a/jsk_apc2016_common/samples/FA_I_sensor2_precise_toward_table3.dat b/jsk_apc2016_common/samples/FA_I_sensor2_precise_toward_table3.dat
new file mode 100644
index 000000000..b577bb68b
--- /dev/null
+++ b/jsk_apc2016_common/samples/FA_I_sensor2_precise_toward_table3.dat
@@ -0,0 +1,50 @@
+50 10487.3
+49 10490.4
+48 10487.0
+47 10494.1
+46 10493.9
+45 10494.8
+44 10495.0
+43 10495.0
+42 10497.9
+41 10497.2
+40 10495.7
+39 10498.1
+38 10513.1
+37 10540.6
+36 10557.1
+35 10585.8
+34 10615.1
+33 10637.5
+32 10677.6
+31 10704.5
+30 10734.8
+29 10776.2
+28 10796.0
+27 10852.9
+26 10891.1
+25 10943.4
+24 11001.1
+23 11056.5
+22 11139.3
+21 11188.5
+20 11275.1
+19 11365.7
+18 11473.2
+17 11608.5
+16 11701.2
+15 11900.2
+14 12010.3
+13 12209.3
+12 12443.9
+11 12754.6
+10 13071.5
+9 13371.1
+8 13968.4
+7 14370.8
+6 15108.8
+5 15763.4
+4 16382.3
+3 18087.2
+2 19402.3
+1 21551.7
diff --git a/jsk_apc2016_common/samples/FA_I_sensor2_rough_from_table.dat b/jsk_apc2016_common/samples/FA_I_sensor2_rough_from_table.dat
new file mode 100644
index 000000000..d9d68051c
--- /dev/null
+++ b/jsk_apc2016_common/samples/FA_I_sensor2_rough_from_table.dat
@@ -0,0 +1,10 @@
+0 37681.2
+10 38823.0
+20 16756.8
+30 12152.3
+40 10777.8
+50 10409.3
+60 10239.7
+70 10126.8
+80 10041.7
+90 9998.5
diff --git a/jsk_apc2016_common/samples/FA_I_sensor3_rough_from_table.dat b/jsk_apc2016_common/samples/FA_I_sensor3_rough_from_table.dat
new file mode 100644
index 000000000..af71d471c
--- /dev/null
+++ b/jsk_apc2016_common/samples/FA_I_sensor3_rough_from_table.dat
@@ -0,0 +1,10 @@
+0 31195.5
+10 30814.5
+20 14836.8
+30 10882.6
+40 9733.72
+50 9351.4
+60 9135.29
+70 9037.03
+80 8959.58
+90 8920.19
diff --git a/jsk_apc2016_common/samples/FA_I_sensor4_rough_from_table.dat b/jsk_apc2016_common/samples/FA_I_sensor4_rough_from_table.dat
new file mode 100644
index 000000000..798687682
--- /dev/null
+++ b/jsk_apc2016_common/samples/FA_I_sensor4_rough_from_table.dat
@@ -0,0 +1,10 @@
+0 33409.4
+10 33185.5
+20 16666.5
+30 11886.8
+40 10676.3
+50 10291.4
+60 10033.9
+70 9898.8
+80 9824.48
+90 9766.08
diff --git a/jsk_apc2016_common/samples/IMG_4617.JPG b/jsk_apc2016_common/samples/IMG_4617.JPG
new file mode 100644
index 000000000..d7584e246
Binary files /dev/null and b/jsk_apc2016_common/samples/IMG_4617.JPG differ
diff --git a/jsk_apc2016_common/samples/IMG_4622.JPG b/jsk_apc2016_common/samples/IMG_4622.JPG
new file mode 100644
index 000000000..e2bf88277
Binary files /dev/null and b/jsk_apc2016_common/samples/IMG_4622.JPG differ
diff --git a/jsk_apc2016_common/samples/IMG_4623.JPG b/jsk_apc2016_common/samples/IMG_4623.JPG
new file mode 100644
index 000000000..51d28ad0d
Binary files /dev/null and b/jsk_apc2016_common/samples/IMG_4623.JPG differ
diff --git a/jsk_apc2016_common/samples/IMG_4624.JPG b/jsk_apc2016_common/samples/IMG_4624.JPG
new file mode 100644
index 000000000..10aeebe16
Binary files /dev/null and b/jsk_apc2016_common/samples/IMG_4624.JPG differ
diff --git a/jsk_apc2016_common/samples/IMG_4625.JPG b/jsk_apc2016_common/samples/IMG_4625.JPG
new file mode 100644
index 000000000..b279b3459
Binary files /dev/null and b/jsk_apc2016_common/samples/IMG_4625.JPG differ
diff --git a/jsk_apc2016_common/samples/README.md b/jsk_apc2016_common/samples/README.md
new file mode 100644
index 000000000..c93906d4b
--- /dev/null
+++ b/jsk_apc2016_common/samples/README.md
@@ -0,0 +1,42 @@
+samples
+=======
+
+**samples** includes files used for picking an APC object from table by Fetch.
+
+
+Usage
+-----
+
+```
+$ roslaunch jsk_apc2016_common sample_fetch_start.launch
+```
+
+Then type this command in another terminal.
+
+```
+$ $(rospack find jsk_apc2016_common)/samples/sample-fetch-pick.l
+```
+
+If you want to change the target item using GUI, type this in another terminal and change the value of /label_to_mask/label_value.
+
+```
+$ rosrun rqt_reconfigure rqt_reconfigure
+```
+
+
+Record
+------
+
+```
+$ roslaunch jsk_apc2016_common record.launch
+```
+
+
+Play
+----
+
+```
+$ rossetlocal
+$ roslaunch jsk_apc2016_common play.launch bagfile:=`pwd`/.ros/fetch_demo.bag ### defined in record.launch
+$ rviz -d $(rospack find jsk_apc2016_common)/samples/config/sample_object_segmentation_3d.rviz
+```
diff --git a/jsk_apc2016_common/samples/config/sample_object_segmentation_3d.rviz b/jsk_apc2016_common/samples/config/sample_object_segmentation_3d.rviz
index b5fe472dc..d8c3b860f 100644
--- a/jsk_apc2016_common/samples/config/sample_object_segmentation_3d.rviz
+++ b/jsk_apc2016_common/samples/config/sample_object_segmentation_3d.rviz
@@ -3,11 +3,9 @@ Panels:
Help Height: 0
Name: Displays
Property Tree Widget:
- Expanded:
- - /Global Options1
- - /Status1
+ Expanded: ~
Splitter Ratio: 0.5
- Tree Height: 643
+ Tree Height: 167
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
@@ -26,7 +24,7 @@ Panels:
Experimental: false
Name: Time
SyncMode: 0
- SyncSource: ""
+ SyncSource: PointCloud2
Visualization Manager:
Class: ""
Displays:
@@ -70,9 +68,9 @@ Visualization Manager:
Position Transformer: XYZ
Queue Size: 10
Selectable: true
- Size (Pixels): 3
- Size (m): 0.01
- Style: Points
+ Size (Pixels): 1
+ Size (m): 0.005
+ Style: Flat Squares
Topic: /head_camera/depth_registered/points
Unreliable: false
Use Fixed Frame: true
@@ -296,6 +294,384 @@ Visualization Manager:
Update Interval: 0
Value: true
Visual Enabled: true
+ - Class: moveit_rviz_plugin/MotionPlanning
+ Enabled: true
+ Move Group Namespace: ""
+ MoveIt_Goal_Tolerance: 0
+ MoveIt_Planning_Attempts: 10
+ MoveIt_Planning_Time: 5
+ MoveIt_Use_Constraint_Aware_IK: true
+ MoveIt_Warehouse_Host: 127.0.0.1
+ MoveIt_Warehouse_Port: 33829
+ MoveIt_Workspace:
+ Center:
+ X: 0
+ Y: 0
+ Z: 0
+ Size:
+ X: 2
+ Y: 2
+ Z: 2
+ Name: MotionPlanning
+ Planned Path:
+ Color Enabled: false
+ Interrupt Display: false
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ bellows_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ bellows_link2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ elbow_flex_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ estop_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ forearm_roll_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ gripper_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ head_camera_depth_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ head_camera_depth_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ head_camera_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ head_camera_rgb_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ head_camera_rgb_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ head_pan_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ head_tilt_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ l_gripper_finger_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ l_wheel_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ laser_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ r_gripper_finger_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ r_wheel_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ shoulder_lift_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ shoulder_pan_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ torso_fixed_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ torso_lift_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ upperarm_roll_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ wrist_flex_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ wrist_roll_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Loop Animation: false
+ Robot Alpha: 0.5
+ Robot Color: 150; 50; 150
+ Show Robot Collision: false
+ Show Robot Visual: true
+ Show Trail: false
+ State Display Time: 0.05 s
+ Trajectory Topic: /move_group/display_planned_path
+ Planning Metrics:
+ Payload: 1
+ Show Joint Torques: false
+ Show Manipulability: false
+ Show Manipulability Index: false
+ Show Weight Limit: false
+ TextHeight: 0.08
+ Planning Request:
+ Colliding Link Color: 255; 0; 0
+ Goal State Alpha: 1
+ Goal State Color: 250; 128; 0
+ Interactive Marker Size: 0
+ Joint Violation Color: 255; 0; 255
+ Planning Group: arm
+ Query Goal State: true
+ Query Start State: false
+ Show Workspace: false
+ Start State Alpha: 1
+ Start State Color: 0; 255; 0
+ Planning Scene Topic: move_group/monitored_planning_scene
+ Robot Description: robot_description
+ Scene Geometry:
+ Scene Alpha: 0.9
+ Scene Color: 50; 230; 50
+ Scene Display Time: 0.2
+ Show Scene Geometry: true
+ Voxel Coloring: Z-Axis
+ Voxel Rendering: Occupied Voxels
+ Scene Robot:
+ Attached Body Color: 150; 50; 150
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ bellows_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ bellows_link2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ elbow_flex_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ estop_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ forearm_roll_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ gripper_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ head_camera_depth_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ head_camera_depth_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ head_camera_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ head_camera_rgb_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ head_camera_rgb_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ head_pan_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ head_tilt_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ l_gripper_finger_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ l_wheel_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ laser_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ r_gripper_finger_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ r_wheel_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ shoulder_lift_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ shoulder_pan_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ torso_fixed_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ torso_lift_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ upperarm_roll_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ wrist_flex_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ wrist_roll_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Robot Alpha: 1
+ Show Robot Collision: false
+ Show Robot Visual: true
+ Value: true
+ - Alpha: 1
+ Class: jsk_rviz_plugin/PolygonArray
+ Color: 25; 255; 0
+ Enabled: true
+ Name: PolygonArray
+ Topic: /multi_plane_estimate/output_polygon
+ Unreliable: false
+ Value: true
+ coloring: Auto
+ normal length: 0.1
+ only border: true
+ show normal: true
+ - Class: rviz/Marker
+ Enabled: true
+ Marker Topic: /arrow_marker0
+ Name: Marker
+ Namespaces:
+ "": true
+ Queue Size: 100
+ Value: true
+ - Class: rviz/Marker
+ Enabled: true
+ Marker Topic: /arrow_marker1
+ Name: Marker
+ Namespaces:
+ "": true
+ Queue Size: 100
+ Value: true
+ - Class: rviz/Marker
+ Enabled: true
+ Marker Topic: /arrow_marker2
+ Name: Marker
+ Namespaces:
+ "": true
+ Queue Size: 100
+ Value: true
+ - Class: rviz/Marker
+ Enabled: true
+ Marker Topic: /arrow_marker3
+ Name: Marker
+ Namespaces:
+ "": true
+ Queue Size: 100
+ Value: true
+ - Class: rviz/Marker
+ Enabled: true
+ Marker Topic: /arrow_marker4
+ Name: Marker
+ Namespaces:
+ "": true
+ Queue Size: 100
+ Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
@@ -320,30 +696,32 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
- Distance: 2.97777
+ Distance: 0.660681
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
- X: 0.887903
- Y: 0.311074
- Z: 0.819679
+ X: 0.751763
+ Y: 0.0214136
+ Z: 0.781115
Name: Current View
Near Clip Distance: 0.01
- Pitch: 0.250398
+ Pitch: 0.750398
Target Frame:
Value: Orbit (rviz)
- Yaw: 6.1736
+ Yaw: 4.86861
Saved: ~
Window Geometry:
Displays:
collapsed: false
- Height: 846
+ Height: 847
Hide Left Dock: false
Hide Right Dock: false
- QMainWindow State: 000000ff00000000fd000000040000000000000176000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000005fb000000120053006500670052006500730075006c00740100000028000001550000001600fffffffb00000014005400610072006700650074004d00610073006b0100000183000001690000001600fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ce0000003efc0100000002fb0000000800540069006d00650100000000000005ce000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000033d000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Motion Planning:
+ collapsed: false
+ QMainWindow State: 000000ff00000000fd0000000400000000000002a2000002c5fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000000e8000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000116000001d7000001cc00ffffff00000001000000d1000002c5fc0200000005fb000000120053006500670052006500730075006c00740100000028000001550000001600fffffffb00000014005400610072006700650074004d00610073006b01000001830000016a0000001600fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ce0000003efc0100000002fb0000000800540069006d00650100000000000005ce000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000024f000002c500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
SegResult:
collapsed: false
Selection:
@@ -357,5 +735,5 @@ Window Geometry:
Views:
collapsed: false
Width: 1486
- X: 199
- Y: 124
+ X: 418
+ Y: 285
diff --git a/jsk_apc2016_common/samples/fetch_gripper.STL b/jsk_apc2016_common/samples/fetch_gripper.STL
new file mode 100644
index 000000000..cd696e0ee
Binary files /dev/null and b/jsk_apc2016_common/samples/fetch_gripper.STL differ
diff --git a/jsk_apc2016_common/samples/fetch_indiv.launch b/jsk_apc2016_common/samples/fetch_indiv.launch
new file mode 100755
index 000000000..33c157342
--- /dev/null
+++ b/jsk_apc2016_common/samples/fetch_indiv.launch
@@ -0,0 +1,20 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/jsk_apc2016_common/samples/for_fetch_grope.rviz b/jsk_apc2016_common/samples/for_fetch_grope.rviz
new file mode 100644
index 000000000..f0ddf10b9
--- /dev/null
+++ b/jsk_apc2016_common/samples/for_fetch_grope.rviz
@@ -0,0 +1,305 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ - /Marker1
+ Splitter Ratio: 0.5
+ Tree Height: 369
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.588679
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: Image
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.03
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame:
+ Value: true
+ - Class: rviz/Marker
+ Enabled: true
+ Marker Topic: /arrow_marker0
+ Name: Marker
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: true
+ - Class: rviz/Marker
+ Enabled: true
+ Marker Topic: /arrow_marker1
+ Name: Marker
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: true
+ - Class: rviz/Marker
+ Enabled: true
+ Marker Topic: /arrow_marker2
+ Name: Marker
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: true
+ - Class: rviz/Marker
+ Enabled: true
+ Marker Topic: /arrow_marker3
+ Name: Marker
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: true
+ - Class: rviz/Marker
+ Enabled: true
+ Marker Topic: /arrow_marker4
+ Name: Marker
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: true
+ - Class: rviz/Image
+ Enabled: true
+ Image Topic: /apply_context_to_label_proba/output
+ Max Value: 1
+ Median window: 5
+ Min Value: 0
+ Name: Image
+ Normalize Range: true
+ Queue Size: 2
+ Transport Hint: raw
+ Unreliable: false
+ Value: true
+ - Alpha: 1
+ Class: rviz/RobotModel
+ Collision Enabled: false
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ bellows_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ bellows_link2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ elbow_flex_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ estop_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ forearm_roll_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ gripper_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ head_camera_depth_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ head_camera_depth_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ head_camera_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ head_camera_rgb_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ head_camera_rgb_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ head_pan_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ head_tilt_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ l_gripper_finger_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ l_wheel_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ laser_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ r_gripper_finger_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ r_wheel_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ shoulder_lift_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ shoulder_pan_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ torso_fixed_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ torso_lift_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ upperarm_roll_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ wrist_flex_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ wrist_roll_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Name: RobotModel
+ Robot Description: robot_description
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Fixed Frame: base_link
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Topic: /initialpose
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 7.09855
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.06
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 0
+ Y: 0
+ Z: 0
+ Name: Current View
+ Near Clip Distance: 0.01
+ Pitch: 0.795398
+ Target Frame:
+ Value: Orbit (rviz)
+ Yaw: 0.780398
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 846
+ Hide Left Dock: false
+ Hide Right Dock: false
+ Image:
+ collapsed: false
+ QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000200000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000022e000000be0000001600ffffff000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000022b000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1200
+ X: 366
+ Y: 51
diff --git a/jsk_apc2016_common/samples/image1.png b/jsk_apc2016_common/samples/image1.png
new file mode 100644
index 000000000..e92e340eb
Binary files /dev/null and b/jsk_apc2016_common/samples/image1.png differ
diff --git a/jsk_apc2016_common/samples/image2.png b/jsk_apc2016_common/samples/image2.png
new file mode 100644
index 000000000..59e58b774
Binary files /dev/null and b/jsk_apc2016_common/samples/image2.png differ
diff --git a/jsk_apc2016_common/samples/measure-sensor-characteristics.l b/jsk_apc2016_common/samples/measure-sensor-characteristics.l
new file mode 100755
index 000000000..1171f4209
--- /dev/null
+++ b/jsk_apc2016_common/samples/measure-sensor-characteristics.l
@@ -0,0 +1,95 @@
+#!/usr/bin/env roseus
+
+(ros::load-ros-manifest "jsk_recognition_msgs")
+(ros::load-ros-manifest "geometry_msgs")
+(ros::load-ros-manifest "sensor_msgs")
+(ros::load-ros-manifest "roseus")
+(ros::load-ros-package "force_proximity_ros")
+
+(require "package://fetcheus/fetch-interface.l")
+
+(fetch-init t)
+
+(load "package://jsk_apc2016_common/samples/publish_FA_I_sensor_marker.l")
+
+;; initialize pose of the real fetch robot
+(defun init()
+ (send *fetch* :reset-pose)
+ (send *fetch* :head :neck-p :joint-angle 15) ;; see down
+ (send *ri* :angle-vector (send *fetch* :angle-vector) 10000)
+ (send *ri* :stop-grasp)
+ (send *ri* :wait-interpolation))
+
+(defun arm-set (&optional (torso 235.876) (time 5000))
+ ;; pos #f(800 0 770) rpy #f(0 1.57 0)
+ (send *fetch* :angle-vector (float-vector torso 0.007561 9.1053 -179.963 28.8795 -180.039 109.729 -0.011606 0.0 0.0))
+ (send *ri* :angle-vector (send *fetch* :angle-vector) time)
+ (send *ri* :wait-interpolation))
+
+;; order the real fetch robot to follow the joint angle of the model robot
+(defun move (time &optional (trial-number 100))
+ (let ((test nil) (i 0))
+ (while (and (null test) (< i trial-number))
+ (setq i (+ i 1))
+ (setq test (send *ri* :angle-vector (send *fetch* :angle-vector) time)))
+ (send *ri* :wait-interpolation)))
+
+
+(defun cb-proximity (proximity-msg)
+ (setq proximity-sum (+ proximity-sum (send proximity-msg :proximity)))
+ (setq count-spin (+ count-spin 1)))
+
+
+(defun echo-proximity()
+ (ros::subscribe
+ "/proximity_sensor0"
+ force_proximity_ros::proximity #'cb-proximity 100))
+
+(defun write-to-file(val1 val2)
+ (with-open-file (out "FA_I_sensor.dat"
+ :direction :output
+ :if-exists :append
+ :if-does-not-exist :create)
+ (princ (format nil "~A ~A~%" val1 val2) out)))
+
+;;be careful when use this function
+;;table-height ~ 770
+(defun measure ()
+ (echo-proximity)
+ (arm-set 235.876 5000)
+ (dotimes (i 50)
+ (unix:sleep 1)
+ (setq proximity-sum 0)
+ (setq count-spin 0)
+ (ros::spin-once)
+ (while (< count-spin 100)
+ ;(print count-spin)
+ )
+ (setq average-proximity (/ (float proximity-sum) (float count-spin)))
+ (write-to-file i average-proximity)
+ (format t "i : ~A~%" i)
+ (send *fetch* :torso :waist-z :joint-angle (+ 235.876 i))
+ (send *ri* :angle-vector-raw (send *fetch* :angle-vector))
+ (send *ri* :wait-interpolation)
+ ))
+
+
+(defun measure-closing()
+ (echo-proximity)
+ (arm-set 285.876 5000)
+ (dotimes (i 50)
+ (unix:sleep 1)
+ (setq proximity-sum 0)
+ (setq count-spin 0)
+ (ros::spin-once)
+ (while (< count-spin 100)
+ ;;(print count-spin)
+ )
+ (setq average-proximity (/ (float proximity-sum) (float count-spin)))
+ (write-to-file (- 50 i) average-proximity)
+ (format t "50 - i : ~A~%" (- 50 i))
+ ;;(send *fetch* :torso :waist-z :joint-angle (+ 235.876 i))
+ (send *fetch* :torso :waist-z :joint-angle (- 285.876 i))
+ (send *ri* :angle-vector-raw (send *fetch* :angle-vector))
+ (send *ri* :wait-interpolation)
+ ))
diff --git a/jsk_apc2016_common/samples/play.launch b/jsk_apc2016_common/samples/play.launch
new file mode 100644
index 000000000..432156caa
--- /dev/null
+++ b/jsk_apc2016_common/samples/play.launch
@@ -0,0 +1,19 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/jsk_apc2016_common/samples/publish_FA_I_sensor_marker.l b/jsk_apc2016_common/samples/publish_FA_I_sensor_marker.l
new file mode 100755
index 000000000..a751ee1bd
--- /dev/null
+++ b/jsk_apc2016_common/samples/publish_FA_I_sensor_marker.l
@@ -0,0 +1,67 @@
+#!/usr/bin/env roseus
+
+(ros::roseus "publish_FA_I_sensor_marker")
+
+(defun make-arrow (size)
+ (print size)
+ (let ((radius (max 1.0 size))(cylinder) (cone) (arrow))
+ (setq cylinder (make-cylinder (/ radius 2) (* radius 4)))
+ (setq cone (make-cone (float-vector 0 0 (* radius 2)) radius))
+ (send cone :translate (float-vector 0 0 (* radius 4)))
+ (send cylinder :assoc cone)
+ (setq arrow (body+ cone cylinder))
+ arrow))
+
+
+(ros::advertise "/arrow_marker0" visualization_msgs::Marker 5)
+(ros::advertise "/arrow_marker1" visualization_msgs::Marker 5)
+(ros::advertise "/arrow_marker2" visualization_msgs::Marker 5)
+(ros::advertise "/arrow_marker3" visualization_msgs::Marker 5)
+(ros::advertise "/arrow_marker4" visualization_msgs::Marker 5)
+
+(defun publish-FA-I-sensor-marker (&optional (num 0) (size 10))
+ (let ((arrow (make-arrow size))
+ (pub-arrow-topic (format nil "/arrow_marker~A" num))
+ (gripper))
+ (ros::rate 10)
+ (send arrow :set-color :red)
+
+ (cond
+ ((eq num 0)
+ (setq gripper (make-coords))
+ (send gripper :locate #f(-30 0 0))
+ (send gripper :rotate (deg2rad 90) :y))
+ ((eq num 1)
+ (setq gripper (make-coords))
+ (send gripper :locate #f(0 -50 20))
+ (send gripper :rotate (deg2rad -90) :x))
+ ((eq num 2)
+ (setq gripper (make-coords))
+ (send gripper :locate #f(0 50 -20))
+ (send gripper :rotate (deg2rad 90) :x))
+ ((eq num 3)
+ (setq gripper (make-coords))
+ (send gripper :locate #f(30 -60 0))
+ (send gripper :rotate (deg2rad 90) :y))
+ ((eq num 4)
+ (setq gripper (make-coords))
+ (send gripper :locate #f(30 60 0))
+ (send gripper :rotate (deg2rad 90) :y))
+ (t (format t "arg error!~%")))
+
+ (let* ((header (instance std_msgs::header :init
+ :stamp (ros::time-now)
+ :frame_id "/gripper_link"))
+ (arrow-msg (object->marker-msg arrow header :coords gripper)))
+
+ (ros::publish pub-arrow-topic arrow-msg)
+
+ ;; for array msg
+ ;; you shold set up namespace (:ns ) or id (:id) for marker array
+ (send arrow-msg :ns "test_arrow")
+ ;;(print header)
+ )))
+
+;(publish-FA-I-sensor-marker)
+;(make-arrow 300)
+;(make-sensor-coords)
diff --git a/jsk_apc2016_common/samples/record.launch b/jsk_apc2016_common/samples/record.launch
new file mode 100644
index 000000000..30d2e1473
--- /dev/null
+++ b/jsk_apc2016_common/samples/record.launch
@@ -0,0 +1,36 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/jsk_apc2016_common/samples/sample-fetch-pick.l b/jsk_apc2016_common/samples/sample-fetch-pick.l
old mode 100644
new mode 100755
index b7569076a..1332ab7b3
--- a/jsk_apc2016_common/samples/sample-fetch-pick.l
+++ b/jsk_apc2016_common/samples/sample-fetch-pick.l
@@ -1,20 +1,37 @@
#!/usr/bin/env roseus
+
(ros::load-ros-manifest "jsk_recognition_msgs")
(ros::load-ros-manifest "geometry_msgs")
+(ros::load-ros-manifest "sensor_msgs")
+;(ros::load-ros-manifest "jsk_pcl_ros")
+(ros::load-ros-manifest "std_msgs")
+(ros::load-ros-manifest "roseus")
+(ros::load-ros-package "force_proximity_ros")
+(ros::load-ros-package "ar_track_alvar_msgs")
(require "package://fetcheus/fetch-interface.l")
+(ros::advertise "stop_moveit" std_msgs::string 100)
+(setq msg (instance std_msgs::string :init))
+
+(setq *co* (instance collision-object-publisher :init))
(fetch-init t)
+(load "package://jsk_apc2016_common/samples/publish_FA_I_sensor_marker.l")
+
+;; initialize pose of the real fetch robot
(defun init()
(send *fetch* :reset-pose)
- (send *fetch* :head :neck-p :joint-angle 15)
- (send *ri* :angle-vector (send *fetch* :angle-vector) 5000)
+ (send *fetch* :head :neck-p :joint-angle 15) ;; see down
+ (send *ri* :angle-vector (send *fetch* :angle-vector) 10000)
(send *ri* :stop-grasp)
(send *ri* :wait-interpolation))
-(defun recon()
+
+;; recognize position of the target object.
+;; frame_id : /base_link
+(defun recog-target()
(setq boxes_msg
(one-shot-subscribe
"/cluster_indices_decomposer_target/boxes"
@@ -24,54 +41,346 @@
(one-shot-subscribe
"/cluster_indices_decomposer_target/centroid_pose_array"
geometry_msgs::PoseArray))
-
(setq box (car (send boxes_msg :boxes)))
(setq center (car (send centroids :poses)))
- (setq x (* 1000 (send (send center :position) :x)))
- (setq y (* 1000 (send (send center :position) :y)))
- (setq z (* 1000 (send (send (send box :pose) :position):z)))
- t)
+ (if center
+ (progn (setq x (* 1000 (send (send center :position) :x)))
+ (setq y (* 1000 (send (send center :position) :y))))
+ nil)
+ (if box
+ (setq z (* 1000 (send (send (send box :pose) :position):z)))
+ ;;(format t "cannot find target!~%")))
+ ))
+
+
+;; look at the target object if fetch has already found it.
+(defun look-at()
+ (let ((head-x) (head-y) (head-z))
+ (setq head-x (aref (send (send *fetch* :head-end-coords) :worldpos) 0))
+ (setq head-y (aref (send (send *fetch* :head-end-coords) :worldpos) 1))
+ (setq head-z (aref (send (send *fetch* :head-end-coords) :worldpos) 2))
+ (send *fetch* :head :neck-y :joint-angle (rad2deg (atan (/ (- y head-y) x))))
+ (send *fetch* :head :neck-p :joint-angle (rad2deg (atan (/ (- head-z z) x))))
+ (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 1000 :head-controller)
+ (send *ri* :wait-interpolation :head-controller)))
+
+
+;; search for a target object by swinging his head.
+;; use search-for-object or find-AR-marker in order to find object
+(defun search-for-object()
+ (recog-target)
+ (let ((flag t))
+ (if (and center box)
+ (progn (look-at) (setq flag nil) (format t "111~%"))
+ (progn (send *fetch* :head :neck-y :joint-angle 30) ;; see left
+ (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 1000 :head-controller)
+ (recog-target)))
+
+ (if flag
+ (if (and center box)
+ (progn (look-at) (setq flag nil) (format t "222~%"))
+ (progn (send *fetch* :head :neck-y :joint-angle -30) ;; see right
+ (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 1000 :head-controller)
+ (recog-target))))
+
+ (if flag
+ (if (and center box)
+ (progn (look-at) (setq flag nil) (format t "333~%"))
+ (progn (send *fetch* :head :neck-p :joint-angle -30)
+ (send *fetch* :head :neck-y :joint-angle 0) ;; see up
+ (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 1000 :head-controller)
+ (recog-target))))
+
+ (if flag
+ (if (and center box)
+ (progn (look-at) (setq flag nil) (format t "444~%"))
+ (progn (send *fetch* :head :neck-p :joint-angle 30) ;; see down
+ (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 1000 :head-controller)
+ (recog-target))))
+
+ (if flag
+ (if (and center box)
+ (progn (look-at) (setq flag nil))
+ (progn (send *fetch* :head :neck-p :joint-angle 0) ;; see front
+ (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 1000 :head-controller))))
+
+ (if (null flag)
+ (progn (format t "find object!~%") t)
+ (progn (format t "cannot find object!~%") nil))))
+
+
+;; use search-for-object or find-AR-marker in order to find object
+(defun find-AR-marker()
+ "test documentation"
+ (let ((AR-marker-msg) (AR-marker-pose) (AR-marker-coords))
+ (setq AR-marker-msg
+ (one-shot-subscribe
+ "ar_pose_marker"
+ ar_track_alvar_msgs::AlvarMarkers))
+ (if AR-marker-msg
+ (progn
+ (setq AR-marker-pose (send (send (car (send AR-marker-msg :markers)) :pose) :pose))
+ ;;(setq AR-marker-coords (ros::tf-pose->coords AR-marker-pose))
+ (setq x (* 1000 (send AR-marker-pose :position :x)))
+ (setq y (* 1000 (send AR-marker-pose :position :y)))
+ (setq z (* 1000 (send AR-marker-pose :position :z)))
+ (setq table-height z)
+ (look-at))
+ (format t "cannot find AR marker!~%"))))
-(defun test(x y z)
- (send *fetch* :inverse-kinematics (make-coords :pos (float-vector x y z) :rpy (float-vector 0 (/ pi 2) 0)) :debug-view t))
-(defun test2(x y z)
- (send *fetch* :inverse-kinematics (make-coords :pos (float-vector x y z)) :debug-view t))
-(defun move (time)
- (send *ri* :angle-vector (send *fetch* :angle-vector) time)
- (send *ri* :wait-interpolation))
+;; order the real fetch robot to follow the joint angle of the model robot
+(defun move (time &optional (trial-number 100))
+ (let ((test nil) (i 0))
+ (while (and (null test) (< i trial-number))
+ (setq i (+ i 1))
+ (setq test (send *ri* :angle-vector (send *fetch* :angle-vector) time)))
+ (send *ri* :wait-interpolation)))
+
(defun try-grasp()
(send *ri* :go-grasp :effort 60))
-;;try grasping from above (TODO: adjust x to center of object; x+= K(dimension y) ?)
-(defun trial()
- (recon)
- (test (- x 300) y (+ z 150))
- (move 6000)
- (test x y (+ z 150)) ; move end-coords x +300
- (move 3000)
- (test x y (+ z 50))
- (move 3000)
- (try-grasp)
- )
-
-;;try grasping from front
-(defun trial2()
- (recon)
- (setq z-off (/ (send (send box :dimensions) :z) 3))
- (setq z (- z z-off))
- (test2 (- x 200) y z)
- (move 6000)
- (test2 (+ x 50) y z) ; move end-coords x +250
- (move 3000)
- (try-grasp)
- ;(test2 (+ x 50) y (+ z 100)) ; move end-coords z +100
- ;(move 3000)
- )
-
-;;TODO: avoid table!!
-;;move to put x in range (700 ~ 850 ?)
-;;elmer's glue: centroid too deep
+
+(defun grope-from-front()
+ ;before use this function, do below
+ ;(send *ri* :angle-vector (send *fetch* :inverse-kinematics (make-coords :pos (v+ (float-vector (- x 100) y table-height) (float-vector 0 width-sum i)))))
+
+ ;;proximity1,2 are for width.
+ ;;proximity0,3,4 are for depth.
+ (echo-proximity)
+ (unix:sleep 1)
+
+ (let (
+ ;(proximity0-init-value) (proximity1-init-value) (proximity2-init-value)
+ (proximity0-raw) (proximity1-raw) (proximity2-raw)
+ (proximity0 0) (proximity1 0) (proximity2 0)
+ (i 0) (width-sum 0))
+
+ (FA-I-calibration)
+ ;(ros::spin-once)
+ ;;sensor calibration (far away from the target)
+ ;; (setq proximity0-init-value (send proximity0-obj :average))
+ ;; (setq proximity1-init-value (send proximity1-obj :average))
+ ;; (setq proximity2-init-value (send proximity2-obj :average))
+
+ (setq grasp-flag-front nil)
+ ;(while (null grasp-flag-front)
+ (while (< proximity0 15000)
+ (setq proximity0-raw (send proximity0-obj :average))
+ (setq proximity1-raw (send proximity1-obj :average))
+ (setq proximity2-raw (send proximity2-obj :average))
+
+ (setq proximity0 (- proximity0-raw proximity0-init-value))
+ (setq proximity1 (- proximity1-raw proximity1-init-value))
+ (setq proximity2 (- proximity2-raw proximity2-init-value))
+
+ (format t "i : ~A~%" i)
+ (format t "proximity0 : ~A~%" proximity0)
+ (format t "proximity1 : ~A~%" proximity1)
+ (format t "proximity2 : ~A~%" proximity2)
+
+ (setq width-sum (+ width-sum (/ (- proximity2 proximity1) 100)))
+ (setq i (+ i 10))
+
+ (send *fetch* :inverse-kinematics (make-coords :pos (v+ (float-vector (- x 100) y table-height) (float-vector i width-sum 0))))
+ (move 1000)
+
+ (ros::spin-once)))
+
+ (try-grasp))
+
+
+(defun FA-I-calibration()
+ (ros::spin-once)
+ (setq proximity0-init-value (send proximity0-obj :average))
+ (setq proximity1-init-value (send proximity1-obj :average))
+ (setq proximity2-init-value (send proximity2-obj :average))
+ (setq proximity3-init-value (send proximity3-obj :average))
+ (setq proximity4-init-value (send proximity4-obj :average)))
+
+
+(defun calc-arrow-size-in-rviz (proximity-msg init-value)
+ (- 10 (* 10 (expt (max (- (send proximity-msg :average) init-value) 1) (/ -1.0 5)))))
+
+;; TODO START ;;
+;; load this file, echo-proximity, fa-i-calibration
+
+(defun cb-proximity0 (proximity-msg)
+ (if (boundp 'proximity0-init-value)
+ (publish-FA-I-sensor-marker 0 (calc-arrow-size-in-rviz proximity-msg proximity0-init-value)))
+ (setq proximity0-obj proximity-msg)
+ (if (equal "T" (send proximity-msg :mode))
+ (setq grasp-flag-front t)))
+
+(defun cb-proximity1 (proximity-msg)
+ (if (boundp 'proximity1-init-value)
+ (publish-FA-I-sensor-marker 1 (calc-arrow-size-in-rviz proximity-msg proximity1-init-value)))
+ (setq proximity1-obj proximity-msg))
+
+(defun cb-proximity2 (proximity-msg)
+ (if (boundp 'proximity2-init-value)
+ (publish-FA-I-sensor-marker 2 (calc-arrow-size-in-rviz proximity-msg proximity2-init-value)))
+ (setq proximity2-obj proximity-msg))
+
+(defun cb-proximity3 (proximity-msg)
+ (if (boundp 'proximity3-init-value)
+ (publish-FA-I-sensor-marker 3 (calc-arrow-size-in-rviz proximity-msg proximity3-init-value)))
+ (setq proximity3-obj proximity-msg)
+ (if (equal "T" (send proximity-msg :mode))
+ (setq grasp-flag-top t)))
+
+(defun cb-proximity4 (proximity-msg)
+ (if (boundp 'proximity4-init-value)
+ (publish-FA-I-sensor-marker 4 (calc-arrow-size-in-rviz proximity-msg proximity4-init-value)))
+ (setq proximity4-obj proximity-msg)
+ (if (equal "T" (send proximity-msg :mode))
+ (setq grasp-flag-top t)))
+
+;; TODO END ;;
+
+(defun echo-proximity()
+ (ros::subscribe
+ "/proximity_sensor0"
+ force_proximity_ros::proximity #'cb-proximity0 10)
+ (ros::subscribe
+ "/proximity_sensor1"
+ force_proximity_ros::proximity #'cb-proximity1 10)
+ (ros::subscribe
+ "/proximity_sensor2"
+ force_proximity_ros::proximity #'cb-proximity2 10)
+ (ros::subscribe
+ "/proximity_sensor3"
+ force_proximity_ros::proximity #'cb-proximity3 10)
+ (ros::subscribe
+ "/proximity_sensor4"
+ force_proximity_ros::proximity #'cb-proximity4 10))
+
+
+
+(defun grope-from-top()
+ ;before use this function, do below
+ ;(send *ri* :angle-vector (send *fetch* :inverse-kinematics (make-coords :pos (float-vector x y (+ table-height 30)) :rpy #f(0 1.57 0))))
+
+ ;;proximity1,2 are for width.
+ ;;proximity0,3,4 is for depth.
+ (echo-proximity)
+
+ (let (;(proximity0-init-value) (proximity3-init-value) (proximity4-init-value)
+ (proximity0-raw) (proximity3-raw) (proximity4-raw)
+ (proximity0 0) (proximity3 0) (proximity4 0)
+ (i 0) (width-sum 0))
+
+ (FA-I-calibration)
+ ;(ros::spin-once)
+ ;;sensor calibration (far away from the target)
+ ;; (setq proximity0-init-value (send proximity0-obj :average))
+ ;; (setq proximity3-init-value (send proximity3-obj :average))
+ ;; (setq proximity4-init-value (send proximity4-obj :average))
+
+ (setq grasp-flag-top nil)
+ (while (null grasp-flag-top)
+ (setq proximity0-raw (send proximity0-obj :average))
+ (setq proximity3-raw (send proximity3-obj :average))
+ (setq proximity4-raw (send proximity4-obj :average))
+
+ (setq proximity0 (- proximity0-raw proximity0-init-value))
+ (setq proximity3 (- proximity3-raw proximity3-init-value))
+ (setq proximity4 (- proximity4-raw proximity4-init-value))
+
+ (format t "i : ~A~%" i)
+ (format t "proximity0 : ~A~%" proximity0)
+ (format t "proximity3 : ~A~%" proximity3)
+ (format t "proximity4 : ~A~%" proximity4)
+
+
+ (setq width-sum (+ width-sum (/ (- proximity4 proximity3) 100)))
+ (setq i (- i 1))
+
+ (send *fetch* :inverse-kinematics (make-coords :pos (v+ (float-vector x y (+ table-height 30)) (float-vector 0 width-sum i)) :rpy #f(0 1.57 0)))
+ (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 1000)
+
+ (ros::spin-once))
+
+ (send *fetch* :inverse-kinematics (make-coords :pos (v+ (float-vector x y (+ table-height 30)) (float-vector 0 width-sum (+ i 20))) :rpy #f(0 1.57 0)))
+ (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000)
+ (send *ri* :wait-interpolation)
+ (try-grasp)
+
+ (send *fetch* :inverse-kinematics (make-coords :pos (float-vector x y (+ table-height 30)) :rpy #f(0 1.57 0)))
+ (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000)))
+
+
+;;try to pick object.
+(defun pick()
+ (send *ri* :angle-vector (send *fetch* :reset-pose))
+ (send *ri* :stop-grasp)
+ (send *ri* :wait-interpolation)
+ (send msg :data "start")
+ (ros::publish "stop_moveit" msg)
+
+
+ (if (search-for-object)
+ (progn
+ (send msg :data "stop")
+ (ros::publish "stop_moveit" msg)
+ ;(send *fetch* :inverse-kinematics (make-coords :pos (float-vector (- x 200) y (+ z 200))) :debug-view t)
+ (send *fetch* :inverse-kinematics (make-coords :pos (v+ (float-vector x y z) #f(-200 0 200)) :debug-view t))
+ (move 10000)
+ (let ((flag nil) (i 0))
+ (while (null flag)
+ ;(send *fetch* :inverse-kinematics (make-coords :pos (float-vector x y (+ z i))))
+ (send *fetch* :inverse-kinematics (make-coords :pos (v+ (float-vector x y z) (float-vector -100 0 i))))
+ (setq flag (move 5000 1))
+ (setq i (+ i 5)))
+ (setq table-height (- i 5))
+ (send *ri* :wait-interpolation)
+
+ (grope-from-front)
+ ;; grope for the target by using proximity sensor(s) on fetch's hand
+ ;; (let ((proximity0 0) (threshold0 13000) (proximity-msg0)
+ ;; (proximity1 0) (threshold1 13000) (proximity-msg1)
+ ;; (j 0) (rend-y) (lend-y))
+ ;; (while (< proximity0 threshold0)
+ ;; (setq proximity-msg0
+ ;; (one-shot-subscribe
+ ;; "/proximity_sensor0"
+ ;; force_proximity_ros::proximity))
+ ;; (setq proximity0 (send proximity-msg0 :average))
+ ;; (send *fetch* :inverse-kinematics (make-coords :pos (v+ (float-vector x y z) (float-vector 0 j 0))))
+ ;; (move 1000)
+ ;; (setq j (+ j 5)))
+ ;; (setq rend-y (+ y j))
+
+ ;; (setq j 0)
+ ;; (while (< proximity1 threshold1)
+ ;; (setq proximity-msg1
+ ;; (one-shot-subscribe
+ ;; "/proximity_sensor1"
+ ;; force_proximity_ros::proximity))
+ ;; (setq proximity1 (send proximity-msg1 :average))
+ ;; (send *fetch* :inverse-kinematics (make-coords :pos (v+ (float-vector x y z) (float-vector 0 j 0))))
+ ;; (move 1000)
+ ;; (setq j (- j 5)))
+ ;; (setq lend-y (+ y j))
+ ;; (send *fetch* :inverse-kinematics (make-coords :pos (float-vector x (/ (+ rend-y lend-y) 2) z)))
+ ;; (move 3000))
+
+ ;(send *fetch* :inverse-kinematics (make-coords :pos (float-vector (- x 200) y (+ z 200))))
+ (send *fetch* :inverse-kinematics (make-coords :pos (v+ (float-vector x y z) #f(-200 0 200))) :debug-view t)
+ (move 1000)
+ ;(send *fetch* :inverse-kinematics (make-coords :pos (float-vector (- x 300) y (+ z 100))))
+ (send *fetch* :inverse-kinematics (make-coords :pos (v+ (float-vector x y z) #f(-300 0 100))) :debug-view t)
+ (move 2000)
+ (send *fetch* :inverse-kinematics (make-coords :pos (float-vector 350 600 (+ z 100))) :debug-view t)
+ (move 5000)
+ (send *fetch* :inverse-kinematics (make-coords :pos (float-vector 350 600 100)) :debug-view t :rotation-axis nil)
+ (move 5000)
+ (send *ri* :stop-grasp)
+ (init))
+ (format t "trial canceled.~%"))))
+
+;; elmer's glue: centroid too deep
+;; fetch mistakes his hand for collision object.
diff --git a/jsk_apc2016_common/samples/sample_fetch_object_segmentation_3d.launch b/jsk_apc2016_common/samples/sample_fetch_object_segmentation_3d.launch
index 105700f1a..083c61787 100644
--- a/jsk_apc2016_common/samples/sample_fetch_object_segmentation_3d.launch
+++ b/jsk_apc2016_common/samples/sample_fetch_object_segmentation_3d.launch
@@ -21,7 +21,7 @@
-
+
+
+
+ args="standalone jsk_pcl/ClusterPointIndicesDecomposer">
@@ -42,6 +44,8 @@
queue_size: 1000
+
+
@@ -54,13 +58,16 @@
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/jsk_arc2017_common/config/label_names.yaml b/jsk_arc2017_common/config/label_names.yaml
new file mode 100644
index 000000000..036def1d1
--- /dev/null
+++ b/jsk_arc2017_common/config/label_names.yaml
@@ -0,0 +1,43 @@
+label_names:
+ - __background__
+ - avery_binder
+ - balloons
+ - band_aid_tape
+ - bath_sponge
+ - black_fashion_gloves
+ - burts_bees_baby_wipes
+ - colgate_toothbrush_4pk
+ - composition_book
+ - crayons
+ - duct_tape
+ - epsom_salts
+ - expo_eraser
+ - fiskars_scissors
+ - flashlight
+ - glue_sticks
+ - hand_weight
+ - hanes_socks
+ - hinged_ruled_index_cards
+ - ice_cube_tray
+ - irish_spring_soap
+ - laugh_out_loud_jokes
+ - marbles
+ - measuring_spoons
+ - mesh_cup
+ - mouse_traps
+ - pie_plates
+ - plastic_wine_glass
+ - poland_spring_water
+ - reynolds_wrap
+ - robots_dvd
+ - robots_everywhere
+ - scotch_sponges
+ - speed_stick
+ - table_cloth
+ - tennis_ball_container
+ - ticonderoga_pencils
+ - tissue_box
+ - toilet_brush
+ - white_facecloth
+ - windex
+ - __shelf__
diff --git a/jsk_arc2017_common/launch/object_segmentation_3d.launch b/jsk_arc2017_common/launch/object_segmentation_3d.launch
new file mode 100644
index 000000000..887bc4867
--- /dev/null
+++ b/jsk_arc2017_common/launch/object_segmentation_3d.launch
@@ -0,0 +1,101 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+ backend: torch
+ gpu: $(arg GPU)
+ model_name: fcn32s_bilinear
+ model_file: $(find jsk_arc2017_common)/data/models/fcn32s_arc2017_dataset_v1_20170417.pth
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ approximate_sync: true
+ queue_size: 100
+ keep_organized: true
+
+
+
+
+
+ step_x: 2
+ step_y: 2
+
+
+
+
+
+ min_size: 10
+ max_size: 10000
+ tolerance: 0.05
+
+
+
+
+
+
+ approximate_sync: false
+ queue_size: 1000
+ align_boxes: true
+ align_boxes_with_plane: false
+ target_frame_id: $(arg FIXED_FRAME)
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+