# pengyu_sim

**Repository Path**: potato77/pengyu_sim

## Basic Information

- **Project Name**: pengyu_sim
- **Description**: No description available
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 1
- **Forks**: 0
- **Created**: 2026-04-03
- **Last Updated**: 2026-05-18

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# pengyu_sim

`pengyu_sim` 是一个基于 ROS 的多平台动力学仿真工作区，当前重点覆盖 3 类对象：

- `uav_simulator`：无人机动力学与基础传感器仿真
  - 其中 `px4_control_simulator` 负责把 UAV 仿真包装成 MAVROS/PX4 风格接口，因此可以直接与 `Sunray_v2` 的 UAV 控制代码联调

- `ugv_simulator`：无人车动力学、可视化与终端控制
- `hot_wheels_simulator`：空地两栖平台动力学、可视化与终端控制

## 下载与编译方法
```bash
## 下载
git clone https://gitee.com/potato77/pengyu_sim
## 编译
cd pengyu_sim
catkin_make
## source
source devel/setup.bash
```

## TODO

1. `map_generator` 和 `local_sensing` 包目前主要从 `marsim` 中复制过来，动力学部分已经重写。
2. 继续验证当前自研动力学与 `uav_pid_controller` / `Sunray_v2` 控制链路闭环后的系统响应。
3. 梳理动力学中的各个环节公式，并补成文档。
4. 多段输入暂不优先，先完成动力学正确性验证。

## 与Sunray进行联合调试仿真

### 当前仿真中的 MAVROS/PX4 替代关系

当前 `pengyu_sim` 中的替代关系如下：

- `fake_mavros_bridge_node`
  负责模拟：
  - `/uav<id>/mavros/state`
  - 模式切换
  - 解锁/上锁
  - 默认连接状态
- `px4_control_sim_node`
  负责模拟：
  - `/uav<id>/mavros/setpoint_raw/local`
  - `/uav<id>/mavros/setpoint_raw/attitude`
  - 内部姿态/角速度到电机 RPM 的控制转换
- `quadrotor_dynamics_node`
  负责模拟：
  - 机体动力学
  - odom
  - IMU

所以如果你的目标是“让 `sunray_uav_control` 误以为自己接到了 MAVROS/PX4”，当前这套链路已经够用了。

### `pengyu_sim` 与 `Sunray_v2` 联合仿真框架

可以把联合仿真理解成下面这两条主链路：

```text
UAV:
Sunray_v2/sunray_uav_control
        |
        |  MAVROS 风格 setpoint / mode / arming
        v
fake_mavros_bridge_node
        |
        |  /uavN/mavros/setpoint_raw/*
        v
px4_control_sim_node
        |
        |  /uavN/pengyu_sim/cmd_RPM
        v
quadrotor_dynamics_node
        |
        |  /uavN/pengyu_sim/odom / imu / navsat
        +-----------------------> uav_visualization_node
        |
        +-----------------------> localization_fusion ------> Sunray_v2/sunray_uav_control

UGV:
Sunray_v2/sunray_ugv_control
        |
        |  /ugvN/sunray/cmd_vel
        v
ugv_dynamics_node
        |
        |  /ugvN/pengyu_sim/odom / imu
        +-----------------------> localization_fusion ------> Sunray_v2/sunray_ugv_control

Hot Wheels:
terminal_control / 自定义上层控制
        |
        |  cmd_vel / cmd_rpm / mode_switch
        v
hot_wheels_dynamics_node -----> hot_wheels_visualization_node
```

如果按职责拆开看：

- `fake_mavros_bridge_node` 负责把 UAV 的 `pengyu_sim` 原生状态包装成 MAVROS 语义，同时接住模式切换、解锁等服务。
- `px4_control_sim_node` 负责把 MAVROS 风格 setpoint 继续变成电机 RPM。
- `quadrotor_dynamics_node`、`ugv_dynamics_node`、`hot_wheels_dynamics_node` 负责真正的动力学推进和基础传感器发布。
- `localization_fusion` 位于控制器和仿真状态之间，向 `Sunray_v2` 提供它期望的定位输入。

统一约定：

- 每个机器人都使用 `agent_name` 和 `agent_id` 标识，`agent_id` 从 1 开始编号。

- 话题前缀统一为 `/<agent_name><agent_id>`，例如 `agent_name:=uav agent_id:=1` 对应 `/uav1`。

