Robot Operating System——深度解析监控Parameters修改的底层实现

在《Robot Operating System——AsyncParametersClient监控Parameters的增删改行为》和《Robot Operating System——ParameterEventHandler监控Parameters的增删改行为》中，我们看到了三种方案的使用案例。本文我们将深度解析它们的底层实现。

AsyncParametersClient

在ROS 2中，我们看到很多同步方案的底层实际是通过对异步方案的封装实现的。本例中分析的同步方案SyncParametersClient也是如此。

// /opt/ros/jazzy/include/rclcpp/rclcpp/parameter_client.hpp
class SyncParametersClient
{
public:
  RCLCPP_SMART_PTR_DEFINITIONS(SyncParametersClient)
  ……
  RCLCPP_PUBLIC
  SyncParametersClient(
    rclcpp::Executor::SharedPtr executor,
    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
    const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
    const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
    const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
    const std::string & remote_node_name = "",
    const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS())
  : executor_(executor), node_base_interface_(node_base_interface)
  {
    async_parameters_client_ =
      std::make_shared<AsyncParametersClient>(
      node_base_interface,
      node_topics_interface,
      node_graph_interface,
      node_services_interface,
      remote_node_name,
      qos_profile);
  }

所以我们直接分析底层的异步方案AsyncParametersClient的实现。

用于设置监控的AsyncParametersClient::on_parameter_event方法底层实现如下

  /**
   * The NodeT type only needs to have a method called get_node_topics_interface()
   * which returns a shared_ptr to a NodeTopicsInterface, or be a
   * NodeTopicsInterface pointer itself.
   */
  template<
    typename CallbackT,
    typename NodeT,
    typename AllocatorT = std::allocator<void>>
  static typename rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
  on_parameter_event(
    NodeT && node,
    CallbackT && callback,
    const rclcpp::QoS & qos = rclcpp::ParameterEventsQoS(),
    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
      rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
  ))
  {
    return rclcpp::create_subscription<rcl_interfaces::msg::ParameterEvent>(
      node,
      "/parameter_events",
      qos,
      std::forward<CallbackT>(callback),
      options);
  }

我们看到其底层实际创建了一个"/parameter_events"主题的订阅者，然后使用我们传递的回调来处理相关事件。这样在Parameters发生变动时，ROS 2底层会发布一条"/parameter_events"主题的事件消息，于是我们就会监控到其变化。

通过这段代码，我们可以看到AsyncParametersClient的方案还是非常简单的。这就需要我们在自己在回调函数中完成更多的逻辑判断。

而后面解析的ParameterEventHandler则帮我们做了很多“分门别类”的工作。

ParameterEventHandler

ParameterEventHandler的构造函数做了上述订阅"/parameter_events"主题的工作。

  /// Construct a parameter events monitor.
  /**
   * \param[in] node The node to use to create any required subscribers.
   * \param[in] qos The QoS settings to use for any subscriptions.
   */
  template<typename NodeT>
  explicit ParameterEventHandler(
    NodeT node,
    const rclcpp::QoS & qos =
    rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)))
  : node_base_(rclcpp::node_interfaces::get_node_base_interface(node))
  {
    auto node_topics = rclcpp::node_interfaces::get_node_topics_interface(node);

    callbacks_ = std::make_shared<Callbacks>();

    event_subscription_ = rclcpp::create_subscription<rcl_interfaces::msg::ParameterEvent>(
      node_topics, "/parameter_events", qos,
      [callbacks = callbacks_](const rcl_interfaces::msg::ParameterEvent & event) {
        callbacks->event_callback(event);
      });
  }

只是它的回调函数包含了两个回调函数集合：

• Map结构的parameter_callbacks_。
• List结构的event_callbacks_。

// https://github.com/ros2/rclcpp/blob/jazzy/rclcpp/include/rclcpp/parameter_event_handler.hpp
  using CallbacksContainerType = std::list<ParameterCallbackHandle::WeakPtr>;
  
  struct Callbacks
  {
    std::recursive_mutex mutex_;

    // Map container for registered parameters
    std::unordered_map<
      std::pair<std::string, std::string>,
      CallbacksContainerType,
      StringPairHash
    > parameter_callbacks_;

    std::list<ParameterEventCallbackHandle::WeakPtr> event_callbacks_;

    /// Callback for parameter events subscriptions.
    RCLCPP_PUBLIC
    void
    event_callback(const rcl_interfaces::msg::ParameterEvent & event);
  };
  
