本节沿用之前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> ¶ms)->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