- 无人机默认 `agent_name:=uav`，无人车默认 `agent_name:=ugv`，风火轮两栖平台默认 `agent_name:=hot_wheels`。

- 集群数量统一使用  `swarm_num`，初始编号与位姿统一使用 `agent_id_k / init_x_k / init_y_k / init_yaw_k`。

### 与 Sunray 联调时的推荐启动顺序（单个无人机）

> `pengyu_sim` 可以理解为PX4_SITL+MAVROS 的替身，可以模拟 `sunray_uav_control` 发出的各种MAVROS底层控制指令

- 步骤 1：启动 `pengyu_sim`的无人机模拟器`uav_simulator`

  ```bash
  ## 可以通过init_x、init_y、init_z、init_yaw来配置无人机的初始位置
  ## agent_id 可以任意设置，但后续 Sunray_v2 启动程序的参数也要对应同一编号
  ## 默认不启动仿真器里的rviz可视化，因为sunray中会有可视化程序
  ## UAV 参数文件集中在：$(find uav_simulator)/config/modules/{dynamics,imu,pid}.yaml
  ## PX4控制算法配置文件在：$(find px4_control_simulator)/config/px4_control_sim.yaml
  ## 质量、重力、电机系数等动力学参数从 dynamics.yaml 读取，IMU 参数从 imu.yaml 读取
  roslaunch px4_control_simulator px4_uav_simulator.launch agent_name:=uav agent_id:=1
  ```

  - 这一步启动了三个节点
    - `fake_mavros_bridge_node` 负责把 UAV 的 `pengyu_sim` 原生状态包装成 MAVROS 话题发布，同时接住模式切换、解锁等MAVROS服务。
    - `px4_control_sim_node` 负责把 MAVROS 风格 setpoint 继续变成电机 RPM。
    - `quadrotor_dynamics_node` 订阅电机RPM话题，解算无人机动力学并发布odom话题。

- 步骤 2：启动`Sunray`中的定位模块 `localization_fusion`（ `pengyu_sim`产生的`odom`消息会经过定位节点转发至`Sunray`其他模块）

  ```bash
  ## source_id:=5代表定位来源于pengyu_sim的odom
  roslaunch localization_fusion localization_fusion.launch source_id:=5 agent_name:=uav agent_id:=1
  ```

- 步骤 3：启动`Sunray`中的无人机控制模块  `sunray_uav_control`

  ```bash
  ## 注意修改配置文件/Sunray_v2/control/sunray_uav_control/config/sunray_control_config.yaml中的PX4控制器类型
  ## 包括了无人机控制节点uav_control_node、打印节点uav_control_monitor_node、可视化节点rviz_visualization_uav_control_node、rviz
  roslaunch sunray_uav_control uav_control.launch agent_name:=uav agent_id:=1
  ```

- 步骤 4：启动`Sunray`中的无人车地面站 `uav_control_panel`用于监控和下发无人机控制指令进行测试

  ```bash
  roslaunch control_tools uav_control_panel.launch 
  ```

- 步骤 4：发送控制指令，通常通过 `sunray` 的终端控制节点来发布 `UAVControlCMD` 指令。

  ```bash
  rosrun control_tools uav_terminal_control_node
  ```

### 与 Sunray 联调 - 单无人机规划验证

> 这一链路是在“单个无人机控制联调”基础上，进一步把 `Sunray_v2/planning/sunray_planning` 接进来，验证规划器能否正确接收规划指令、跟随控制状态机并输出 `uav_control_cmd`。

- 规划模块入口：

  ```bash
  /home/amov/Sunray_v2/planning/sunray_planning/launch/sunray_planning.launch
  ```

- 这个 launch 的默认关键话题关系如下：

  - 订阅 `/uav1/sunray/planning_cmd`
  - 订阅 `/uav1/sunray/fsm/state`
  - 发布 `/uav1/sunray/uav_control_cmd`
  - 发布 `/uav1/sunray/planning_state`

