本文资源获取:https://download.csdn.net/download/2203_75650990/89967717

  笔者购买的是yahboom官方的X3类型的小车,本章节会是对yahboom官方代码的解析(只是导航nav部分),以及一些从ROS1过渡到ROS2的简单知识。

本章中关键词解释:

PC:自己的电脑

宿主机/嵌入式端:Jetson Orin NX

容器:通过docker镜像在宿主机/PC中创建的一种虚拟环境

1.yahboom导航代码解析

        首先yahboom官方代码是基于ROS2的框架,并且并非直接部署在开发板上,而是部署在docker镜像中。这样处理的好处不少,首先就是很强的可移植性,并且大大减少了开发板环境的限制,对于部署项目非常方便。但是docker镜像的缺点也很明显,那就是对开发人员非常不友好,网络连接不好好配置会经常断联不说,开发的时候常常因为忘记及时保存容器,导致好不容易写好的代码全部丢失(这一点在Windows的docker Desktop中得到改善,但是非常不建议设置为关闭容器后立刻注销容器)(docker的定义和用法因为篇幅受限,暂时不做解释)

所以,对于官方的docker,我们需要做进一步处理。

通过ssh远程访问容器内部:

# 进入容器
./run_docker # 这是官方的一个启动容器的脚本,后面会有介绍
# 下载ssh:
sudo apt-get install openssh-server
vim /etc/ssh/sshd_config
# 设置(注意,此处修改的是ssh的config文件,有一个特别需要注意的地方,那就是因为我们的容器运行在嵌入式平台上,而在开发的时候,我们很多时候都需要通过ssh连接嵌入式平台,而ssh的默认端口为22号,这个设定无论是容器内还是容器外均一致,但是我们容器启动时(./run_docker脚本中)有设置 --net=host 所以容器会和宿主机共用一套网络,所以在此处我们需要修改容器内部的ssh端口port为其他的端口,避免冲突):
PermitRootLogin yes
PasswordAuthentication yes
PermitEmptyPasswords yes
# 设置完毕后记得保存容器为新的镜像(宿主机内运行)
docker ps # 找到对应的容器编号
docker commit 9f538156a263 myros2:1.0

容器的启动脚本:

docker run -it \ # docker容器的标准启动
--net=host \ # 这里将容器的网络与宿主机合为一体
--env="DISPLAY" \ # 下面就是一些配置,不需要修改
--env="QT_X11_NO_MITSHM=1" \
-v /tmp/.X11-unix:/tmp/.X11-unix \
--security-opt apparmor:unconfined \
-v /home/jetson/temp:/root/yahboomcar_ros2_ws/temp \
-v /home/jetson/rosboard:/root/rosboard \
-v /home/jetson/maps:/root/maps \
-v /dev/bus/usb/001/010:/dev/bus/usb/001/014 \
-v /dev/bus/usb/001/012:/dev/bus/usb/001/015 \
--device=/dev/astradepth \
--device=/dev/astrauvc \
--device=/dev/video0 \
--device=/dev/myserial \
--device=/dev/rplidar \
--device=/dev/input \
-p 9090:9090 \ # 这里设置端口其实没有很大用处
-p 8888:8888 \
-p 2222:44 \
mysshimage:1.1 /bin/bash # 这里就是启动的镜像名字(名字+编号与commit指令创建时一致)和/bin/bash进入控制台

        这样设置完毕后,每次启动容器后,只需要运行service ssh start就可以从自己的主机PC上直接通过ssh访问容器内部了(注意PC和嵌入式端一定要在同一个网络下)

        随后就是对yahboom官方的导航代码的解析,因为笔者是刚接触ROS2,所以解析比较浅薄,并且只解析了关键节点,此处只展示launch文件中的一些要点,并不会很详细的研究相关代码的运行原理。这里我们通过VSCode的ssh远程开发代码,更加方便:

        注意,这里VSCode开发中和ROS1一样要遵循C++的开发原则,可以设置快捷键一键编译ctrl+shift+b编译:colcon build。但是因为ROS2似乎没有单独的插件,所以这套系统依然是ROS1的,也就是不能用右键一键创建包,只能在命令行中进入工作空间的src下用指令创建包:

ros2 pkg create 包名 --build-type ament_cmake --dependencies 依赖库

        在我们能够进入yahboom的代码空间后,就可以着手整理代码的大体情况,特别是导航部分,得到具体的整理结果如下:

该部分可以按照运行的语句,分为三点进行介绍:

1.底板部分

        底板部分主要就是启动硬件方面的通道,比如图中所示的一些硬件的数据处理,这里是最重要的部分,负责与硬件进行交互,负责处理和接收硬件传感器的数据信息并分析:

2.RVIZ显示启动

该部分是用于启动rviz界面。

3.nav2导航模块启动

        该部分则用于开启nav2导航,此处启动nav与ROS1不同,在ROS1中:首先加载地图文件,第二步启动静态变换,第三步启动amcl,第四步启动move_base就可以实现导航全流程。ROS2中前两步差不多。但是amcl和move_base启动不太相同,amcl被集成到了nav2_controller节点,而move_base的全局规划和本地规避功能被拆分集成到了nav2_planner和nav2_controller节点。此外,nav2架构中引入了一个全新的nav2_bt_navigator节点,作为行为树导航器,协调并管理其他节点的执行。nav2_waypoint_follower节点则专门用于跟踪全局规划出的路径。这些节点都可以通过bringup_launch.py一次性启动。

2.ROS2:插件安装和接口编写

        ROS2相比于ROS1,从结构层面有了较大的变化,其中最直观的就是去掉了roscore的设计,现在每个节点都是独立运行,不需要依附于roscore。而ROS2保留了ROS1的大部分指令,ROS2的指令与ROS1指令也是极为相似,如rosrun=ros2 run,rostopic=ros2 topic,以此类推。ROS2的安装也比ROS1简单方便,基本不会出现很离谱的报错情况:

安装ROS2的bug:
在vim /etc/hosts中插入aw.githubusercontent.com所在的服务器IP,这个IP通过https://www.ipaddress.com/ 获知

再次重申一下我们的工程设想:创建三个包,第一个是插件包,主要用来定义规划器接口,最开始我的工程是在ROS1上运行的,所以是继承nav_core::BaseGlobalPlanne类并实现其中的方法。然后我采用了工厂模式,动态创建了算法.so共享库文件,通过传递共享库的地址到参数服务器上,然后由插件包去获取对应算法共享库的地址并调用。第二个包是接口包,定义了算法需要实现的方法,第三个包是算法包,主要是继承接口包,并实现其中的方法。

经过查询资料:ROS2中添加自定义路径规划器插件的流程与ROS1有一些区别:

插件继承的基类不同

在ROS1中,是继承了nav_core::BaseGlobalPlanner类来定义路径规划器插件。而在ROS2中,需要继承nav2_core::controller::PluginBase类。

构建系统不同

ROS1中使用的是catkin构建系统,而ROS2使用的是colcon构建系统。相应地,package.xmlCMakeLists.txt文件的格式和内容也有所不同。

依赖项不同

由于ROS2的架构和组件与ROS1不同,所以您需要链接不同的依赖项,例如nav2_corerclcpp等。

创建包

ros2 pkg create my_planner_plugin --build-type ament_cmake --dependencies angles nav2_regulated_pure_pursuit_controller nav2_costmap_2d geometry_msgs nav2_core nav2_util rclcpp my_planner_interface nav_msgs pluginlib
​
ros2 pkg create my_planner_interface --build-type ament_cmake --dependencies angles geometry_msgs nav_msgs rclcpp nav2_costmap_2d
​
ros2 pkg create my_astar --build-type ament_cmake --dependencies angles nav2_regulated_pure_pursuit_controller nav2_costmap_2d nav2_core nav2_util rclcpp tf2 tf2_geometry_msgs tf2_ros my_planner_interface

