【开发环境】搭建PX4+ROS2+MAVROS2+Simulink+Optitrack实物联合仿真环境_mavros --remap

本篇文章介绍如何搭建使用ROS2对PX4固件与Matlab/Simulink进行联合实物仿真的环境，以及如何安装所有所需软件和构建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

Ubuntu中的安装过程

下载并编译PX4固件代码

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

mkdir -p px4src_v1.13.0
1

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

cd px4src_v1.13.0/
1

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

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

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

cd Firmware/
1

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

git submodule update --init --recursive
1

安装ROS2

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

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

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

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

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

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

安装VRPN动捕数据转换ROS2话题库

安装VRPN库

执行以下命令安装VRPN库。

git clone https://github.com/vrpn/vrpn.git
mkdir -p vrpn/build
cd vrpn/build
cmake ..
make 
sudo make install
1
2
3
4
5
6

拉取vrpn_client_ros2节点代码并配置VRPN server参数

执行以下命令拉取vrpn_client_ros2代码。

git clone https://github.com/efc-robot/vrpn_client_ros2
1

拉取完成后，编辑代码中的配置文件 vrpn_client_ros2/src/vrpn_listener/config/params.yaml。

/vrpn_listener:
     ros__parameters:
        server: 192.168.1.2               # VRPN server在局域网内的IP
        port: 3883                        # VRPN server的服务端口
        frame_id: "world"                 # reference frame参数
        mainloop_frequency: 100.0         # 主循环频率，频率越高，越不容易丢失VRPN数据包，但同时计算负载越大
        refresh_trackers_frequency: 1.0   # 追踪器刷新频率，频率越高，发现新的VRPN tracker的实时性越强
        tracker_mainloop_frequency: 100.0 # 追踪器主循环频率，频率越高，越不容易丢失VRPN数据包，但同时计算负载越大
1
2
3
4
5
6
7
8

• 根据动捕软件的数据发送设置，配置 server 和 port 参数。
• 不推荐修改其余配置项，默认即可。

编译软件包

执行以下命令编译vrpn_client_ros2，其中 ${ROS_DISTRO} 替换为实际使用的ROS2版本。

source /opt/ros/${ROS_DISTRO}/setup.bash
cd vrpn_client_ros2/src
colcon build
1
2
3

启动vrpn_client_ros2节点

执行以下命令运行vrpn_client_ros2节点。

cd vrpn_client_ros2/src
source install/setup.bash
ros2 launch vrpn_listener sync_entity_state.launch
1
2
3

重命名话题名 /vrpn/fly/pose 为 /mavros/vision_pose/pose

编辑代码中的launch文件vrpn_client_ros2/src/vrpn_listener/launch/sync_entity_state.launch。

改为以下代码。

<launch>
<arg name="server" default="localhost"/>
<node name="vrpn_listener" pkg="vrpn_listener" exec="vrpn_listener"  output="screen">
    <param from="$(find-pkg-share vrpn_listener)/config/params.yaml"/>
    <remap from="/vrpn/fly/pose" to="/mavros/vision_pose/pose"/>
</node>
</launch>
1
2
3
4
5
6
7

之后重新编译软件包并运行节点即可。

安装MAVROS2

创建ROS2工作空间。

mkdir -p ~/mavros2_ws/src
cd ~/mavros2_ws
1
2

下载MAVLink和MAVROS2源码。

source /opt/ros/foxy/setup.bash
rosinstall_generator --format repos mavlink | tee /tmp/mavlink.repos
rosinstall_generator --format repos --upstream mavros | tee -a /tmp/mavros.repos
vcs import src < /tmp/mavlink.repos
vcs import src < /tmp/mavros.repos
rosdep install --from-paths src --ignore-src -y
1
2
3
4
5
6

安装依赖数据集。

sudo ./src/mavros/mavros/scripts/install_geographiclib_datasets.sh
1

从github官网下载MAVROS2的ros2分支的最新版本代码，替换mavros2_ws/src文件夹中的mavros文件夹。

MAVROS2

编译工作空间。

source ~/mavros2_ws/install/setup.bash
colcon build
1
2

