mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 00:37:46 +08:00
44 lines
1.4 KiB
Python
Executable File
44 lines
1.4 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
import rclpy
|
|
from rclpy.node import Node
|
|
from rclpy.qos import DurabilityPolicy
|
|
from rclpy.qos import HistoryPolicy
|
|
from rclpy.qos import QoSProfile
|
|
from tf2_msgs.msg import TFMessage
|
|
|
|
class StaticTransformRepublisher(Node):
|
|
|
|
def __init__(self):
|
|
super().__init__('static_transform_republisher')
|
|
qos = QoSProfile(
|
|
depth=1,
|
|
durability=DurabilityPolicy.TRANSIENT_LOCAL,
|
|
history=HistoryPolicy.KEEP_LAST,
|
|
)
|
|
self.publisher_ = self.create_publisher(TFMessage, '/tf_static', qos)
|
|
self.data = TFMessage()
|
|
self.subscription = self.create_subscription(
|
|
TFMessage,
|
|
'/tf_static_old',
|
|
self.listener_callback,
|
|
10)
|
|
self.subscription # prevent unused variable warning
|
|
|
|
def listener_callback(self, msg):
|
|
if len(self.data.transforms) == 0:
|
|
self.data = msg
|
|
else:
|
|
self.data.transforms = self.data.transforms + msg.transforms
|
|
self.get_logger().info('"Received /tf_static_old and republising latched /tf_static"')
|
|
self.publisher_.publish(self.data)
|
|
|
|
def main(args=None):
|
|
rclpy.init(args=args)
|
|
static_transform_republisher = StaticTransformRepublisher()
|
|
rclpy.spin(static_transform_republisher)
|
|
minimal_publisher.destroy_node()
|
|
rclpy.shutdown()
|
|
|
|
if __name__ == '__main__':
|
|
main()
|