注册插件

  1. 创建一个插件包,用于定义插件类并注册插件。

  2. 创建一个接口包,定义算法需要实现的纯虚函数接口。

  3. 创建一个或多个算法包,继承接口包中定义的接口,并实现具体的算法逻辑。

  4. 在插件包中实例化算法包中的具体算法类,并将其注册为插件。

  5. 修改Navigation2配置文件,指定要加载的插件类型。

具体插件配置流程和要点如下:

1.插件包需要包含pluginlib库,同时在cpp中引用:

PLUGINLIB_EXPORT_CLASS(my_planner_namespace::MyPlannerPlugin, nav2_core::GlobalPlanner)

这里的my_planner_namespace::MyPlannerPlugin是插件的工作空间和类名

2.建立my_planner_plugin.xml文件,其中填入:

<library path="my_planner_plugin"> <!--注意这里是插件包包名-->
    <!--注意name是‘包名/自定义’-->
    <class name="my_planner_plugin/my_plugin"  
    type="my_planner_namespace::MyPlannerPlugin" 
    base_class_type="nav2_core::GlobalPlanner">
        <description>My custom global planner plugin</description>
    </class>
</library>

name:插件名:(插件包名)/(自己定义一个名称)

type:第一点中的(工作空间名)/(类名)

base_class_type:"nav2_core::GlobalPlanner"

3.cmake文件:

