ROS2学习CH3 参数通信

ROS2学习CH3 参数通信

参数通信

在实际的项目当中,一定会伴随着很多的配置参数,比如经典的PID调参。显然对于这种需要不断改变参数值的场景,直接在代码中修改参数值并重新编译是非常不方便的。在ROS2中,提供了参数通信的机制,可以在程序运行时动态地修改参数值,而不需要重新编译程序。这种方法不仅提高了开发效率,也使得程序的调试和优化变得更加灵活。

可以使用ros2 param list命令来查看当前节点的参数列表,使用ros2 param get <node_name> <param_name>命令来获取指定节点的参数值,使用ros2 param set <node_name> <param_name> <value>命令来设置指定节点的参数值。

比如运行一个turtlesim节点:

bash 复制代码
ros2 run turtlesim turtlesim_node

查看其参数结果如下

可以看到有很多的参数,比如background_bbackground_gbackground_r(这三个参数分别表示背景的蓝色、绿色和红色分量)等。我们可以使用以下命令来获取和设置这些参数的值:

可以看到,使用ros2 param get命令可以获取指定参数的值,同时他会告诉我们参数的类型(图中写的是Integer value,即整型)。且使用ros2 param set命令将背景的红色分量设置为255后,turtlesim的背景颜色就变成了类似于粉色的颜色。说明turtlesim这个节点的background_r参数的值既可以在线读取,也可以在线修改。这就是ROS2参数通信的一个典型应用场景。

同时,如果此时使用ros2 service list命令查看当前节点的服务列表,会发现有很多带有parameter字样的服务,这些服务就是ROS2参数通信机制的实现方式。比如turtlesim_node节点提供了/turtlesim/set_parameters服务,用于设置参数值;提供了/turtlesim/get_parameters服务,用于获取参数值;当我们使用ros2 param set命令设置参数值时,实际上就是调用了/turtlesim/set_parameters服务。这也是后面使用编程去实现参数通信的基础。

使用ROS2参数

在ROS2中,参数是节点的一个重要组成部分。每个节点都可以定义自己的参数,并且可以在运行时动态地修改这些参数。参数的定义和使用可以通过代码实现,下面就是一个简单的示例,展示了如何在ROS2节点中定义和使用参数。

cpp 复制代码
#include "rclcpp/rclcpp.hpp"
#include "rcl_interfaces/msg/set_parameters_result.hpp"

using SetParametersResult = rcl_interfaces::msg::SetParametersResult;


class ParamShow : public rclcpp::Node
{
    private:
        int num1_;
        int num2_;
        // 参数回调函数的句柄,当该shared_ptr被销毁时,回调函数也会被注销。
        rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_;
        rclcpp::TimerBase::SharedPtr timer_;

    public:
        ParamShow() : Node("param_show")
        {
            // 1. 声明参数
            this->declare_parameter("num1", 0); // "xxx"是注册到ros的参数名, 0是默认值
            this->declare_parameter("num2", 0);

            // 2. 获取参数
            this->get_parameter("num1", num1_); // 获取参数值并赋值给num1_,如果参数不存在,则使用默认值0
            this->get_parameter("num2", num2_);

            // 3. 打印参数,调试
            RCLCPP_INFO(this->get_logger(), "num1: %d, num2: %d", num1_, num2_);

            // 4. 注册参数回调函数(即当参数设置服务被调用且参数发生变化时,该函数会被调用)
            param_callback_handle_ = this->add_on_set_parameters_callback(
                [this](const std::vector<rclcpp::Parameter> & parameters)-> rcl_interfaces::msg::SetParametersResult
                {
                    rcl_interfaces::msg::SetParametersResult result; // 参数设置结果
                    result.successful = true;
                    result.reason = "success"; // 自己可以根据需要设置 reason 字段,表示参数设置的结果信息
                    // 参数名与参数值使用类似哈希表的键值对形式存储,因此使用参数名来获取对应的参数值
                    for (const auto & param : parameters)
                    {
                        if (param.get_name() == "num1")
                        {
                            num1_ = param.as_int(); // 根据实际要求选择"as_xxxxx"
                            RCLCPP_INFO(this->get_logger(), "Updated num1: %d", num1_);
                        }
                        else if (param.get_name() == "num2")
                        {
                            num2_ = param.as_int();
                            RCLCPP_INFO(this->get_logger(), "Updated num2: %d", num2_);
                        }
                    }
                    return result;
                }
            );
            // 5. 创建一个定时器,用于定期打印参数值
            timer_ = this->create_wall_timer(
                std::chrono::seconds(1),
                [this]()
                {
                    RCLCPP_INFO(this->get_logger(), "num1: %d, num2: %d", num1_, num2_);
                }
            );
        }
};


