【开发环境-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

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

cd px4src_v1.13.0/

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

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

clone成功之后,你会发现文件夹路径下面多了一个文件夹Firmware,我们使用lscd命令进入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.13FoxyUbuntu 20.04release/1.13
v1.14FoxyUbuntu 20.04release/1.14
v1.14HumbleUbuntu 22.04release/1.14
v1.14RollingUbuntu 22.04release/1.14
mainFoxyUbuntu 22.04main
mainHumbleUbuntu 22.04main
mainRollingUbuntu 22.04main

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服务

  • 20
    点赞
  • 70
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 3
    评论
评论 3
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

后厂村路练习生

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值