1. pid参数调节的topic
1.1 移植
pid的输入输出作为topic分别输出，这个只需要创建一个int类型的pub即可
void BaseDriver::init_pid_debug() {
if (config_.out_pid_debug_enable) {
const char* input_topic_name[MAX_MOTOR_COUNT] = {"motor1_input", "motor2_input", "motor3_input", "motor4_input"};
const char* output_topic_name[MAX_MOTOR_COUNT] = {"motor1_output", "motor2_output", "motor3_output", "motor4_output"};
for (size_t i = 0; i < MAX_MOTOR_COUNT; i++) {
pid_input_pub_[i] = this->create_publisher<std_msgs::msg::Int32>(input_topic_name[i], 10);
pid_output_pub_[i] = this->create_publisher<std_msgs::msg::Int32>(output_topic_name[i], 10);
}
}
}
void BaseDriver::update_pid_debug() {
if (config_.out_pid_debug_enable) {
frame_->interact(ID_GET_PID_DATA);
for (size_t i = 0; i < MAX_MOTOR_COUNT; i++) {
pid_input_msg_[i].data = DataHolder::get()->pid_data.input[i];
pid_output_msg_[i].data = DataHolder::get()->pid_data.output[i];
pid_input_pub_[i]->publish(pid_input_msg_[i]);
pid_output_pub_[i]->publish(pid_output_msg_[i]);
}
}
}
1.2 运行测试
在launch文件中打开out_pid_debug_enable后启动
...
{"out_pid_debug_enable": True},
ros2 launch pibot_bringup bringup_launch.py
查看topic列表
➜ ros2 topic list
/cmd_vel
/motor1_input
/motor1_output
/motor2_input
/motor2_output
/motor3_input
/motor3_output
/motor4_input
/motor4_output
/odom
/parameter_events
/rosout
/tf
1.3 rqt plot
安装rqt-plot
sudo apt-get install ros-galactic-rqt-plot
安装后rqt未发现plot的插件，原因暂时位置
2. 动态参数
2.1 移植
不同于ROS1中的dynamic_reconfigure,ROS2中我们直接使用declare_parameter声明参数，可以在rqt-reconfigure中动态配置，之前我们在声明时新增了一个只读的约束
这里我们还可以新增其他约束以限制参数设置的范围
rcl_interfaces::msg::ParameterDescriptor descriptor;
descriptor.description = "";
descriptor.name = "name";
descriptor.integer_range.resize(1);
descriptor.integer_range[0].from_value = 10;
descriptor.integer_range[0].to_value = 1000;
descriptor.integer_range[0].step = 1;
node->declare_parameter<uint16_t>("wheel_diameter", rp->wheel_diameter, descriptor);
同时我们设置一个参数修改的回调通知，来根据设置的参数下发至下位机
rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr callback_handle_;
callback_handle_ = node->add_on_set_parameters_callback(std::bind(&BaseDriverConfig::SetParametersCallback,
this,
std::placeholders::_1,
node,
rp));
回调中我们需要循环所有参数列表，并且判断参数名称设置相应的变量
rcl_interfaces::msg::SetParametersResult BaseDriverConfig::SetParametersCallback(const std::vector<rclcpp::Parameter>& parameters, rclcpp::Node* node, Robot_parameter* rp) {
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
result.reason = "success";
for (auto& param : parameters) {
RCLCPP_INFO(node->get_logger(), "param %s update", param.get_name().c_str());
if (param.get_name() == "motor1_exchange_flag") {
RCLCPP_INFO(node->get_logger(), "++param %d", rp->motor_nonexchange_flag);
std::bitset<8> val(rp->motor_nonexchange_flag);
val[0] = !param.as_bool();
rp->motor_nonexchange_flag = val.to_ulong();
RCLCPP_INFO(node->get_logger(), "--param %d", rp->motor_nonexchange_flag);
} else if (param.get_name() == "motor2_exchange_flag") {
std::bitset<8> val(rp->motor_nonexchange_flag);
val[1] = !param.as_bool();
rp->motor_nonexchange_flag = val.to_ulong();
} else if (param.get_name() == "motor3_exchange_flag") {
std::bitset<8> val(rp->motor_nonexchange_flag);
val[2] = !param.as_bool();
rp->motor_nonexchange_flag = val.to_ulong();
} else if (param.get_name() == "motor4_exchange_flag") {
std::bitset<8> val(rp->motor_nonexchange_flag);
val[3] = !param.as_bool();
rp->motor_nonexchange_flag = val.to_ulong();
} else if (param.get_name() == "encoder1_exchange_flag") {
std::bitset<8> val(rp->encoder_nonexchange_flag);
val[0] = !param.as_bool();
rp->encoder_nonexchange_flag = val.to_ulong();
} else if (param.get_name() == "encoder2_exchange_flag") {
std::bitset<8> val(rp->encoder_nonexchange_flag);
val[1] = !param.as_bool();
rp->encoder_nonexchange_flag = val.to_ulong();
} else if (param.get_name() == "encoder3_exchange_flag") {
std::bitset<8> val(rp->encoder_nonexchange_flag);
val[2] = !param.as_bool();
rp->encoder_nonexchange_flag = val.to_ulong();
} else if (param.get_name() == "encoder4_exchange_flag") {
std::bitset<8> val(rp->encoder_nonexchange_flag);
val[3] = !param.as_bool();
rp->encoder_nonexchange_flag = val.to_ulong();
} else if (param.get_name() == "model_type") {
rp->model_type = param.as_int();
} else if (param.get_name() == "wheel_diameter") {
rp->wheel_diameter = param.as_int();
} else if (param.get_name() == "wheel_track") {
rp->wheel_track = param.as_int();
} else if (param.get_name() == "encoder_resolution") {
rp->encoder_resolution = param.as_int();
} else if (param.get_name() == "do_pid_interval") {
rp->do_pid_interval = param.as_int();
} else if (param.get_name() == "kp") {
rp->kp = param.as_int();
} else if (param.get_name() == "ki") {
rp->ki = param.as_int();
} else if (param.get_name() == "kd") {
rp->kd = param.as_int();
} else if (param.get_name() == "ko") {
rp->ko = param.as_int();
} else if (param.get_name() == "cmd_last_time") {
rp->cmd_last_time = param.as_int();
} else if (param.get_name() == "max_v_liner_x") {
rp->max_v_liner_x = param.as_int();
} else if (param.get_name() == "max_v_liner_y") {
rp->max_v_liner_y = param.as_int();
} else if (param.get_name() == "max_v_angular_z") {
rp->max_v_angular_z = param.as_int();
} else if (param.get_name() == "imu_type") {
rp->imu_type = param.as_int();
} else if (param.get_name() == "motor_ratio") {
rp->motor_ratio = param.as_int();
}
}
DataHolder::dump_params(rp);
param_update_flag_ = true;
return result;
}
该回调被调用会设置一个update_flag的变量，主线程会处理执行一次参数同步操作
2.2 运行测试
ros2 launch pibot_bringup bringup_launch.py
ros2 run rqt_reconfigure rqt_reconfigure
不同于ROS1的dynamic_reconfigure, 显示的参数不会按照我们声明的顺序，而是按照字母排序，会显得有点杂乱。
image.png
同时暂时无法配置枚举类型，对于之前的model_type，在ui中无法下拉列表选择，略显麻烦
本文代码https://gitee.com/pibot/pibot_bringup/tree/25ed34acda8a6e850a1c96fcee8d3a762374a135
