Merge pull request #2549 from MiyabiTane/new_node

doc/jsk_pcl_ros_utils/nodes/xyz_to_screenpoint.md

@@ -0,0 +1,36 @@
+# xyz_to_screenpoint.py
+
+## What Is This
+
+Convert (x, y, z) 3-D coordinate to (u, v) coordinate on a image using camerainfo of sensor.
+
+
+## Subscribing Topic
+
+* `~input` (`geometry_msgs/PointStamped`)
+
+  Input point to represent (x, y, z) 3-D coordinate.
+
+* `~input/camera_info` (`sensor_msgs/CameraInfo`)
+
+  CameraInfo of sensor.
+
+
+## Publishing Topic
+
+* `~output` (`geometry_msgs/PointStamped`)
+
+  Output point to represent (u, v) image coordinate. Only x and y fileds are used.
+  The header frame_id uses the information of `~input/camera_info`.
+
+
+## Parameters
+
+None.
+
+
+## Sample
+
+```bash
+roslaunch jsk_pcl_ros_utils sample_xyz_to_screenpoint.launch
+```

jsk_pcl_ros_utils/CMakeLists.txt

@@ -320,6 +320,7 @@ if (CATKIN_ENABLE_TESTING)
     add_rostest(test/tf_transform_cloud.test)
     add_rostest(test/transform_pointcloud_in_bounding_box.test)
     add_rostest(test/centroid_publisher.test)
+    add_rostest(test/xyz_to_screenpoint.test)
     roslaunch_add_file_check(test/subtract_point_indices.test)
   endif()
 endif()

jsk_pcl_ros_utils/sample/sample_xyz_to_screenpoint.launch

@@ -0,0 +1,21 @@
+<launch>
+
+  <param name="/use_sim_time" value="true" />
+
+  <node name="rosbag_play"
+        pkg="rosbag" type="play"
+        args="$(find jsk_pcl_ros_utils)/sample/data/arc2017_4objects.bag --clock --loop">
+  </node>
+
+  <node name="reference_point_publisher"
+        pkg="rostopic" type="rostopic"
+        args="pub -r 1 -s reference_point_publisher/output geometry_msgs/PointStamped
+              '{header: {stamp: 'now', frame_id: 'camera_link'}, point: {x: 500, y: 100, z: 100}}'"/>
+
+  <node name="xyz_to_screenpoint"
+        pkg="jsk_pcl_ros_utils" type="xyz_to_screenpoint.py">
+    <remap from="~input/camera_info" to="camera/rgb/camera_info" />
+    <remap from="~input" to="reference_point_publisher/output" />
+  </node>
+
+</launch>

jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py

@@ -0,0 +1,59 @@
+#!/usr/bin/env python
+
+import rospy
+import tf2_ros
+import tf2_geometry_msgs
+
+from sensor_msgs.msg import CameraInfo
+from geometry_msgs.msg import PointStamped
+from geometry_msgs.msg import PoseStamped
+from image_geometry import PinholeCameraModel
+
+
+class XYZToScreenPoint(object):
+    def __init__(self):
+        self.cameramodels = PinholeCameraModel()
+        self.is_camera_arrived = False
+        self.frame_id = None
+        self.tf_buffer = tf2_ros.Buffer(rospy.Duration(1200.0))
+        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
+
+        self.pub = rospy.Publisher("~output", PointStamped, queue_size=1)
+
+        rospy.Subscriber('~input/camera_info', CameraInfo, self.camera_info_cb)
+        rospy.Subscriber('~input', PointStamped, self.point_stamped_cb)
+
+    def camera_info_cb(self, msg):
+        self.cameramodels.fromCameraInfo(msg)
+        self.frame_id = msg.header.frame_id
+        self.is_camera_arrived = True
+
+    def point_stamped_cb(self, msg):
+        if not self.is_camera_arrived:
+            return
+        pose_stamped = PoseStamped()
+        pose_stamped.pose.position.x = msg.point.x
+        pose_stamped.pose.position.y = msg.point.y
+        pose_stamped.pose.position.z = msg.point.z
+        try:
+            transform = self.tf_buffer.lookup_transform(self.frame_id, msg.header.frame_id, rospy.Time(0), rospy.Duration(1.0))
+        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
+            rospy.logerr('lookup_transform failed: {}'.format(e))
+            return
+        position_transformed = tf2_geometry_msgs.do_transform_pose(pose_stamped, transform).pose.position
+        pub_point = (position_transformed.x, position_transformed.y, position_transformed.z)
+        u, v = self.cameramodels.project3dToPixel(pub_point)
+        rospy.logdebug("u, v : {}, {}".format(u, v))
+        pub_msg = PointStamped()
+        pub_msg.header = msg.header
+        pub_msg.header.frame_id = self.frame_id
+        pub_msg.point.x = u
+        pub_msg.point.y = v
+        pub_msg.point.z = 0
+        self.pub.publish(pub_msg)
+
+
+if __name__ == '__main__':
+    rospy.init_node("xyz_to_screenpoint")
+    xyz_to_screenpoint = XYZToScreenPoint()
+    rospy.spin()

jsk_pcl_ros_utils/test/xyz_to_screenpoint.test

@@ -0,0 +1,16 @@
+<launch>
+  <include file="$(find jsk_pcl_ros_utils)/sample/sample_xyz_to_screenpoint.launch">
+  </include>
+
+  <test test-name="test_xyz_to_screenpoint"
+        name="test_xyz_to_screenpoint"
+        pkg="jsk_tools" type="test_topic_published.py"
+        retry="3">
+    <rosparam>
+      topic_0: /xyz_to_screenpoint/output
+      timeout_0: 30
+    </rosparam>
+  </test>
+
+</launch>
+