- 推荐启动顺序：

  - 步骤 1.1：启动 `pengyu_sim` 全局点云发布器（如果规划器自己会开rviz，这里就别开rviz了）

    ```bash
    ## 可在launch文件中选择加载的PCD文件
    ## 启动后，发布全局点云话题为/map_generator/global_cloud,这个话题发布频率为0.1Hz
    roslaunch map_generator map_publisher.launch launch_rviz:=false
    ```

  - 步骤 1.2：启动 `pengyu_sim` 局部点云渲染程序

    ```bash
    ## 启动后，发布的局部点云话题为/<agent_name><agent_id>/pengyu_sim/cloud(频率大约为10hz)
    roslaunch local_sensing local_sensing_mid360.launch
    ```

  - 步骤 1.3：启动 `pengyu_sim` 单无人机仿真

    ```bash
    roslaunch px4_control_simulator px4_uav_simulator.launch agent_name:=uav agent_id:=1
    ```

  - 步骤 2：启动 `Sunray` 定位模块 `localization_fusion`

    ```bash
    roslaunch localization_fusion localization_fusion.launch source_id:=5 agent_name:=uav agent_id:=1
    ```

  - 步骤 3：启动 `Sunray` 无人机控制模块 `sunray_uav_control`

    ```bash
    ## 规划模块默认会跟随控制状态机，因此这里的控制模块需要先起来
    roslaunch sunray_uav_control uav_control.launch uav_name:=uav uav_id:=1 enable_rviz:=true
    ```

  - 步骤 4：启动 `Sunray` 规划模块 `sunray_planning`

    ```bash
    ## planner_type 默认是 diff
    ## 默认会订阅 /uav1/sunray/planning_cmd 和 /uav1/sunray/fsm/state
    ## 默认会发布 /uav1/sunray/uav_control_cmd 和 /uav1/sunray/planning_state
    roslaunch sunray_planning sunray_planning.launch uav_name:=uav uav_id:=1 planner_type:=diff
    ```

  - 步骤 5：发送规划指令并观察规划状态、控制输出和飞行轨迹

    ```bash
    rostopic echo /uav1/sunray/planning_state
    ```

- 常用可调参数：

  - `planner_type`：当前 launch 默认是 `diff`
  - `process_rate_hz`：规划主循环频率，默认 `50.0`
  - `state_pub_rate_hz`：规划状态发布频率，默认 `10.0`
  - `auto_hover_on_timeout`：规划指令超时后是否自动悬停，默认 `true`
  - `follow_control_fsm`：是否跟随控制状态机，默认 `true`

- 使用注意：

  - 如果 `uav_name`、`uav_id` 与仿真器、定位模块、控制模块不一致，规划器会挂到错误的话题前缀上。
  - 如果控制模块没有正常发布 `/uav1/sunray/fsm/state`，规划器即使启动成功，也可能无法进入预期工作状态。
  - 规划验证阶段建议先保持 `follow_control_fsm:=true`，等链路确认稳定后再测试更激进的参数组合。

### 与 Sunray 联调时的推荐启动顺序（集群无人机，swarm_num可以配置为3~10）

> 限于launch文件的预设写法和预设阵型限制，集群数量目前仅支持3-10。

- 步骤 1：启动 `pengyu_sim`的无人机模拟器`uav_simulator`

  ```bash
  roslaunch px4_control_simulator px4_uav_simulator_swarm.launch swarm_num:=6
  ```

- 步骤 2：启动`Sunray`中的定位模块 `localization_fusion`（ `pengyu_sim`产生的`odom`消息会经过定位节点转发至`Sunray`其他模块）

  ```bash
  roslaunch localization_fusion localization_fusion_swarm.launch source_id:=5 agent_name:=uav agent_num:=6
  ```

- 步骤 3：启动`Sunray`中的无人机控制模块  `sunray_uav_control`

  ```bash
  ## 配置文件在：control/sunray_uav_control/config/sunray_control_config.yaml
  roslaunch sunray_uav_control uav_control_swarm.launch agent_name:=uav agent_num:=6
  ```

- 步骤 4：启动`Sunray`中的无人机集群控制

  ```bash
  roslaunch sunray_swarm_control swarm_control_uav_sim.launch swarm_num:=6
  ```

- 步骤5：启动集群监控和控制面板

  ```bash
  ## 面板控制（推荐）
  roslaunch sunray_swarm_control swarm_control_panel.launch
  ## 终端控制
  roslaunch sunray_swarm_control uav_swarm_cmd_pub_terminal.launch
  ```

### 与 Sunray 联调时的推荐启动顺序（单台无人车）

> `pengyu_sim` 可以理解为无人车底盘控制器的替身，可以模拟 `sunray_ugv_control` 发出的`cmd_vel`指令

