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 仿真硬件

基于基础环境安装，在Jetson Orin Nano 8GB硬件上进行仿真测试：

至少需要安装、更新以下软件包：

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

3.2 Gazebo Harmonic安装

参考Gazebo Harmonic, Binary Installation on Ubuntu。

Step 1: 安装基本工具

$ sudo apt-get update
$ sudo apt-get install curl nano

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: 启用 Ubuntu Universe 仓库

$ sudo apt install software-properties-common
$ sudo add-apt-repository universe

Step 5: 安装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源

$ 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: 安装基本工具

$ sudo apt update
$ sudo apt-get install python3 python3-pip python3-vcstool python3-rosdep2 default-jre
$ pip3 install -U colcon-common-extensions

Step 2: 安装microxrceddsgen

$ cd ~
$ 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 3: 新建工程环境

新建工程目录：

$ cd ~
$ mkdir ros2_ws
$ cd ros2_ws
$ mkdir src

导入代码：

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

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

$ export https_proxy=http://192.168.1.10:808
$ curl -O https://raw.githubusercontent.com/ArduPilot/ardupilot/master/Tools/ros2/ros2.repos
$ vcs import  --recursive --input ros2.repos src

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

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

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

若已经初始化过，可以执行以下命令：

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

Step 4: ROS2环境和依赖更新

设置环境：

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

更新依赖：

$ rosdep update
$ rosdep install --from-paths src --ignore-src

由于网络等问题，请使用代理：

$ export https_proxy=http://192.168.1.10:808
$ rosdep update
$ rosdep install --from-paths src --ignore-src -r -y

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

Step 5: 编译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+

若内存不足，执行以下命令（Jetson Orin Nano 8GB经常出现内存不足问题）：

$ sudo fallocate -l 4G /swapfile  # Allocate 4GB of swap
$ sudo chmod 600 /swapfile        # Set proper permissions
$ sudo mkswap /swapfile           # Set up the swap file
$ sudo swapon /swapfile           # Enable the swap file
$ sudo swapon --show              # Check if swap is enabled

确保swap始终打开，执行以下命令：

$ echo '/swapfile none swap sw 0 0' | sudo tee -a /etc/fstab

将环境内所有ros包进行编译，执行以下命令：

$ colcon build                        # parallel compile
or
$ colcon build --executor sequential  # sequential compile

单独编译某个ros包：

$ colcon build --packages-select ardupilot_dds_tests

测试ardupilot_dds_tests

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

3.5 ROS2启动SITL

参考:

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

// for 4.5
$ 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

// for 4.6
$ ros2 launch ardupilot_sitl sitl_dds_udp.launch.py transport:=udp4 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

$ sudo apt-get install build-essential libgtk-3-dev
$ pip3 install MAVProxy wxPython

Step 4: mavprxoy启动地图&控制台

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

Step 5: mavproxy操作飞机

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

3.6 Gazebo+ROS2启动SITL

参考：

Step 1: 工作环境代码clone

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

Step 2: 指定gazbo版本

$ export GZ_VERSION=harmonic

Step 3: 更新ROS2依赖库

设置环境：

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

更新依赖：

$ sudo apt-get update
$ rosdep update
$ rosdep install --from-paths src --ignore-src -r

由于网络等问题，请使用代理：

$ export https_proxy=http://192.168.1.10:808
$ sudo apt-get update
$ rosdep update
$ rosdep install --from-paths src --ignore-src -r

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

Step 4: 编译Ardupilot ROS2组件

$ cd ~/ros2_ws
$ colcon build --packages-up-to ardupilot_gz_bringup ardupilot_sitl ardupilot_gazebo ardupilot_gz_application ardupilot_gz_description ardupilot_gz_gazebo

测试

$ source install/setup.bash
$ colcon test --packages-select ardupilot_sitl ardupilot_dds_tests ardupilot_gazebo ardupilot_gz_application 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 rviz:=true use_gz_tf:=true  //Iris Runway (Copter)
or
$ ros2 launch ardupilot_gz_bringup iris_maze.launch.py rviz:=true use_gz_tf:=true  //Iris Maze (Copter)
or
$ ros2 launch ardupilot_gz_bringup wildthumper_playpen.launch.py rviz:=true use_gz_tf:=true  //WildThumper (Rover)

另一个窗口启动MavProxy进行控制

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

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

在第三个终端中运行：

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

4. 总结

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

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

4.1 测试代码库

为了更好的统一笔者的环境，这里有一份修改好的git协议脚本：SnapLearnGazebo/lesson_04_ardupilot

ros2.repos + ros2_inc_gz.repos = ros2_gz.repos

  • 集中获取代码方式一：
$ cd ~/cd ros2_ws
$ vcs import --recursive --input https://raw.githubusercontent.com/SnapDragonfly/SnapLearnGazebo/main/lesson_04_ardupilot/ros2_gz.repos src
  • 集中获取代码方式二：
$ cd ~/cd ros2_ws
$ vcs import --recursive --input https://raw.githubusercontent.com/SnapDragonfly/SnapLearnGazebo/main/lesson_04_ardupilot/ros2.repos src
$ vcs import --recursive --input https://raw.githubusercontent.com/SnapDragonfly/SnapLearnGazebo/main/lesson_04_ardupilot/ros2_inc_gz.repos src

4.2 Gazebo学习

5. 参考资料

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

6. 补充

6.1 环境设置问题

daniel@nvidia:~/ros2_ws$ colcon build --executor sequential

