赞
踩
本篇文章介绍如何搭建使用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
并自动使用新的话题定义。
新建文件夹,文件夹名字可以任意取。
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
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,推荐用鱼香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工作空间。
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监听器的第三个终端上看到打印的数据。
为了与Ubuntu中的PX4固件仿真相连接,我们需要在Windows上的Matlab上使用ROS2进行通信。
可以在Matlab官网看到Matlab使用ROS Toolbox工具箱的环境依赖,我的Matlab安装后就自带了ROS Toolbox工具箱。
ROS Toolbox System Requirements
可以看到Matlab 2022b版本对应的环境为Python 3.9和Visual Studio 2019。
安装之后可以在任意终端中查询Python版本。
python --version
Visual Studio 2019 version 16.11 Release Notes
安装时勾选【python开发】和【使用C++的桌面开发】。
在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-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包已经编译成功。
打开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】即可。
整体架构如下图所示。
首先需要关闭Windows的Windows Defender防火墙,并允许其他网络用户联机。
打开【控制面板】-【网络和共享中心】找到【更改适配器选项】。
双击已连接的局域网【WLAN】。
单机【属性】,在【共享】栏中勾选【允许其他网络用户通过此计算机的Internet连接来联机(N)】。
确保两台设备在同一网段,并可以互相ping
通。
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
打开终端,使用命令安装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 #关闭开机自启
无人机的解锁是通过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官方给出了一个示例,该示例演示了如何从具有PX4自动驾驶仪的模拟无人机接收传感器读数和自动驾驶仪状态,并发送控制命令来导航模拟无人机。
Control a Simulated UAV Using ROS 2 and PX4 Bridge
可以在Matlab命令行中输入以下命令打开该例程所在位置。
openExample('uav_ros/ControlASimulatedUAVUsingROS2AndPX4BridgeExample')
参考资料:
ROS 2 User Guide (PX4-ROS 2 Bridge)
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。