- 步骤 1：启动 `pengyu_sim`的无人车模拟器`ugv_simulator`

  ```bash
  ## agent_id 可以任意设置，但后续 Sunray_v2 启动程序的参数也要对应同一编号
  ## 启动时可直接指定 agent_name、agent_id、drive_type（mecanum 或 differential）
  ## 其余动力学与初始位姿参数在：src/ugv_simulator/launch/ugv_simulator.yaml
  roslaunch ugv_simulator ugv_simulator.launch agent_name:=ugv agent_id:=1 drive_type:=mecanum
  ```
  
- 步骤 2：启动`Sunray`中的定位模块 `localization_fusion`（ `pengyu_sim`产生的`odom`消息会经过定位节点转发至`Sunray`其他模块）

  ```bash
  ## source_id:=5代表定位来源于pengyu_sim的odom
  roslaunch localization_fusion localization_fusion.launch source_id:=5 agent_name:=ugv agent_id:=1
  ```

- 步骤 3：启动`Sunray`中的无人车控制模块 `sunray_ugv_control`

  ```bash
  ## 配置文件在：control/sunray_ugv_control/config/sunray_control_config.yaml，可以修改底盘类型（一般默认使用麦克纳姆轮底盘）
  ## 包括了无人车控制节点ugv_control_node、打印节点ugv_control_monitor_node、可视化节点rviz_visualization_ugv_control_node、rviz
  roslaunch sunray_ugv_control ugv_control.launch agent_name:=ugv agent_id:=1 enable_rviz:=true
  ```

- 步骤 4：启动`Sunray`中的无人车地面站 `ugv_control_panel`用于监控和下发无人车控制指令进行测试

  ```bash
  roslaunch sunray_ugv_control ugv_control_panel.launch 
  ```

### 与 Sunray 联调时的推荐启动顺序（集群无人车，swarm_num可以配置为3~10）

- 步骤 1：启动 `pengyu_sim`的无人车模拟器`ugv_simulator`

  ```bash
  ## swarm_num代表集群数量，后续 Sunray_v2 启动程序的集群数量也要一致
  ## 启动时可直接指定 agent_name、swarm_num、drive_type（mecanum 或 differential）
  ## 各车初始位姿参数在：src/ugv_simulator/launch/ugv_simulator.yaml
  roslaunch ugv_simulator ugv_simulator.launch agent_name:=ugv swarm_num:=6 drive_type:=mecanum
  ```
  
- 步骤 2：启动`Sunray`中的定位模块 `localization_fusion`（ `pengyu_sim`产生的`odom`消息会经过定位节点转发至`Sunray`其他模块）

  ```bash
  roslaunch localization_fusion localization_fusion_swarm.launch source_id:=5 agent_name:=ugv agent_num:=6
  ```

- 步骤 3：启动`Sunray`中的无人车控制模块 `sunray_ugv_control`

  ```bash
  ## 配置文件在：control/sunray_ugv_control/config/sunray_control_config.yaml，可以修改底盘类型
  ## 此时不启动rviz
  roslaunch sunray_ugv_control ugv_control_swarm.launch swarm_num:=6
  ```

- 步骤 4：启动`Sunray`中的无人车集群控制

  ```bash
  roslaunch sunray_swarm_control swarm_control_ugv_sim.launch swarm_num:=6
  ```

- 步骤5：启动集群监控和控制面板

  ```bash
  ## 面板控制（推荐）
  roslaunch sunray_swarm_control swarm_control_panel.launch
  ## 终端控制
  roslaunch sunray_swarm_control ugv_swarm_cmd_pub_terminal.launch
  ```

## Hot Wheels 仿真启动步骤(张海鹏、曾浩然专用)

- 步骤 1：启动 `hot_wheels_simulator`

  ```bash
  roslaunch hot_wheels_simulator hot_wheels_simulator.launch agent_name:=hot_wheels agent_id:=1
  ```

- 步骤 2：启动终端控制程序

  ```bash
  roslaunch hot_wheels_simulator hot_wheels_terminal_control.launch agent_name:=hot_wheels agent_id:=1
  ```

  终端中目前提供 3 个演示入口：

  - `1`：发布地面模式 `cmd_vel`
  - `2`：切换到空中模式并飞到目标点
  - `3`：先在地面画圈，再起飞到空中画圈

## pengyu_sim核心程序说明

当前工作区内的主要功能包都在 `src/` 下，可以按下面的方式快速理解：