新建一个终端，测试MAVROS2能否正常启动。

source /opt/ros/foxy/setup.bash
source ~/mavros2_ws/install/setup.bash
ros2 launch mavros px4.launch
1
2
3

MAVROS2连接仿真和实机需要的参数配置

默认情况下，PX4使用固定的UDP端口与地面站（如QGroundControl）、机载电脑（如MAVSDK、MAVROS）和仿真器（如Gazebo）进行MAVLink通信。

PX4的UDP端口14550用于与地面站进行通信。地面站侦听此端口上的连接，QGroundControl默认侦听此端口。

PX4的UDP端口14540用于与Offboard模式板外电脑进行通信。Offboard模式板外电脑应侦听此端口上的连接。

MAVROS2连接仿真

source /opt/ros/foxy/setup.bash
source ~/mavros2_ws/install/setup.bash
ros2 launch mavros px4.launch fcu_url:=udp://:14540@127.0.0.1:14555
1
2
3

MAVROS2连接实机

这里192.168.1.11是无人机上WiFi模块的IP地址。

source /opt/ros/foxy/setup.bash
source ~/mavros2_ws/install/setup.bash
ros2 launch mavros px4.launch fcu_url:=udp://:14550@192.168.1.11:14555
1
2
3

MAVROS2控制无人机常用命令

查看所有ROS2话题：

ros2 topic list
1

查看MAVROS2连接状态：

ros2 topic echo /mavros/state
1

查看ROS2话题发布频率：

ros2 topic hz /mavros/state
1

解锁：

ros2 service call /mavros/cmd/arming mavros_msgs/srv/CommandBool "{value: True}"
1

上锁：

ros2 service call /mavros/cmd/arming mavros_msgs/srv/CommandBool "{value: False}"
1

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
1

安装Visual Studio 2019

Visual Studio 2019 version 16.11 Release Notes

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

下载mavros_msgs包并编译

在使用Matlab的ROS Toolbox工具箱编译与PX4联合调试的代码，Matlab官方给出的示例是使用ROS2配合microRTPS连接无人机，并发送控制命令来导航模拟无人机。

Control a Simulated UAV Using ROS 2 and PX4 Bridge

但是实际中使用更多的是MAVROS2连接的方法，所以这里选择了编译MAVROS2消息来实现与Matlab的联合调试。

MAVROS2

在官网下载MAVROS2的源码，使用Matlab中的ros2genmsg命令生成ROS2消息。

Generate custom messages from ROS 2 definitions

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

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

解压，将其中的mavros_msgs文件夹放在PX4-ROS2-Simulink/custom文件夹下。

下载geographic_msgs消息包，一起放到custom文件夹下进行编译。

打开官网链接进行下载，注意需要下载的是ROS2版本的。

ros-geographic-info/geographic_info

解压后将其中的geographic_msgs文件夹复制到custom文件夹下

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

解压，将其中的geographic_msgs文件夹放在PX4-ROS2-Simulink/custom文件夹下。

最后的结构为下列所示。

PX4-ROS2-Simulink
├─custom
|  ├─geographic_msgs
|  |   ├─include
|  |   ├─msg
|  |   ├─srv
|  |   ├─test
|  |   ├─CHANGELOG.rst
|  |   ├─CMakeLists.txt
|  |   ├─geographic_msgs_mapping_rule.yaml
|  |   ├─mainpage.dox
|  |   └─package.xml
|  └─mavros_msgs
|      ├─include
|      ├─msg
|      ├─srv
|      ├─CHANGELOG.rst
|      ├─CMakeLists.txt
|      ├─mavros_msgs_mapping_rule.yaml
|      └─package.xml
└─others
   └─mavros-ros2.zip
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

用Matlab打开PX4-ROS2-Simulink文件夹，编译即可。

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

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

这说明mavros_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】即可。

在Simulink中编译ROS2节点并部署在Ubuntu中

关闭Windows防火墙

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

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

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

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

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

Ubuntu中开启SSH服务

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

sudo apt install openssh-server
1

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

sudo ufw allow ssh
1