struct ParameterEventCallbackHandle
{
  RCLCPP_SMART_PTR_DEFINITIONS(ParameterEventCallbackHandle)

  using ParameterEventCallbackType =
    std::function<void (const rcl_interfaces::msg::ParameterEvent &)>;

  ParameterEventCallbackType callback;
};
  
struct ParameterCallbackHandle
{
  RCLCPP_SMART_PTR_DEFINITIONS(ParameterCallbackHandle)

  using ParameterCallbackType = std::function<void (const rclcpp::Parameter &)>;

  std::string parameter_name;
  std::string node_name;
  ParameterCallbackType callback;
};

监控全部Parameters

add_parameter_event_callback会监控所有Node上Parameters的变动。

我们看到它在底层实际是将我们传入的回调插入到上述回调函数List的头部（emplace_front）。这说明add_parameter_event_callback可以被反复调用，也就说我们可以设置多个不同的回调来监控所有Parameters变动。

ParameterEventCallbackHandle::SharedPtr
ParameterEventHandler::add_parameter_event_callback(
  ParameterEventCallbackType callback)
{
  std::lock_guard<std::recursive_mutex> lock(callbacks_->mutex_);
  auto handle = std::make_shared<ParameterEventCallbackHandle>();
  handle->callback = callback;
  callbacks_->event_callbacks_.emplace_front(handle);

  return handle;
}

监控Node上Parameters的变动

add_parameter_callback会将传入的Parameter的名称以及所在Node上的名称，保存到回调结构parameter_callbacks_的Key中，Value就是我们传入的回调函数List的头部。这样在数据结构上，我们让Pair<ParameterName，NodeName>和Callback产生了映射。

通过这段底层实现，可以发现，我们是可以针对同一Pair<ParameterName，NodeName>设置多个回调监控。

ParameterCallbackHandle::SharedPtr
ParameterEventHandler::add_parameter_callback(
  const std::string & parameter_name,
  ParameterCallbackType callback,
  const std::string & node_name)
{
  std::lock_guard<std::recursive_mutex> lock(callbacks_->mutex_);
  auto full_node_name = resolve_path(node_name);

  auto handle = std::make_shared<ParameterCallbackHandle>();
  handle->callback = callback;
  handle->parameter_name = parameter_name;
  handle->node_name = full_node_name;
  // the last callback registered is executed first.
  callbacks_->parameter_callbacks_[{parameter_name, full_node_name}].emplace_front(handle);

  return handle;
}

触发

当 "/parameter_events"收到发布的事件后，Callbacks::event_callback会被调用。
它会先挨个遍历针对具体Parameter的回调结构parameter_callbacks_

void
ParameterEventHandler::Callbacks::event_callback(const rcl_interfaces::msg::ParameterEvent & event)
{
  std::lock_guard<std::recursive_mutex> lock(mutex_);

  for (auto it = parameter_callbacks_.begin(); it != parameter_callbacks_.end(); ++it) {

确定其是否和事件中的ParameterName以及NodeName一致。如果一直则提取出Parameter结构；如果不一致就继续遍历寻找。

    rclcpp::Parameter p;
    if (get_parameter_from_event(event, p, it->first.first, it->first.second)) {

找到一致的Callback就直接调用

        auto shared_handle = cb->lock();
        if (nullptr != shared_handle) {
          shared_handle->callback(p);
        } else {
          cb = it->second.erase(cb);
        }
      }
    }
  }

处理完针对具体Node和Parameter的回调后，会去处理针对所有Node、所有Parameters的回调。

  for (auto event_cb = event_callbacks_.begin(); event_cb != event_callbacks_.end(); ++event_cb) {
    auto shared_event_handle = event_cb->lock();
    if (nullptr != shared_event_handle) {
      shared_event_handle->callback(event);
    } else {
      event_cb = event_callbacks_.erase(event_cb);
    }
  }
}

总结

ParameterEventHandler包含了AsyncParametersClient的实现，且比其功能更丰富。AsyncParametersClient只能设置一个全部Parameter监控回调函数，而ParameterEventHandler可以设置多个。而且ParameterEventHandler还可以针对不同Node、不同Parameter设置多个不同的回调，但是AsyncParametersClient没有这个能力。