Starting >>> ros_gz_image
--- stderr: ros_gz_image
Traceback (most recent call last):
  File "/opt/ros/humble/share/ament_cmake_core/cmake/package_templates/templates_2_cmake.py", line 21, in <module>
    from ament_package.templates import get_environment_hook_template_path
ModuleNotFoundError: No module named 'ament_package'
CMake Error at /opt/ros/humble/share/ament_cmake_core/cmake/ament_cmake_package_templates-extras.cmake:41 (message):
  execute_process(/usr/bin/python3
  /opt/ros/humble/share/ament_cmake_core/cmake/package_templates/templates_2_cmake.py
  /home/daniel/ros2_ws/build/ros_gz_image/ament_cmake_package_templates/templates.cmake)
  returned error code 1
Call Stack (most recent call first):
  /opt/ros/humble/share/ament_cmake_core/cmake/ament_cmake_coreConfig.cmake:41 (include)
  /opt/ros/humble/share/ament_cmake/cmake/ament_cmake_export_dependencies-extras.cmake:15 (find_package)
  /opt/ros/humble/share/ament_cmake/cmake/ament_cmakeConfig.cmake:41 (include)
  CMakeLists.txt:13 (find_package)


---
Failed   <<< ros_gz_image [0.41s, exited with code 1]

Summary: 18 packages finished [32.0s]
  1 package failed: ros_gz_image
  1 package had stderr output: ros_gz_image
  7 packages not processed

解决方案：

$ source /opt/ros/humble/setup.bash
$ colcon build --executor sequential

6.2 libEGL warning

看到这个问题不要惊讶，直接忽略，因为Gazebo可以正常使用。

6.3 ignition-gazebo6问题

daniel@nvidia:~/ros2_ws$ colcon build --executor sequential

--- stderr: ros_ign_gazebo
CMake Error at CMakeLists.txt:44 (find_package):
  By not providing "Findignition-gazebo6.cmake" in CMAKE_MODULE_PATH this
  project has asked CMake to find a package configuration file provided by
  "ignition-gazebo6", but CMake did not find one.

  Could not find a package configuration file provided by "ignition-gazebo6"
  with any of the following names:

    ignition-gazebo6Config.cmake
    ignition-gazebo6-config.cmake

  Add the installation prefix of "ignition-gazebo6" to CMAKE_PREFIX_PATH or
  set "ignition-gazebo6_DIR" to a directory containing one of the above
  files.  If "ignition-gazebo6" provides a separate development package or
  SDK, be sure it has been installed.


gmake: *** [Makefile:267: cmake_check_build_system] Error 1
---
Failed   <<< ros_ign_gazebo [0.47s, exited with code 2]

Summary: 12 packages finished [24.7s]
  1 package failed: ros_ign_gazebo
  1 package had stderr output: ros_ign_gazebo
  13 packages not processed

解决方案：

$ sudo apt-get install libignition-gazebo6-dev
$ echo $CMAKE_PREFIX_PATH
$ export CMAKE_PREFIX_PATH=/usr/lib/aarch64-linux-gnu/cmake/ignition-gazebo6:$CMAKE_PREFIX_PATH
$ echo $CMAKE_PREFIX_PATH
$ colcon build --executor sequential

6.4 IgnProtobuf 问题

daniel@nvidia:~/ros2_ws$ colcon build --executor sequential

Starting >>> ros_gz_image
--- stderr: ros_gz_image
CMake Error at /usr/lib/aarch64-linux-gnu/cmake/ignition-transport11/ignition-transport11-config.cmake:92 (find_package):
  By not providing "FindIgnProtobuf.cmake" in CMAKE_MODULE_PATH this project
  has asked CMake to find a package configuration file provided by
  "IgnProtobuf", but CMake did not find one.

  Could not find a package configuration file provided by "IgnProtobuf" with
  any of the following names:

    IgnProtobufConfig.cmake
    ignprotobuf-config.cmake

  Add the installation prefix of "IgnProtobuf" to CMAKE_PREFIX_PATH or set
  "IgnProtobuf_DIR" to a directory containing one of the above files.  If
  "IgnProtobuf" provides a separate development package or SDK, be sure it
  has been installed.
Call Stack (most recent call first):
  CMakeLists.txt:58 (find_package)


---
Failed   <<< ros_gz_image [3.39s, exited with code 1]

Summary: 18 packages finished [37.1s]
  1 package failed: ros_gz_image
  1 package had stderr output: ros_gz_image
  7 packages not processed

解决方案：

$ sudo apt-get install libignition-msgs8-8-protobuf23
$ sudo apt-get install libignition-msgs8-8
$ rm -rf build/ install/ log/
$ colcon build --executor sequential
# 开源 # c++
Logo
DAMO开发者矩阵

DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐

cover

具身机器人中AI（DEEPSEEK）与PLC、驱动器协同发展研究：突破数据困境与指令精确控制（3）

avatar DAMO开发者矩阵
cover

第二十六章:Seaborn库实现统计数据可视化

avatar DAMO开发者矩阵

优雅～～Spring Boot 整合多数据源的姿势

最常见的单一应用中最多涉及到一个数据库，即是一个数据源（Datasource那么顾名思义，多数据源就是在一个单一应用中涉及到了两个及以上的数据库了。urlusernamepassword这三个属性已经唯一确定了一个数据库了，DataSource则是依赖这三个创建出来的。则多数据源即是配置多个DataSource（暂且这么理解）。动态数据源简单的说就是能够自由切换的数据源，类似于一个动态路由的感觉，

avatar DAMO开发者矩阵