ROS 消息过滤器MessageFilter

最新推荐文章于 2024-05-08 15:43:29 发布

氢键H-H

最新推荐文章于 2024-05-08 15:43:29 发布

阅读量8.7k

点赞数 20

分类专栏：其他文章标签： python 自动驾驶 ROS

本文为氢键H-H原创文章，未经允许请勿用于商业用途，转载请注明出处

本文链接：https://blog.csdn.net/qq_32618327/article/details/121655428

版权

其他专栏收录该内容

12 篇文章 2 订阅

订阅专栏

ROS 消息过滤器MessageFilter

1. 缘由
2. MessageFilter
3. Subscriber订阅者
4. TimeSynchronizer
5. ApproximateTimeSynchronizer

1. 缘由

机器人应用中，传感器的感知频率很少可以一致，
如里程计的数据可达到50Hz，图像的频率达到30Hz，而激光雷达数据采集可以达到10Hz

那么时间肯定不同步了，而部分融合算法需要将传感器数据进行时间同步后才能进行融合
咋整？
整个工具过滤或同步一下时间戳

2. MessageFilter

消息过滤器message_filters类似一个消息缓存
当消息到达消息过滤器的时候，可能并不会立即输出，而是在稍后的时间点里满足一定条件下输出

举个例子，比如时间同步器，它接收来自多个源的不同类型的消息
并且仅当它们在具有相同时间戳的每个源上接收到消息时才输出它们
也就是起到了一个消息同步输出的效果

3. Subscriber订阅者

订阅者过滤器是对ROS订阅的封装，为其他过滤器提供源代码
订阅者过滤器无法将另一个过滤器的输出作为其输入，而是使用ROS主题作为其输入

sub = message_filters.Subscriber("chatter", String)
sub.registerCallback(callback)

等同于

rospy.Subscriber("chatter", String, callback)

4. TimeSynchronizer

4.1. 时间同步器

如果需要绝对时间同步，那么需要时间戳相同

TimeSynchronizer过滤器通过包含在其头中的时间戳来同步输入通道
并以单个回调的形式输出它们需要相同数量的通道

4.2. 举例

比如image和camera_info，图像和相机信息可能需要完全同步

4.2.1. 发布文件

#!/usr/bin/env python3

import rospy  # 导入rospy包 ROS的python客户端
from sensor_msgs.msg import Image, CameraInfo  # sensor_msgs.msg包的Image, CameraInfo消息类型


# 定义发布函数 talker
def talker():
    image_pub = rospy.Publisher('image', Image, queue_size=1)  # 定义发布器 image_pub 发布image
    camera_info_pub = rospy.Publisher('camera_info', CameraInfo, queue_size=1)  # 定义发布器 camera_info_pub 发布camera_info
    rospy.init_node('talker', anonymous=True)  # 初始化节点talker
    rate = rospy.Rate(1)  # 初始化循环频率1HZ的对象rate

    # 在程序没退出的情况下
    while not rospy.is_shutdown():
        # 时间戳
        current_time = rospy.Time.now()

        # 创建并发布image消息
        image_msg = Image()
        image_msg.header.stamp = current_time
        rospy.loginfo("image:" + str(image_msg.header.stamp))  # 消息打印到屏幕上
        image_pub.publish(image_msg)  # 发布消息

        # 创建并发布新image消息
        image_msg = Image()
        image_msg.header.stamp = rospy.Time.now()
        rospy.loginfo("image:" + str(image_msg.header.stamp))  # 消息打印到屏幕上
        image_pub.publish(image_msg)  # 发布消息

        # 创建并发布camera_info消息
        camera_info_msg = CameraInfo()
        camera_info_msg.header.stamp = current_time
        rospy.loginfo("camera_info:" + str(camera_info_msg.header.stamp))  # 消息打印到屏幕上
        camera_info_pub.publish(camera_info_msg)  # 发布消息
        rate.sleep()  # 休眠


if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass

4.2.2. 订阅文件

#!/usr/bin/env python3

import rospy  # 导入rospy包 ROS的python客户端
import message_filters  # 导入message_filters包 消息过滤器
from sensor_msgs.msg import Image, CameraInfo  # sensor_msgs.msg包的Image, CameraInfo消息类型


