【开发环境-PX4路径规划算法】搭建PX4+Gazebo+ROS2+microRTPS+Matlab+Simulink路径规划算法设计联合仿真环境

本篇文章介绍如何搭建使用ROS2对PX4固件与Matlab/Simulink进行联合仿真的环境，提供了ROS2-PX4桥接体系结构和应用程序管道的概述，以及如何安装所有所需软件和构建ROS2应用程序。

环境：

MATLAB : R2022b
Ubuntu ：20.04 LTS
Windows ：Windows 10
ROS ：ROS2 Foxy
Python: 3.8.2
Visual Studio ：Visual Studio 2019
PX4 ：1.13.0

ROS2的应用程序管道非常简单，这要归功于本地通信中间件（DDS/RTPS）。microRTPS桥接工具由运行在PX4上的客户端和运行在计算机上的服务端组成，它们进行通信以提供uORB和ROS2话题格式之间的双向数据交换和话题转换。使得可以创建直接与PX4的uORB话题接口的ROS2订阅服务器或发布服务器节点，其结构如下图所示。

ROS 2使用px4_msgs包和px4_ROS_com包来确保使用匹配的话题定义来创建客户端和服务端代码。

px4_msgs包：px4 ROS话题定义，当构建该项目时会生成相应的兼容ROS2节点的话题类型，以及IDL文件，由fastddsgen用于生成microRTPS代码。

px4_ros_com包：服务端发布者和订阅者的microRTPS代码模板，构建过程运行一个fastddsgen实例来生成micrortps_agent的代码，该代码可编译为单个可执行文件。

当px4_msgs发生变化时，PX4固件会自动更新px4_msgs并自动使用新的话题定义。

Ubuntu中的安装过程

下载并编译PX4固件代码

新建文件夹，文件夹名字可以任意取。

mkdir -p px4src_v1.13.0

之后我们使用ls、cd命令进入这个文件夹。

cd px4src_v1.13.0/

需要下载PX4 1.13版本的固件，请使用以下命令。

git clone -b v1.13.0 https://github.com/PX4/Firmware.git

clone成功之后，你会发现文件夹路径下面多了一个文件夹Firmware，我们使用ls、cd命令进入Firmware文件夹中。

cd Firmware/

Firmware里面就是PX4的源码，但是它依赖了很多其他的库，所以此时不完整还不能用，我们需要更新他的依赖。

git submodule update --init --recursive

安装Fast DDS

eProsima Fast DDS是使用C++实现的对象管理组（Object Management Group，OMG）数据分发服务（Data Distribution Service，DDS）规范和实时发布-订阅（Real Time Publish Subscribe，RTPS）协议的工具。

对应不同Ubuntu版本有对应的软件版本。

Ubuntu 18.04: Fast RTPS 1.8.4 (or later) and Fast-RTPS-Gen 1.0.4 (not later!).
Ubuntu 20.04: Fast DDS 2.0.2 (or later) and Fast-RTPS-Gen 1.0.4 (not later!).

Java是必备的，官方建议使用Java JDK 11，如果没有JAVA环境，请使用以下命令安装。

sudo apt install openjdk-11-jdk

运行以下命令建立Foonathan内存依赖环境。

git clone https://github.com/eProsima/foonathan_memory_vendor.git
cd foonathan_memory_vendor
mkdir build && cd build
cmake ..
sudo cmake --build . --target install

运行以下命令克隆Fast DDS项目并安装。

git clone --recursive https://github.com/eProsima/Fast-DDS.git -b v2.0.2 ~/FastDDS-2.0.2
cd ~/FastDDS-2.0.2
mkdir build && cd build
cmake -DTHIRDPARTY=ON -DSECURITY=ON ..
make -j$(nproc --all)
sudo make install

在Fast DDS安装完成之后，运行以下命令克隆Fast-RTPS-Gen项目并安装。

git clone --recursive https://github.com/eProsima/Fast-DDS-Gen.git -b v1.0.4 ~/Fast-RTPS-Gen
cd ~/Fast-RTPS-Gen/gradle/wrapper

将gradle-wrapper.properties文件中的distributionUrl修改为以下值。

distributionUrl=https\://services.gradle.org/distributions/gradle-6.8.3-bin.zip

之后运行以下命令。

cd ~/Fast-RTPS-Gen
./gradlew assemble && sudo env "PATH=$PATH" ./gradlew install

安装ROS2

安装ROS2，推荐用鱼香ROS的一键安装。

鱼香ROS网站上线|一行代码安装ROS/ROS2/解决rosdep问题|小鱼脚本

wget http://fishros.com/install -O fishros && bash fishros

之后按照终端中的提示进行安装即可。

安装ROS2后还需要安装一些依赖环境。

sudo apt install python3-colcon-common-extensions
sudo apt install ros-foxy-eigen3-cmake-module
sudo pip3 install -U empy pyros-genmsg setuptools

建立ROS2工作空间

打开一个终端，建立ROS2工作空间。

mkdir -p ~/px4_ros_com_ros2/src

克隆px4_msgs包和px4_ros_com包。

