Gazebo 仿真中跑通 PX4 + MID360 + FAST-LIO2 + EGO-Planner 完整链路

本文记录一条已经跑通的仿真链路，并重点说明一键启动脚本的用法：

Gazebo + PX4 SITL
  -> MID360-like LiDAR + IMU
  -> FAST-LIO2
  -> EGO-Planner
  -> MAVROS
  -> PX4 Offboard

最终效果：执行 start_sim_px4_mid360_fastlio_ego.sh start 后，发送目标点，再切 OFFBOARD 并解锁，无人机即可按 EGO-Planner 规划轨迹飞行。

一、环境与文件

默认环境：

Ubuntu 22.04
ROS 2 Humble
Gazebo Harmonic
PX4-Autopilot SITL
FAST_LIO_ROS2
ego-planner-swarm ROS2
MAVROS ROS2

默认路径：

组件	默认路径
PX4	`~/PX4-Autopilot`
FAST-LIO2	`~/fastlio_ws/src/FAST_LIO_ROS2`
EGO-Planner	`~/ros2_ws/src/ego-planner-swarm`
MAVROS 里程计桥接	`~/ros2_ws/src/external_odom_to_mavros`
EGO 到 MAVROS 桥接	`~/ros2_ws/src/ego_to_mavros`
一键启动脚本	`~/code/start_sim_px4_mid360_fastlio_ego.sh`

本文用到的脚本和配置：

文件	作用
`~/code/start_sim_px4_mid360_fastlio_ego.sh`	一键启动、停止、发送目标点
`ego_planner/launch/ego_fastlio_mid360_tuned.launch.py`	EGO-Planner 参数和话题映射
`ego_planner/launch/ego_fastlio_mid360_rviz.launch.py`	RViz、静态 TF、目标点桥接
`ego_planner/launch/ego_fastlio_mid360.rviz`	EGO 专用 RViz 配置
`fast_lio/scripts/pointcloud_self_filter.py`	过滤输入 FAST-LIO 前的机体附近点云
`~/fastlio_ws/src/FAST_LIO_ROS2/config/gz_mid360.yaml`	Gazebo MID360 对应 FAST-LIO2 配置

二、一键启动脚本

启动完整链路：

1	~/code/start_sim_px4_mid360_fastlio_ego.sh start

常用命令：

命令	作用
`start`	启动完整仿真链路
`restart`	停止后重新启动
`stop`	停止 tmux 会话
`status`	查看 tmux 窗口状态
`attach`	进入 tmux 会话查看各节点终端
`goal`	发布一个测试目标点

脚本默认优先使用 tmux，没有 tmux 时会尝试使用 gnome-terminal。如果想稳定使用 attach/status/stop，推荐安装并使用 tmux。

脚本启动顺序如下：

gazebo
px4
gz_bridge
self_filter      # ENABLE_SELF_FILTER=true 时启用
fast_lio
ego_planner
ego_rviz
mavros
odom_to_mavros
ego_to_mavros

其中点云链路是：

Gazebo LiDAR
  -> /mid360/points_raw
  -> pointcloud_self_filter
  -> /mid360/points
  -> FAST-LIO2

如果关闭自滤波，Gazebo 点云会直接桥接到 /mid360/points：

1 2	ENABLE_SELF_FILTER=false \ ~/code/start_sim_px4_mid360_fastlio_ego.sh restart

三、快速飞行流程

启动后先确认关键话题有输出：

source /opt/ros/humble/setup.bash

ros2 topic hz /mid360/points
ros2 topic hz /livox/imu
ros2 topic hz /Odometry
ros2 topic hz /cloud_registered
ros2 topic hz /drone_0_planning/pos_cmd
ros2 topic hz /mavros/setpoint_raw/local

发送测试目标点：

1	~/code/start_sim_px4_mid360_fastlio_ego.sh goal

goal 子命令默认发送到 /rviz_goal_pose，再由 rviz_goal_bridge 补上高度并转发到 /move_base_simple/goal。默认目标是：

1
2
3

GOAL_X=1.0
GOAL_Y=0.0
GOAL_Z=1.0

指定目标：

1 2	GOAL_X=1.0 GOAL_Y=0.5 GOAL_Z=1.0 \ ~/code/start_sim_px4_mid360_fastlio_ego.sh goal

也可以直接向 EGO-Planner 发目标：

