diff --git a/doc/jsk_pcl_ros_utils/nodes/xyz_to_screenpoint.md b/doc/jsk_pcl_ros_utils/nodes/xyz_to_screenpoint.md new file mode 100644 index 0000000000..d5bd2c3dbd --- /dev/null +++ b/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 +``` diff --git a/jsk_pcl_ros_utils/CMakeLists.txt b/jsk_pcl_ros_utils/CMakeLists.txt index 93b93654cc..231dda1b06 100644 --- a/jsk_pcl_ros_utils/CMakeLists.txt +++ b/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() diff --git a/jsk_pcl_ros_utils/sample/sample_xyz_to_screenpoint.launch b/jsk_pcl_ros_utils/sample/sample_xyz_to_screenpoint.launch new file mode 100644 index 0000000000..2e1e8d3954 --- /dev/null +++ b/jsk_pcl_ros_utils/sample/sample_xyz_to_screenpoint.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + diff --git a/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py b/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py new file mode 100755 index 0000000000..bf24ea4be8 --- /dev/null +++ b/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() diff --git a/jsk_pcl_ros_utils/test/xyz_to_screenpoint.test b/jsk_pcl_ros_utils/test/xyz_to_screenpoint.test new file mode 100644 index 0000000000..1d656a623c --- /dev/null +++ b/jsk_pcl_ros_utils/test/xyz_to_screenpoint.test @@ -0,0 +1,16 @@ + + + + + + + topic_0: /xyz_to_screenpoint/output + timeout_0: 30 + + + + +