【PX4、RTPS、ROS2、Simulink联合仿真】在Simulink中使用ROS2发布消息通过microRTPS中间件控制无人机进入Offboard模式起飞悬停并在Gazebo中可视化

已于 2024-05-27 11:21:34 修改
阅读量1.2k 收藏 24
点赞数 22
分类专栏： PX4-AutoPilot 文章标签： 无人机 ROS2 Simulink Offboard Gazebo 仿真
于 2024-02-21 15:58:58 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_45031928/article/details/136213665
版权

在Simulink中使用ROS2发布消息通过microRTPS中间件控制无人机进入Offboard模式起飞悬停并在Gazebo中可视化

本篇文章介绍如何使用ROS2控制无人机进入Offboard模式起飞悬停并在Gazebo中可视化，提供了Matlab/Simulink源代码，以及演示效果图。

环境：

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的代码，该代码可编译为单个可执行文件。

这样在Ubuntu中就生成了一个可以调用uORB话题接口的ROS2节点，这个节点可以和运行在同一局域网下的Matlab/Simulink上的ROS2节点进行通信，以实现PX4&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')

运行所需的环境配置

请确保已经安装前一篇文章配置好了PX4+Gazebo+ROS2+FastDDS+Matlab+Simulink联合调试环境。

【PX4-AutoPilot教程-开发环境】搭建PX4+Gazebo+ROS2+FastDDS+Matlab+Simulink联合调试环境

PX4&Simulink&Gazebo联合仿真实现方法

建立Simulink模型并完成基本配置

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

在这里插入图片描述

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

在这里插入图片描述

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

在这里插入图片描述

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

在这里插入图片描述

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

在这里插入图片描述

整体框架

整体框架如下，主体是对时钟进行判断，1-3秒是触发Arm子系统，3-5秒是触发Enable Offboard Control子系统，5秒后是触发Takeoff子系统。

在这里插入图片描述

各子系统实现原理

Arm子系统

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

在这里插入图片描述

无人机的解锁是通过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话题。

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

在这里插入图片描述

Enable Offboard Control子系统

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

在这里插入图片描述

无人机进入Offboard模式也是通过vehicle_command话题进行的。

在源码Firmware/msg/vehicle_command.msg中可以检索到设置系统模式的命令ID是：

uint16 VEHICLE_CMD_DO_SET_MODE = 176			# Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty|

这里的注释写的是将第一个参数param1设为模式的ID号，之后param2到param7设置为空，但是这里的注释好像写错了。

在源码Firmware/src/modules/commander/Commander.cpp中，官方写的调节模式的命令是：

send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_OFFBOARD);

send_vehicle_command()函数的定义为：

static bool send_vehicle_command(const uint32_t cmd, const float param1 = NAN, const float param2 = NAN,
				 const float param3 = NAN,  const float param4 = NAN, const double param5 = static_cast<double>(NAN),
				 const double param6 = static_cast<double>(NAN), const float param7 = NAN)
{
	vehicle_command_s vcmd{};
	vcmd.command = cmd;
	vcmd.param1 = param1;
	vcmd.param2 = param2;
	vcmd.param3 = param3;
	vcmd.param4 = param4;
	vcmd.param5 = param5;
	vcmd.param6 = param6;
	vcmd.param7 = param7;

	uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
	vcmd.source_system = vehicle_status_sub.get().system_id;
	vcmd.target_system = vehicle_status_sub.get().system_id;
	vcmd.source_component = vehicle_status_sub.get().component_id;
	vcmd.target_component = vehicle_status_sub.get().component_id;

	uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
	vcmd.timestamp = hrt_absolute_time();
	return vcmd_pub.publish(vcmd);
}

可以看出需要将param1赋值为1，将param2赋值为PX4_CUSTOM_MAIN_MODE_OFFBOARD才能切换为Offboard模式。

查询PX4_CUSTOM_MAIN_MODE_OFFBOARD的定义，在源码Firmware/src/modules/commander/px4_custom_mode.h中找到：

