transform_to_tf.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 import rospy
3 import tf
4 from geometry_msgs.msg import TransformStamped
5 
6 def callback(transform):
7  global br
8  global frame_id
9  global child_frame_id
10  local_frame_id = transform.header.frame_id
11  local_child_frame_id = transform.child_frame_id
12  if not local_frame_id:
13  local_frame_id = frame_id
14  if not local_child_frame_id:
15  local_child_frame_id = child_frame_id
16  br.sendTransform(
17  (transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z),
18  (transform.transform.rotation.x, transform.transform.rotation.y, transform.transform.rotation.z, transform.transform.rotation.w),
19  transform.header.stamp,
20  child_frame_id,
21  frame_id)
22 
23 if __name__ == "__main__":
24 
25  rospy.init_node("transform_to_tf", anonymous=True)
26 
27  frame_id = rospy.get_param('~frame_id', 'world')
28  child_frame_id = rospy.get_param('~child_frame_id', 'transform')
29 
31  rospy.Subscriber("transform", TransformStamped, callback, queue_size=1)
32  rospy.spin()


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Fri Jun 7 2019 21:55:04