1 2	ros2 topic pub --once /move_base_simple/goal geometry_msgs/msg/PoseStamped \ "{header: {frame_id: 'world'}, pose: {position: {x: 1.0, y: 0.5, z: 1.0}, orientation: {w: 1.0}}}"

EGO-Planner 成功规划时，终端通常会看到：

Triggered!
[FSM]: from WAIT_TARGET to GEN_NEW_TRAJ
refine_success=1
[FSM]: from GEN_NEW_TRAJ to EXEC_TRAJ

确认 /mavros/setpoint_raw/local 稳定发布后，切到 Offboard：

1 2	ros2 service call /mavros/set_mode mavros_msgs/srv/SetMode \ "{base_mode: 0, custom_mode: 'OFFBOARD'}"

解锁：

1 2	ros2 service call /mavros/cmd/arming mavros_msgs/srv/CommandBool \ "{value: true}"

四、核心话题

作用	话题
Gazebo LiDAR 转 ROS 后点云	`/mid360/points`
Gazebo IMU 转 ROS 后 IMU	`/livox/imu`
FAST-LIO2 里程计	`/Odometry`
FAST-LIO2 当前帧局部点云	`/cloud_registered`
FAST-LIO2 累计地图	`/Laser_map`
RViz 原始目标点	`/rviz_goal_pose`
EGO-Planner 输入目标	`/move_base_simple/goal`
EGO-Planner 位置指令	`/drone_0_planning/pos_cmd`
MAVROS 外部视觉位姿	`/mavros/vision_pose/pose`
MAVROS raw setpoint	`/mavros/setpoint_raw/local`

EGO-Planner 的避障输入使用 /cloud_registered。/Laser_map 是累计地图，适合观察建图效果，不建议作为规划输入。

五、RViz 使用

脚本会启动：

1	ros2 launch ego_planner ego_fastlio_mid360_rviz.launch.py

该 launch 文件做三件事：

1
2
3

1. 发布 world -> camera_init 静态 TF
2. 发布 world -> map 静态 TF
3. 启动 rviz_goal_bridge，把 /rviz_goal_pose 转成 /move_base_simple/goal

主要显示项：

显示项	话题	说明
Odometry	`/Odometry`	FAST-LIO2 位姿
FAST-LIO Map	`/Laser_map`	累计地图，仅用于观察
FAST-LIO Cloud	`/cloud_registered`	当前帧局部点云
Map Inflate	`/drone_0_grid/grid_map/occupancy_inflate`	EGO 膨胀占据栅格
Goal Point	`/drone_0_plan_vis/goal_point`	目标点
Optimal Traj	`/drone_0_plan_vis/optimal_list`	优化轨迹
Drone Path	`/drone_0_vis/path`	飞行路径

在 RViz 中可以用 2D Goal Pose 点目标。RViz 先发到 /rviz_goal_pose，rviz_goal_bridge 再使用 GOAL_Z 作为默认高度转发给 EGO-Planner。

修改 RViz 点目标默认高度：

1 2	GOAL_Z=1.5 \ ~/code/start_sim_px4_mid360_fastlio_ego.sh restart

六、常用参数

启动脚本支持环境变量覆盖。常用参数如下：

参数	默认值	说明
`BACKEND`	`auto`	启动后端，支持 `auto/tmux/gnome`
`FASTLIO_RVIZ`	`true`	是否打开 FAST-LIO2 自带 RViz
`ENABLE_SELF_FILTER`	`true`	是否过滤机体附近点云
`GOAL_X/Y/Z`	`1.0/0.0/1.0`	`goal` 子命令的默认目标
`EGO_MAX_VEL`	`0.45`	EGO 最大速度
`EGO_MAX_ACC`	`0.7`	EGO 最大加速度
`EGO_INFLATION`	`0.30`	障碍物硬膨胀半径
`EGO_COLLISION_DIST`	`0.35`	优化器软安全距离
`EGO_LAMBDA_COLLISION`	`0.8`	避障代价权重
`EGO_VIRTUAL_CEIL`	`8.0`	EGO 虚拟天花板
`EGO_TO_MAVROS_LIMITS`	`passthrough`	桥接层是否额外限幅，可设为 `safe`
`AUTO_OFFBOARD`	`false`	是否自动切 Offboard
`AUTO_ARM`	`false`	是否自动解锁

提高避障保守程度：

