pose_array_to_pose.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 from __future__ import absolute_import
4 from __future__ import division
5 from __future__ import print_function
6 
7 import dynamic_reconfigure.server
8 from geometry_msgs.msg import PoseArray
9 from geometry_msgs.msg import PoseStamped
10 from jsk_topic_tools import ConnectionBasedTransport
11 from jsk_topic_tools.log_utils import logerr_throttle
12 from jsk_recognition_utils.cfg import PoseArrayToPoseConfig
13 import rospy
14 
15 
16 class PoseArrayToPose(ConnectionBasedTransport):
17 
18  def __init__(self):
19  super(self.__class__, self).__init__()
20  self.pub = self.advertise('~output', PoseStamped, queue_size=1)
21  dynamic_reconfigure.server.Server(PoseArrayToPoseConfig,
22  self._config_callback)
23 
24  def subscribe(self):
25  self.sub = rospy.Subscriber('~input', PoseArray, self._convert)
26 
27  def unsubscribe(self):
28  self.sub.unregister()
29 
30  def _config_callback(self, config, level):
31  self.index = config.index
32  return config
33 
34  def _convert(self, msg):
35  pose_msg = PoseStamped()
36  pose_msg.header = msg.header
37  if self.index < 0:
38  return
39  elif self.index < len(msg.poses):
40  pose_msg.pose = msg.poses[self.index]
41  self.pub.publish(pose_msg)
42  else:
43  logerr_throttle(10,
44  'Invalid index {} is specified '
45  'for pose array whose size is {}'
46  .format(self.index, len(msg.poses)))
47 
48 
49 if __name__ == '__main__':
50  rospy.init_node('pose_array_to_pose')
52  rospy.spin()
def _config_callback(self, config, level)


jsk_recognition_utils
Author(s):
autogenerated on Wed May 27 2020 03:59:48