ROS2参数的进阶使用 一个节点内修改其他节点的参数（C++）

---

前言

提示：本文使用的ROS2版本为Humble，Ubuntu22.04，Humble以下的版本笔者未做过测试，不知道是否支持此功能

在网上所有介绍ROS2的文章中，基本都会教大家如何在命令行里面设置全局参数，比如我们熟悉的 设置参数：

ros param set <node_name> <param_name>

或者说在程序里面修改参数，我们可以这样写

auto node = std::make_shared<rclcpp::Node>("test_node");
node->get_param("param_name",param);	//第一个是参数名称，第二个是把参数赋给哪个值

很多教程到这里的介绍就戛然而止了，但总感觉还有许多用法没有说明白，加上国内ros2的教程比较少，今天由笔者介绍几种常见的参数的进阶用法。

---

一、ROS1与ROS2参数的不同

在ROS1中参数全部都在一个Parameter Server上面，任意节点都可以随意的设置和读取其他节点的参数。但是在ROS2中，这部分发生了很大变化。首先由于ROS2没有了中心节点，所以也不存在一个Parameter Server了。每个节点的参数由节点本身去维护。所以要设置或获取其他节点的参数就要用调用服务的方法去调用其他节点设置参数。
但是ROS2中，也多了许多ROS1没有的高级特性，比如动态监测参数等等。

二、进阶使用技巧

1.动态监测参数变化

1.1 监测本节点的参数变化

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

#include "rclcpp/rclcpp.hpp"
#include <memory>//使用智能指针
class TestNode : public rclcpp::Node
{

public:
    TestNode()
    : Node("test_node")
    {
        //声明参数
        this->declare_parameter<std::uint8_t>("frequency", frequency);
        this->get_parameter("frequency", frequency);

        timer_ms = std::chrono::milliseconds(1000/frequency);
        param_subscriber_ = std::make_shared<rclcpp::ParameterEventHandler>(this);
        //Lambda表达式
        auto fre_cb = [this](const rclcpp::Parameter & p) {
        //做自己的操作
            if( p.as_int() > 100)
            {
                RCLCPP_INFO(this->get_logger(), "frequency must be less than 100Hz!");
                return;
            }
           this->frequency = p.as_int();
        }
        //注册参数变更回调
        cb_handle_ = param_subscriber_->add_parameter_callback("frequency", fre_cb);
   }
private:     
	//事件触发机制句柄   
	std::shared_ptr<rclcpp::ParameterEventHandler> param_subscriber_;
    std::shared_ptr<rclcpp::ParameterCallbackHandle> cb_handle_;
    uint16_t frequency;
    std::chrono::milliseconds timer_ms;
};

主要就是通过ParameterEventHandler类注册一个回调完成对参数的监测，当然，这只是监测了本节点的参数，有没有方法可以监测其他节点的参数呢？

1.2 监测其他节点参数的动态变化

监测其他节点的参数动态变化同样只需要通过ParameterEventHandler类注册一个回调，只是在注册的时候我们除了指明参数名称和回调函数外，还需要指明该参数所在的节点，否则就只会在本节点下寻找。我们在上一小节代码的基础上，新建一个节点，其中的回调句柄的写法为：

auto remote_node_name = std::string("test_node");
auto remote_param_name = std::string("frequency");
cb_handle_ = param_subscriber_->add_parameter_callback(remote_param_name , fre_cb, remote_node_name );

1.3 监测一个节点下所有参数的动态变化

前面我们了解了如何针对单个参数的监测，每监测一个参数就需要创一个ParameterEventHandler类，然后注册对应的回调，有没有办法只注册一次，然后监测节点下所有参数的变化呢？
在介绍监测多个动态参数变化前，我想有必要先认识下rclcpp::node_interfaces::NodeParametersInterface这个类，之前我们说过，参数是围绕节点的配置，依附于节点，那么这个类就给我们提供了操作某个节点下参数的接口。创建节点后可以通过 node->get_node_parameters_interface()获取这个类。
除此之外，我们还认识一个常用的msg类型rcl_interfaces::msg::SetParametersResult，在终端输入查看其类型

ros2 interface show rcl_interfaces/msg/SetParametersResult

主要就是显示参数设置的结果和返回失败的原因。
好了，下面笔者讲讲我监测节点下所有参数的操作，直接先上代码

auto node = std::make_shared<rclcpp::Node>("test_node");
auto parameters_interface = node->get_node_parameters_interface();
//回调句柄
std::shared_ptr<rclcpp::node_interfaces::OnSetParametersCallbackHandle> handle;
declare_params();//自己写的参数声明函数，在这个里面声明所有参数，再此就不做展开论述
auto update_param_cb = [](const std::vector<rclcpp::Parameter> &parameters){return update(parameters);};
handle = parameters_interface_->add_on_set_parameters_callback(update_param_cb);

