RflySimSDK v4.10
RflySimSDK说明文档
载入中...
搜索中...
未找到
Simulink/DLL 综合模型 ROS 转发原理说明

1. 系统架构 (System Architecture)

整个系统由三部分组成：**RflySim 仿真端**、**转发桥接节点 (Bridge Node)**、**ROS 控制端**。

graph TD subgraph Windows ["RflySim Windows"] Sim["CopterSim/Simulink DLL"] Sim -->|"(UDP 30101+)"| Bridge Bridge -->|"(UDP 30100+)"| Sim end subgraph Linux ["WSL / Linux"] Bridge["Bridge Node (C++/Python)"] Bridge -->|"Pub: /mavros/..."| ROS["ROS Control Algo"] ROS -->|"Sub: /mavros/..."| Bridge end

关键组件 (Key Components)

  1. RflySim 综合模型 (DLL): 运行在 CopterSim 平台，负责无人机动力学解算。如果不使用 PX4 SITL，模型直接通过 UDP 在端口 30100 (Recv) / 30101 (Send) 上通信。
  2. UDP Bridge (转发节点): 本例程的核心。它充当中间人，将 RflySim 的 UDP 协议转换为 ROS 消息（Topics/Services）。
    • C++版: udp_ros_bridge，高性能，支持 ROS1/ROS2 自动编译。
    • Python版: DllSimCtrlAPIROS.py，灵活易用，适合快速原型开发。
  3. ROS 算法: 标准的 ROS 节点（如 Ros12MultiUav.py），通过 mavros 兼容的话题控制无人机。

2. 通信协议细节 (Communication Protocol Details)

2.1 端口分配 (Port Mapping)

系统默认支持多机，通过 CopterID 区分端口。

通信方向 (Direction) 源端口 (Source) 目标端口 (Dest) 描述 (Description)
RflySim -> Bridge (上行) Random 30100 + ID*2 - 1 仿真状态数据 (State Data)
ID=1 -> 30101
ID=2 -> 30103
Bridge -> RflySim (下行) Random 30100 + (ID-1)*2 控制指令数据 (Control Cmd)
ID=1 -> 30100
ID=2 -> 30102

2.2 数据结构 (Data Structures)

上行数据包 (SOut2Simulator) - 168 Bytes

用于传输飞机状态到 ROS。

字段 (Field) 类型 (Type) 偏移 (Offset) 说明 (Note)
Checksum int32 0 校验位 (123456789)
CopterID int32 4 飞机 ID
VehicleType int32 8 机型
VelE float[3] 16 地球坐标系速度 (NED)
AngEuler float[3] 28 欧拉角
AngQuatern float[4] 40 四元数
PosE double[3] 112 地球坐标系位置 (NED)
PosGPS double[3] 136 GPS (Lat, Lon, Alt)

下行数据包 (SILIntFloat) - 120 Bytes

用于从 ROS 发送控制指令到仿真。

字段 (Field) 类型 (Type) 偏移 (Offset) 说明 (Note)
Checksum int32 0 校验位 (1234567897)
CopterID int32 4 飞机 ID
InInts int32[8] 8 控制标志位 (Flags, Modes)
InFloats float[20] 40 控制量 (Pos, Vel, Yaw...)
  • 控制标志位 (InInts):
    • ICmd: 启用标志，如 CmdArmed | CmdPosition (解锁+位置控制)。
    • IOffboard: 模式标志，如 HasPos | HasYaw (位置+偏航有效) 或 HasVel | FRD (机体速度有效)。

3. 调用流程 (Calling Workflow)

3.1 启动阶段 (Startup)

  1. 启动仿真: 运行 MulticopterNOpx4.bat，CopterSim 加载 DLL 模型，开始监听 30100 等端口。
  2. 启动 Bridge:
    • C++: build_bridge.sh 编译并运行 udp_ros_bridge_node。
    • Python: start_all.sh 运行 SimulinkAndDllMavrosStart.py，内部实例化 UdpRosBridge 类。
    • Bridge 绑定 UDP 端口 30101, 30103... 并注册 ROS 节点。

3.2 运行阶段 (Run Loop)

A. 状态更新 (Sim -> ROS)

  1. RflySim 每毫秒发送 UDP 包。
  2. Bridge 的接收线程 (recvLoop) 收到数据。
  3. 解析 168 字节数据包 (parseSOut2Simulator)。
  4. 填充 ROS 消息：
    • geometry_msgs::PoseStamped -> /mavros/local_position/pose
    • geometry_msgs::TwistStamped -> /mavros/local_position/velocity_local
    • sensor_msgs::NavSatFix -> /mavros/global_position/global
  5. 发布消息 (Publish)。

B. 控制指令 (ROS -> Sim)

  1. 用户脚本 (Ros12MultiUav.py) 计算期望位置/速度。
  2. 用户脚本发布消息 mavros_msgs/PositionTarget 到 /mavros/setpoint_raw/local。
  3. Bridge 的订阅回调 (handleSetpoint) 被触发。
  4. Bridge 根据 type_mask 判断控制模式：
    • 位置控制: 提取 xyz，设置 HasPos。
    • 速度控制: 提取 vx,vy,vz，设置 HasVel。
  5. 封装 120 字节 SILIntFloat 结构体。
  6. 通过 UDP 发送到 30100 (ID1) 或 30102 (ID2)。
  7. RflySim 接收指令并驱动模型运动。

4. C++ 与 Python 实现对比 (Comparison)

特性 (Feature) C++ Demo Python Demo
性能 (Performance) 高，适合高频率闭环控制 中，受限于 GIL，适合中低频控制
并发 (Concurrency) 多线程 + select IO 复用 多线程 (threading)
ROS 兼容性 源码级兼容 ROS1/ROS2 (通过宏切换) 依赖 rospy 或 rclpy 库
部署 (Deployment) 需要编译 (colcon/catkin) 直接运行，无需编译
代码位置 udp_ros_bridge.cpp DllSimCtrlAPIROS.py

5. 总结 (Summary)

两个例程本质上实现了**相同的协议转换逻辑**。用户可以根据开发习惯选择：

  • 如果需要修改底层通信协议或追求极致性能，建议基于 C++ Demo 修改。
  • 如果只是进行简单的算法验证或利用 Python 丰富的生态（如 PyTorch/TensorFlow），建议使用 **Python Demo**。 无论哪种方式，其对外暴露的 ROS 接口（Topic/Service）是一致的，因此上层控制算法（如 Ros12MultiUav.py）可以通用。
制作者 doxygen 1.15.0