前文添加了简单的测试保证编译通过，本文将开始移植实现一个控制下发
1. base_driver的移植
- 首先把
base_driver.cpp添加到pibot_driver中编译
diff --git a/CMakeLists.txt b/CMakeLists.txt
index c1a7568..ea8fa95 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,6 +10,7 @@ find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
add_executable(pibot_driver src/main.cpp
+ src/base_driver.cpp
)
- 注释ROS1相关以及大部分影响编译的函数实现
如替换#include <ros/ros.h>---->#include <rclcpp/rclcpp.hpp>
/home/pibot/ros2_ws/src/pibot_bringup/src/base_driver.cpp:1:10: fatal error: base_driver.h: No such file or directory
需要在cmake新增include_directories目录
include_directories(
include include
)
同时#include "base_driver.h"修改为#include "pibot_bringup/base_driver.h"
2. 实现Node
ROS2中编译一个node，需要从rclcpp::Node继承,我们删除BaseDriver中的单例，并添加至从rclcpp::Node继承
3. 实例化Node
在main中添加Node实例,并调用loop
#include "pibot_bringup/base_driver.h"
int main(int argc, char* argv[]) {
rclcpp::init(argc, argv);
auto driver = std::make_shared<BaseDriver>();
driver->work_loop();
rclcpp::shutdown();
return 0;
}
- 工作线程运行
在loop中修改
ros::Rate----->rclcpp::WallRate
ros::ok----->rclcpp::ok
void BaseDriver::work_loop() {
// ros::Rate loop(bdg.freq);
rclcpp::WallRate loop(100);
while (rclcpp::ok()) {
// update_param();
// update_odom();
// update_pid_debug();
// update_speed();
// if (bdg.use_imu) {
// if (Data_holder::get()->parameter.imu_type == IMU_TYPE_GY65
// || Data_holder::get()->parameter.imu_type == IMU_TYPE_GY85
// || Data_holder::get()->parameter.imu_type == IMU_TYPE_GY87) {
// update_imu();
// }
// }
loop.sleep();
}
}
- 替换日志输出
ROS_INFO--->RCLCPP_INFO
ROS_WARN--->RCLCPP_WARN
ROS_ERROR--->RCLCPP_ERROR
RCLCPP_XXX接受一个参数标识， Node类中直接使用this->get_logger()，如
RCLCPP_INFO(this->get_logger(), "BaseDriver startup...");
4. 添加cmd_vel的订阅
cmd_vel为标准的速度消息，订阅后转换为串口命令发送给下位机
订阅消息
cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>("/cmd_vel", // 配置后面移植，先硬编代替
10,
std::bind(&BaseDriver::cmd_vel_callback, this, std::placeholders::_1));
返回值定义
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
头文件#include <geometry_msgs/Twist.h>----> #include <geometry_msgs/msg/twist.hpp>
订阅函数subscribe---->create_subscription
订阅返回类型ros::Subscriber---->rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr
消息类型(回调参数类型)geometry_msgs::Twist---->geometry_msgs::msg::Twist
ROS1到ROS2的很多消息定义类似，大部分一样只是多了一个msg域
至此消息定义已添加完成，使用colcon --log-base /dev/null build --symlink-install --event-handlers console_direct+ log-编译可以看到，出现了链接错误，显然少了依赖项目
在CMakeLists.txt添加geometry_msgs的依赖即可正常编译
find_package(geometry_msgs REQUIRED)
...
ament_target_dependencies(pibot_driver
rclcpp
geometry_msgs)
5. 运行测试
尝试ros2 run运行该node, 提示找不到包
➜ ros2 run pibot_bringup pibot_driver
Package 'pibot_bringup' not found
由ROS1的经验，我们执行这里需要生效环境变量，执行. install/setup.bash 再次运行我们可以看到日志输出
➜ . install/setup.bash
➜ ros2 run pibot_bringup pibot_driver
[INFO] [1651597084.976378233] [pibot_dirver]: BaseDriver startup...
[INFO] [1651597084.976500411] [pibot_dirver]: subscribe cmd topic on [/cmd_vel]
[INFO] [1651597086.171644990] [pibot_dirver]: init ok
打开另外的终端输入ros2 topic list和ros2 topic info /cmd_vel可以查看订阅的topic信息
➜ ros2 topic list
/cmd_vel
/parameter_events
/rosout
➜ ros2 topic info /cmd_vel
Type: geometry_msgs/msg/Twist
Publisher count: 0
Subscription count: 1
可以使用下面命令发布消息
ros2_ws ros2 topic pub --rate 20 /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 1.0}}"
[INFO] [1651597691.617713960] [pibot_dirver]: cmd_vel:[0.500 0.000 1.200]
[INFO] [1651597691.667825693] [pibot_dirver]: cmd_vel:[0.500 0.000 1.200]
[INFO] [1651597691.717775974] [pibot_dirver]: cmd_vel:[0.500 0.000 1.200]
可以看到pibot_driver接收到订阅的callback，时间间隔为50ms
本文代码https://gitee.com/pibot/pibot_bringup/tree/bc942bdb49ec5a358a0a13f8359eb5e7878e2350