ArduPilot开源代码之ROS2Humble+CartographerSLAM+SITL+Gazebo

1. 源由

从测试的角度分析问题，大致有以下几个逻辑过程：

1. 【白盒】逻辑走读
2. 【灰盒】单元测试
3. 【黑盒】系统测试(仿真)

这个Ardupilot的代码用于无人机，更要面临程序异常导致的坠机问题。

因此，仿真环境的验证就显得尤为重要。可以避免：

1. 设备损坏
2. 事故发生
3. 快速验证

当然，仿真也有一些不现实的问题，但是它是与实际飞行最接近的环境。在实际试飞前，依然需要做各种用例和方法来确保飞行安全。

2. 仿真环境

Ardupilot仿真环境的组成：

• Ardupilot SITL / Flight Control
• Gazebo Harmonic / Robotics Simulator
• ROS2 Humble / Companion Computer
• Cartographer SLAM / Non-GPS Position

3. 环境搭建

3.1 仿真硬件

基于《Linux 36.3@Jetson Orin Nano之系统安装》，在Jetson Orin Nano 8GB硬件上进行仿真测试。

3.2 Gazebo Harmonic安装

参考Gazebo Harmonic, Binary Installation on Ubuntu：

• Step 1: 更新系统

$ sudo apt-get update
$ sudo apt-get install lsb-release gnupg

• Step 2: 添加Gazebo秘钥

$ curl https://packages.osrfoundation.org/gazebo.gpg --output /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg

由于网络等问题，curl可以分步骤执行，以下是curl分布执行所需要使用到的命令集合：

$ export https_proxy=http://192.168.1.10:808
$ curl https://packages.osrfoundation.org/gazebo.gpg --output gazebo.gpg
$ sudo mv gazebo.gpg /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg

注：相关代理命令请参考apt-get通过代理更新系统。

• Step 3: 添加Gazebo源

$ echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null

• Step 4: 安装Gazebo Harmonic

$ sudo apt-get update
$ sudo apt-get install gz-harmonic

• 其他（若要删除Gazebo）

$ sudo apt remove gz-harmonic && sudo apt autoremove

3.3 ROS2 Humble系统安装

参考ROS2 Humble, Debian packages for ROS 2 on Ubuntu：

• Step 1: 设置环境

$ locale  # check for UTF-8

$ sudo apt update && sudo apt install locales
$ sudo locale-gen en_US en_US.UTF-8
$ sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
$ export LANG=en_US.UTF-8

$ locale  # verify settings

• Step 2: 添加ROS2秘钥

$ sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg

由于网络等问题，curl可以分步骤执行，以下是curl分布执行所需要使用到的命令集合：

$ export https_proxy=http://192.168.1.10:808
$ curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o ros-archive-keyring.gpg
$ sudo mv ros-archive-keyring.gpg /usr/share/keyrings/

• Step 3: 添加ROS2源

$ sudo apt install software-properties-common
$ sudo add-apt-repository universe
$ echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null

• Step 4: 安装ROS2 Humble

$ sudo apt update
$ sudo apt install ros-humble-desktop

• 其他（ROS2环境设置）

$ source /opt/ros/humble/setup.bash

3.4 ROS2 Ardupilot工作环境

参考Companion Computers - ROS 2：

• Step 1: 新建工程环境

$ cd ~
$ mkdir ros2_ws
$ cd ~/ros2_ws
$ vcs import --recursive --input  https://raw.githubusercontent.com/ArduPilot/ardupilot/master/Tools/ros2/ros2.repos src

由于网络等问题，vcs可以分步骤执行，以下是vcs分布执行所需要使用到的命令集合：

$ curl -O https://raw.githubusercontent.com/ArduPilot/ardupilot_gz/main/ros2_gz.repos
$ vcs import --input ros2_gz.repos src

部分代码库可能有雨容量较大，下载经常出现问题，采用SSH可以解决：

$ diff -urN ros2_gz.repos.bak ros2_gz.repos
--- ros2_gz.repos.bak   2024-07-30 18:26:51.804632126 +0800
+++ ros2_gz.repos       2024-07-30 18:27:53.341738354 +0800
@@ -1,11 +1,11 @@
 repositories:
   ardupilot:
     type: git
-    url: https://github.com/ArduPilot/ardupilot.git
+    url: git@github.com:ArduPilot/ardupilot.git
     version: master
   ardupilot_gazebo:
     type: git
-    url: https://github.com/ArduPilot/ardupilot_gazebo.git
+    url: git@github.com:ArduPilot/ardupilot_gazebo.git
     version: ros2
   ardupilot_gz:
     type: git
@@ -13,7 +13,7 @@
     version: main
   ardupilot_sitl_models:
     type: git
-    url: https://github.com/ArduPilot/SITL_Models.git
+    url: git@github.com:ArduPilot/SITL_Models.git
     version: main
   micro_ros_agent:
     type: git

采用分布下载代码库，需要对每个模块的子模块进行代码同步：

$ cd src/<module>
$ git submodule update --init --recursive

• Step 2: ROS2环境和依赖更新

$ sudo apt update
$ rosdep update
$ source /opt/ros/humble/setup.bash
$ rosdep install --from-paths src --ignore-src

• Step 3: 安装microxrceddsgen

$ sudo apt install default-jre
$ cd ~/ros2_ws
$ git clone --recurse-submodules https://github.com/ardupilot/Micro-XRCE-DDS-Gen.git
$ cd Micro-XRCE-DDS-Gen
$ ./gradlew assemble
$ echo "export PATH=\$PATH:$PWD/scripts" >> ~/.bashrc