这样，当node节点下的参数被修改，就会触发回调函数update_para,_cb，这个匿名函数里面又调用了自己写的一个update的函数传递了一个Parameter类的vector容器，我们就可以在自己写的函数中完成对参数更新后的操作。下面是一个update()实现的写法：

rcl_interfaces::msg::SetParametersResult update(const std::vector<rclcpp::Parameter> &parameters) {
      for (const auto &param: parameters) {
        if (param.get_name() == (prefix_ + "param_name1")) {
            param1 = param.as_string_array();
        }
        if (param.get_name() == (prefix_ + "param_name2")) {
            param2 = param.as_string_array();
        }
        if (param.get_name() == (prefix_ + "param_name3")) {
            param3 = param.as_double();
        }
     //...添加更多的参数
}

所做操作无非也是遍历改变的参数，然后根据参数名字更新本地变量。

1.4 监测所有参数

既然我们可以监测一个节点下所有参数，有没有一种方法监测全局所有参数的变化呢？这里思路还是差不多，我们直接上官方的示例代码，如何监测所有参数的变化。

auto cb3 =
     [fqn, remote_param_name, &node](const rcl_interfaces::msg::ParameterEvent & event) {
        // Look for any updates to parameters in "/a_namespace" as well as any parameter changes
        // to our own node ("this_node")
        std::regex re("(/a_namespace/.*)|(/this_node)");
        if (regex_match(event.node, re)) {
        // Now that we know the event matches the regular expression we scanned for, we can
        // use 'get_parameter_from_event' to get a specific parameter name that we're looking for
        rclcpp::Parameter p;
        if (rclcpp::ParameterEventsSubscriber::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::ParameterEventsSubscriber::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_handler->add_parameter_event_callback(cb3);

这里官方给了两种做法，第一种是通过get_parameter_from_event( event, p, remote_param_name, fqn)函数来监测我们想要的特定的参数，第二种是采用get_parameters_from_event(event)直接返回所有更新的参数。

2.launch文件读取yaml的参数并加载到节点中

如果参数数量比较大，通常我们的做法是不会在程序里面一个个去进行声明，而是编写一个yaml文件去统一修改我们的参数。那么在C++中如何通过launch文件去加载我们的参数文件呢？
首先ROS2和ROS1的很大不同就是ROS2的launch文件采用了python编写，这里网上有许多参考教程，笔者在此就不一一赘述。如何把配置文件通过launch加载，我们直接举个例子吧。我们在功能包的目录下新建个config文件夹，然后在里面新建参数文件，例如my_config.yaml，在里面配置好我们要用的一些参数，比如

#namespace
#my_namespace //第一层是命名空间，可以不要
# node_name
node1:
  # parameters
  ros__parameters:
    # 下面就填写我们要用的参数，记得yaml文件的写法，记得冒号后面加空格
    enable_serial: false    #调试模式下关闭
    isMagnetic: true
    frequency: 100

node2:
  # parameters
  ros__parameters:
    enable_serial: false   
    frequency: 50

接着我们编写启动文件，下面给一个例子，一般来说照着官方的模板套即可，很多函数我也不清楚是干嘛的。

from ament_index_python.packages import get_package_share_directory
import launch
import launch.actions
import launch.substitutions
import launch_ros.actions
import os.path

def generate_launch_description():
    para_dir = os.path.join(get_package_share_directory('<your_package_name>'), 'config', 'my_config.yaml')
    print(para_dir)
    return launch.LaunchDescription([
        launch.actions.DeclareLaunchArgument(
            'node_prefix',
            # if launch file's prefix is "_lanuch.py", change '.' into '_'
            default_value=[launch.substitutions.EnvironmentVariable('USER'), '.'],
            description='Prefix for node names'
        ),
        launch.actions.DeclareLaunchArgument(
            'namespace', default_value='',
            description='Top-level namespace'
        ),
        launch.actions.DeclareLaunchArgument(
            'use_sim_time', default_value='false',
            description='Use simulation (Gazebo) clock if true'
        ),
		#重点！！配置各个节点的启动
        launch_ros.actions.Node(
            package='<your_package_name>',
            #namespace='',//可以没有命名空间
            executable='node1',
            parameters=[para_dir],
            output='screen'
        ),

        launch_ros.actions.Node(
        package='<your_package_name>',
        executable='node2',
        parameters=[para_dir],
        output='screen'
        )
    ])

首先这个例子是我们开启了两个节点，通过get_package_share_directory()指定了参数文件的路径并且print打印了一下
然后在每个节点的配置那里指明parameters=[para_dir]。
除此之外，我们还需要把这个yaml文件暴露出来，所以需要在CMakeLists.txt文件里面添加：

install(DIRECTORY launch
DESTINATION share/${PROJECT_NAME})

install(DIRECTORY
  config
  DESTINATION share/${PROJECT_NAME}
)

这样我们通过启动文件加载后，在终端ros2 param list可以查看我们所加载的参数。每次修改yaml文件后，需要重新clocon build 才能把config下的yaml文件导入到共享空间中去，否则ros2读取的yaml文件还是更改之前的这个就比较抽象，我们更希望参数可以热更新。

3.获取或者设置其他节点的参数

前面我们总结过如何监测参数的动态变化，那我们就想单纯获取或者设置其他节点的参数，有没有办法呢？
ROS2每个节点的参数由节点本身去维护。所以要设置或获取其他节点的参数就要用调用服务的方法去调用其他节点设置参数。很幸运，我们不用为了每个参数自己去写一个服务，可以通过参数服务器去修改，实际上我们在终端的操作，笔者猜测可能也是通过参数服务器去修改获取。
下面介绍两种访问参数服务器的方法，一种是同步，一种是异步。

3.1 同步访问参数服务器

我们先看同步：

uto node = std::make_shared<rclcpp::Node>("test_node");
std::string target_node_name = "/am_node";
// 创建参数客户端
auto parameter_client = std::make_shared<rclcpp::SyncParametersClient>(node,target_node_name);

// 等待参数服务器启动
while (!parameter_client->wait_for_service(std::chrono::seconds(1))) {
  if (!rclcpp::ok()) {
    RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the parameter service. Exiting.");
    return -1;
  }
  RCLCPP_INFO(node->get_logger(), "Waiting for the parameter service to start...");
}

// 修改节点的参数值
  std::vector<rclcpp::Parameter> params;
  params.push_back(rclcpp::Parameter("frequency", 120));

  auto result = parameter_client->set_parameters_atomically(params);
  if(result.successful)
  {
      RCLCPP_INFO(node->get_logger(), "Parameter set successfully");
  }
  else
  {
      RCLCPP_ERROR(node->get_logger(), "Failed to set parameter");
  }

上述代码我们首先创建了个同步的参数访问客户端rclcpp::SyncParametersClient，然后把需要修改的参数压入容器内，通过set_parameters_atomically()设置参数，由于是同步访问，所以线程会在该处阻塞，直至服务器返回结果，当然这个也可以指定最大超时时间,比如1000ms

auto result = parameter_client->set_parameters_atomically(params,std::chrono::duration<int64_t, std::milli>(1000));

同理，我们也可以获取参数值

 std::string parameter_name = "frequency";
   std::vector<rclcpp::Parameter> parameters;

   parameters = parameter_client->get_parameters({parameter_name});
   if (!parameters.empty()) {
     const auto& parameter = parameters.front();
     if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) {
       int value = parameter.as_int();
       RCLCPP_INFO(node->get_logger(), "Parameter value: %d", value);
     }
   } else {
     // 参数获取失败
     RCLCPP_ERROR(node->get_logger(), "Failed to get parameter.");
   }

3.2 异步访问参数服务器

异步访问和同步访问的步骤也很类似，最主要的区别就是异步访问不会阻塞线程，访问结果可以自行决定什么时候去查看，下面是一个异步访问的例子：

// 创建参数客户端
auto parameter_client = std::make_shared<rclcpp::AsyncParametersClient>(node,target_node_name);
auto future_set = parameter_client->set_parameters_atomically(params);
 // 等待参数设置完成
 //rclcpp::spin_until_future_complete(node, future_set);

 //检查参数是否已成功更改
 if (future_set.wait_for(std::chrono::seconds(1)) == std::future_status::ready) {
   auto result = future_set.get();
   if (result.successful) {
     RCLCPP_INFO(node->get_logger(), "Parameter set successfully");
   } else {
     RCLCPP_ERROR(node->get_logger(), "Failed to set parameter");
   }
 } else {
   RCLCPP_ERROR(node->get_logger(), "Parameter set timed out");
    }

调用set_parameters_atomically()后会返回std::shared_future类，这是一个C++中访问异步操作结果的机制，我们可以通过这个句柄，在之后自己想要访问结果的时候查看结果，而不会阻塞进程。当然，你也可以通过rclcpp::spin_until_future_complete(node, future_set);阻塞进程直到共享状态就绪，当然，这个也就和同步访问一样了。
总之，当我们使用异步访问时候，主要还是想给个信号，说我想设置/获取参数，然后线程就继续干自己的事情了，空闲时候或延时一段时间后再去查一下有没有设置/获取成功。

---

总结

本文在阅读官方文档的基础上，提出了几种ROS2中参数的进阶用法，例如参数监测，利用参数服务器访问其他节点的参数等等，弥补了部分国内ROS2教程细节的空白。限笔者水平有限，以上若有不正确的地方，欢迎大家批评指正。
如果对您有帮助，麻烦点个star★★★加个关注吧！我会继续努力分享更多ROS2小知识