一些常用的命令。

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

建立Simulink模型

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

这里使用Pulse Generator模块生成一个10Hz的脉冲信号，作为位置信息发送的频率。

SendPosition模块内部是使用mavros_msgs包中的mavros/setpoint_position/local话题，该话题定义了位置信息。

mavros_msgs/PositionTarget Message定义

以下是生成位置轨迹的Matlab代码。

function y = fcn(t)

a = 4;%x轴上的范围
b = 6;%y轴上的范围
w = 0.2;%角速度

if t < 5
    position_x = 0;
    position_y = 0;
    position_z = 2;
else
    t = t-5;%去掉轨迹飞行开始前的时间

    position_x = b * sin(w * t) .* cos(w * t);
    position_y = a * sin(w * t);
    position_z = 2;
end

y = [position_x position_y position_z];
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

编译并部署ROS2节点

这里需要确保Windows电脑和Ubuntu电脑在同一网段，并且可以互相ping通。

在Simulink模型的ROS栏目里，点击【Manage Remote Device】设置Ubuntu所在的设备的相关配置。

【Device address】设置为Ubuntu电脑的IP地址，【Username】设置为Ubuntu电脑的用户名，【Password】设置为Ubuntu电脑的密码，【ROS 2 folder】选择为ROS2的安装地址，【ROS 2 workspace】选择一个ROS2工作空间，之后可以点击【Test】进行测试。

测试成功后可以点击Simulink模型栏目中的【Build & Load】中的【Build & Load】，程序会自动编译并将节点部署在Ubuntu中的ROS2工作空间中。

动捕系统配置

OptiTrack MOTIVE软件手册

数据流设置

Motive 软件可以通过多种方式将捕捉到的数据实时传输到其他程序，常用的传输方式是 VRPN。可以通过菜单栏【View】-【Data Streaming Pane】对数据传输进行设置。

这里需要按下图配置VRPN的相关参数，其中【Local Interface】设置为运行Motive软件所在电脑的IP地址。

创建刚体

将标记点固定在需要进行观测的无人机上，放置在标定好的动捕场地中，在 MOTIVE 软件的 3D 视图中可以看到对应的标记点，如下图所示，无人机上固定有四个标记点。

框选住刚体模型所有标记点，点击右键选择【Rigid Body】下的【Create From Selected Markers】，完成刚体创建。

这时 MOTIVE 软件右下角会出现该刚体的属性窗口，可以对该刚体进行重命名。

实机飞行测试

无人机相关设置

将无人机的模式切换设置为遥控器的某个开关切换，切换需要的三个模式：自稳模式、位置模式和Offboard模式。

确保无人机与Ubuntu电脑在同一局域网，使用QGC地面站可以直接与无人机进行无线连接。

启动vrpn_client_ros2节点

执行以下命令运行vrpn_client_ros2节点。

cd vrpn_client_ros2/src
source install/setup.bash
ros2 launch vrpn_listener sync_entity_state.launch
1
2
3

启动MAVROS2

新建一个终端，启动MAVROS2。

source /opt/ros/foxy/setup.bash
source ~/mavros2_ws/install/setup.bash
ros2 launch mavros px4.launch fcu_url:=udp://:14550@192.168.1.11:14555
1
2
3

启动完vrpn_client_ros2节点和MAVROS2后，无人机就收到了位置信息，正常情况下就可以切换到位置模式了。

启动ROS2节点InfinityFlightReal

新建一个终端，启动ROS2节点InfinityFlightReal。

cd ~/trajectoryfly_ws
source /opt/ros/foxy/setup.bash
source ~/mavros2_ws/install/setup.bash
ros2 run infinityflightreal InfinityFlightReal
1
2
3
4

先启动ROS2节点InfinityFlightReal再使用遥控器切换到offboard模式，无人机就会沿设定的轨迹飞行了。

---

参考资料：

【PX4｜Mavros】Ubuntu 20.04 配置 ROS2 下的 PX4 开发环境（包括 ROS2 foxy 和 Mavros2）

How to use mavros2

Optitrack与ROS详细教程以及Motive的使用