大纲
- AsyncParametersClient
- ParameterEventHandler
- 监控全部Parameters
- 监控Node上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_PUBLICSyncParametersClient(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>::SharedPtron_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.hppusing CallbacksContainerType = std::list<ParameterCallbackHandle::WeakPtr>;struct Callbacks{std::recursive_mutex mutex_;// Map container for registered parametersstd::unordered_map<std::pair<std::string, std::string>,CallbacksContainerType,StringPairHash> parameter_callbacks_;std::list<ParameterEventCallbackHandle::WeakPtr> event_callbacks_;/// Callback for parameter events subscriptions.RCLCPP_PUBLICvoidevent_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没有这个能力。