mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-09-13 06:40:19 +08:00
45 lines
1.3 KiB
Python
Executable File
45 lines
1.3 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
import rclpy
|
|
from rclpy.node import Node
|
|
from geometry_msgs.msg import TransformStamped
|
|
from tf2_ros import TransformBroadcaster
|
|
|
|
class TransformToTf(Node):
|
|
|
|
def __init__(self):
|
|
super().__init__('transform_to_tf')
|
|
|
|
self.declare_parameter('frame_id', 'world')
|
|
self.declare_parameter('child_frame_id', 'transform')
|
|
self.frame_id = self.get_parameter('frame_id').get_parameter_value().string_value
|
|
self.child_frame_id = self.get_parameter('child_frame_id').get_parameter_value().string_value
|
|
|
|
self.tf_broadcaster = TransformBroadcaster(self)
|
|
|
|
self.subscription = self.create_subscription(
|
|
TransformStamped,
|
|
'transform',
|
|
self.callback,
|
|
1)
|
|
self.subscription # prevent unused variable warning
|
|
|
|
def callback(self, t):
|
|
if not t.header.frame_id:
|
|
t.header.frame_id = self.frame_id
|
|
if not t.child_frame_id:
|
|
t.child_frame_id = self.child_frame_id
|
|
|
|
self.tf_broadcaster.sendTransform(t)
|
|
|
|
|
|
def main(args=None):
|
|
rclpy.init(args=args)
|
|
transform_to_tf = TransformToTf()
|
|
rclpy.spin(transform_to_tf)
|
|
transform_to_tf.destroy_node()
|
|
rclpy.shutdown()
|
|
|
|
if __name__ == "__main__":
|
|
main()
|
|
|