From 44fd84bd5a4c42230755c1ef6c7ecd3422487ef9 Mon Sep 17 00:00:00 2001 From: MiyabiTane Date: Wed, 18 Nov 2020 00:51:09 +0900 Subject: [PATCH 01/16] add node that convert 3D coordinate to 2D coordinate --- .../nodes/xyz_to_screenpoint.md | 35 +++++++++++++++ .../sample/sample_xyz_to_screenpoint.launch | 21 +++++++++ .../scripts/xyz_to_screenpoint.py | 45 +++++++++++++++++++ .../test/xyz_to_screenpoint.test | 16 +++++++ 4 files changed, 117 insertions(+) create mode 100644 doc/jsk_pcl_ros_utils/nodes/xyz_to_screenpoint.md create mode 100644 jsk_pcl_ros_utils/sample/sample_xyz_to_screenpoint.launch create mode 100755 jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py create mode 100644 jsk_pcl_ros_utils/test/xyz_to_screenpoint.test 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..6028dae36a --- /dev/null +++ b/doc/jsk_pcl_ros_utils/nodes/xyz_to_screenpoint.md @@ -0,0 +1,35 @@ +# 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. The header frame_id is ignored. + +* `~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 and the header frame_id is ignored. + + +## Parameters + +None. + + +## Sample + +```bash +roslaunch jsk_pcl_ros_utils sample_xyz_to_screenpoint.launch +``` 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..e0cecdd046 --- /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..825025588c --- /dev/null +++ b/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py @@ -0,0 +1,45 @@ +#!/usr/bin/env python + +import rospy + +from sensor_msgs.msg import CameraInfo +from geometry_msgs.msg import PointStamped + +from image_geometry import PinholeCameraModel + +class XYZToScreenPoint: + def __init__(self): + self.cameramodels = PinholeCameraModel() + self.is_camera_arrived = False + + def fromCameraInfo_cb(self, msg): + self.cameramodels.fromCameraInfo(msg) + self.is_camera_arrived = True + + def project3dToPixel_cb(self, msg): + if not self.is_camera_arrived: + return + point = (msg.point.x, msg.point.y, msg.point.z) + x, y = self.cameramodels.project3dToPixel(point) + rospy.logdebug("x, y : %lf, %lf", x, y) + # publish info + pub = rospy.Publisher("~output", PointStamped, queue_size=1) + pub_msg = PointStamped() + pub_msg.header = msg.header + pub_msg.point.x = x + pub_msg.point.y = y + pub_msg.point.z = 0 + pub.publish(pub_msg) + + def subscribeCameraInfo(self): + rospy.Subscriber('~input/camera_info', CameraInfo, self.fromCameraInfo_cb) + rospy.Subscriber('~input', PointStamped, self.project3dToPixel_cb) + + +if __name__ == '__main__': + rospy.init_node("xyz_to_screenpoint") + xyz_to_screenpoint = XYZToScreenPoint() + xyz_to_screenpoint.subscribeCameraInfo() + 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..b51f94fc69 --- /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 + + + + + From b95a4ea90158bf5d23b957c395cbbd21b9fbce98 Mon Sep 17 00:00:00 2001 From: Miyabi Tanemoto <52348980+MiyabiTane@users.noreply.github.com> Date: Wed, 18 Nov 2020 12:12:47 +0900 Subject: [PATCH 02/16] Update jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py Co-authored-by: Shingo Kitagawa --- jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py b/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py index 825025588c..50e36e1b68 100755 --- a/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py +++ b/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py @@ -20,8 +20,8 @@ def project3dToPixel_cb(self, msg): if not self.is_camera_arrived: return point = (msg.point.x, msg.point.y, msg.point.z) - x, y = self.cameramodels.project3dToPixel(point) - rospy.logdebug("x, y : %lf, %lf", x, y) + u, v = self.cameramodels.project3dToPixel(point) + rospy.logdebug("u, v : {}, {}".format(u, v)) # publish info pub = rospy.Publisher("~output", PointStamped, queue_size=1) pub_msg = PointStamped() @@ -42,4 +42,3 @@ def subscribeCameraInfo(self): xyz_to_screenpoint.subscribeCameraInfo() rospy.spin() - From 788e3911f4f19f58dc21689c433c0f3390fc98ef Mon Sep 17 00:00:00 2001 From: Miyabi Tanemoto <52348980+MiyabiTane@users.noreply.github.com> Date: Wed, 18 Nov 2020 12:13:03 +0900 Subject: [PATCH 03/16] Update jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py Co-authored-by: Shingo Kitagawa --- jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py b/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py index 50e36e1b68..6f746811b6 100755 --- a/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py +++ b/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py @@ -26,8 +26,8 @@ def project3dToPixel_cb(self, msg): pub = rospy.Publisher("~output", PointStamped, queue_size=1) pub_msg = PointStamped() pub_msg.header = msg.header - pub_msg.point.x = x - pub_msg.point.y = y + pub_msg.point.x = u + pub_msg.point.y = v pub_msg.point.z = 0 pub.publish(pub_msg) @@ -41,4 +41,3 @@ def subscribeCameraInfo(self): xyz_to_screenpoint = XYZToScreenPoint() xyz_to_screenpoint.subscribeCameraInfo() rospy.spin() - From 544c30d0dee1b8e116ed371d3b780c6d4c5496ca Mon Sep 17 00:00:00 2001 From: Miyabi Tanemoto <52348980+MiyabiTane@users.noreply.github.com> Date: Wed, 18 Nov 2020 12:13:20 +0900 Subject: [PATCH 04/16] Update jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py Co-authored-by: Shingo Kitagawa --- jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py b/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py index 6f746811b6..010c797290 100755 --- a/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py +++ b/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py @@ -16,7 +16,7 @@ def fromCameraInfo_cb(self, msg): self.cameramodels.fromCameraInfo(msg) self.is_camera_arrived = True - def project3dToPixel_cb(self, msg): + def point_stamped_cb(self, msg): if not self.is_camera_arrived: return point = (msg.point.x, msg.point.y, msg.point.z) From 88d36a083856e12f3e4743f61489762efbb86c14 Mon Sep 17 00:00:00 2001 From: Miyabi Tanemoto <52348980+MiyabiTane@users.noreply.github.com> Date: Wed, 18 Nov 2020 12:13:28 +0900 Subject: [PATCH 05/16] Update jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py Co-authored-by: Shingo Kitagawa --- jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py b/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py index 010c797290..18995587ac 100755 --- a/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py +++ b/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py @@ -12,7 +12,7 @@ def __init__(self): self.cameramodels = PinholeCameraModel() self.is_camera_arrived = False - def fromCameraInfo_cb(self, msg): + def camera_info_cb(self, msg): self.cameramodels.fromCameraInfo(msg) self.is_camera_arrived = True From 6fd50f68e742575c926056453d5f6d3c0844790a Mon Sep 17 00:00:00 2001 From: Miyabi Tanemoto <52348980+MiyabiTane@users.noreply.github.com> Date: Wed, 18 Nov 2020 12:13:35 +0900 Subject: [PATCH 06/16] Update jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py Co-authored-by: Shingo Kitagawa --- jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py b/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py index 18995587ac..13ac70d6b7 100755 --- a/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py +++ b/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py @@ -7,7 +7,7 @@ from image_geometry import PinholeCameraModel -class XYZToScreenPoint: +class XYZToScreenPoint(object): def __init__(self): self.cameramodels = PinholeCameraModel() self.is_camera_arrived = False From c5b6ce731a3e4487df2153625b01c1636aef998a Mon Sep 17 00:00:00 2001 From: Miyabi Tanemoto <52348980+MiyabiTane@users.noreply.github.com> Date: Wed, 18 Nov 2020 12:13:47 +0900 Subject: [PATCH 07/16] Update jsk_pcl_ros_utils/sample/sample_xyz_to_screenpoint.launch Co-authored-by: Shingo Kitagawa --- jsk_pcl_ros_utils/sample/sample_xyz_to_screenpoint.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_pcl_ros_utils/sample/sample_xyz_to_screenpoint.launch b/jsk_pcl_ros_utils/sample/sample_xyz_to_screenpoint.launch index e0cecdd046..3bc0ab94b1 100644 --- a/jsk_pcl_ros_utils/sample/sample_xyz_to_screenpoint.launch +++ b/jsk_pcl_ros_utils/sample/sample_xyz_to_screenpoint.launch @@ -13,7 +13,7 @@ '{header: {stamp: 'now', frame_id: camera}, point: {x: 0, y: 0, z: 1}}'"/> + pkg="jsk_pcl_ros_utils" type="xyz_to_screenpoint.py"> From 5f94a4bfad8b81ddaf2c4ae197b01ee072996697 Mon Sep 17 00:00:00 2001 From: Miyabi Tanemoto <52348980+MiyabiTane@users.noreply.github.com> Date: Wed, 18 Nov 2020 12:14:03 +0900 Subject: [PATCH 08/16] Update jsk_pcl_ros_utils/test/xyz_to_screenpoint.test Co-authored-by: Shingo Kitagawa --- jsk_pcl_ros_utils/test/xyz_to_screenpoint.test | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/jsk_pcl_ros_utils/test/xyz_to_screenpoint.test b/jsk_pcl_ros_utils/test/xyz_to_screenpoint.test index b51f94fc69..1d656a623c 100644 --- a/jsk_pcl_ros_utils/test/xyz_to_screenpoint.test +++ b/jsk_pcl_ros_utils/test/xyz_to_screenpoint.test @@ -3,9 +3,9 @@ + name="test_xyz_to_screenpoint" + pkg="jsk_tools" type="test_topic_published.py" + retry="3"> topic_0: /xyz_to_screenpoint/output timeout_0: 30 From adf7c9af418288df6832d16a5b5c695280c2cd6c Mon Sep 17 00:00:00 2001 From: MiyabiTane Date: Wed, 18 Nov 2020 12:33:11 +0900 Subject: [PATCH 09/16] move pub and sub to init --- .../scripts/xyz_to_screenpoint.py | 22 +++++++++++-------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py b/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py index 13ac70d6b7..cba6bd616c 100755 --- a/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py +++ b/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py @@ -11,10 +11,18 @@ class XYZToScreenPoint(object): def __init__(self): self.cameramodels = PinholeCameraModel() self.is_camera_arrived = False + self.header = None + + # publish info + 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.is_camera_arrived = True + self.header = msg.header def point_stamped_cb(self, msg): if not self.is_camera_arrived: @@ -22,22 +30,18 @@ def point_stamped_cb(self, msg): point = (msg.point.x, msg.point.y, msg.point.z) u, v = self.cameramodels.project3dToPixel(point) rospy.logdebug("u, v : {}, {}".format(u, v)) - # publish info - pub = rospy.Publisher("~output", PointStamped, queue_size=1) pub_msg = PointStamped() - pub_msg.header = msg.header + pub_msg.header = self.header pub_msg.point.x = u pub_msg.point.y = v pub_msg.point.z = 0 - pub.publish(pub_msg) - - def subscribeCameraInfo(self): - rospy.Subscriber('~input/camera_info', CameraInfo, self.fromCameraInfo_cb) - rospy.Subscriber('~input', PointStamped, self.project3dToPixel_cb) + self.pub.publish(pub_msg) + # def subscribeCameraInfo(self): + if __name__ == '__main__': rospy.init_node("xyz_to_screenpoint") xyz_to_screenpoint = XYZToScreenPoint() - xyz_to_screenpoint.subscribeCameraInfo() + # xyz_to_screenpoint.subscribeCameraInfo() rospy.spin() From cf9149827d3e5d2d026acb9dcba039c1d289a148 Mon Sep 17 00:00:00 2001 From: Miyabi Tanemoto <52348980+MiyabiTane@users.noreply.github.com> Date: Wed, 18 Nov 2020 21:30:09 +0900 Subject: [PATCH 10/16] Update jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py Co-authored-by: Shingo Kitagawa --- jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py b/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py index cba6bd616c..83abecfc09 100755 --- a/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py +++ b/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py @@ -22,7 +22,7 @@ def __init__(self): def camera_info_cb(self, msg): self.cameramodels.fromCameraInfo(msg) self.is_camera_arrived = True - self.header = msg.header + self.frame_id = msg.header.frame_id def point_stamped_cb(self, msg): if not self.is_camera_arrived: From 39d88f0a5c7321b90b7371774e67f9ccab94f3a3 Mon Sep 17 00:00:00 2001 From: Miyabi Tanemoto <52348980+MiyabiTane@users.noreply.github.com> Date: Wed, 18 Nov 2020 21:31:16 +0900 Subject: [PATCH 11/16] Update jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py Co-authored-by: Shingo Kitagawa --- jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py b/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py index 83abecfc09..57f056b671 100755 --- a/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py +++ b/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py @@ -31,7 +31,8 @@ def point_stamped_cb(self, msg): u, v = self.cameramodels.project3dToPixel(point) rospy.logdebug("u, v : {}, {}".format(u, v)) pub_msg = PointStamped() - pub_msg.header = self.header + 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 From d922bf8b77cd0bf068197a78a6817926c040debc Mon Sep 17 00:00:00 2001 From: MiyabiTane Date: Wed, 18 Nov 2020 21:44:19 +0900 Subject: [PATCH 12/16] modify --- jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py b/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py index 57f056b671..4410a294c7 100755 --- a/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py +++ b/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py @@ -4,16 +4,15 @@ from sensor_msgs.msg import CameraInfo from geometry_msgs.msg import PointStamped - from image_geometry import PinholeCameraModel + class XYZToScreenPoint(object): def __init__(self): self.cameramodels = PinholeCameraModel() self.is_camera_arrived = False - self.header = None + self.frame_id = None - # publish info self.pub = rospy.Publisher("~output", PointStamped, queue_size=1) rospy.Subscriber('~input/camera_info', CameraInfo, self.camera_info_cb) @@ -21,8 +20,8 @@ def __init__(self): def camera_info_cb(self, msg): self.cameramodels.fromCameraInfo(msg) - self.is_camera_arrived = True self.frame_id = msg.header.frame_id + self.is_camera_arrived = True def point_stamped_cb(self, msg): if not self.is_camera_arrived: @@ -38,11 +37,8 @@ def point_stamped_cb(self, msg): pub_msg.point.z = 0 self.pub.publish(pub_msg) - # def subscribeCameraInfo(self): - if __name__ == '__main__': rospy.init_node("xyz_to_screenpoint") xyz_to_screenpoint = XYZToScreenPoint() - # xyz_to_screenpoint.subscribeCameraInfo() rospy.spin() From c8217ee5fa102a3a9c6820dcd8ca3af1a904719c Mon Sep 17 00:00:00 2001 From: MiyabiTane Date: Thu, 19 Nov 2020 14:54:03 +0900 Subject: [PATCH 13/16] lookup transform frame_id --- .../sample/sample_xyz_to_screenpoint.launch | 2 +- .../scripts/xyz_to_screenpoint.py | 19 +++++++++++++++++-- 2 files changed, 18 insertions(+), 3 deletions(-) diff --git a/jsk_pcl_ros_utils/sample/sample_xyz_to_screenpoint.launch b/jsk_pcl_ros_utils/sample/sample_xyz_to_screenpoint.launch index 3bc0ab94b1..2e1e8d3954 100644 --- a/jsk_pcl_ros_utils/sample/sample_xyz_to_screenpoint.launch +++ b/jsk_pcl_ros_utils/sample/sample_xyz_to_screenpoint.launch @@ -10,7 +10,7 @@ + '{header: {stamp: 'now', frame_id: 'camera_link'}, point: {x: 500, y: 100, z: 100}}'"/> diff --git a/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py b/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py index 4410a294c7..a3bc22a194 100755 --- a/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py +++ b/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py @@ -1,9 +1,12 @@ #!/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 @@ -12,6 +15,8 @@ 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) @@ -26,8 +31,18 @@ def camera_info_cb(self, msg): def point_stamped_cb(self, msg): if not self.is_camera_arrived: return - point = (msg.point.x, msg.point.y, msg.point.z) - u, v = self.cameramodels.project3dToPixel(point) + 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: + print(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 From e152f920e8c315cb9e46681dfc4cdbf40d91183d Mon Sep 17 00:00:00 2001 From: MiyabiTane Date: Thu, 19 Nov 2020 15:04:56 +0900 Subject: [PATCH 14/16] update doc --- doc/jsk_pcl_ros_utils/nodes/xyz_to_screenpoint.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/doc/jsk_pcl_ros_utils/nodes/xyz_to_screenpoint.md b/doc/jsk_pcl_ros_utils/nodes/xyz_to_screenpoint.md index 6028dae36a..d5bd2c3dbd 100644 --- a/doc/jsk_pcl_ros_utils/nodes/xyz_to_screenpoint.md +++ b/doc/jsk_pcl_ros_utils/nodes/xyz_to_screenpoint.md @@ -9,7 +9,7 @@ Convert (x, y, z) 3-D coordinate to (u, v) coordinate on a image using camerainf * `~input` (`geometry_msgs/PointStamped`) - Input point to represent (x, y, z) 3-D coordinate. The header frame_id is ignored. + Input point to represent (x, y, z) 3-D coordinate. * `~input/camera_info` (`sensor_msgs/CameraInfo`) @@ -20,7 +20,8 @@ Convert (x, y, z) 3-D coordinate to (u, v) coordinate on a image using camerainf * `~output` (`geometry_msgs/PointStamped`) - Output point to represent (u, v) image coordinate. Only x and y fileds are used and the header frame_id is ignored. + 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 From 92919663292f025c51f93f1c6582ea232c344425 Mon Sep 17 00:00:00 2001 From: Miyabi Tanemoto <52348980+MiyabiTane@users.noreply.github.com> Date: Thu, 19 Nov 2020 23:48:38 +0900 Subject: [PATCH 15/16] Update jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py Co-authored-by: Shingo Kitagawa --- jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py b/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py index a3bc22a194..bf24ea4be8 100755 --- a/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py +++ b/jsk_pcl_ros_utils/scripts/xyz_to_screenpoint.py @@ -38,7 +38,7 @@ def point_stamped_cb(self, msg): 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: - print(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) From 6d68589efa93283772ab522ec0af6cab456da298 Mon Sep 17 00:00:00 2001 From: MiyabiTane Date: Wed, 2 Dec 2020 00:37:08 +0900 Subject: [PATCH 16/16] add xyz_to_screenpoint.test --- jsk_pcl_ros_utils/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/jsk_pcl_ros_utils/CMakeLists.txt b/jsk_pcl_ros_utils/CMakeLists.txt index 6fd6146d1e..3f584fdb23 100644 --- a/jsk_pcl_ros_utils/CMakeLists.txt +++ b/jsk_pcl_ros_utils/CMakeLists.txt @@ -316,6 +316,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()