| 包 | 主要职责 | 典型入口 |
| --- | --- | --- |
| `uav_simulator` | 无人机动力学、IMU/里程计/NavSat 仿真，以及 UAV 自带 RViz 可视化。 | `uav_simulator.launch` |
| `px4_control_simulator` | MAVROS/PX4 风格接口仿真，包含 `px4_control_sim_node`、`fake_mavros_bridge_node` 和 UAV 终端控制工具。 | `px4_uav_simulator.launch` `px4_uav_simulator_swarm.launch` `terminal_control.launch` |
| `ugv_simulator` | 无人车动力学仿真，话题风格对齐 `Sunray_v2`。 | `ugv_simulator.launch` |
| `hot_wheels_simulator` | 空地两栖平台动力学，支持地面差速、空中四旋翼和模式切换。 | `hot_wheels_simulator.launch` `hot_wheels_cluster.launch` |
| `uav_pid_controller` | UAV PID 控制器，把位置/姿态目标转换成电机 RPM。 | `uav_pid_controller.launch` |
| `map_generator` | 发布全局点云地图，提供仿真环境。 | `map_generator` 相关 launch |
| `local_sensing` | 基于地图和位姿生成局部点云/深度感知结果。 | `local_sensing` |
| `test_interface` | 快速联调入口，把简单目标点/轨迹转换成控制输入。 | `test_interface_node` |
| `sim_debug` | 轨迹和状态调试辅助。 | `sim_debug` 相关 launch |
| `quadrotor_msgs` | 四旋翼相关消息定义。 | 消息包 |
| `pose_utils` | 位姿、旋转、坐标变换工具。 | 基础库 |
| `rviz_plugins` | 自定义 RViz 插件。 | RViz 插件 |
| `uav_utils` | UAV 通用头文件和脚本工具。 | 基础库/脚本 |
| `waypoint_generator` | 生成点、圆、8 字、分段等航点。 | `waypoint_generator` 节点 |
| `odom_visualization` | 旧版可视化包，当前主要保留给老测试链路兼容。 | 兼容用途 |

主要关系可以粗略理解为：

```text
map_generator -----------------> local_sensing
                                      ^
                                      |
test_interface / sim_debug / waypoint_generator / 上层控制器
                   |                  |                  |
                   v                  v                  v
         uav_simulator <-> px4_control_simulator     ugv_simulator
                   |                                      |
                   v                                      v
           uav_visualization                    localization_fusion / 上层控制

hot_wheels_terminal_control ----> hot_wheels_simulator ----> hot_wheels_visualization

quadrotor_msgs / pose_utils / uav_utils / rviz_plugins
                提供消息定义、基础工具和显示扩展
```

### UAV 核心模块

`px4_uav_simulator.launch` 会启动 `quadrotor_dynamics_node`、`px4_control_sim_node`、`fake_mavros_bridge_node` 和 `uav_visualization_node`。集群入口 `px4_uav_simulator_swarm.launch` 通过 `swarm_num` 控制启动前几台。

`quadrotor_dynamics_node`

根据电机转速推进四旋翼动力学，并发布里程计、IMU 和 GNSS 数据。

ROS参数

| 参数名 | 类型 | 默认值 | 说明 |
| --- | --- | --- | --- |
| `agent_name` | `string` | `uav` | 机器人名前缀 |
| `agent_id` | `int` | `1` | 机器人编号 |
| `dynamics_update_rate` | `double` | `200.0` | 动力学积分和状态发布频率，当前 launch 默认从 `config/modules/dynamics.yaml` 读取 |
| `init_x/y/z` | `double` | `0.0` | 初始位置 |
| `init_yaw` | `double` | `0.0` | 初始航向角 |
| `cmd_timeout` | `double` | `0.1` | 控制输入超时 |
| `dynamics/mass` | `double` | `1.5` | 机体质量 |
| `dynamics/arm_length` | `double` | `0.25` | 机臂长度 |
| `motor/omega_max` | `double` | `1200.0` | 最大电机转速 |

订阅话题

| 话题名 | 消息类型 | 说明 |
| --- | --- | --- |
| `/uav<id>/pengyu_sim/cmd_RPM` | `std_msgs/Float32MultiArray` | 四个电机的期望转速 |

发布话题

