0. 简介
作者最近发现ROS2目前的功能越来越完善了，其中也新增了很多比较好用的高级玩法，这里作者来一个个向大家展示。这里是小鱼做的ROS2官方文档的中文翻译平台，可以学习和推荐一下
1. 动态参数
1.1 代码编写
对于动态参数，大家学过ROS1的话应该都应该有所耳闻吧，ROS1的动态参数的操作还需要dynamic_reconfigure，ROS2中我们直接使用declare_parameter声明参数，可以在rqt-reconfigure中动态配置，之前我们在声明时新增了一个只读的约束。我们这里参考gitee中的代码。
首先和ROS1一样设置cfg文件：
#!/usr/bin/env python
PACKAGE = 'pibot_bringup'
from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, str_t, double_t, bool_t, int_t
gen = ParameterGenerator()
model_type_enum = gen.enum([ gen.const("2WD_Diff", int_t, 1, "Two-wheel Diff Drive"),
gen.const("4WD_Diff", int_t, 2, "Four-wheel Diff Drive"),
gen.const("3WD_Omni", int_t, 101, "Three-wheel Omni Drvie"),
# gen.const("4WD_Omni", int_t, 102, "Four-wheel Omni Drvie"),
gen.const("4WD_Mecanum", int_t, 201, "Four-wheel Mecanum Drvie"),
gen.const("UNKNOWN", int_t, 255, "unknown model")],
"pibot dirver list")
gen.add("model_type", int_t, 0, "model type", 1, 1, 255, edit_method = model_type_enum)
gen.add("motor1_exchange_flag", bool_t, 0, "exchange motor1 wire", False)
gen.add("motor2_exchange_flag", bool_t, 0, "exchange motor2 wire", False)
gen.add("motor3_exchange_flag", bool_t, 0, "exchange motor3 wire", False)
gen.add("motor4_exchange_flag", bool_t, 0, "exchange motor4 wire", False)
gen.add("encoder1_exchange_flag", bool_t, 0, "exchange encoder1 wire", False)
gen.add("encoder2_exchange_flag", bool_t, 0, "exchange encoder2 wire", False)
gen.add("encoder3_exchange_flag", bool_t, 0, "exchange encoder3 wire", False)
gen.add("encoder4_exchange_flag", bool_t, 0, "exchange encoder4 wire", False)
gen.add("wheel_diameter", int_t, 0, "The diameter of the wheel", 30, 10, 500)
gen.add("wheel_track", int_t, 0, "The track of the wheel", 300, 50, 1000)
gen.add("encoder_resolution", int_t, 0, "The resolution of the encoder", 1560, 1 , 32000)
gen.add("motor_ratio", int_t, 0, "The ratio of the motor", 1, 1, 1000)
gen.add("do_pid_interval", int_t, 0, "The interval of do pid", 10, 1, 80)
gen.add("kp", int_t, 0, "Kp value", 20, 0, 10000)
gen.add("ki", int_t, 0, "Ki value", 20, 0, 32000)
gen.add("kd", int_t, 0, "Kd value", 20, 0, 1000)
gen.add("ko", int_t, 0, "Ko value", 20, 0, 1000)
gen.add("cmd_last_time", int_t, 0, "cmd_last_time value", 200, 0, 1000)
gen.add("max_v_liner_x", int_t, 0, "liner x", 60, 0, 500)
gen.add("max_v_liner_y", int_t, 0, "liner y", 0, 0, 500)
gen.add("max_v_angular_z", int_t, 0, "angular z", 120, 0, 2000)
imu_type_enum = gen.enum([ gen.const("GY65", int_t, 49, "mpu6050"),
gen.const("GY85", int_t, 69, "adxl345_itg3200_hmc5883l"),
gen.const("GY87", int_t, 71, "mpu6050_hmc5883l"),
gen.const("nonimu", int_t, 255, "disable imu")],
"imu type list")
gen.add("imu_type", int_t, 0, "imu type", 69, 1, 255, edit_method = imu_type_enum)
exit(gen.generate(PACKAGE, "pibot_bringup", "pibot_driver"))
同时还可以新增其他约束以限制参数设置的范围
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);
同时我们设置一个参数修改的回调通知，来根据设置的参数下发至下位机
#include <vector>
#include <rclcpp/rclcpp.hpp>
struct Robot_parameter {
union {
char buff[64];
struct
{
unsigned short wheel_diameter; //轮子直径 mm
unsigned short wheel_track; //差分：轮距， 三全向轮：直径，四全向：前后轮距+左右轮距 mm
unsigned short encoder_resolution; //编码器分辨率
unsigned char do_pid_interval; // pid间隔 (ms)
unsigned short kp;
unsigned short ki;
unsigned short kd;
unsigned short ko; // pid参数比例
unsigned short cmd_last_time; //命令持久时间ms 超过该时间会自动停止运动
unsigned short max_v_liner_x; // 最大x线速度
unsigned short max_v_liner_y; // 最大y线速度
unsigned short max_v_angular_z; // 最大角速度
unsigned char imu_type; // imu类型 参见IMU_TYPE
unsigned short motor_ratio; // 电机减速比
unsigned char model_type; // 运动模型类型 参见MODEL_TYPE
unsigned char motor_nonexchange_flag; // 电机标志参数 1 正接 0 反接(相当于电机线交换)
unsigned char encoder_nonexchange_flag; // 编码器标志参数 1 正接 0 反接(相当于编码器ab相交换)
};
};
};
class MyNode : public rclcpp::Node
{
public:
MyNode()
{
Robot_parameter* rp;
// Declare parameters first
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);
// Then create callback
callback_handle_= this->add_on_set_parameters_callback(
std::bind(&MyNode::SetParametersCallback, this, std::placeholders::_1,rp));
}
private:
// This will get called whenever a parameter gets updated
rcl_interfaces::msg::SetParametersResult SetParametersCallback(
const std::vector<rclcpp::Parameter> & parameters, Robot_parameter* rp)
{
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
result.reason = "success";
for (auto& param : parameters) {
RCLCPP_INFO(this->get_logger(), "param %s update", param.get_name().c_str());
if (param.get_name() == "motor1_exchange_flag") {
RCLCPP_INFO(this->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(this->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;
}
// Need to hold a pointer to the callback
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr callback_handle_;
rcl_interfaces::msg::ParameterDescriptor descriptor;
};
该回调被调用会设置一个update_flag的变量，主线程会处理执行一次参数同步操作
1.2 运行测试
ros2 launch pibot_bringup bringup_launch.py
ros2 run rqt_reconfigure rqt_reconfigure
不同于ROS1的dynamic_reconfigure, 显示的参数不会按照我们声明的顺序，而是按照字母排序，会显得有点杂乱。
2. 动态插件
ROS2的动态插件和ROS1其实差别不太大，但是写法还是有所差距的，这里也举个栗子，这里参照了官网的程序
安装pluginlib
sudo apt-get install ros-foxy-pluginlib
创建一个新包
cd dev_ws/src
ros2 pkg create --build-type ament_cmake polygon_base --dependencies pluginlib --node-name area_node
编辑文件dev_ws/src/polygon_base/include/polygon_base/regular_polygon.hpp并添加以下内容
#ifndef POLYGON_BASE_REGULAR_POLYGON_HPP
#define POLYGON_BASE_REGULAR_POLYGON_HPP
namespace polygon_base
{
class RegularPolygon
{
public:
virtual void initialize(double side_length) = 0;
virtual double area() = 0;
virtual ~RegularPolygon(){}
protected:
RegularPolygon(){}
};
} // namespace polygon_base
#endif // POLYGON_BASE_REGULAR_POLYGON_HPP
在dev_ws/src/polygon_base/CMakeLists.txt添加。在ament_target_dependencies命令后添加以下行
install(
DIRECTORY include/
DESTINATION include
)
在命令之前添加此ament_package命令
ament_export_include_directories(
include
)
创建插件包
cd dev_ws/src
ros2 pkg create --build-type ament_cmake polygon_plugins --dependencies polygon_base pluginlib --library-name polygon_plugins
编辑文件 dev_ws/src/polygon_plugins/src/polygon_plugins.cpp
#include <polygon_base/regular_polygon.hpp>
#include <cmath>
namespace polygon_plugins
{
class Square : public polygon_base::RegularPolygon
{
public:
void initialize(double side_length) override
{
side_length_ = side_length;
}
double area() override
{
return side_length_ * side_length_;
}
protected:
double side_length_;
};
class Triangle : public polygon_base::RegularPolygon
{
public:
void initialize(double side_length) override
{
side_length_ = side_length;
}
double area() override
{
return 0.5 * side_length_ * getHeight();
}
double getHeight()
{
return sqrt((side_length_ * side_length_) - ((side_length_ / 2) * (side_length_ / 2)));
}
protected:
double side_length_;
};
}
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(polygon_plugins::Square, polygon_base::RegularPolygon)
PLUGINLIB_EXPORT_CLASS(polygon_plugins::Triangle, polygon_base::RegularPolygon)
创建XML文件，在dev_ws/src/polygon_plugins/plugins.xml添加以下内容
