tf_to_transform.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import argparse
4 from geometry_msgs.msg import Quaternion
5 from geometry_msgs.msg import Transform
6 from geometry_msgs.msg import TransformStamped
7 from geometry_msgs.msg import Vector3
8 import rospy
9 from std_msgs.msg import Header
10 import tf
11 
12 
13 class TFToTransform(object):
14  def __init__(self, parent_frame_id, child_frame_id, duration, rate):
15  self.duration = rospy.get_param('~duration', duration)
16  self.parent_frame_id = rospy.get_param('~parent_frame_id',
17  parent_frame_id)
18  self.child_frame_id = rospy.get_param('~child_frame_id',
19  child_frame_id)
20  rate = rospy.get_param('~rate', rate)
21  self.pub = rospy.Publisher('~output', TransformStamped, queue_size=1)
23  rospy.Timer(rospy.Duration(1. / rate), self._publish_transform)
24 
25  def _publish_transform(self, event):
26  try:
27  now = rospy.Time.now()
28  self.listener.waitForTransform(
29  self.parent_frame_id,
30  self.child_frame_id,
31  now,
32  timeout=rospy.Duration(self.duration)
33  )
34  except Exception:
35  rospy.logerr(
36  "cannot get transform from '%s' to '%s' in '%.2f' [s]" %
37  (self.parent_frame_id, self.child_frame_id, self.duration))
38  return
39 
40  trans, rot = self.listener.lookupTransform(
41  self.parent_frame_id,
42  self.child_frame_id,
43  now
44  )
45  tf_msg = Transform(
46  translation=Vector3(
47  x=trans[0],
48  y=trans[1],
49  z=trans[2]
50  ),
51  rotation=Quaternion(
52  x=rot[0],
53  y=rot[1],
54  z=rot[2],
55  w=rot[3]
56  )
57  )
58  tf_stamped_msg = TransformStamped(
59  header=Header(
60  stamp=now,
61  frame_id=self.parent_frame_id
62  ),
63  child_frame_id=self.child_frame_id,
64  transform=tf_msg
65  )
66  self.pub.publish(tf_stamped_msg)
67 
68 if __name__ == '__main__':
69  parser = argparse.ArgumentParser()
70  parser.add_argument('parent_frame_id', nargs='?',
71  help='parent frame id', default=None)
72  parser.add_argument('child_frame_id', nargs='?',
73  help='child frame id', default=None)
74  parser.add_argument('--duration', '-d', type=float, default=1,
75  help='Duration to resolve tf. default: 1 [s]')
76  parser.add_argument('--rate', '-r', type=float, default=1,
77  help='Rate of publication. default: 1 [hz]')
78  args = parser.parse_args(rospy.myargv()[1:])
79  rospy.init_node('tf_to_transform')
80  tf_pub = TFToTransform(
81  args.parent_frame_id,
82  args.child_frame_id,
83  args.duration,
84  args.rate,
85  )
86  rospy.spin()
def _publish_transform(self, event)
def __init__(self, parent_frame_id, child_frame_id, duration, rate)


jsk_topic_tools
Author(s): Kei Okada , Yusuke Furuta
autogenerated on Tue Feb 6 2018 03:45:19