ROS自主导航机器人

PIBOT移植ROS2记录(2)

2022-05-04  本文已影响0人  PIBOT导航机器人

前文添加了简单的测试保证编译通过,本文将开始移植实现一个控制下发

1. base_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
                       )
/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;
}
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 listros2 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

上一篇下一篇

猜你喜欢

热点阅读