git clone -b release/1.13 https://github.com/PX4/px4_ros_com.git ~/px4_ros_com_ros2/src/px4_ros_com
git clone -b release/1.13 https://github.com/PX4/px4_msgs.git ~/px4_ros_com_ros2/src/px4_msgs

编译ROS2工作空间。

cd ~/px4_ros_com_ros2/src/px4_ros_com/scripts
source build_ros2_workspace.bash

检查安装是否成功

打开一个终端，启动PX4 Gazebo仿真，这里默认已经安装并能编译PX4 1.13版本的固件。

make px4_sitl_rtps gazebo

新打开一个终端，启动以UDP作为传输协议的microrts_agent守护进程。

source ~/px4_ros_com_ros2/install/setup.bash
micrortps_agent -t UDP

新打开一个终端，使用提供的启动文件启动一个“监听器”。

source ~/px4_ros_com_ros2/install/setup.bash
ros2 launch px4_ros_com sensor_combined_listener.launch.py

如果网桥工作正常，可以在启动ROS监听器的第三个终端上看到打印的数据。

Windows中的安装过程

为了与Ubuntu中的PX4固件仿真相连接，我们需要在Windows上的Matlab上使用ROS2进行通信。

可以在Matlab官网看到Matlab使用ROS Toolbox工具箱的环境依赖，我的Matlab安装后就自带了ROS Toolbox工具箱。

ROS Toolbox System Requirements

可以看到Matlab 2022b版本对应的环境为Python 3.9和Visual Studio 2019。

安装python3.9

Python Releases for Windows

安装之后可以在任意终端中查询Python版本。

python --version

安装Visual Studio 2019

Visual Studio 2019 version 16.11 Release Notes

安装时勾选【python开发】和【使用C++的桌面开发】。

下载px4_msgs包并编译

在GitHub下载px4_msgs包的Zip压缩包，这里注意需要根据PX4和ROS版本选择对应的分支。

PX4版本	ROS 2版本	Ubuntu版本	branch分支
v1.13	Foxy	Ubuntu 20.04	release/1.13
v1.14	Foxy	Ubuntu 20.04	release/1.14
v1.14	Humble	Ubuntu 22.04	release/1.14
v1.14	Rolling	Ubuntu 22.04	release/1.14
main	Foxy	Ubuntu 22.04	main
main	Humble	Ubuntu 22.04	main
main	Rolling	Ubuntu 22.04	main

px4_msgs官方下载地址

新建一个PX4-ROS2-Simulink文件夹用来存储工程文件（名字可以任取），在其下新建custom文件夹、others文件夹。

将下载的px4_msgs包的Zip压缩包放在PX4-ROS2-Simulink/others文件夹下。

将Zip压缩包解压到custom文件夹下，并将文件夹名从px4_msgs-main改为px4_msgs。

最后的结构为下列所示。

PX4-ROS2-Simulink
├─custom
|  └─px4_msgs
|      ├─.github
|      ├─msg
|      ├─srv
|      ├─.gitignore
|      ├─CMakeLists.txt
|      ├─CONTRIBUTING.md
|      ├─LICENSE
|      ├─package.xml
|      └─README.md
└─others
   └─px4_msgs-main.zip

用Matlab打开PX4-ROS2-Simulink文件夹，在Matlab命令行中输入以下命令。

folderPath = fullfile(pwd,"custom");
ros2genmsg(folderPath);

等待一段时间后会在custom生成一个新文件夹matlab_msg_gen。

这说明px4_msgs包已经编译成功。

安装UAV Toolbox Support Package for PX4 Autopilots硬件支持包

打开Matlab，点击【附加功能】中的【获取附加功能】。

搜索并安装UAV Toolbox Support Package for PX4 Autopilots硬件支持包，安装完之后点击右侧的设置按钮。

点击【Next】-【Install】安装Python 3.8.2，安装完毕后点击【Next】。

点击文字中的【link】。

点击【at this link】。

下载【PX4.Windows.Cygwin.Toolchain.0.8.msi】。

运行并安装【PX4.Windows.Cygwin.Toolchain.0.8.msi】。

点击【Verify Installation】。

之后下载PX4源码并编译，点击【Verify】，程序会自动运行。

选择【Design Flight Controller in Simulink】。

如果只仿真的话选择【PX4 Host Target】即可。

点击【Build Firmware】编译固件。

编译完成后Matlab命令行界面显示如下。

编译成功后点击【Next】-【Next】-【Finish】即可。

连接Ubuntu下的PX4 GAZEBO仿真和Windows下的Matlab/Simulink仿真

整体架构如下图所示。

首先需要关闭Windows的Windows Defender防火墙，并允许其他网络用户联机。

打开【控制面板】-【网络和共享中心】找到【更改适配器选项】。

双击已连接的局域网【WLAN】。

单机【属性】，在【共享】栏中勾选【允许其他网络用户通过此计算机的Internet连接来联机(N)】。

确保两台设备在同一网段，并可以互相ping通。

测试ROS2连接是否成功

Ubuntu下打开一个终端，启动PX4 Gazebo仿真。

make px4_sitl_rtps gazebo