| 话题名 | 消息类型 | 说明 |
| --- | --- | --- |
| `/uav<id>/pengyu_sim/odom` | `nav_msgs/Odometry` | 无人机里程计 |
| `/uav<id>/pengyu_sim/imu` | `sensor_msgs/Imu` | IMU 数据 |
| `/uav<id>/pengyu_sim/navsat` | `sensor_msgs/NavSatFix` | GNSS 数据 |

`px4_control_sim_node`

将 MAVROS 风格控制指令转换成 `pengyu_sim` 使用的电机 RPM 指令。

ROS参数

| 参数名 | 类型 | 默认值 | 说明 |
| --- | --- | --- | --- |
| `agent_name` | `string` | `uav` | 机器人名前缀 |
| `agent_id` | `int` | `1` | 机器人编号 |
| `px4_update_rate` | `double` | `100.0` | PX4 控制转换更新频率 |
| `limits/max_vel_xy` | `double` | `0.8` | 最大水平速度 |
| `limits/max_vel_z` | `double` | `0.6` | 最大垂直速度 |
| `limits/max_tilt_deg` | `double` | `15.0` | 最大倾角 |
| `px4_control_sim/enable_status_print` | `bool` | `true` | 是否打印状态 |

订阅话题

| 话题名 | 消息类型 | 说明 |
| --- | --- | --- |
| `/uav<id>/mavros/setpoint_raw/local` | `mavros_msgs/PositionTarget` | 位置/速度控制指令 |
| `/uav<id>/mavros/setpoint_raw/attitude` | `mavros_msgs/AttitudeTarget` | 姿态控制指令 |
| `/uav<id>/mavros/state` | `mavros_msgs/State` | 飞控状态 |
| `/uav<id>/pengyu_sim/odom` | `nav_msgs/Odometry` | 当前位姿反馈 |

发布话题

| 话题名 | 消息类型 | 说明 |
| --- | --- | --- |
| `/uav<id>/pengyu_sim/cmd_RPM` | `std_msgs/Float32MultiArray` | 电机转速指令 |

`fake_mavros_bridge_node`

把 `pengyu_sim` 原生状态包装成 MAVROS 接口，同时提供常用模式切换、解锁和参数服务。

ROS参数

| 参数名 | 类型 | 默认值 | 说明 |
| --- | --- | --- | --- |
| `agent_name` | `string` | `uav` | 机器人名前缀 |
| `agent_id` | `int` | `1` | 机器人编号 |
| `fake_mavros_bridge/mavros_publish_rate` | `double` | `50.0` | MAVROS 风格状态话题发布频率 |
| `fake_mavros_bridge/start_connected` | `bool` | `true` | 初始连接状态 |
| `fake_mavros_bridge/start_armed` | `bool` | `true` | 初始解锁状态 |
| `fake_mavros_bridge/start_mode` | `string` | `OFFBOARD` | 初始飞行模式 |

订阅话题

| 话题名 | 消息类型 | 说明 |
| --- | --- | --- |
| `/uav<id>/pengyu_sim/odom` | `nav_msgs/Odometry` | 原生位姿状态 |
| `/uav<id>/pengyu_sim/imu` | `sensor_msgs/Imu` | 原生 IMU 状态 |
| `/uav<id>/mavros/setpoint_raw/local` | `mavros_msgs/PositionTarget` | 本地控制指令 |
| `/uav<id>/mavros/setpoint_raw/attitude` | `mavros_msgs/AttitudeTarget` | 姿态控制指令 |

发布话题

| 话题名 | 消息类型 | 说明 |
| --- | --- | --- |
| `/uav<id>/mavros/state` | `mavros_msgs/State` | 飞控状态 |
| `/uav<id>/mavros/extended_state` | `mavros_msgs/ExtendedState` | 扩展状态 |
| `/uav<id>/mavros/sys_status` | `mavros_msgs/SysStatus` | 系统状态 |
| `/uav<id>/mavros/estimator_status` | `mavros_msgs/EstimatorStatus` | 估计器状态 |
| `/uav<id>/mavros/local_position/odom` | `nav_msgs/Odometry` | MAVROS 风格里程计 |
| `/uav<id>/mavros/imu/data` | `sensor_msgs/Imu` | MAVROS 风格 IMU |

### UGV 核心模块

`ugv_simulator.launch` 只启动一个 `ugv_dynamics_node`，单车和集群共用这一套入口。

`ugv_dynamics_node`

根据地面速度指令推进无人车动力学，并发布里程计和 IMU 数据。