enum PX4_CUSTOM_MAIN_MODE {
	PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
	PX4_CUSTOM_MAIN_MODE_ALTCTL,
	PX4_CUSTOM_MAIN_MODE_POSCTL,
	PX4_CUSTOM_MAIN_MODE_AUTO,
	PX4_CUSTOM_MAIN_MODE_ACRO,
	PX4_CUSTOM_MAIN_MODE_OFFBOARD,
	PX4_CUSTOM_MAIN_MODE_STABILIZED,
	PX4_CUSTOM_MAIN_MODE_RATTITUDE_LEGACY,
	PX4_CUSTOM_MAIN_MODE_SIMPLE /* unused, but reserved for future use */
};

PX4_CUSTOM_MAIN_MODE_OFFBOARD对应的数字是6。

综上，通过ROS2对无人机进入Offboard模式的方法为：

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

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

在这里插入图片描述

Takeoff子系统

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

在这里插入图片描述

offboard_control_mode话题是Offboard模式的心跳包，为了保证飞行的安全性，心跳包必须以最低2Hz的频率发布，PX4在两个Offboard命令之间有一个500ms的延时，如果超过此延时，系统会将回到无人机进入Offboard模式之前的最后一个模式。

在源码Firmware/msg/offboard_control_mode.msg中可以看到offboard_control_mode话题的定义。

# Off-board control mode

uint64 timestamp		# time since system start (microseconds)

bool position
bool velocity
bool acceleration
bool attitude
bool body_rate
bool actuator

因为要进行位置控制所以需要将position赋值为true。

trajectory_setpoint话题是期望的位置，在源码Firmware/msg/vehicle_local_position_setpoint.msg中可以看到trajectory_setpoint话题的定义。

# Local position setpoint in NED frame
# setting something to NaN means the state should not be controlled

uint64 timestamp	# time since system start (microseconds)

float32 x		# in meters NED
float32 y		# in meters NED
float32 z		# in meters NED
float32 yaw		# in radians NED -PI..+PI
float32 yawspeed	# in radians/sec
float32 vx		# in meters/sec
float32 vy		# in meters/sec
float32 vz		# in meters/sec
float32[3] acceleration # in meters/sec^2
float32[3] jerk # in meters/sec^3
float32[3] thrust	# normalized thrust vector in NED

# TOPICS vehicle_local_position_setpoint trajectory_setpoint

其中trajectory_setpoint话题和vehicle_local_position_setpoint话题的内容是一样的，源码Firmware/msg/tools/urtps_bridge_topics.yaml中可以看到以下代码。

  - msg:     vehicle_local_position_setpoint
    receive: true
  - msg:     trajectory_setpoint # multi-topic / alias of vehicle_local_position_setpoint
    base:    vehicle_local_position_setpoint
    receive: true

可以看出trajectory_setpoint话题是基于vehicle_local_position_setpoint话题的。

这里需要注意坐标系是NED坐标系，即北东地坐标系，所以想让无人机飞起来，z的赋值应该为负数。

综上，通过ROS2对无人机进入Offboard模式起飞悬停的方法为：

订阅/fmu/timesync/out获得时间戳–>position设置为true、x设置为0、y设置为0、z设置为-5、target_system设置为1–>发布offboard_control_mode话题和trajectory_setpoint话题。

SendCommand子系统中使用ROS2 Blank Message获得offboard_control_mode的话题类型和trajectory_setpoint的话题类型，导入获取到的时间戳、传入参数、期望位置等，并使用ROS2 Publish模块发布这些话题。

在这里插入图片描述

实现效果

打开一个终端，启动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

运行Simulink模型，可以看到Gazebo中的无人机已经进入Offboard模式并起飞悬停在5m的高度。

在这里插入图片描述

在这里插入图片描述

参考资料：

PX4 Gazebo Simulation

Control a Simulated UAV Using ROS 2 and PX4 Bridge

后厂村路练习生
关注
  • 22
    点赞
  • 踩
  • 24
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 6
    评论
