jsk_recognition_msgs: people_pose_array_to_pose

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 from geometry_msgs.msg import PoseArray
00004 from jsk_topic_tools import ConnectionBasedTransport
00005 import rospy
00006 
00007 from jsk_recognition_msgs.msg import PeoplePoseArray
00008 
00009 
00010 class PeoplePoseArrayToPoseArray(ConnectionBasedTransport):
00011 
00012     def __init__(self):
00013         super(PeoplePoseArrayToPoseArray, self).__init__()
00014         self.pub = self.advertise('~output', PoseArray, queue_size=1)
00015 
00016     def subscribe(self):
00017         self.sub = rospy.Subscriber('~input', PeoplePoseArray, self._cb)
00018 
00019     def unsubscribe(self):
00020         self.sub.unregister()
00021 
00022     def _cb(self, msg):
00023         out_msg = PoseArray()
00024         for person_pose in msg.poses:
00025             out_msg.poses.extend(person_pose.poses)
00026         out_msg.header = msg.header
00027         self.pub.publish(out_msg)
00028 
00029 
00030 if __name__ == '__main__':
00031     rospy.init_node('people_pose_array_to_pose_array')
00032     app = PeoplePoseArrayToPoseArray()
00033     rospy.spin()