4.5 在C++节点中使用参数

bohu832025-01-10 8:51

本节沿用之前4.3 节小海龟控制例子。

4.5.1 参数声明与设置

打开src/demo_cpp_service/src/turtle_control.cpp文件

添加测试代码

cpp 复制代码 
        this->declare_parameter("k",1.0);
        this->declare_parameter("max_speed",1.0);
        this->get_parameter("k",k_);
        this->get_parameter("max_speed",max_speed_);

重新构建后，运行节点

ros2 run demo_cpp_service turtle_control

查看参数列表

bohu@bohu-TM1701:~/chapt4/chapt4_ws$ ros2 param list

/turtle_controller:

k

max_speed

qos_overrides./parameter_events.publisher.depth

qos_overrides./parameter_events.publisher.durability

qos_overrides./parameter_events.publisher.history

qos_overrides./parameter_events.publisher.reliability

use_sim_time

可以看到节点声明的参数了。还可以使用rqt软件直接修改

4.5.2 接受参数事件

上面设置参数只是修改ros2参数，节点的参数值没有修改，需要增加参数设置回调函数。

修改之前的turtle_control.cpp.

cpp 复制代码 
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "turtlesim/msg/pose.hpp"
#include "chapt4_interfaces/srv/patrol.hpp"
#include "rcl_interfaces/msg/set_parameters_result.hpp"
using Patrol = chapt4_interfaces::srv::Patrol;
using SetParametersResult = rcl_interfaces::msg::SetParametersResult;
using namespace std;


class TurtleControlNode : public rclcpp::Node
{
public:
    TurtleControlNode() : Node("turtle_controller")
    {   
        this->declare_parameter("k",1.0);
        this->declare_parameter("max_speed",1.0);
        this->get_parameter("k",k_);
        this->get_parameter("max_speed",max_speed_);
        paramter_callback_handle_ = this->add_on_set_parameters_callback(
            [&](const vector<rclcpp::Parameter> &params)->SetParametersResult {
                for(auto  param: params){
                    RCLCPP_INFO(this->get_logger(),"更新参数的值%s=%f",param.get_name().c_str(),param.as_double());
                    if(param.get_name() =="k"){
                        k_ = param.as_double();
                    }else if(param.get_name() =="max_speed"){
                        max_speed_ = param.as_double(); 
                    }
                }
                auto result = SetParametersResult();
                result.successful = true;
                return result;
            });
        patrol_service_ =  this->create_service<Patrol>(
        "patrol",[&](const shared_ptr<Patrol::Request> request,
            shared_ptr<Patrol::Response> response) -> void {
          // 判断巡逻点是否在模拟器边界内
          if ((0 < request->target_x && request->target_x < 12.0f)
           && (0 < request->target_y && request->target_y < 12.0f)) {
            target_x_ = request->target_x;
            target_y_ = request->target_y;
            response->result = Patrol::Response::SUCCESS;
          }else{
            response->result = Patrol::Response::FAIL;
          }
        });

        //调用继承来父类来创建发布者  
        publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("/turtle1/cmd_vel",10);
        //订阅者
        subscriber_ = this->create_subscription<turtlesim::msg::Pose>("/turtle1/pose",10,std::bind(&
        TurtleControlNode::on_pose_received_,this,placeholders::_1));
    }
private:
    void on_pose_received_(const turtlesim::msg::Pose::SharedPtr pose) {
    //1获取当前位置
    auto current_x = pose->x;
    auto current_y = pose->y;
    RCLCPP_INFO(get_logger(),"当前：x=%f,y=%f",current_x,current_y);
    //2计算与目标距离，以及当前海龟的角度差
    auto distance = sqrt(
        (target_x_-current_x)*(target_x_-current_x)+
        (target_y_-current_y)*(target_y_-current_y)
    );
    auto angle = atan2((target_y_-current_y),(target_x_-current_x))-pose->theta;

    //3控制策略：
    auto msg = geometry_msgs::msg::Twist();
    if(distance>0.1){
        if(fabs(angle)>0.2)
        {
            msg.angular.z = fabs(angle);
        }else{
            msg.linear.x = k_*distance;
        }
    }

    //4 限制最大速度
    if(msg.linear.x>max_speed_){
        msg.linear.x = max_speed_;
    }
    publisher_->publish(msg);
  }
private:
    OnSetParametersCallbackHandle::SharedPtr paramter_callback_handle_;
    rclcpp::Service<Patrol>::SharedPtr patrol_service_;
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr  subscriber_; //订阅者智能指针
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;   //发布者智能指针  
    double target_x_{1.0}; //目标位置X,
    double target_y_{1.0}; 目标位置Y
    double k_{1.0};//比例系数
    double max_speed_{2.0}; //最大速度    
};