测试

$ source ~/.bashrc
$ microxrceddsgen -version
openjdk version "11.0.23" 2024-04-16
OpenJDK Runtime Environment (build 11.0.23+9-post-Ubuntu-1ubuntu122.04.1)
OpenJDK 64-Bit Server VM (build 11.0.23+9-post-Ubuntu-1ubuntu122.04.1, mixed mode)
microxrceddsgen version: null

• Step 4: 编译ROS2 Arudpilot工作环境

$ cd ~/ros2_ws
$ colcon build --packages-up-to ardupilot_dds_tests

若编译报错，执行以下命令：

$ colcon build --packages-up-to ardupilot_dds_tests --event-handlers=console_cohesion+

测试

$ cd ~/ros2_ws
$ source ./install/setup.bash
$ colcon test --packages-select ardupilot_dds_tests
$ colcon test-result --all --verbose

3.5 ROS2启动SITL

参考:

• ROS 2 with SITL

• ArduPilot ROS 2 packages

• Step 1: 编译SITL

$ source /opt/ros/humble/setup.bash
$ cd ~/ros2_ws/
$ colcon build --packages-up-to ardupilot_sitl

或者参考《Ardupilot开源飞控工程项目编译回顾》，可以直接在ardupilot工程下编译。

• Step 2: ROS2启动SITL

$ source ~/ros2_ws/install/setup.bash
$ ros2 launch ardupilot_sitl sitl_dds_udp.launch.py transport:=udp4 refs:=$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/dds_xrce_profile.xml synthetic_clock:=True wipe:=False model:=quad speedup:=1 slave:=0 instance:=0 defaults:=$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/copter.parm,$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/dds_udp.parm sim_address:=127.0.0.1 master:=tcp:127.0.0.1:5760 sitl:=127.0.0.1:5501

• Step 3: mavprxoy启动地图&控制台

$ mavproxy.py --console --map --aircraft test --master=:14550

• Step 4: mavproxy操作飞机

> mode guided
> arm throttle
> takeoff 5.0
> velocity 10 0 0
> mode RTL

3.6 Gazebo+ROS2启动SITL

参考：

• ROS 2 with Gazebo

• 章节3.4 ROS2 Ardupilot工作环境

• Step 1: 工作环境代码clone

$ cd ~/ros2_ws
$ vcs import --input https://raw.githubusercontent.com/ArduPilot/ardupilot_gz/main/ros2_gz.repos --recursive src

• Step 2: 指定gazbo版本

$ export GZ_VERSION=harmonic

• Step 3: 更新ROS2依赖库

$ cd ~/ros2_ws
$ source /opt/ros/humble/setup.bash
$ sudo apt update
$ rosdep update
$ rosdep install --from-paths src --ignore-src -r

• Step 4: 编译Ardupilot ROS2组件

$ cd ~/ros2_ws
$ colcon build --packages-up-to ardupilot_gz_bringup

测试

$ cd ~/ros2_ws
$ source install/setup.bash
$ colcon test --packages-select ardupilot_sitl ardupilot_dds_tests ardupilot_gazebo ardupilot_gz_applications ardupilot_gz_description ardupilot_gz_gazebo ardupilot_gz_bringup
$ colcon test-result --all --verbose

• Step 5: ROS2在Gazebo启动SITL模拟

$ source install/setup.bash


$ ros2 launch ardupilot_gz_bringup iris_runway.launch.py  //Iris Runway (Copter)
or
$ ros2 launch ardupilot_gz_bringup iris_maze.launch.py  //Iris Maze (Copter)
or
$ ros2 launch ardupilot_gz_bringup wildthumper.launch.py  //WildThumper (Rover)

3.7 CartographerSLAM+ROS2启动SITL

参考：Cartographer SLAM with ROS 2 in SITL

• Step 1: ardupilot_ros组件clone

$ cd ~/ros2_ws/src
$ git clone git@github.com:ArduPilot/ardupilot_ros.git

• Step 2: 依赖库安装

$ cd ~/ros2_ws
$ rosdep install --from-paths src --ignore-src -r --skip-keys gazebo-ros-pkgs

• Step 3: 编译ardupilot_ros/ardupilot_gz_bringup

$ cd ~/ros2_ws
$ source ./install/setup.bash
$ colcon build --packages-up-to ardupilot_ros ardupilot_gz_bringup

• Step 4: 360度2D激光雷达无人机迷宫仿真

启动 RViz 和 Gazebo

$ source ~/ros2_ws/install/setup.sh
$ ros2 launch ardupilot_gz_bringup iris_maze.launch.py

启动 Google Cartographer 生成 SLAM，检查在 RVIZ 中是否正确生成了地图。

在另一个终端中运行：

$ source ~/ros2_ws/install/setup.sh
$ ros2 launch ardupilot_ros cartographer.launch.py

4. 总结

本章在Jetson Orin Nano 8GB板子上，整理了Ardupilot当前在ROS2Humble+CartographerSLAM+SITL+Gazebo下的仿真运行步骤。

后续，希望能够在此基础上，进一步整理仿真环境的必要资料，比如，地图、云层、三维FPV视频等等：

• GSoC24: Visual Follow-me using AI
• GSoC 2024: High Altitude Non-GPS Navigation

5. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计
【6】ArduPilot开源代码之EKF系列研读
【7】ArduPilot开源代码之AP_DAL研读系列