怎样同时订阅多个话题，融合多种传感器的数据呢？

XZP

我现在是想用本车的激光雷达测前方小车的速度，我的想法是用测量得到的距离做差分得到两车的相对速度，然后加上本车的速度得到前车的速度。但是需要同时订阅vel_raw话题和scan话题，我在网上看到可以用message filters同步时间，但是vel_raw话题不包含header，我是想自定义一个话题消息，就是带有header的vel_raw。请问怎么实现呢？或者有没有其他更好的方法融合多个传感器呢？谢谢

小助理

您可以自定义一个消息类型，包含激光雷达数据和车速数据，并在消息头部添加时间戳信息。然后在订阅这两个话题时，使用message_filters库中的TimeSynchronizer类来同步它们的时间戳，从而实现数据的融合。

以下是一个示例代码，假设您自定义的消息类型为MyMessage，包含vel_raw和scan两个字段：

import rospy
from sensor_msgs.msg import LaserScan
from my_package.msg import MyMessage
from message_filters import TimeSynchronizer, Subscriber

def callback(vel_raw, scan):
    # 在这里进行数据融合的操作
    # ...

if __name__ == '__main__':
    rospy.init_node('data_fusion_node')

    vel_raw_sub = Subscriber('/vel_raw', MyMessage)
    scan_sub = Subscriber('/scan', LaserScan)

    ts = TimeSynchronizer([vel_raw_sub, scan_sub], 10)
    ts.registerCallback(callback)

    rospy.spin()

在上面的代码中，我们使用Subscriber类来订阅vel_raw和scan两个话题，并将它们传递给TimeSynchronizer类的构造函数。TimeSynchronizer类的第二个参数表示时间戳的最大误差，单位为秒。在registerCallback函数中，我们将回调函数callback注册到TimeSynchronizer类中，当两个话题的时间戳同步时，callback函数将被调用。

在callback函数中，您可以进行数据融合的操作，例如计算两车的相对速度。由于您自定义的消息类型包含了时间戳信息，因此您可以使用该信息来确保数据的同步和正确性。

当然，如果您的数据量比较大，可以考虑使用rosbag来记录数据，然后在离线状态下进行数据融合和分析。

weijiz

@xzp 首先要获取到本车车速应该用的是Twist信息，或者里程计信息里面的Twist信息。cmd_vel只是速度控制指令，不是机器人实际的运动速度。这个信息是有header的。

用激光雷达测相对速度感觉也不是一个很好的方法，激光雷达的点比较稀疏，特征少。很难把目标车从点云中找出来。感觉用uwb，或者两个车都有全局的map坐标系比较靠谱。

XZP

@weijiz 感谢您的回复，这个方法应用起来确实不太可能，但这是一个课程作业，只是想尝试一下。我想问的是如何订阅小车的带时间戳的速度信息呢？我觉得融合多种传感器并匹配时间对自动驾驶来说是个常用的功能吧。因为TimeSynchronizer需要消息格式带时间戳。还有，如何查看ROS所有可以订阅的话题和其对应的消息格式呢？

weijiz

@xzp rostopic list 获取所有话题，rostopic info 话题名可以看话题信息。也可以 @小助理他会帮你回答基础的问题

小助理

@xzp 如果您想订阅带时间戳的速度信息，可以使用ROS中的nav_msgs/Odometry消息类型，该消息类型包含了机器人的位置、速度和姿态信息，并且带有时间戳。您可以在小车的控制节点中发布该消息类型，然后在数据融合节点中订阅该话题，使用TimeSynchronizer类来同步时间戳。
以下是一个示例代码，假设您的小车控制节点发布的话题名为/odom：
import rospy
from nav_msgs.msg import Odometry
from sensor_msgs.msg import LaserScan
from my_package.msg import MyMessage
from message_filters import TimeSynchronizer, Subscriber

def callback(odom, scan):
# 在这里进行数据融合的操作
# …

if name == ‘main’:
rospy.init_node(‘data_fusion_node’)

odom_sub = Subscriber('/odom', Odometry)
scan_sub = Subscriber('/scan', LaserScan)

ts = TimeSynchronizer([odom_sub, scan_sub], 10)
ts.registerCallback(callback)

rospy.spin()

在上面的代码中，我们使用Subscriber类来订阅/