jsk_pcl_ros: publish_clicked_point

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 import tf
00005 
00006 from jsk_recognition_msgs.msg import *
00007 from geometry_msgs.msg import *
00008 
00009 def clicked_point_cb(msg):
00010     listener.waitForTransform('odom', msg.header.frame_id, msg.header.stamp, rospy.Duration(1.0))
00011     p_conv = listener.transformPoint('odom', msg)
00012     bbox_array = BoundingBoxArray()
00013     bbox_array.header = p_conv.header
00014     bbox = BoundingBox()
00015     bbox.header = p_conv.header
00016     bbox.pose = Pose(position=p_conv.point)
00017     bbox.dimensions.x = 1
00018     bbox.dimensions.y = 1
00019     bbox.dimensions.z = 1
00020     bbox_array.boxes.append(bbox)
00021     bbox_pub.publish(bbox_array)
00022 
00023 rospy.init_node('clicked_bbox')
00024 listener = tf.TransformListener()
00025 point_sub = rospy.Subscriber('clicked_point', PointStamped, clicked_point_cb)
00026 bbox_pub = rospy.Publisher('bbox_with_clicked_point', BoundingBoxArray)
00027 rospy.spin()