int main(int argc, char* argv[])
{
    rclcpp::init(argc, argv);

    auto node = std::make_shared<ParamShow>();
    
    rclcpp::spin(node);

    rclcpp::shutdown();

    return 0;
}

可以看到,使用可变参数需要做到步骤如下:

  1. 声明参数:使用declare_parameter方法声明参数,并指定默认值。
  2. 获取参数:使用get_parameter方法获取参数的值,并将其存储在类的成员变量中。
  3. 注册参数回调函数:使用add_on_set_parameters_callback方法注册一个回调函数,当参数设置服务被调用且参数发生变化时,该函数会被调用。在回调函数中,可以根据参数名获取对应的参数值,并进行相应的处理。

同时我们引用了头文件rcl_interfaces/msg/set_parameters_result.hpp,该头文件定义了SetParametersResult消息类型,用于表示参数设置的结果。通过在回调函数中返回一个SetParametersResult对象,可以告诉调用方参数设置的结果信息。

如果做一个类比,该回调函数类似于服务端的回调函数,当客户端调用服务时,服务端会执行回调函数并返回结果,即SetParametersResult对象,表示参数设置的结果信息。

我们可以使用两种方法来修改参数的值:

  1. 使用命令行工具:可以使用ros2 param set <node_name> <param_name> <value>命令来设置参数的值。
  2. 使用rqt工具:可以使用rqt工具来修改参数的值。rqt是一个基于Qt的图形化工具,可以方便地查看和修改ROS2系统中的各种信息,包括参数。

当使用命令行时,我们可以在终端中输入以下命令来修改参数的值:

bash 复制代码
# ros2 param set /节点名 参数名 参数值  
# 注意节点名前面需要加上斜杠"/",表示这是一个全局节点名
ros2 param set /param_show num1 10

可以看到,使用ros2 param set命令将num1参数的值设置为10后,终端中会打印出参数修改后的值。同时,程序中注册的参数回调函数也会被调用,并打印出更新后的参数值。

当使用rqt工具时,我们可以在终端中输入以下命令来启动rqt:

bash 复制代码
rqt

在rqt界面中,选择"Plugins" -> "Configuration" -> "Dynamic Reconfigure",即可打开参数编辑器。在参数编辑器中,可以选择要修改的节点,并修改其参数的值。修改后按下回车,程序中注册的参数回调函数也会被调用,并打印出更新后的参数值。

如下图

使用其他节点修改参数

前面提到过,ros2中参数通信实际上是通过服务来实现的,因此我们可以使用其他节点去调用参数设置服务来修改另一个节点的参数值。下面是一个简单的示例,展示了如何在一个客户端节点中调用参数设置服务来修改另一个节点的参数值。

cpp 复制代码
#include "rclcpp/rclcpp.hpp"
#include "rcl_interfaces/msg/parameter_value.hpp"
#include "rcl_interfaces/msg/parameter_type.hpp"
#include "rcl_interfaces/srv/set_parameters.hpp"

using SetParameters = rcl_interfaces::srv::SetParameters;

class ParamChange : public rclcpp::Node
{
    private:


    public:
        ParamChange() : Node("param_change")
        {

        }
        // 调用服务端的set_parameters服务
        SetParameters::Response::SharedPtr call_set_parameters(const rcl_interfaces::msg::Parameter &param)
        {
            auto param_client = this->create_client<SetParameters>("/param_show/set_parameters");// 创建客户端,连接到param_show节点的set_parameters服务

            // 等待服务端上线
            while(!param_client->wait_for_service(std::chrono::seconds(1)))
            {
                if(!rclcpp::ok())
                {
                    RCLCPP_ERROR(this->get_logger(), "Waiting for service interrupted, exiting.");
                    return nullptr;
                }
                RCLCPP_INFO(this->get_logger(), "Service not available, waiting again...");
            }

            // 构造请求的对象
            auto request = std::make_shared<SetParameters::Request>();
            request->parameters.push_back(param);

            // 发送请求
            auto future = param_client->async_send_request(request);
            // 等待响应
            rclcpp::spin_until_future_complete(this->get_node_base_interface(), future);

            auto response = future.get();
            return response;
        }

        // 更新参数
        void update_server_param_num1(int num1)
        {
            // 创建参数对象
            auto param = rcl_interfaces::msg::Parameter();
            param.name = "num1";
            // 创建参数值

            auto param_value = rcl_interfaces::msg::ParameterValue(); // 创建参数值对象
            // 设置参数值之前必须指定参数类型,否则会报错
            param_value.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
            param_value.integer_value = num1;
            param.value = param_value; // 将参数值对象赋值给参数对象
            // 请求更新参数
            auto response = call_set_parameters(param); // 调用服务端的set_parameters服务
            if(response == nullptr)
            {
                RCLCPP_ERROR(this->get_logger(), "Failed to call set_parameters service.");
                return;
            }
            for(auto result : response->results)
            {
                if(result.successful)
                {
                    RCLCPP_INFO(this->get_logger(), "Parameter 'num1' updated successfully.");
                }
                else
                {
                    RCLCPP_ERROR(this->get_logger(), "Failed to update parameter 'num1'.Reason: %s", result.reason.c_str());
                }
            }
        }

        void update_server_param_num2(int num2)
        {
            // 创建参数对象
            auto param = rcl_interfaces::msg::Parameter();
            param.name = "num2";
            // 创建参数值
            
            auto param_value = rcl_interfaces::msg::ParameterValue();
            param_value.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
            param_value.integer_value = num2;

            param.value = param_value;
            // 请求更新参数
            auto response = call_set_parameters(param);
            if(response == nullptr)
            {
                RCLCPP_ERROR(this->get_logger(), "Failed to call set_parameters service.");
                return;
            }
            for(auto result : response->results)
            {
                if(result.successful)
                {
                    RCLCPP_INFO(this->get_logger(), "Parameter 'num2' updated successfully.");
                }
                else
                {
                    RCLCPP_ERROR(this->get_logger(), "Failed to update parameter 'num2'.Reason: %s", result.reason.c_str());
                }
            }
        }
};


int main(int argc, char* argv[])
{
    rclcpp::init(argc, argv);

    auto node = std::make_shared<ParamChange>();

    node->update_server_param_num1(10);
    node->update_server_param_num2(20);

    rclcpp::spin(node);

    rclcpp::shutdown();

    return 0;
}

可以看到,在节点中修改另一个节点的参数值主要分为以下几个步骤:

  1. 创建参数对象
  2. 创建参数值对象
  3. 设置参数值,并将参数值对象赋值给参数对象
  4. 调用set_parameters服务更新参数

最后,我们在main函数中创建了一个ParamChange节点,并调用了update_server_param_num1update_server_param_num2方法来修改param_show节点的参数值。当然你可以设计一些结合定时器或者其他逻辑来动态地修改参数值,这样就可以实现更加灵活的参数通信机制。

当然,运行的结果如下图所示:

可以看到当我们在param_change节点中修改了param_show节点的参数值后,param_show节点的参数回调函数被调用,并打印出更新后的参数值。这说明我们成功地实现了跨节点的参数通信机制。