ros2 topic时间同步 代码

下面是使用时间同步节点的ROS2代码示例：

创建一个时间同步节点：

import rclpy
from rclpy.node import Node
from rclpy.time import Time
from std_msgs.msg import Time as TimeMsg

class TimeSyncNode(Node):
    def __init__(self):
        super().__init__('time_sync_node')
        self.publisher = self.create_publisher(TimeMsg, 'sync_time', 10)
        self.timer = self.create_timer(1, self.publish_time)
        self.subscription = self.create_subscription(
            TimeMsg,
            'input_time',
            self.callback,
            10)

        self.last_time = None

    def callback(self, msg):
        if self.last_time is None:
            self.last_time = msg.data
        else:
            offset = (msg.data - self.last_time) / 2.0
            sync_time = msg.data - offset
            time_msg = TimeMsg()
            time_msg.data = sync_time
            self.publisher.publish(time_msg)
            self.last_time = sync_time

    def publish_time(self):
        time_msg = TimeMsg()
        time_msg.data = Time().now().to_msg()
        self.publisher.publish(time_msg)

def main(args=None):
    rclpy.init(args=args)
    node = TimeSyncNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

在callback函数中，我们首先判断是否是第一次接收到消息，如果是，则将该消息时间戳作为起始时间戳；否则，计算出两个时间戳的偏移量，并将偏移量的一半加到当前时间戳中，得到同步后的时间戳，并发布到`sync_time`话题上。

在`publish_time`函数中，我们将当前系统时间戳发布到`sync_time`话题上。

在需要同步时间戳的节点中，添加一个订阅器，订阅`sync_time`话题，并将订阅回调中收到的时间戳与消息时间戳作差，得到时间戳偏移量，然后将该偏移量加到消息时间戳中，得到同步后的时间戳。

下面是一个使用时间同步节点的订阅器的ROS2代码示例：

import rclpy
from rclpy.node import Node
from std_msgs.msg import Time as TimeMsg

class SubscriberNode(Node):
    def __init__(self):
        super().__init__('subscriber_node')
        self.subscription = self.create_subscription(
            TimeMsg,
            'sync_time',
            self.callback,
            10)

    def callback(self, msg):
        offset = (self.get_clock().now().to_msg() - msg.data).to_sec()
        # do something with synchronized timestamp

def main(args=None):
    rclpy.init(args=args)
    node = SubscriberNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

在订阅回调中，我们首先计算出当前系统时间戳与收到的同步时间戳的偏移量，然后将该偏移量加到消息时间戳中，得到同步后的时间戳。

值得注意的是，在使用时间同步节点的时候，需要确保所有节点都使用相同的时钟类型。在ROS2中，可以使用系统时钟或ROS2内置的仿真时钟，具体可以参考ROS2官方文档。

下面是使用时间插值器的ROS2代码示例：

创建一个时间插值器：

import rclpy
from rclpy.node import Node
from message_filters import TimeSynchronizer, Subscriber
from sensor_msgs.msg import Image
from geometry_msgs.msg import PoseStamped

class TimeInterpolatorNode(Node):
    def __init__(self):
        super().__init__('time_interpolator_node')
        self.publisher = self.create_publisher(PoseStamped, 'sync_pose', 10)

        image_sub = Subscriber(self, Image, 'input_image')
        pose_sub = Subscriber(self, PoseStamped, 'input_pose')
        self.time_sync = TimeSynchronizer([image_sub, pose_sub], 10)
        self.time_sync.registerCallback(self.callback)

    def callback(self, image_msg, pose_msg):
        interpolated_time = (image_msg.header.stamp + pose_msg.header.stamp) / 2.0
        sync_pose = PoseStamped()
        sync_pose.header.stamp = interpolated_time
        sync_pose.pose = pose_msg.pose
        self.publisher.publish(sync_pose)

def main(args=None):
    rclpy.init(args=args)
    node = TimeInterpolatorNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

在回调函数中，我们首先计算出两个消息的时间戳的平均值，得到插值后的时间戳，然后将该时间戳与一个需要同步时间戳的消息一起发布到`sync_pose`话题上。

在需要插值时间戳的节点中，添加一个订阅器，订阅`sync_pose`话题，并将订阅回调中收到的时间戳与消息时间戳作差，得到时间戳偏移量，然后将该偏移量加到消息时间戳中，得到同步后的时间戳。

下面是一个使用时间插值器的订阅器的ROS2代码示例：

import rclpy
from rclpy.node import Node
from std_msgs.msg import Time as TimeMsg
from geometry_msgs.msg import PoseStamped

class SubscriberNode(Node):
    def __init__(self):
        super().__init__('subscriber_node')
        self.subscription = self.create_subscription(
            PoseStamped,
            'sync_pose',
            self.callback,
            10)

    def callback(self, msg):
        offset = (self.get_clock().now().to_msg() - msg.header.stamp).to_sec()
        # do something with synchronized timestamp

def main(args=None):
    rclpy.init(args=args)
    node = SubscriberNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

在订阅回调中，我们首先计算出当前系统时间戳与收到的同步时间戳的偏移量，然后将该偏移量加到消息时间戳中，得到同步后的时间戳。