# 回调函数
def callback(data_image, data_amera_info):
    rospy.loginfo(rospy.get_caller_id() + "I heard %s ,%s", data_image.header.stamp, data_amera_info.header.stamp)  # 消息打印到屏幕上


def listener():
    rospy.init_node('listener', anonymous=True)  # 初始化节点listener

    # 创建消息过滤器订阅器
    sub_image = message_filters.Subscriber("image", Image)
    camera_info_sub = message_filters.Subscriber("camera_info", CameraInfo)

    # 设置时间同步器 fs等待消息[sub_image, camera_info_sub]，queue_size 10
    sync_listener = message_filters.TimeSynchronizer([sub_image, camera_info_sub], 10)

    # 设置回调函数
    sync_listener.registerCallback(callback)

    rospy.spin()  # 保持主进程一直循环


if __name__ == '__main__':
    listener()

4.2.3. 运行效果

运行效果，同一时间戳的话题将触发回调函数，新时间戳的image被忽略

在这里插入图片描述

5. ApproximateTimeSynchronizer

5.1. 接近时间同步器

很明显，不同的传感器时间戳几乎不可能相同
那么就可能就需要给出个时间误差范围

自适应算法来匹配基于其时间戳的消息，可自定义接近的时间，而非绝对时间要一模一样

比如话题“odom” 50Hz和“image”1Hz，通过ApproximateTime ，设置接近时间为1s
那么就可以正常订阅到消息，并同步回调输出了

5.2. 举例

简单举例，用字符串代替里程和图像消息

5.2.1. odom发布文件

#!/usr/bin/env python3

import rospy  # 导入rospy包 ROS的python客户端
from std_msgs.msg import String  # 导入std_msgs.msg包的String消息类型


# 定义发布函数 talker1
def talker():
    pub = rospy.Publisher('odom', String, queue_size=1)  # 定义发布器 pub 发布odom
    rospy.init_node('odom_talker', anonymous=True)  # 初始化节点odom_talker
    rate = rospy.Rate(50)  # 初始化循环频率50HZ的对象rate

    # 在程序没退出的情况下
    while not rospy.is_shutdown():
        hello_str = "odom %s" % rospy.get_time()
        rospy.loginfo(hello_str)  # 消息打印到屏幕上
        pub.publish(hello_str)  # 发布消息
        rate.sleep()  # 休眠


if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass

5.2.2. image发布文件

#!/usr/bin/env python3

import rospy  # 导入rospy包 ROS的python客户端
from std_msgs.msg import String  # 导入std_msgs.msg包的String消息类型


# 定义发布函数 talker2
def talker():
    pub = rospy.Publisher('image', String, queue_size=1)  # 定义发布器 pub 发布image
    rospy.init_node('image_talker', anonymous=True)  # 初始化节点image_talker
    rate = rospy.Rate(1)  # 初始化循环频率1HZ的对象rate

    # 在程序没退出的情况下
    while not rospy.is_shutdown():
        hello_str = "image %s" % rospy.get_time()
        rospy.loginfo(hello_str)  # 消息打印到屏幕上
        pub.publish(hello_str)  # 发布消息
        rate.sleep()  # 休眠


if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass

5.2.3. 话题订阅文件

#!/usr/bin/env python3

import rospy  # 导入rospy包 ROS的python客户端
import message_filters  # 导入message_filters包 消息过滤器
from std_msgs.msg import String  # 导入std_msgs.msg包的String消息类型


# 回调函数
def callback(data_odom, data_image):
    rospy.loginfo(rospy.get_caller_id() + "I heard %s ,%s", data_odom.data, data_image.data)  # 消息打印到屏幕上


def listener():
    rospy.init_node('listener', anonymous=True)  # 初始化节点listener

    # 创建消息过滤器订阅器
    sub_odom = message_filters.Subscriber("odom", String)
    sub_image = message_filters.Subscriber("image", String)

    # 设置接近时间同步器 fs等待消息[sub_odom, sub_image]，queue_size 10 和接近时间1s，allow_headerless允许非时间戳header.stamp比较，采用ROS时间
    sync_listener = message_filters.ApproximateTimeSynchronizer([sub_odom, sub_image], 10, 1, allow_headerless=True)

    # 设置回调函数
    sync_listener.registerCallback(callback)

    rospy.spin()  # 保持主进程一直循环


if __name__ == '__main__':
    listener()