ROS2回调处理机制executor+callbackgroup

ros2的节点中可能需要订阅多个话题（下面内容假设有两个话题订阅者），每个话题的回调函数执行时间会有所区别。默认情况下，该节点的所有回调函数被放在一个调用队列中，一个执行完之后再执行另一个。如果一个回调函数执行的时间太长，会导致另一个订阅者接受的数据因为来不及及时处理而被扔掉。为了避免这种情况，两个订阅者的回调函数最好独立执行，也就是说各排各的队，而不是排在一个队中。

回调管理

为了说明ROS2的回调函数处理机制，先对涉及的几个概念简要介绍。

Executor

rclpy中有SingleThreadedExecutor和MultiThreadedExecutor两种类型的executor，rclcpp还包含StaticSingleThreadedExecutor。SingleThreadedExecutor使用一个线程排队处理多个消息或事件。MultiThreadedExecutor可创建多个线程（可配置）以并行处理多个消息或事件。

CallbackGroup

ROS2 以组的形式组织节点的回调。在MultiThreadedExecutor的情况下，实际的并行性取决于回调组属性。默认情况下所有定时器、话题、动作等的回调函数都放在同一个回调组中。ROS2提供两种类型的回调组：

• 互斥回调组：Mutually Exclusive Callback Group

• 重入回调组：Reentrant Callback Group

Mutually Exclusive Callback Group中的回调函数顺序执行。

Reentrant Callback Group中的回调并发执行，只要来一个消息就调用一个回调函数，同一回调的不同实例可以并发执行，比如一个订阅者前一个消息的回调函数还没处理完，后一个消息又来了，那么它会不等前一个回调函数执行完就并发执行该回调的地2个实例。

属于不同回调组（任何类型）的回调总是可以相互并行执行。

实例

talker：以1hz的频率发布消息。

from typing import List
import rclpy
from rclpy.context import Context
from rclpy.node import Node
from rclpy.parameter import Parameter
from std_msgs.msg import Float64

class TestPublisher(Node):
    def __init__(self, node_name):
        super().__init__(node_name)
        self.publisher1 = self.create_publisher(Float64, 'topic1', 10)
        time_period = 1
        self.timer = self.create_timer(time_period, self.timer1_callback)
        self.i = 0.1

    def timer1_callback(self):
        msg = Float64()
        msg.data = self.i
        self.publisher1.publish(msg)
        self.get_logger().info('Publishing: "%f"' % msg.data)
        self.i = self.i + 1

    
def main(args=None):
    rclpy.init(args=args)
    publisher1 = TestPublisher('talkers')
    rclpy.spin(publisher1)
    publisher1.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

listener：包含两个订阅者，其中一个订阅者执行时间很长（中间sleep了5s）。

情形一：两个订阅者属于同一个Mutually Exclusive Callback Group。

import rclpy
import time
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup
from std_msgs.msg import Float64


class TestSubscriberCallback(Node):
    def __init__(self, node_name):
        super().__init__(node_name)
        cb_group1 = MutuallyExclusiveCallbackGroup()
        cb_group2 = MutuallyExclusiveCallbackGroup()
        self.subscription1 = self.create_subscription(
            Float64,
            'topic1',
            self.listener1_callback,
            10,
            callback_group=cb_group1
        )

        self.subscription2 = self.create_subscription(
            Float64,
            'topic1',
            self.listener2_callback,
            10,
            callback_group=cb_group1
        )
        self.l1 = []
  
    def listener1_callback(self, msg):
        self.l1.append(msg.data)
        # self.get_logger().info('I heard:"%f"' % msg.data)
        print(self.l1)

    def listener2_callback(self, msg):
        dis_min = 1000
        idx_min = 0
        for idx, item in enumerate(self.l1):
            if abs(item - msg.data) < dis_min:
                idx_min = idx
        time.sleep(5)
        self.get_logger().info('closest idx:"%i"' % idx_min)

def main(args=None):
    rclpy.init(args=args)
    listener = TestSubscriberCallback('listeners')
    executor = MultiThreadedExecutor()
    executor.add_node(listener)
    executor.spin()
    listener.destroy_node()
    rclpy.shutdown()


if __name__=='__main__':
    main()

执行结果：

 可以看到两个订阅者的回调函数其实是顺序执行的，由于一个回调函数执行时间太长，导致另一个回调的数据来不及处理而被冲掉。

情形二：两个订阅者各属于一个Mutually Exclusive Callback Group。

 两个回调函数互不干扰，各自排各自的队，顺序执行。

情形三：两个订阅者属于同一个Reentrant Callback Group。

 listener2_callback的一个实例还没执行完，新消息就到了，于是新的实例又开始执行。即，同一回调的不同实例也可以并发执行。

参考

https://docs.ros.org/en/foxy/How-To-Guides/Using-callback-groups.html