ROS参数

| 参数名 | 类型 | 默认值 | 说明 |
| --- | --- | --- | --- |
| `agent_name` | `string` | `ugv` | 机器人名前缀 |
| `agent_id` | `int` | `1` | 机器人编号 |
| `swarm_num` | `int` | `1` | 启动的无人车数量 |
| `drive_type` | `string` | `mecanum` | 底盘类型，`mecanum` 或 `differential` |
| `publish_rate` | `double` | `100.0` | 状态更新频率 |
| `cmd_timeout` | `double` | `0.2` | 控制输入超时 |
| `linear_acc_limit` | `double` | `3.0` | 线速度加速度限制 |
| `angular_acc_limit` | `double` | `6.0` | 角速度加速度限制 |

订阅话题

| 话题名 | 消息类型 | 说明 |
| --- | --- | --- |
| `/ugv<id>/sunray/ugv_control/cmd_vel` | `geometry_msgs/Twist` | 底盘速度指令 |

发布话题

| 话题名 | 消息类型 | 说明 |
| --- | --- | --- |
| `/ugv<id>/pengyu_sim/odom` | `nav_msgs/Odometry` | 无人车里程计 |
| `/ugv<id>/pengyu_sim/imu` | `sensor_msgs/Imu` | IMU 数据 |

### 全局点云生成模块

`map_pub`

读取 PCD 地图文件，完成降采样、坐标偏移和可选边界补充后，发布静态全局点云地图，供局部感知和仿真环境使用。

启动方式

```bash
roslaunch map_generator map_publisher.launch
```

ROS参数

| 参数名 | 类型 | 默认值 | 说明 |
| --- | --- | --- | --- |
| `map_name` | `string` | `$(find map_generator)/resource/yundrone.pcd` | 输入的 PCD 地图文件路径 |
| `add_boundary` | `int` | `0` | 是否额外添加边界点 |
| `is_bridge` | `int` | `0` | 是否启用桥接场景分支 |
| `downsample_res` | `double` | `0.1` | 地图体素降采样分辨率 |
| `map_offset_x` | `double` | `0.0` | 地图整体 `x` 偏移 |
| `map_offset_y` | `double` | `0.0` | 地图整体 `y` 偏移 |
| `map_offset_z` | `double` | `0.0` | 地图整体 `z` 偏移 |

订阅话题

| 话题名 | 消息类型 | 说明 |
| --- | --- | --- |
| 无 | 无 | 该节点不订阅外部话题 |

发布话题

| 话题名 | 消息类型 | 说明 |
| --- | --- | --- |
| `/map_generator/global_cloud` | `sensor_msgs/PointCloud2` | 全局静态点云地图，锁存发布 |

### 局部点云渲染模块

`pcl_render_node`

结合全局地图和当前位姿，使用 PCL 点云渲染链路生成局部可见点云、深度图、传感器位姿、动态障碍点云等感知结果。

启动方式

```bash
roslaunch local_sensing local_sensing_mid360.launch
```

或

```bash
roslaunch local_sensing local_sensing_os128.launch
```

ROS参数

| 参数名 | 类型 | 默认值 | 说明 |
| --- | --- | --- | --- |
| `agent_name` | `string` | `uav` | 机器人名前缀 |
| `agent_id` | `int` | `1` | 机器人编号 |
| `swarm_num` | `int` | `1` | 集群数量，会映射到内部多机感知参数 |
| `sensing_horizon` | `double` | 无默认值 | 感知距离 |
| `sensing_rate` | `double` | 无默认值 | 局部点云刷新频率 |
| `estimation_rate` | `double` | 无默认值 | 传感器位姿发布频率 |
| `downsample_res` | `double` | 无默认值 | 点云降采样分辨率 |
| `dynobj_enable` | `bool/int` | 无默认值 | 是否启用动态障碍物仿真 |
| `collisioncheck_enable` | `bool/int` | 无默认值 | 是否启用碰撞检测 |

订阅话题

| 话题名 | 消息类型 | 说明 |
| --- | --- | --- |
| `/<agent_name><agent_id>/pengyu_sim/odom` | `nav_msgs/Odometry` | 当前载体位姿输入，由程序按 `agent_name` 和 `agent_id` 自动生成 |
| `global_map_topic` | `sensor_msgs/PointCloud2` | 全局点云地图输入，默认 `/map_generator/global_cloud`，仅 `pcl_render_node` 使用 |

