1. pid參數(shù)調(diào)節(jié)的topic
1.1 移植
pid的輸入輸出作為topic分別輸出，這個(gè)只需要?jiǎng)?chuàng)建一個(gè)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 運(yùn)行測(cè)試
在launch文件中打開out_pid_debug_enable后啟動(dòng)
...
{"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未發(fā)現(xiàn)plot的插件，原因暫時(shí)位置
2. 動(dòng)態(tài)參數(shù)
2.1 移植
不同于ROS1中的dynamic_reconfigure,ROS2中我們直接使用declare_parameter聲明參數(shù)，可以在rqt-reconfigure中動(dòng)態(tài)配置，之前我們?cè)诼暶鲿r(shí)新增了一個(gè)只讀的約束
這里我們還可以新增其他約束以限制參數(shù)設(shè)置的范圍
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);
同時(shí)我們?cè)O(shè)置一個(gè)參數(shù)修改的回調(diào)通知，來根據(jù)設(shè)置的參數(shù)下發(fā)至下位機(jī)
rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr callback_handle_;
callback_handle_ = node->add_on_set_parameters_callback(std::bind(&BaseDriverConfig::SetParametersCallback,
this,
std::placeholders::_1,
node,
rp));
回調(diào)中我們需要循環(huán)所有參數(shù)列表，并且判斷參數(shù)名稱設(shè)置相應(yīng)的變量
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;
}
該回調(diào)被調(diào)用會(huì)設(shè)置一個(gè)update_flag的變量，主線程會(huì)處理執(zhí)行一次參數(shù)同步操作
2.2 運(yùn)行測(cè)試
ros2 launch pibot_bringup bringup_launch.py
ros2 run rqt_reconfigure rqt_reconfigure
不同于ROS1的dynamic_reconfigure, 顯示的參數(shù)不會(huì)按照我們聲明的順序，而是按照字母排序，會(huì)顯得有點(diǎn)雜亂。
image.png
同時(shí)暫時(shí)無法配置枚舉類型，對(duì)于之前的model_type，在ui中無法下拉列表選擇，略顯麻煩
本文代碼https://gitee.com/pibot/pibot_bringup/tree/25ed34acda8a6e850a1c96fcee8d3a762374a135
