在ROS1中,Parameter参数机制默认是无法实现动态监控的(需要配合专门的动态机制),比如正在使用的参数被其他节点改变了,如果不重新查询的话,就无法确定改变之后的值。ROS2最新版本中添加了参数的事件触发机制ParameterEventHandler, 当参数被改变后,可以通过回调函数的方式,动态发现参数修改结果。

注意:此功能目前只支持Galactic及以上的ROS2版本

 一、创建功能包

在工作空间dev_ws的src文件夹中创建功能包:
ros2 pkg create --build-type ament_cmake cpp_parameter_event_handler --dependencies rclcpp
由于在创建功能包时,使用了 --dependencies参数,会自动添加一些依赖项在 package.xml和 CMakeLists.txt文件中,所以就不需要特意设置了。但是我们最好还是打开package.xml文件,在其中填写完整以下这些描述信息。
<description>C++ parameter tutorial</description>
<maintainer email="huchunxu@guyuehome.com">Hu Chunxu</maintainer>
<license>Apache License 2.0</license>

二、编写C++代码节点

在dev_ws/src/cpp_parameter_event_handler/src路径下,创建一个新的代码文件parameter_event_handler.cpp,然后复制粘贴以下代码:
#include <memory>
#include "rclcpp/rclcpp.hpp"

class SampleNodeWithParameters : public rclcpp::Node
{
public:
  SampleNodeWithParameters()
  : Node("node_with_parameters")
  {
    this->declare_parameter("an_int_param", 0);

    // Create a parameter subscriber that can be used to monitor parameter changes
    // (for this node's parameters as well as other nodes' parameters)
    param_subscriber_ = std::make_shared<rclcpp::ParameterEventHandler>(this);

    // Set a callback for this node's integer parameter, "an_int_param"
    auto cb = [this](const rclcpp::Parameter & p) {
        RCLCPP_INFO(
          this->get_logger(), "cb: Received an update to parameter \"%s\" of type %s: \"%ld\"",
          p.get_name().c_str(),
          p.get_type_name().c_str(),
          p.as_int());
      };
    cb_handle_ = param_subscriber_->add_parameter_callback("an_int_param", cb);
  }

private:
  std::shared_ptr<rclcpp::ParameterEventHandler> param_subscriber_;
  std::shared_ptr<rclcpp::ParameterCallbackHandle> cb_handle_;
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<SampleNodeWithParameters>());
  rclcpp::shutdown();

  return 0;
}

2.1 代码解析

我们来解释一下以上代码的关键部分。
头文件的部分先包含必要的头文件,稍后要用到的ParameterEventHandler类,就是在rclcpp/rclcpp.hpp中定义的。
SampleNodeWithParameters()
: Node("node_with_parameters")
{
  this->declare_parameter("an_int_param", 0);

  // Create a parameter subscriber that can be used to monitor parameter changes
  // (for this node's parameters as well as other nodes' parameters)
  param_subscriber_ = std::make_shared<rclcpp::ParameterEventHandler>(this);

  // Set a callback for this node's integer parameter, "an_int_param"
  auto cb = [this](const rclcpp::Parameter & p) {
      RCLCPP_INFO(
        this->get_logger(), "cb: Received an update to parameter \"%s\" of type %s: \"%ld\"",
        p.get_name().c_str(),
        p.get_type_name().c_str(),
        p.as_int());
    };
  cb_handle_ = param_subscriber_->add_parameter_callback("an_int_param", cb);
}
在SampleNodeWithParameters类的构造函数中,声明了一个整型数参数an_int_param,参数值默认是0,然后创建了ParameterEventHandler实例,用于监控参数的变化。最后创建了一个lambda函数,并设置为参数变化时的回调函数。

2.2 添加配置选项

打开功能包的CMakeLists.txt文件,添加如下代码编译的配置项:
add_executable(parameter_event_handler src/parameter_event_handler.cpp)
ament_target_dependencies(parameter_event_handler rclcpp)

install(TARGETS
  parameter_event_handler
  DESTINATION lib/${PROJECT_NAME}
)

三、编译运行

首先确认功能包的依赖是否有安装完成,如果又缺少的依赖则会自动安装:
rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y
在终端中回到dev_ws工作空间的根目录下,使用如下命令编译工作空间:
colcon build --packages-select cpp_parameter_event_handler
打开一个新的终端,进入到dev_ws工作空间根目录下,先设置工作空间的环境变量,接下来就可以运行编译完成的节点了。
. install/setup.bash
ros2 run cpp_parameter_event_handler parameter_event_handler
 
节点成功运行后,为了测试参数监控的功能,我们可以故意通过命令行改变参数值,看下是否会触发回调函数:
ros2 param set node_with_parameters an_int_param 43
 
在运行节点终端中,很快就可以看到回调函数的输出:
[INFO] [1606950498.422461764] [node_with_parameters]: cb: Received an update to parameter "an_int_param" of type integer: "43"
输出的日志中,告知我们参数被修改为了43,说明回调函数成功被调用,参数值也顺利变更为修改后的结果。

四、监控其他节点参数的变化

刚才的参数是节点自身声明的,如果想要监控其他节点声明的参数该怎么办呢?这里我们用到ROS2自带例程parameter_blackboard做一个尝试,我们通过修改刚才的程序,实现对parameter_blackboard中一个double型参数的监控。
首先需要更新刚才parameter_event_handler.cpp程序中的构造函数。
// 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 = [this](const rclcpp::Parameter & p) {
    RCLCPP_INFO(
      this->get_logger(), "cb2: Received an update to parameter \"%s\" of type: %s: \"%.02lf\"",
      p.get_name().c_str(),
      p.get_type_name().c_str(),
      p.as_double());
  };
auto remote_node_name = std::string("parameter_blackboard");
auto remote_param_name = std::string("a_double_param");
cb_handle2_ = param_subscriber_->add_parameter_callback(remote_param_name, cb2, remote_node_name);
然后添加一个回调函数的句柄:
private:
  std::shared_ptr<rclcpp::ParameterEventHandler> param_subscriber_;
  std::shared_ptr<rclcpp::ParameterCallbackHandle> cb_handle_;
  std::shared_ptr<rclcpp::ParameterCallbackHandle> cb_handle2_;  // Add this
接下来重新编译功能包。
colcon build --packages-select cpp_parameter_event_handler
编译成功后,就先运行监控节点。
. install/setup.bash
ros2 run cpp_parameter_event_handler parameter_event_handler
然后运行parameter_blackboard节点。
ros2 run demo_nodes_cpp parameter_blackboard
好啦,接下来我们在终端中尝试修改parameter_blackboard中的参数,看下parameter_event_handler是否可以发现。
ros2 param set parameter_blackboard a_double_param 3.45
运行parameter_event_handler节点的终端中,立刻就可以看到回调函数打印的参数变更日志了:
[INFO] [1606952588.237531933] [node_with_parameters]: cb2: Received an update to parameter "a_double_param" of type: double: "3.45"
 
好啦,今天我们一起学习了ROS2中参数的监控方法,无论是节点自己声明的参数,还是其他节点声明的参数,只要我们知道节点名和参数名,就可以通过ParameterEventHandler来监控参数值的变化了。