[WIP] test program for 708yamaguchi 2017 spring

jsk_apc2016_common/launch/object_segmentation_3d.launch

@@ -8,12 +8,12 @@
   <rosparam command="load" file="$(find jsk_apc2016_common)/config/label_names.yaml" />
 
   <node name="fcn_object_segmentation"
-        pkg="jsk_perception" type="fcn_object_segmentation.py">
+        pkg="jsk_perception" type="fcn_object_segmentation.py" output="screen">
     <remap from="~input" to="$(arg INPUT_IMAGE)" />  <!-- rgb timestamp -->
     <rosparam subst_value="true">
       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
     </rosparam>
     <remap from="~target_names" to="label_names" />
   </node>

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()

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()

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()

jsk_apc2016_common/package.xml

@@ -45,6 +45,7 @@
   <run_depend>tf2_ros</run_depend>
   <run_depend>tf2_eigen</run_depend>
   <run_depend>pcl_ros</run_depend>
+  <run_depend>moveit_commander</run_depend>
 
   <test_depend>roslint</test_depend>
 

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

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

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