Ubuntu下新打开一个终端，启动以UDP作为传输协议的microrts_agent守护进程。

source ~/px4_ros_com_ros2/install/setup.bash
micrortps_agent -t UDP

Windows下在Matlab命令行中运行以下命令查看ROS2话题。

ros2 topic list

如果结果是一长串话题说明连接成功。

/fmu/collision_constraints/out
/fmu/debug_array/in
/fmu/debug_key_value/in
/fmu/debug_value/in
/fmu/debug_vect/in
/fmu/offboard_control_mode/in
/fmu/onboard_computer_status/in
/fmu/optical_flow/in
/fmu/position_setpoint/in
/fmu/position_setpoint_triplet/in
/fmu/sensor_combined/out
/fmu/telemetry_status/in
/fmu/timesync/in
/fmu/timesync/out
/fmu/trajectory_bezier/in
/fmu/trajectory_setpoint/in
/fmu/trajectory_waypoint/out
/fmu/vehicle_command/in
/fmu/vehicle_control_mode/out
/fmu/vehicle_local_position_setpoint/in
/fmu/vehicle_mocap_odometry/in
/fmu/vehicle_odometry/out
/fmu/vehicle_status/out
/fmu/vehicle_trajectory_bezier/in
/fmu/vehicle_trajectory_waypoint/in
/fmu/vehicle_trajectory_waypoint_desired/out
/fmu/vehicle_visual_odometry/in
/parameter_events
/rosout
/timesync_status

Ubuntu中开启SSH服务

打开终端，使用命令安装SSH服务。

sudo apt install openssh-server

开启防火墙ssh的服务端口。

sudo ufw allow ssh

一些常用的命令。

systemctl status ssh         #查看ssh服务状态
systemctl stop ssh           #关闭ssh服务
systemctl start ssh          #开启ssh服务
systemctl restart ssh        #重启ssh服务
sudo systemctl enable ssh    #设置开启自启
sudo systemctl disable ssh   #关闭开机自启

测试通过ROS2发布话题控制无人机解锁

无人机的解锁是通过vehicle_command话题进行的，它的定义在源码Firmware/msg/vehicle_command.msg中，这个话题是地面站/nsh等终端发送的控制指令用的。

我们可以从任意已经编译过的固件中的Firmware\build\px4_fmu-v5_default\uORB\topics\vehicle_command.h文件中看到vehicle_command话题的结构体定义。

	uint64_t timestamp;
	double param5;
	double param6;
	float param1;
	float param2;
	float param3;
	float param4;
	float param7;
	uint32_t command;
	uint8_t target_system;
	uint8_t target_component;
	uint8_t source_system;
	uint8_t source_component;
	uint8_t confirmation;
	bool from_external;
	uint8_t _padding0[2]; // required for logger

可以看到其结构为：

时间戳+command命令+目标系统号+目标组件号+发出命令系统号+发出命令组件号+收到命令次数+数据包

在源码Firmware/msg/vehicle_command.msg中可以检索到解锁的命令ID是：

uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400		# Arms / Disarms a component |1 to arm, 0 to disarm|

可以在注释中看到用法，只需将param1的值赋值为1即可解锁。

综上，通过ROS2对无人机进行解锁的方法为：

订阅/fmu/timesync/out获得时间戳–>command设置为400、param1设置为1、target_system设置为1–>发布/fmu/vehicle_command/in话题。

在Matlab工作文件夹中新建一个文件夹models用来存放Simulink模型，新建一个模型我这里命名为ARM_DISARM.slx，双击使用Simulink打开。

在【建模】栏打开【模型设置】，【求解器】栏中【求解器类型】选为【定步长】。

【硬件实现】栏中【Hardware board】选择【ROS2】。

【代码生成】栏中【接口】勾选【连续时间】。

仿真调速界面勾选【启用调速以减慢仿真】。

建立Simulink模型，对时钟进行判断，3秒后触发Arm子系统。

Arm子系统中使用ROS2 Subscribe模块订阅/fmu/timesync/out话题，并使用Bus Selector分解话题获取时间戳，将时间戳传入子系统。

使用ROS2 Blank Message获得px4_msgs/vehicle_command的话题类型，导入获取到的时间戳、命令编号、传入参数等，并使用ROS2 Publish模块发布该话题。

Ubuntu中启动Gazebo仿真和microrts_agent守护进程，运行Simulink模型，可以看到Gazebo中的无人机已经解锁了。

Matlab官方例程Control a Simulated UAV Using ROS 2 and PX4 Bridge

Matlab官方给出了一个示例，该示例演示了如何从具有PX4自动驾驶仪的模拟无人机接收传感器读数和自动驾驶仪状态，并发送控制命令来导航模拟无人机。

Control a Simulated UAV Using ROS 2 and PX4 Bridge

可以在Matlab命令行中输入以下命令打开该例程所在位置。

openExample('uav_ros/ControlASimulatedUAVUsingROS2AndPX4BridgeExample')

---

参考资料：

ROS 2 User Guide (PX4-ROS 2 Bridge)

Control a Simulated UAV Using ROS 2 and PX4 Bridge

Ubuntu 20.04 开启SSH服务