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
添加以下内容
<library path="polygon_plugins">
<class type="polygon_plugins::Square" base_class_type="polygon_base::RegularPolygon">
<description>This is a square plugin.</description>
</class>
<class type="polygon_plugins::Triangle" base_class_type="polygon_base::RegularPolygon">
<description>This is a triangle plugin.</description>
</class>
</library>
在dev_ws/src/polygon_plugins/CMakeLists.txt
添加以下内容
add_library(polygon_plugins src/polygon_plugins.cpp)
target_include_directories(polygon_plugins PUBLIC
<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
<INSTALL_INTERFACE:include>)
ament_target_dependencies(
polygon_plugins
polygon_base
pluginlib
)
pluginlib_export_plugin_description_file(polygon_base plugins.xml)
install(
TARGETS polygon_plugins
EXPORT export_${PROJECT_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
在ament_package命令之前,添加
ament_export_libraries(
polygon_plugins
)
ament_export_targets(
export_${PROJECT_NAME}
)
使用插件。在dev_ws/src/polygon_base/src/area_node.cpp添加以下内容
#include <pluginlib/class_loader.hpp>
#include <polygon_base/regular_polygon.hpp>
int main(int argc, char** argv)
{
// To avoid unused parameter warnings
(void) argc;
(void) argv;
pluginlib::ClassLoader<polygon_base::RegularPolygon> poly_loader("polygon_base", "polygon_base::RegularPolygon");
try
{
std::shared_ptr<polygon_base::RegularPolygon> triangle = poly_loader.createSharedInstance("polygon_plugins::Triangle");
triangle->initialize(10.0);
std::shared_ptr<polygon_base::RegularPolygon> square = poly_loader.createSharedInstance("polygon_plugins::Square");
square->initialize(10.0);
printf("Triangle area: %.2f\n", triangle->area());
printf("Square area: %.2f\n", square->area());
}
catch(pluginlib::PluginlibException& ex)
{
printf("The plugin failed to load for some reason. Error: %s\n", ex.what());
}
return 0;
}
构建和运行
cd dev_ws
colcon build --packages-select polygon_base polygon_plugins
. install/setup.bash
运行节点
ros2 run polygon_base area_node
运行结果
Triangle area: 43.30
Square area: 100.00
3. Component多节点生成
这部分在之前的一篇文章中就已经提及了,这里就不详细展开说了:文章链接:https://hermit.blog.csdn.net/article/details/125492694
4. TF2坐标系变换
4.1 发布TF
#include <tf2_ros/transform_broadcaster.h>
std::unique_ptr<tf2_ros::TransformBroadcaster> broadcaster;
broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(nodePtr);
geometry_msgs::msg::TransformStamped transform;
transform.header.stamp = node->now();
transform.header.frame_id = "odom";
transform.child_frame_id = "base_link";
// Fill in transform.transform.translation
// Fill in transform.transform.rotation
broadcaster->sendTransform(transform);
4.2 监听TF
#include "tf2_ros/transform_listener.h"
std::shared_ptr<tf2_ros::Buffer> tf_buffer;
std::shared_ptr<tf2_ros::TransformListener> tf_listener;
rclcpp::Node node("name_of_node");
tf_buffer.reset(new tf2_ros::Buffer(node.get_clock()));
tf_listener.reset(new tf2_ros::TransformListener(*tf_buffer_));
4.3 TF变换
TF2 可以通过提供实现的包进行扩展。tf2_geometry_msgs
程序包为 msgs 提供这些。下面的示例使用 geometry_msg::msg::PointStamed
,但这应该适用于 msgs 中的任何数据类型:
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
tf2::Quaternion quat_tf;
geometry_msgs::Quaternion quat_msg = ...;
tf2::convert(quat_msg , quat_tf);
// or
tf2::fromMsg(quat_msg, quat_tf);
// or for the other conversion direction
quat_msg = tf2::toMsg(quat_tf);
convert()
即 将 msg 类型 转换 为 tf 类型fromMsg()
即 将 msg 类型 转换 为 tf 类型toMsg()
即 将 tf 类型 转换 为 msg 类型
transform 函数还可以接受超时。然后它将等待转换可用的时间达到这个时间量:
tf_buffer->transform(in, out, "target_frame", tf2::durationFromSec(1.0));
4.4 获取最新TF
常见的工作方式是获得“最新”转换。在 ros2中,这可以使用 tf2::TimePointZero
来实现,但是需要使用 lookupTransform
然后调用 doTransform (基本上就是在内部进行转换) :
geometry_msgs::msg::PointStamped in, out;
geometry_msgs::msg::TransformStamped transform =
tf_buffer->lookupTransform("target_frame",
in.header.frame_id,
tf2::TimePointZero);
tf2::doTransform(in, out, transform);
5. 懒订阅
ROS2 还没有订户连接回叫。此代码创建一个函数,定期调用该函数来检查我们是否需要启动或停止订阅者:
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/float64.hpp>
class LazyPublisherEx : rclcpp::Node
{
public:
LazyPublisherEx(const rclcpp::NodeOptions & options) :
Node("lazy_ex", options)
{
// Setup timer
timer = this->create_wall_timer(
std::chrono::seconds(1),
std::bind(&LazyPublisher::periodic, this));
}
private:
void periodic()
{
if (pub_.get_subscription_count() > 0)
{
// We have a subscriber, do any setup required
}
else
{
// Subscriber has disconnected, do any shutdown
}
}
rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr pub_;
rclcpp::TimerBase::SharedPtr timer_;
};
6. colcon 编译
6.1 Build
用于在工作区构建 ROS2 包。
生成所有包:
colcon build
要避免在调整 Python 脚本、配置文件和启动文件时重新构建:
colcon build --symlink-install
要修复大多数构建问题,特别是在向工作区添加或删除包或安装新的 RMW 实现的情况下,请清除 CMake 缓存。
colcon build --cmake-clean-cache
6.2 CMake 参数
colcon build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo
6.3 测试
要测试并将结果显示在屏幕上,请执行以下操作:
colcon test
colcon test-result --verbose
构建/测试单个包:
colcon <verb> --packages-select <package-name>
6.4格式化
将输出显示到屏幕上:
colcon <verb> --event-handlers console_direct+
7. 参考链接
https://blog.csdn.net/baimei4833953/article/details/124577887
https://fishros.com/d2lros2foxy/#/
https://docs.ros.org/en/foxy/Tutorials/Beginner-Client-Libraries/Pluginlib.html
评论(0)
您还未登录,请登录后发表或查看评论