pose_array_to_pose.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 from __future__ import absolute_import
00004 from __future__ import division
00005 from __future__ import print_function
00006 
00007 import dynamic_reconfigure.server
00008 from geometry_msgs.msg import PoseArray
00009 from geometry_msgs.msg import PoseStamped
00010 from jsk_topic_tools import ConnectionBasedTransport
00011 from jsk_topic_tools.log_utils import logerr_throttle
00012 from jsk_recognition_utils.cfg import PoseArrayToPoseConfig
00013 import rospy
00014 
00015 
00016 class PoseArrayToPose(ConnectionBasedTransport):
00017 
00018     def __init__(self):
00019         super(self.__class__, self).__init__()
00020         self.pub = self.advertise('~output', PoseStamped, queue_size=1)
00021         dynamic_reconfigure.server.Server(PoseArrayToPoseConfig,
00022                                           self._config_callback)
00023 
00024     def subscribe(self):
00025         self.sub = rospy.Subscriber('~input', PoseArray, self._convert)
00026 
00027     def unsubscribe(self):
00028         self.sub.unregister()
00029 
00030     def _config_callback(self, config, level):
00031         self.index = config.index
00032         return config
00033 
00034     def _convert(self, msg):
00035         pose_msg = PoseStamped()
00036         pose_msg.header = msg.header
00037         if self.index < 0:
00038             return
00039         elif self.index < len(msg.poses):
00040             pose_msg.pose = msg.poses[self.index]
00041             self.pub.publish(pose_msg)
00042         else:
00043             logerr_throttle(10,
00044                             'Invalid index {} is specified '
00045                             'for pose array whose size is {}'
00046                             .format(self.index, len(msg.poses)))
00047 
00048 
00049 if __name__ == '__main__':
00050     rospy.init_node('pose_array_to_pose')
00051     PoseArrayToPose()
00052     rospy.spin()


jsk_recognition_utils
Author(s):
autogenerated on Sun Oct 8 2017 02:42:48