people_pose_array_to_pose_array.py

Go to the documentation of this file.

#!/usr/bin/env python
 
from geometry_msgs.msg import PoseArray
from jsk_topic_tools import ConnectionBasedTransport
import rospy
 
from jsk_recognition_msgs.msg import PeoplePoseArray
 
 
class PeoplePoseArrayToPoseArray(ConnectionBasedTransport):
 
    def __init__(self):
        super(PeoplePoseArrayToPoseArray, self).__init__()
        self.pub = self.advertise('~output', PoseArray, queue_size=1)
 
    def subscribe(self):
        self.sub = rospy.Subscriber('~input', PeoplePoseArray, self._cb)
 
    def unsubscribe(self):
        self.sub.unregister()
 
    def _cb(self, msg):
        out_msg = PoseArray()
        for person_pose in msg.poses:
            out_msg.poses.extend(person_pose.poses)
        out_msg.header = msg.header
        self.pub.publish(out_msg)
 
 
if __name__ == '__main__':
    rospy.init_node('people_pose_array_to_pose_array')
    app = PeoplePoseArrayToPoseArray()
    rospy.spin()