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

搭建PX4+ROS2+MAVROS2+Simulink+OptiTrack实物联合仿真环境

- Ubuntu中的安装过程
- - 下载并编译PX4固件代码
  - 安装ROS2
  - 安装VRPN动捕数据转换ROS2话题库
  - - 安装VRPN库
    - 拉取vrpn_client_ros2节点代码并配置VRPN server参数
    - 编译软件包
    - 启动vrpn_client_ros2节点
    - 重命名话题名 /vrpn/fly/pose 为 /mavros/vision_pose/pose
  - 安装MAVROS2
  - MAVROS2连接仿真和实机需要的参数配置
  - - MAVROS2连接仿真
    - MAVROS2连接实机
  - MAVROS2控制无人机常用命令
- Windows中的安装过程
- - 安装python3.9
  - 安装Visual Studio 2019
  - 下载mavros_msgs包并编译
  - 安装UAV Toolbox Support Package for PX4 Autopilots硬件支持包
- 在Simulink中编译ROS2节点并部署在Ubuntu中
- - 关闭Windows防火墙
  - Ubuntu中开启SSH服务
  - 建立Simulink模型
  - 编译并部署ROS2节点
- 动捕系统配置
- - 数据流设置
  - 创建刚体
- 实机飞行测试
- - 无人机相关设置
  - 启动vrpn_client_ros2节点
  - 启动MAVROS2
  - 启动ROS2节点InfinityFlightReal

本篇文章介绍如何搭建使用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

之后我们使用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

安装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

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

安装VRPN库

执行以下命令安装VRPN库。

git clone https://github.com/vrpn/vrpn.git
mkdir -p vrpn/build
cd vrpn/build
cmake ..
make 
sudo make install

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

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

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

拉取完成后，编辑代码中的配置文件 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数据包，但同时计算负载越大

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

编译软件包

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

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

启动vrpn_client_ros2节点

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

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

在这里插入图片描述

重命名话题名 /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>

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

安装MAVROS2

创建ROS2工作空间。

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

下载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

安装依赖数据集。

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

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

MAVROS2

编译工作空间。

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

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

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

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

在这里插入图片描述

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

在这里插入图片描述

MAVROS2控制无人机常用命令

查看所有ROS2话题：

ros2 topic list

查看MAVROS2连接状态：

ros2 topic echo /mavros/state

查看ROS2话题发布频率：

ros2 topic hz /mavros/state

解锁：

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

上锁：

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

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++的桌面开发】。

在这里插入图片描述

下载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

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

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

等待一段时间后会在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

开启防火墙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   #关闭开机自启

建立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];

编译并部署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

启动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

启动完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

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

在这里插入图片描述

参考资料：

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

How to use mavros2

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