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
+
+
+
+
+