发布话题

| 话题名 | 消息类型 | 说明 |
| --- | --- | --- |
| `/<agent_name><agent_id>/pengyu_sim/cloud` | `sensor_msgs/PointCloud2` | 局部渲染点云，随 `sensing_rate` 发布 |
| `/<agent_name><agent_id>/pengyu_sim/sensor_cloud` | `sensor_msgs/PointCloud2` | 传感器可见点云，随 `sensing_rate` 发布 |
| `/<agent_name><agent_id>/pengyu_sim/dyn_cloud` | `sensor_msgs/PointCloud2` | 动态障碍物点云，随 `sensing_rate` 更新 |
| `/<agent_name><agent_id>/pengyu_sim/depth_img` | `sensor_msgs/Image` | 深度图像，随 `sensing_rate` 发布 |
| `/<agent_name><agent_id>/pengyu_sim/sensor_pose` | `geometry_msgs/PoseStamped` | 传感器位姿，由 `odom` 乘固定传感器外参后得到，随 `estimation_rate` 发布 |
| `/<agent_name><agent_id>/pengyu_sim/uav_cloud` | `sensor_msgs/PointCloud2` | 机体点云，收到其他无人机 `odom` 时更新 |

点云话题说明

| 话题名 | 具体含义 |
| --- | --- |
| `/<agent_name><agent_id>/pengyu_sim/cloud` | 世界坐标系下的最终局部渲染结果。除静态环境外，在功能开启且目标可见时，也会包含动态障碍物和其他无人机机体点云。 |
| `/<agent_name><agent_id>/pengyu_sim/sensor_cloud` | 传感器坐标系下的局部点云，本质上是把 `cloud` 变换到当前传感器坐标系后的结果，因此其内容与 `cloud` 对应，只是坐标系不同。 |
| `/<agent_name><agent_id>/pengyu_sim/dyn_cloud` | 动态障碍物点云，只单独发布仿真中额外生成的动态目标，可看作 `cloud` 中动态障碍物部分的拆分输出。 |
| `/<agent_name><agent_id>/pengyu_sim/uav_cloud` | 其他无人机机体模型对应的点云，用于多机情况下把其他无人机也作为环境障碍渲染出来，可看作 `cloud` 中其他无人机部分的拆分输出。 |

### Hot Wheels 核心模块

`hot_wheels_dynamics_node`

模拟空地两栖平台。地面模式下响应 `cmd_vel`，空中模式下响应四电机 `rpm`，并支持模式切换。

ROS参数

| 参数名 | 类型 | 默认值 | 说明 |
| --- | --- | --- | --- |
| `agent_name` | `string` | `hot_wheels` | 机器人名前缀 |
| `agent_id` | `int` | `1` | 机器人编号 |
| `publish_rate` | `double` | `100.0` | 状态更新频率 |
| `initial_mode` | `int` | `0` | 初始模式 |
| `init_x/y/z` | `double` | `0.0` | 初始位置 |
| `init_yaw` | `double` | `0.0` | 初始航向角 |
| `cmd_timeout` | `double` | `0.25` | 地面控制超时 |
| `rpm_timeout` | `double` | `0.25` | 空中控制超时 |
| `ground_max_linear_x` | `double` | `1.5` | 地面最大线速度 |
| `max_rpm` | `double` | `2200.0` | 空中模式最大电机转速 |

订阅话题

| 话题名 | 消息类型 | 说明 |
| --- | --- | --- |
| `/hot_wheels<id>/pengyu_sim/cmd_vel` | `geometry_msgs/Twist` | 地面模式速度指令 |
| `/hot_wheels<id>/pengyu_sim/cmd_rpm` | `std_msgs/Float32MultiArray` | 空中模式电机转速指令 |
| `/hot_wheels<id>/pengyu_sim/mode_switch` | `std_msgs/UInt8` | 模式切换指令 |

发布话题

| 话题名 | 消息类型 | 说明 |
| --- | --- | --- |
| `/hot_wheels<id>/pengyu_sim/odom` | `nav_msgs/Odometry` | 里程计数据 |
| `/hot_wheels<id>/pengyu_sim/imu` | `sensor_msgs/Imu` | IMU 数据 |
| `/hot_wheels<id>/pengyu_sim/mode_state` | `std_msgs/UInt8` | 当前模式状态 |