int main(int argc,char *argv[])
{
    rclcpp::init(argc,argv);
    auto node = std::make_shared<TurtleControlNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

主要是增加回调函数add_on_set_parameters_callback

重新构建后运行节点，打开rqt 修改属性值

bohu@bohu-TM1701:~/chapt4/chapt4_ws$ ros2 run demo_cpp_service turtle_control

[INFO] [1736411168.111954645] [turtle_controller]: 更新参数的值k=2.000000

[INFO] [1736411195.539882142] [turtle_controller]: 更新参数的值k=3.000000

[INFO] [1736411207.634454220] [turtle_controller]: 更新参数的值max_speed=2.000000

4.5.3 修改其他节点的参数

参数机制是基于服务通信实现的，之前python节点验证过。先看下涉及SetParameters数据类型：

bohu@bohu-TM1701:~/chapt4/chapt4_ws$ ros2 interface show rcl_interfaces/srv/SetParameters

A list of parameters to set.

Parameter[] parameters

string name

ParameterValue value

uint8 type

bool bool_value

int64 integer_value

float64 double_value

string string_value

byte[] byte_array_value

bool[] bool_array_value

int64[] integer_array_value

float64[] double_array_value

string[] string_array_value

Indicates whether setting each parameter succeeded or not and why.

SetParametersResult[] results

bool successful

string reason

这也是为什么要引入相关头文件,修改后的patrol_client.cpp代码

cpp 复制代码 
#include <chrono>
#include <cstdlib>
#include <ctime>
#include "rclcpp/rclcpp.hpp"
#include "chapt4_interfaces/srv/patrol.hpp"
#include "rcl_interfaces/msg/parameter.hpp"
#include "rcl_interfaces/msg/parameter_value.hpp"
#include "rcl_interfaces/msg/parameter_type.hpp"
#include "rcl_interfaces/srv/set_parameters.hpp"
using SetP = rcl_interfaces::srv::SetParameters;
using Patrol = chapt4_interfaces::srv::Patrol;
using namespace std;
using namespace std::chrono_literals;//可以使用10s


class PatrolClient : public rclcpp::Node
{
public:
    PatrolClient() : Node("patrol_client")
    {   
        patrol_client_ = this->create_client<Patrol>("patrol");
        timer_ = this->create_wall_timer(10s,[&]()->void{
            //1检测服务端是否上线
            while(!patrol_client_->wait_for_service(1s)){
                RCLCPP_ERROR(this->get_logger(),"等待服务上线过程中，rclcpp异常，退出");
                return;
            }
            RCLCPP_INFO(this->get_logger(),"等待服务上线....");
            //2 构造请求对象
            auto request = std::make_shared<Patrol::Request>();
            request->target_x = rand()%15;
            request->target_y = rand()%15;
            RCLCPP_INFO(this->get_logger(),"已准备好目标点%f,%f",request->target_x ,request->target_y );
            //3 发送请求
            this->patrol_client_->async_send_request(request,[&](rclcpp::Client<Patrol>::SharedFuture result_future)->void{
                auto response = result_future.get();
                if(response->result==Patrol::Response::SUCCESS){
                    RCLCPP_INFO(this->get_logger(),"请求目标点处理成功");
                }else if(response->result==Patrol::Response::FAIL){
                    RCLCPP_INFO(this->get_logger(),"请求目标点处理失败");
                }

            });

        });
    }
    /**发送请求 */
    SetP::Response::SharedPtr call_set_parameters(const rcl_interfaces::msg::Parameter &param)
    {
        auto param_client_ = this->create_client<SetP>("/turtle_controller/set_parameters");
        //1检测服务端是否上线
        while(!patrol_client_->wait_for_service(1s)){
            RCLCPP_ERROR(this->get_logger(),"等待服务上线过程中，rclcpp异常，退出");
            return nullptr;
        }
        RCLCPP_INFO(this->get_logger(),"等待服务上线....");
        //2 构造请求对象
        auto request = std::make_shared<SetP::Request>();
        request->parameters.push_back(param);//添加数据
        //3 发送请求
        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_param_k(double k)
    {
        //1 创建请求参数
        auto param = rcl_interfaces::msg::Parameter();
        param.name = "k";
        //2 创建请求参数值
        auto param_value = rcl_interfaces::msg::ParameterValue();
        param_value.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
        param_value.double_value =k;
        param.value = param_value;
        //3. 请求更新参数并处理
        auto response = this->call_set_parameters(param);
        if(response == NULL){
            RCLCPP_WARN(this->get_logger(), "参数修改失败");
            return;
        }else{
            for(auto result :response->results)
            {
                if(result.successful){
                    RCLCPP_INFO(this->get_logger(),"参数K已经修改为：%f",k);
                }else{
                    RCLCPP_WARN(this->get_logger(), "参数修改失败,reason:%s",result.reason.c_str());
                }
            }
        }

    }

private:
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Client<Patrol>::SharedPtr patrol_client_;
};

int main(int argc,char *argv[])
{
    rclcpp::init(argc,argv);
    auto node = std::make_shared<PatrolClient>();
    node->update_param_k(4.0);
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

增加了call_set_parameters、update_param_k 两个方法，在main增加update_param_k调用。

构建后启动服务端、与客户端。日志分别如下：

bohu@bohu-TM1701:~/chapt4/chapt4_ws$ ros2 run demo_cpp_service turtle_control

[INFO] [1736431217.835118418] [turtle_controller]: 更新参数的值k=4.000000

客户端：

bohu@bohu-TM1701:~/chapt4/chapt4_ws$ ros2 run demo_cpp_service patrol_client

[INFO] [1736431217.834569546] [patrol_client]: 等待服务上线....

[INFO] [1736431217.836297268] [patrol_client]: 参数K已经修改为：4.000000

相关推荐
weixin_7499499042 分钟前
双向列表的实现（C++）
开发语言·c++·链表
xianwu5431 小时前
反向代理模块开发，
linux·开发语言·网络·c++·git
lzb_kkk2 小时前
【C++】JsonCpp库
开发语言·c++·json·1024程序员节
智驾2 小时前
SOLID原则学习，开闭原则
c++·开闭原则·solid
至暗时刻darkest2 小时前
STM32 单片机 练习项目 LED灯闪烁&LED流水灯&蜂鸣器 未完待续
c++·单片机
飞飞是甜咖啡2 小时前
【Orca】Orca - Graphlet 和 Orbit 计数算法
c语言·c++·人工智能·python
SCBAiotAigc2 小时前
VSCode 在Windows下开发时使用Cmake Tools时输出Log乱码以及CPP文件乱码的终极解决方案
c++·vscode·cmake·cmake tools·编码问题
salsm3 小时前
使用 C++ 实现神经网络：从基础到高级优化
开发语言·c++·神经网络
冷眼看人间恩怨3 小时前
【Qt笔记】QTextEdit和QPlainTextEdit 控件详解
c++·笔记·qt
是阿建吖!3 小时前
【Linux】线程池
android·linux·c语言·c++
热门推荐
01图像分割：Pytorch实现UNet++进行医学细胞分割02半导体应用系统一些小知识收集(strip&wafer mapping,EAP&scada)03HCIA-datacom数通题库和录播视频资料04ESP-IDF学习记录（4） ESP-IDF examples目录05渗透测试之SQLMAP工具详解 kali自带SQLmap解释 重点sqlmap --tamper 使用方式详解 搞完你就很nice了06优化手机性能，解决卡顿问题：关闭这3个微信开关，释放内存空间07如何在 Ubuntu 22.04 上安装并开始使用 RabbitMQ08新手学习VR全景需要知道的几个问题09校验 GPT-4 真实性的三个经典问题：快速区分 GPT-3.5 与 GPT-4，并提供免费测试网站10密码学原理技术-第六章-introduction to pulibc-key cryptography