在《Robot Operating System——AsyncParametersClient监控Parameters的增删改行为》一文中,我们通过AsyncParametersClient和SyncParametersClient的on_parameter_event方法对Parameters的变动进行了监控。本文我们将介绍另一种监控工具类ParameterEventHandler的使用。
我们将通过demo_nodes_cpp/src/parameters/parameter_event_handler.cpp来讲解。
创建订阅"/parameter_events"的Node
这类对Parameters行为进行监控,其底层都是通过订阅"/parameter_events"主题的形式实现的(后面我们会对其进行分析)。所以我们第一步需要创建一个Node,并订阅该主题。
int main(int argc, char ** argv)
{
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
rclcpp::init(argc, argv);
const char * node_name = "this_node";
const char * param_name = "an_int_param";
// Create a node with an integer parameter
auto node = rclcpp::Node::make_shared(node_name);
node->declare_parameter(param_name, 0);
// Now, create a parameter subscriber that can be used to monitor parameter changes on
// our own local node as well as other remote nodes
auto param_subscriber = std::make_shared<rclcpp::ParameterEventHandler>(node);
ParameterEventHandler的构造函数中,实现了主题的订阅功能。
///opt/ros/jazzy/include/rclcpp/rclcpp/parameter_event_handler.hpp
/// 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);
});
}
监控自身Node内部Parameter
ParameterEventHandler在API层面可以方便的支持对某个Parameter修改行为的订阅。而在AsyncParametersClient和SyncParametersClient中,我们只能在监控回调中自己判断。
如下例,add_parameter_callback方法对名字是param_name(= “an_int_param”)的Parameter进行了监控。一旦这个参数发生变动,则cb1会被回调。
// First, set a callback for the local integer parameter. In this case, we don't
// provide a node name (the third, optional, parameter).
auto cb1 = [&node](const rclcpp::Parameter & p) {
RCLCPP_INFO(
node->get_logger(),
"cb1: Received an update to parameter \"%s\" of type %s: \"%" PRId64 "\"",
p.get_name().c_str(),
p.get_type_name().c_str(),
p.as_int());
};
auto handle1 = param_subscriber->add_parameter_callback(param_name, cb1);
监控自身Node外部Parameter
我们先创建一个其他命名空间(/a_namespace)的Node。
// Let's create another "remote" node in a separate namespace with its own string parameter
auto remote_node_name = "a_remote_node";
auto remote_node_namespace = "/a_namespace";
auto remote_param_name = "a_string_param";
auto remote_node = rclcpp::Node::make_shared(remote_node_name, remote_node_namespace);
remote_node->declare_parameter(remote_param_name, "default_string_value");
auto remote_thread = std::make_unique<NodeThread>(remote_node);
该Node运行于一个线程中
// A utility class to assist in spinning a separate node
class NodeThread
{
public:
explicit NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base)
: node_(node_base)
{
thread_ = std::make_unique<std::thread>(
[&]()
{
executor_.add_node(node_);
executor_.spin();
executor_.remove_node(node_);
});
}
template<typename NodeT>
explicit NodeThread(NodeT node)
: NodeThread(node->get_node_base_interface())
{}
~NodeThread()
{
executor_.cancel();
thread_->join();
}
protected:
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_;
std::unique_ptr<std::thread> thread_;
rclcpp::executors::SingleThreadedExecutor executor_;
};
然后我们只需要告知add_parameter_callback第三个参数的值,即外部Node的名称(包含命名空间),就可以监听这个Node下名字叫remote_param_name(= “a_string_param”)的Parameter的变动。
// Now, add a callback to monitor any changes to the remote node's parameter. In this
// case, we supply the remote node name.
auto cb2 = [&node](const rclcpp::Parameter & p) {
RCLCPP_INFO(
node->get_logger(), "cb2: Received an update to parameter \"%s\" of type: %s: \"%s\"",
p.get_name().c_str(),
p.get_type_name().c_str(),
p.as_string().c_str());
};
auto fqn = remote_node_namespace + std::string("/") + remote_node_name;
auto handle2 = param_subscriber->add_parameter_callback(
remote_param_name, cb2, fqn);
监听所有Node的所有Parameter的变动
这次我们需要调用add_parameter_event_callback方法,并传入回调即可。因为这会收到全部Node的所有Parameter的变动,所以其信息也非常繁杂。这就需要我们自己在回调函数中做大量的手工处理。
// We can also monitor all parameter changes and do our own filtering/searching
auto cb3 =
[fqn, remote_param_name, &node](const rcl_interfaces::msg::ParameterEvent & event) {
// Use a regular expression to scan for any updates to parameters in "/a_namespace"
// as well as any parameter changes to our own node
std::regex re("(/a_namespace/.*)|(/this_node)");
if (regex_match(event.node, re)) {
// You can use 'get_parameter_from_event' if you know the node name and parameter name
// that you're looking for
rclcpp::Parameter p;
if (rclcpp::ParameterEventHandler::get_parameter_from_event(
event, p,
remote_param_name, fqn))
{
RCLCPP_INFO(
node->get_logger(), "cb3: Received an update to parameter \"%s\" of type: %s: \"%s\"",
p.get_name().c_str(),
p.get_type_name().c_str(),
p.as_string().c_str());
}
// You can also use 'get_parameter*s*_from_event' to enumerate all changes that came
// in on this event
auto params = rclcpp::ParameterEventHandler::get_parameters_from_event(event);
for (auto & p : params) {
RCLCPP_INFO(
node->get_logger(), "cb3: Received an update to parameter \"%s\" of type: %s: \"%s\"",
p.get_name().c_str(),
p.get_type_name().c_str(),
p.value_to_string().c_str());
}
}
};
auto handle3 = param_subscriber->add_parameter_event_callback(cb3);
执行效果
总结
AsyncParametersClient和SyncParametersClient的on_parameter_event的功能和ParameterEventHandler::add_parameter_event_callback比较类似,会通知所有Parameter的变动;但是ParameterEventHandler::add_parameter_callback提供了更细粒度的控制,我们可以通过指定Parameter名称和Node名称让ParameterEventHandler帮我们自动过滤掉我们不关心的事件。