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
9 from std_msgs.msg
import Header
14 def __init__(self, parent_frame_id, child_frame_id, duration, rate):
15 self.
duration = rospy.get_param(
'~duration', duration)
20 rate = rospy.get_param(
'~rate', rate)
21 self.
pub = rospy.Publisher(
'~output', TransformStamped, queue_size=1)
27 now = rospy.Time.now()
28 self.listener.waitForTransform(
32 timeout=rospy.Duration(self.
duration)
36 "cannot get transform from '%s' to '%s' in '%.2f' [s]" %
40 trans, rot = self.listener.lookupTransform(
58 tf_stamped_msg = TransformStamped(
66 self.pub.publish(tf_stamped_msg)
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')