专栏目录
Windows下编译PX4源码并连接simulink
Sanmmul的博客
09-27 5109
需要做pixhawk与matlab的联合仿真，对于常用windows不怎么使用ubuntu开发的同学，在虚拟机上编译px4显得相当繁琐。本文提供了在windows下编译px4源码的方法，可以使用cygwin工具链在控制台进行开发。最后，将pixhawk连接matlab，实现在simulink中传输传感器数据。...
PX4飞控固件项目的ROS2消息定义用于第三方软件使用ROS2节点对PX4内部uORB消息的通信
02-20
uORB消息定义与此包中的ROS2消息的相对应，可以在PX4飞控固件中找到。 当PX4飞控固件中的uORB消息定义发生更改时，CI/CD管道会自动生成更新的ROS消息定义并将其推送到该存储库。ROS消息定义由uorb_to_ROS_msgs.py...
从ROS1到ROS2无人机编程实战指南
lovely_yoshino的博客
06-11 1万+
目前，国内关于ROS的书籍相当稀缺，大部分都是国外翻译版，内容可能更偏向于机器人学的公式推导，而非实战项目。这本书站在初学者的角度，从入门到进阶，再到实战，循序渐进，是学习ROS1和ROS2的最佳选择。尽管本书主要针对ROS1和ROS2的无人机编程，但其中涉及的知识点亦适用于其他机器人编程技术，学习方法和思路同样值得借鉴。本书深入浅出地解释了基础知识，配有编程实例教程，帮助读者更好地理解各个知识点，克服学习过程中的困难与挫折。7.本书丰富的图示和类比有助于简化复杂的原理和流程，让读者更容易理解。
Simulink 和 Gazebo联合仿真控制机械臂【Matlab R2022a】
huangjunsheng123的博客
05-15 2148
官方 demo 文档 1、使用机器人模型和插件控制 Gazebo 1.1 设置 Gazebo 仿真环境 两种选择，一是使用自己的安装有 Gazebo 的 Linux 环境，二是下载提供的 装有 ROS 和 Gazebo 的虚拟机（VM），所需的 Gazebo 插件位于 /home/user/src/GazeboPlugin。 我打算先尝试后者，虚拟机。 ...
ROS系统下gazebo环境中，无人机结合SiamCar算法进行物体跟踪及轨迹评估的实现
最新发布
fRmfNCtMavQj的博客
05-20 483
本文将基于ROS系统中的gazebo仿真环境，利用四旋翼无人机和小车作为目标，结合SiamCar算法完成对物体的跟踪，并给出轨迹对比图等评估指标。通过在rviz上显示小车和无人机的运动轨迹，可以观察到跟踪效果，并保存小车和无人机运动过程中的坐标位置。同样地，在rviz上显示小车和无人机的运动轨迹，并保存坐标位置，以便后续的轨迹对比和评估。ROS系统下gazebo环境中，无人机结合目标跟踪算法（SiamCar），完成对物体的跟踪（可以是小车或者其他的），然后给出轨迹对比图等评估指标。
无人机Mavlink+offboard模式指令概述
诗筱涵的博客
08-06 1743
摘自：https://blog.csdn.net/faith_yu/article/details/52440434 无人机Mavlink+offboard模式指令概述 Faith_yu 2016-09-05 15:09:50 7431 收藏 8 分类专栏： mavlink指令 无人机 offboard模式 版权 无人机如今越来越多，如果你想深入了解无人机，那么了解和应用无人机mavlink指令是应该的。 接下来就介绍一波，下面的内容都是费了很...
关于PX4+ROS+Gazebo的仿真
weixin_48644943的博客
11-02 74
教程参考：XTDrone使用文档 · 语雀
ros2_java：ROS2的Java和Android绑定
02-02
，一个Gradle插件，可以更轻松地在Java和Android项目中使用ROS2。 可以从Gradle Central 安装Gradle插件 运行时环境的示例 ，适用于Android的示例 仅此Java吗？ 不可以，任何针对JVM的语言都可以用来编写ROS2应用...
ROS2编程基础课程文档.pdf
09-12
ROS 2中的默认通信方法使用IDL、DDS和DDS-I RTPS等行业标准，这些标准已广泛应用于从工厂到航空 航天的各种工业应用中。 开源许可证 ROS 2代码在Apache 2.0许可下获得许可，在3条款（或“新”）BSD许可下使用移植...
micrortps_agent：microRTPS桥的microRTPS代理端。 用于通过FastRTPSFastDDS将PX4与DDS世界连接
02-18
micrortps_agent microRTPS桥接器的microRTPS代理端。 用于通过FastRTPS / FastDDS将PX4与DDS世界连接。
PX4-Autopilot：PX4自动驾驶软件
02-03
本适用于想要修改飞行堆栈和中间件（例如，添加新的飞行模式）的软件开发人员，想要支持新的飞行控制器板和外围设备的硬件集成商，以及希望PX4在新的系统上工作的任何人（不支持的机身/车辆。 开发人员应该阅读。 ...
PX4-SIL:循环中的 PX4 软件
07-03
PX4-SIL PX4 软件在循环中。 目标是为 PX4 开发一个有用的开发和测试工具。 通过使用 python，我们可以结合有用的工具，例如 sympy，为独特的车辆自动生成运动方程。 模拟的结构基于周期性过程的执行。 这与在 simulink 和 scicoslab 中基于事件的执行不同，但更接近真实系统上的执行。 用例（目标）： 在 python 中原型新的估算器/控制器模块。 使用纯 python 模块进行测试，或者可以选择使用从实际 C++ 固件代码编译的 cython 模块进行测试。 对每次提交进行集成测试。 开发新车辆的控制系统。 在 sympy 中查找运动方程，将表达式存储在 lambda 函数中以传递给 numpy，与 scipy.integrate.ode 集成。 针对模拟动态测试控制系统。 您可以使用笔记本查看器查看 ipython 笔记本，例如： 访
gazebo模型库
06-12
gazebo模型库
机器人避障（含simulink仿真）
06-04
机器人避障（含simulink仿真），代码齐全。欢迎下载交流
Matlab_Simulink环境下无人机全过程飞行仿真技术研究
09-04
通过阅读学习了解飞行器六自由度模型的建立，仿真。
PX4PSP_v3_0_4_351_R2017b.mltbx
08-09
Pixhawk Pilot Support Package (PSP) Simulink Block Library Pixhawk Target Blocks. 此版本的支持包与MatlabR2017a和R2017b兼容。它也工作在MatlabR2018a的外部模式功能的限制。若要使用此支持包，请遵循以下说明： 并保存.mltbx 文件到可从MatlabR2017b访问的位置。 打开MATLABR2017b并导航到文件位置。 双击.mltbx 文件在MATLAB当前文件夹窗口中。 点击安装然后接受许可协议。
Gazebo機器人仿真學習探索筆記（七）连接ROS
热门推荐
zhangrelay的专栏
10-18 1万+
中文稍后补充，先上官方原版教程。ROS Kinetic 搭配 Gazebo 7附件----官方教程Tutorial: ROS integration overviewAs of Gazebo 1.9 and ROS Hydro, Gazebo no longer has any direct ROS dependencies and is now installed as an Ubuntu st
在ubuntu22.04(LTS)上搭建ROS2+PX4+Gazebo的联合仿真环境
易知识界
01-24 2104
说明：本案例仅仅是跑通了ROS2+PX4+Gazebo的联合仿真，还没有实现使用键盘控制无人机飞行（以后会补充）。
（五）PX4+Gazebo硬件在环（HIL）仿真配置
拾柒.17
04-07 774
本文介绍了Ubuntu 20.04下Gazebo+PX4的硬件在环（HIL）仿真流程。
ros2 humble 使用fast-dds在同一台物理机上发布订阅，如何配置使其使用tcp进行数据传输
06-08
在ROS2 Humble中，Fast DDS默认使用共享内存来优化本地通信性能。想要使用TCP协议进行通信，需要进行以下配置： 1. 首先，在启动节点时，需要设置环境变量`RMW_IMPLEMENTATION`为`rmw_fastrtps_cpp`，以使用Fast ...

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 6
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

打赏作者

后厂村路练习生

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

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

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

打赏作者

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

抵扣说明：

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

余额充值