cmake_minimum_required(VERSION 3.5)
project(my_planner_plugin)
# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# 编译和链接所需的依赖包
find_package(ament_cmake REQUIRED)
find_package(angles REQUIRED)
find_package(nav2_regulated_pure_pursuit_controller REQUIRED)
find_package(nav2_costmap_2d REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav2_core REQUIRED)
find_package(nav2_util REQUIRED)
find_package(rclcpp REQUIRED)
find_package(my_planner_interface REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(pluginlib REQUIRED)
​
include_directories( # 将插件的头文件目录和依赖包的头文件目录加入编译器的搜索路径
  include
  ${my_planner_interface_INCLUDE_DIRS}
)
# 构建插件库,并链接所需的依赖库
add_library(my_planner_plugin SHARED
  src/my_planner_plugin.cpp
)
target_link_libraries(my_planner_plugin
  ${angles_LIBRARIES}
  ${nav2_regulated_pure_pursuit_controller_LIBRARIES}
  ${nav2_costmap_2d_LIBRARIES}
  ${geometry_msgs_LIBRARIES}
  ${nav2_core_LIBRARIES}
  ${nav2_util_LIBRARIES}
  ${rclcpp_LIBRARIES}
  ${my_planner_interface_LIBRARIES}
  ${nav_msgs_LIBRARIES}
  ${pluginlib_LIBRARIES}
)
ament_target_dependencies(${PROJECT_NAME}
  pluginlib
)
target_include_directories(${PROJECT_NAME} PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
  ${my_planner_interface_INCLUDE_DIRS}
)
pluginlib_export_plugin_description_file(nav2_core my_planner_plugin.xml) # 导出插件描述文件
# Install rules
install(TARGETS my_planner_plugin
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin  
)
# 安装插件描述文件
install(FILES my_planner_plugin.xml
  DESTINATION share/${PROJECT_NAME}
)
# 声明依赖和导出头文件
ament_export_dependencies(my_planner_interface)
ament_export_include_directories(include ${my_planner_interface_INCLUDE_DIRS})
ament_package()

4.package中:

<export>
    <build_type>ament_cmake</build_type>
    <nav2_core plugin="${prefix}/my_planner_plugin.xml"/> 
</export>

5.在负责导航的包中,修改对应的导航器的参数文件中的planner_server部分:

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    use_sim_time: False
    planner_plugins: ["GridBased"]
    GridBased:
      # 自定义代码
      plugin: "my_planner_plugin/my_plugin"
      interpolation_resolution: 0.1
      #   plugin: "nav2_navfn_planner/NavfnPlanner"
      #   tolerance: 0.5
      #   use_astar: false
      #   allow_unknown: true

完成以上配置后ROS2就能通过:

参数文件配置 -> 到共享库:包名工作空间/install/<name>/lib中寻找对应的库 -> 到工作空间/install/<name>/share/my_planner_plugin.xml文件 (通过本地代码的xml加载的) -> 加载插件

定义算法接口

        我们需要尽量将算法和我们的插件解耦合,所以我们单独定义一个算法接口包,用于算法实现以及让插件获取算法库。初步思路是定义一个接口包,算法包和插件包共同引用这个接口,算法包实现接口包中的函数,插件包实现接口包中的共享库方法。算法会将自己编译为.so共享库文件,并存放在一个文件夹中,该地址会上传至参数服务器,插件启动时会从参数服务器获取该地址,以运行该算法中的方法。这样就只需要修改这个共享库的地址就可以切换不同的算法,不需要修改插件的代码,实现了完全的解耦。

CMake编写初步指南
1.find_package
find_package(ament_cmake REQUIRED)
find_package(angles REQUIRED)
...

find_package的作用就是找到对应的软件包,REQUIRED表示如果无法找到就会导致cmake报错并终止。写成一行 find_package(... COMPONENTS ...) 或者分开写,本质上是一样的。分开写的好处是可读性更好,更容易管理依赖项。

2.include_directories
include_directories(
  include
  ${my_planner_interface_INCLUDE_DIRS}
)

        当C++代码中使用#include <header.hpp>语句时,编译器需要知道在哪里可以找到这个header.hpp头文件。include_directories命令就是用来告诉编译器应该在哪些目录中搜索头文件。其中include是本地的include目录,也就是当前包文件夹下的include文件夹:

my_package/
├── CMakeLists.txt
├── include/ # 这里
│   ├── my_package/
│   │   ├── header1.hpp
│   │   └── header2.hpp
├── src/
│   ├── source1.cpp
│   └── source2.cpp

        而其中${my_planner_interface_INCLUDE_DIRS}则是通过find_package(my_planner_interface REQUIRED) 获取,由该分享的库,通过

# 安装头文件,制 include/${PROJECT_NAME}/ 目录下所有的 .h 和 .hpp 文件到安装目录的#include/${PROJECT_NAME}/ 路径下
install(
  DIRECTORY include/${PROJECT_NAME}/
  DESTINATION include/${PROJECT_NAME}
  FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp"
)
# 导出包依赖和头文件目录,使得其他包在使用该包时,可以获取到正确的依赖信息和头文件路径。
ament_export_include_directories(include)
ament_export_dependencies(nav2_costmap_2d)

导出的头文件地址。

        以上代码是很重要的部分,其中install指令主要负责将工程的某些生成物安装在对应的位置,比如说此处的代码就是从include/${PROJECT_NAME}/目录中获取.h文件,安装到安装目录install的include/${PROJECT_NAME}/位置中。还可以:

install(
  FILES my_planner_plugin.xml
  DESTINATION share/${PROJECT_NAME}
)

将my_planner_plugin.xml安装到安装目录install的share/${PROJECT_NAME}中。

3.target_link_libraries
target_link_libraries(my_exe
  my_lib
  ${dependency_libs}
)

        为目标(可执行文件/库)链接所需的库依赖项,以上命令就会将 my_lib 库和 ${dependency_libs} 中包含的库链接到 my_exe 可执行文件。与include_directories不同的是,该函数侧重于在链接阶段满足对应的库依赖,而include_directories函数主要侧重于头文件的搜索

4.target_include_directories

这条命令用于为目标添加头文件的搜索路径。它指定了编译器在编译该目标时应该在哪些目录中寻找所需的头文件。

target_include_directories(my_lib
  PUBLIC
    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
    $<INSTALL_INTERFACE:include>
)

可以理解为有针对性的对my_lib设定对应的库,使my_lib可以使用对应库。

示例

        举个例子:三个自己写的工程ABC,其中C是最终输出工程用于顶层的显示和交互,A是底层硬件层,用于硬件交互,B需要调用A的API获取硬件数据,进行分析后,C调用B的分析结果。

三个工程 A、B 和 C,它们之间存在依赖关系:

  • C 依赖于 B

  • B 依赖于 A

工程 A:

  1. include_directories(include) - 将本地的 include 目录添加到头文件搜索路径中,这样 A 的源码就能正确 #include 自己的头文件。

  2. ament_export_include_directories(include) - 导出 include 目录作为 A 的头文件安装路径,这样其他工程(如 B)在链接 A 时,就可以找到 A 的头文件路径。

工程 B:

  1. find_package(A REQUIRED) - 查找并加载 A 工程。

  2. include_directories(...) - 添加 B 自身的头文件路径,以及通过上一步获取的 ${A_INCLUDE_DIRS}。这样 B 的源码就可以正确 #include A 导出的头文件。

  3. target_link_libraries(... ${A_LIBRARIES}) - 将 B 链接到 A 提供的库。

  4. ament_export_include_directories(...) - 导出 B 自身的头文件路径。

  5. ament_export_dependencies(A) - 导出对 A 的依赖关系。

工程 C:

  1. find_package(B REQUIRED) - 查找并加载 B 工程。

  2. include_directories(...) - 添加 C 自身的头文件路径,以及通过上一步获取的 ${B_INCLUDE_DIRS}。这样 C 的源码就可以正确 #include B 导出的头文件。

  3. target_link_libraries(... ${B_LIBRARIES}) - 将 C 链接到 B 提供的库。

接口包的处理

  • 接口包的正确组织方式

my_planner_interface/
├── include/
│   └── my_planner_interface/     # 命名空间文件夹
│       └── my_planner_interface.h # 接口头文件
├── CMakeLists.txt
└── package.xml
  • 接口包的 CMakeLists.txt 配置

cmake_minimum_required(VERSION 3.5)
project(my_planner_interface)
​
# 配置 C++ 标准
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()
​
# 查找必要的依赖包
find_package(ament_cmake REQUIRED)
find_package(nav2_costmap_2d REQUIRED)  # 示例依赖
​
# 声明包含目录
include_directories(
  include
)
​
# 安装头文件
install(
  DIRECTORY include/${PROJECT_NAME}/
  DESTINATION include/${PROJECT_NAME}
)
​
# 导出依赖和包含目录
ament_export_include_directories(include)
ament_export_dependencies(nav2_costmap_2d)
​
ament_package()
  • 接口包的 package.xml 配置
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>my_planner_interface</name>
  <version>0.0.1</version>
  <description>Planner interface package</description>
  <maintainer email="your@email.com">Your Name</maintainer>
  <license>TODO</license>
​
  <buildtool_depend>ament_cmake</buildtool_depend>
  
  <depend>nav2_costmap_2d</depend>
​
  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>
  • 使用接口的包的 CMakeLists.txt 配置

cmake_minimum_required(VERSION 3.5)
project(my_planner_plugin)
​
# 查找接口包
find_package(my_planner_interface REQUIRED)
​
# 添加包含目录
include_directories(
  include
  ${my_planner_interface_INCLUDE_DIRS}  # 添加接口包的头文件目录
)
​
# 创建库并链接
add_library(my_planner_plugin SHARED
  src/my_planner_plugin.cpp
)
​
# 链接依赖
target_link_libraries(my_planner_plugin
  ${my_planner_interface_LIBRARIES}
)
​
# 声明依赖,ROS2特殊库
ament_target_dependencies(my_planner_plugin
  my_planner_interface
)
  • 使用接口的包的 package.xml 配置

<package format="3">
  <!-- ... 其他配置 ... -->
  <depend>my_planner_interface</depend>
  <!-- ... 其他依赖 ... -->
</package>
  • 在代码中使用接口

// 使用完整路径引用头文件
#include "my_planner_interface/my_planner_interface.h"
// 使用命名空间
using namespace my_planner_interface;
// 或者直接使用完整命名空间
my_planner_interface::PlannerInterface* planner;

编写路劲规划算法代码

        此处于上一章中在Gazebo中定义算法接口和算法一模一样,只需要进行两步操作即可:

1.继承上文所述的算法接口并实现其中的方法。

2.在CMakeList中配置动态库的保存,并在插件包的libpath处修改对应算法动态库的地址用于调用算法:

# 部分cmake文件内容,其中比较重要的就是动态库的生成和输出
# 添加包含目录
include_directories(
  include
  ${my_planner_interface_INCLUDE_DIRS}
)

# 添加库
add_library(${PROJECT_NAME} SHARED
  src/astar_planner.cpp
)

# 设置目标属性和依赖
target_include_directories(${PROJECT_NAME} PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
)

# 添加依赖
ament_target_dependencies(${PROJECT_NAME}
  angles
  nav2_regulated_pure_pursuit_controller
  nav2_costmap_2d
  nav2_core
  nav2_util
  rclcpp
  tf2
  tf2_geometry_msgs
  tf2_ros
  my_planner_interface
)

# 设置库输出目录
set(CUSTOM_LIB_DIR "${CMAKE_CURRENT_SOURCE_DIR}/../my_planner_plugin/lib")
set_target_properties(${PROJECT_NAME} PROPERTIES
  LIBRARY_OUTPUT_DIRECTORY ${CUSTOM_LIB_DIR}
)

# 安装库
install(TARGETS ${PROJECT_NAME}
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
)

# 如果有头文件需要安装
install(
  DIRECTORY include/
  DESTINATION include/
)

ament_export_dependencies(my_planner_interface)
ament_export_include_directories(include)
ament_export_libraries(${PROJECT_NAME})

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

      综上,我们系统介绍了ROS2对应配置的部分的重要信息,其中包含了许多C++的知识和Cmake的编写要点,比较杂乱,但是对工程的成功配置必不可少。本章节主要是介绍了如何在ROS2中编写插件和编写接口,顺便介绍了一些CMakeList的编写技巧。其中并没有很多代码相关,所有的代码均采自第二章中的代码,思想基本一致,只是对对应的库进行了替换。

3.运行

按照上述内容配置好后,我们就可以启用自己的插件(特别注意:这里的插件设置一定要正确:name:插件名:(插件包名)/(自己定义一个名称),这里不正确会很难查错,笔者就是在这里的名字定义有误,导致插件系统无法查询到笔者的插件,于是整个插件系统全面瘫痪)。

gmapping建图

按照yahboom官方的指示,运行:

启动建图:

ros2 launch yahboomcar_nav map_gmapping_launch.py 

启动rviz显示地图(PC):

rviz2

控制小车缓慢:

ros2 run yahboomcar_ctrl yahboom_keyboard

获得结果:

将该地图放到yahboomcar_nav 包下的maps文件夹中即可被成功调用(ROS2的nav2必须要导入一张地图才可以使用,所以必须要新建地图)

nav2导航(插件版)

此处笔者实现了一个简单的A* 算法,按照以下顺序运行

启动底盘:

ros2 launch yahboomcar_nav laser_bringup_launch.py

启动rviz(和对应的配置文件):

rviz2

启动导航:

ros2 launch yahboomcar_nav navigation_teb_launch.py

运行成功:

寻路成功:

小车运动:

        至此,小车从虚拟环境到实体化的插件部署已经全部完成,这套流程有很多不完善的地方,特别是借用了他人的平台运行,后续将会陆续更新底层的硬件驱动策略,也就是bringup部分的攻略。其次就是一些对算法的验证和优化策略。

Logo

立足具身智能前沿赛道,致力于搭建全球化、开源化、全栈式技术交流与实践共创平台。

更多推荐