message_filter.ApproximateTimeSynchronizer 方法无法调用回调函数

Question

我正在使用带有 ROS Melodic 的 Ouster OS1 LIDAR。 ROS 包正在发布 LIDAR 数据，我可以在 RViz 中查看它。我想校准 LIDAR 数据并将其与来自 USB 摄像头的数据融合，因此我为摄像头实现了 cv_camera ROS 包，并创建了一个名为 calib 的新包。

但是，我面临一个问题，即 message_filter.ApproximateTimeSynchronizer 方法无法调用回调函数。我订阅的主题是

• cv_camera/image_raw
• cv_camera/camera_info
• 下台/积分

当我尝试回应 ouster/points 主题时，时间戳没有显示。

如何同步数据流之间的时间？ 我在下面给出了使用的代码。

import rospy
import message_filters
from sensor_msgs.msg import Image, CameraInfo, PointCloud2

def callback(image_msg, camera_info_msg, lidar_msg):

# Process synchronized data here
image_sub = message_filters.Subscriber('/cv_camera/image_raw', Image)
info_sub = message_filters.Subscriber('/cv_camera/camera_info', CameraInfo)
lidar_sub = message_filters.Subscriber('/ouster/points', PointCloud2)
ts = message_filters.ApproximateTimeSynchronizer([image_sub, info_sub, lidar_sub], 10, 0.1)
ts.registerCallback(callback)
rospy.init_node('sync_node')
rospy.spin()

Answer 1

使用 ApproximateTimeSyncroniser 同步主题，也许不足为奇，它需要同步每个主题的时间戳信息。如果您的 OS1 没有提供该信息，您需要通过其他方式获取或自己添加（这是更简单的选择）。

一种方法是订阅原始 OS1 主题流，将时间戳信息附加到消息中并在新主题上重新发布它，然后将其输入同步器。如果 Ouster 不这样做，也可以考虑将帧 ID 添加到标题中，以避免帧转换问题。

例如，

import rospy
import message_filters
from sensor_msgs.msg import Image, CameraInfo, PointCloud2

lidar_republisher = rospy.Publisher('/ouster/points_with_timestamp', PointCloud2, queue_size=1)

def add_stamp_to_lidar(lidar_msg):
    stamped_msg = lidar_msg
    stamped_msg.header.stamp = rospy.Time.now()
    stamped_msg.header.frame_id = 'base_link'
    lidar_republisher.pub(stamped_msg)

def callback(image_msg, camera_info_msg, stamped_lidar_msg):
    # Do your thing here
    pass

lidar_sub = rospy.Subscriber('/ouster/points', PointCloud2, add_stamp_to_lidar)
# Process synchronized data here
image_sub = message_filters.Subscriber('/cv_camera/image_raw', Image)
info_sub = message_filters.Subscriber('/cv_camera/camera_info', CameraInfo)
stamped_lidar_sub = message_filters.Subscriber('/ouster/points_with_timestamp', PointCloud2)
ts = message_filters.ApproximateTimeSynchronizer([image_sub, info_sub, stamped_lidar_sub], 10, 0.1)
ts.registerCallback(callback)

rospy.init_node('sync_node')
rospy.spin()

这里的一个危险是，由于您的硬件不同步，您可能会将过时数据与最新数据同步，这意味着您的相机可能会呈现比 LiDAR 更新的帧并显示 LiDAR 无法显示的内容还没有见过，反之亦然。最好的解决方案是从硬件本身获取主题时间戳，因此可能值得看看您是否可以更新 OS1 的固件或驱动程序