EGO_MAX_VEL=0.35 \
EGO_INFLATION=0.35 \
EGO_COLLISION_DIST=0.45 \
EGO_LAMBDA_COLLISION=1.0 \
~/code/start_sim_px4_mid360_fastlio_ego.sh restart

窄通道容易卡住时，可以适当减小安全距离：

EGO_INFLATION=0.20 \
EGO_COLLISION_DIST=0.30 \
EGO_LAMBDA_COLLISION=0.7 \
~/code/start_sim_px4_mid360_fastlio_ego.sh restart

想飞得更快，先小幅提高速度和加速度：

1
2
3

EGO_MAX_VEL=0.8 \
EGO_MAX_ACC=1.2 \
~/code/start_sim_px4_mid360_fastlio_ego.sh restart

提速后如果开始贴墙，优先降速或提高 EGO_INFLATION，不要同时大幅提速和降低安全距离。

七、自动 Offboard

默认推荐手动切 OFFBOARD 和手动解锁，方便先观察 setpoint 是否稳定。

确认链路稳定后，仿真里可以启用自动模式：

AUTO_OFFBOARD=true \
AUTO_ARM=true \
AUTO_START_ON_GOAL=true \
AUTO_START_PRELOAD_TIME=1.5 \
~/code/start_sim_px4_mid360_fastlio_ego.sh restart

自动模式逻辑：

收到有效 EGO pos_cmd
持续发布 setpoint 到 AUTO_START_PRELOAD_TIME 秒
请求 OFFBOARD
确认进入 OFFBOARD 后请求 arm

如果姿态异常或控制不稳，立即回到默认手动模式：

1 2	AUTO_OFFBOARD=false AUTO_ARM=false \ ~/code/start_sim_px4_mid360_fastlio_ego.sh restart

八、排查

1. EGO 没有规划

先查输入：

1
2
3

ros2 topic hz /Odometry
ros2 topic hz /cloud_registered
ros2 topic echo --once /move_base_simple/goal

再看 EGO 终端是否出现：

1
2
3

wait for goal or trigger
plan_success=0
refine_success=0

第一次测试目标不要太远，也不要贴墙。建议 x/y 距离控制在 1m 到 2m，z=1.0。

2. 不能进入 Offboard

检查 MAVROS 连接和 setpoint：

1 2	ros2 topic echo --once /mavros/state ros2 topic hz /mavros/setpoint_raw/local

/mavros/setpoint_raw/local 需要稳定发布，建议高于 20Hz。若没有 setpoint，先确认 EGO 已经收到目标并发布 /drone_0_planning/pos_cmd。

3. 无人机贴墙或撞墙

先降低速度、增加安全距离：

EGO_MAX_VEL=0.35 \
EGO_INFLATION=0.35 \
EGO_COLLISION_DIST=0.45 \
EGO_LAMBDA_COLLISION=1.0 \
~/code/start_sim_px4_mid360_fastlio_ego.sh restart

同时在 RViz 中观察：

1
2
3

/cloud_registered
/drone_0_grid/grid_map/occupancy_inflate
/drone_0_plan_vis/optimal_list

重点确认障碍物是否进入局部地图，以及膨胀层是否把通道堵死。

4. `/Laser_map` 闪烁

/Laser_map 是 FAST-LIO2 每秒发布一次的累计地图。EGO RViz 中已经把 FAST-LIO Map 的 Decay Time 设为 0。

如果手动添加显示项，同样设置：

Topic: /Laser_map
Decay Time: 0
Style: Flat Squares
Use Fixed Frame: true

5. `stop/status/attach` 不生效

这些命令依赖 tmux 会话。如果脚本使用了 gnome-terminal 后端，窗口不会被集中管理，需要手动关闭窗口。

推荐显式使用 tmux：

1 2	BACKEND=tmux \ ~/code/start_sim_px4_mid360_fastlio_ego.sh restart

九、最小命令清单

~/code/start_sim_px4_mid360_fastlio_ego.sh start

~/code/start_sim_px4_mid360_fastlio_ego.sh goal

source /opt/ros/humble/setup.bash

ros2 service call /mavros/set_mode mavros_msgs/srv/SetMode \
  "{base_mode: 0, custom_mode: 'OFFBOARD'}"

ros2 service call /mavros/cmd/arming mavros_msgs/srv/CommandBool \
  "{value: true}"

这就是当前稳定跑通的主流程。