前言

本文把两条链路合在一起：

1
Mid-360S -> FAST-LIO2 -> EGO-Planner -> MAVROS -> PX4 Offboard

目标分三步：

  1. 先跑通 EGO-Planner 官方仿真。
  2. 再接入 Mid-360S + FAST-LIO2，只在 RViz 中看规划。
  3. 最后把 /drone_0_planning/pos_cmd 桥接到 MAVROS Offboard setpoint。

第一次真机测试不要装桨。先看 setpoint，再切 Offboard，最后才低高度短距离飞行。

一、整体链路

完整链路如下：

1
2
3
4
5
6
7
8
9
10
Mid-360S
  -> livox_ros_driver2
  -> FAST-LIO2
  -> /Odometry + /cloud_registered
  -> EGO-Planner
  -> /drone_0_planning/pos_cmd
  -> ego_to_mavros
  -> /uav1/setpoint_raw/local
  -> MAVROS
  -> PX4 Offboard

如果 MAVROS 没有使用命名空间，把 /uav1/... 改成 /mavros/...。

二、安全顺序

建议按下面顺序推进：

1
2
3
4
5
6
7
1. 官方仿真
2. 真实雷达 + FAST-LIO2
3. EGO-Planner 只看 RViz
4. PX4 Position 模式悬停
5. 不装桨检查 Offboard setpoint
6. 不装桨手动切 Offboard
7. 装桨低高度短距离测试

第一次真机参数建议：

项目 建议值
最大水平速度 0.3 ~ 0.5 m/s
最大高度 1.0 ~ 1.2 m
目标距离 1 ~ 2 m
障碍物 先不放，后面再用软质障碍物

不要一开始自动解锁。遥控器要随时能切回 Position / Altitude / Stabilized。

三、准备环境

本文使用：

1
2
3
4
5
6
Ubuntu 22.04
ROS 2 Humble
Livox Mid-360S
FAST_LIO_ROS2
MAVROS ROS2
EGO-Planner ROS2

安装常用依赖：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
sudo apt update
sudo apt install -y \
  git \
  cmake \
  build-essential \
  python3-colcon-common-extensions \
  python3-rosdep \
  libeigen3-dev \
  libpcl-dev \
  libopencv-dev \
  ros-humble-pcl-ros \
  ros-humble-cv-bridge \
  ros-humble-rviz2 \
  ros-humble-rmw-cyclonedds-cpp

建议统一使用 CycloneDDS：

1
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp

确认：

1
echo $RMW_IMPLEMENTATION

如果确认稳定，可以写入 ~/.bashrc。

四、编译工作区

下文统一使用 ~/ros2_ws：

1
2
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src

需要准备这些包：

包 作用
ego-planner-swarm EGO-Planner ROS2
external_odom_to_mavros /Odometry 到 MAVROS 外部视觉
ego_to_mavros EGO pos_cmd 到 MAVROS setpoint

编译：

1
2
3
4
5
cd ~/ros2_ws
source /opt/ros/humble/setup.bash
rosdep install --from-paths src --ignore-src -y
colcon build --symlink-install
source install/setup.bash

检查：

1
2
3
ros2 pkg executables ego_planner
ros2 pkg executables ego_to_mavros
ros2 pkg executables external_odom_to_mavros

ego_to_mavros 只需要已经在工作区中可用。本文不展开源码。

五、先跑官方仿真

先确认 EGO-Planner 本体能跑。

终端 1：

1
2
3
4
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
ros2 launch ego_planner rviz.launch.py

终端 2：

1
2
3
4
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
ros2 launch ego_planner single_run_in_sim.launch.py use_mockamap:=True use_dynamic:=False

检查规划话题：

1
ros2 topic list | grep planning

常见输出：

1
2
3
/drone_0_planning/bspline
/drone_0_planning/pos_cmd
/drone_0_planning/data_display

RViz 中能看到障碍物、无人机模型和轨迹，说明仿真通过。

六、确认 FAST-LIO2 输出

启动 Mid-360S 和 FAST-LIO2 后，先检查两个话题：

1
2
3
4
ros2 topic info /Odometry
ros2 topic info /cloud_registered
ros2 topic hz /Odometry
ros2 topic hz /cloud_registered

推荐结果：

话题 类型 频率
/Odometry nav_msgs/msg/Odometry 约 30Hz 或更高
/cloud_registered sensor_msgs/msg/PointCloud2 5Hz 到 20Hz 均可先测试

第一次不要用 /Laser_map 做避障输入。/Laser_map 是累计地图，点太多，容易卡。

七、启动真机定位链路

终端 1：启动 Mid-360S。

1
2
3
source /opt/ros/humble/setup.bash
source ~/livox_ws/install/setup.bash
ros2 launch livox_ros_driver2 msg_MID360s_launch.py

终端 2：启动 FAST-LIO2。

1
2
3
4
source /opt/ros/humble/setup.bash
source ~/livox_ws/install/setup.bash
source ~/fastlio_ws/install/setup.bash
ros2 launch fast_lio mapping.launch.py config_file:=mid360.yaml rviz:=false

终端 3：启动 MAVROS。

1
2
3
4
5
source /opt/ros/humble/setup.bash
ros2 launch mavros px4.launch \
  fcu_url:="/dev/ttyACM0:921600" \
  namespace:="uav1" \
  tgt_system:=1

终端 4：把 FAST-LIO2 里程计送给 PX4。

1
2
3
4
5
6
7
8
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
ros2 launch external_odom_to_mavros bridge.launch.py \
  input_type:=odometry \
  input_topic:=/Odometry \
  output_topic:=/uav1/vision_pose/pose \
  output_frame_id:=map \
  publish_rate:=30.0

检查：

1
2
3
ros2 topic hz /uav1/vision_pose/pose
ros2 topic echo /uav1/state --once
ros2 topic echo /uav1/local_position/odom --once

重点看：

1
connected: true

如果 PX4 不能进入 Position 模式，先不要继续接 EGO-Planner。

八、启动真实数据规划

终端 5：启动 RViz。

1
2
3
4
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
ros2 launch ego_planner rviz.launch.py

终端 6：启动 EGO-Planner。

1
2
3
4
5
6
7
8
9
10
11
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
ros2 launch ego_planner fastlio_mid360.launch.py \
  odom_topic:=/Odometry \
  cloud_topic:=/cloud_registered \
  max_vel:=0.5 \
  max_acc:=0.8 \
  map_resolution:=0.15 \
  inflation:=0.30 \
  virtual_ceil:=2.0

检查输出：

1
2
ros2 topic list | grep planning
ros2 topic echo /drone_0_planning/pos_cmd --once

没发送目标点前，/drone_0_planning/pos_cmd 可能没有持续输出，这是正常的。

九、发送目标点

第一次目标点不要远：

1
2
3
source /opt/ros/humble/setup.bash
ros2 topic pub --once /move_base_simple/goal geometry_msgs/msg/PoseStamped \
  "{header: {frame_id: 'world'}, pose: {position: {x: 1.0, y: 0.0, z: 1.0}, orientation: {w: 1.0}}}"

规划成功时，EGO-Planner 终端通常会出现：

1
2
3
Triggered!
[TRIG]: from WAIT_TARGET to GEN_NEW_TRAJ
[FSM]: from GEN_NEW_TRAJ to EXEC_TRAJ

RViz 中应看到：

1
2
3
4
局部地图
目标点
初始轨迹
优化后的 B-spline 轨迹

十、检查坐标方向

先看 /Odometry：

1
ros2 topic echo /Odometry --once

手动移动飞机，推荐表现如下：

动作 ROS ENU 中推荐表现
向机头方向移动 x 增大
向机体左侧移动 y 增大
抬高无人机 z 增大

再检查 EGO 输出：

1
2
3
ros2 topic echo /drone_0_grid/grid_map/occupancy_inflate --once
ros2 topic echo /drone_0_planning/bspline --once
ros2 topic echo /drone_0_planning/pos_cmd --once

如果方向不对，先修坐标系，不要切 Offboard。

十一、启动 EGO 到 MAVROS 桥接

确认 RViz 规划正常后，再启动桥接。

第一次不要自动切 Offboard，不要自动解锁：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
ros2 run ego_to_mavros pos_cmd_bridge --ros-args \
  -p pos_cmd_topic:=/drone_0_planning/pos_cmd \
  -p odom_topic:=/Odometry \
  -p setpoint_topic:=/uav1/setpoint_raw/local \
  -p mavros_state_topic:=/uav1/state \
  -p set_mode_service:=/uav1/set_mode \
  -p arm_service:=/uav1/cmd/arming \
  -p publish_rate:=30.0 \
  -p max_vel_xy:=0.3 \
  -p max_vel_z:=0.2 \
  -p max_acc_xy:=0.6 \
  -p max_acc_z:=0.5 \
  -p min_z:=0.2 \
  -p max_z:=1.2 \
  -p max_position_step:=0.5 \
  -p auto_offboard:=false \
  -p auto_arm:=false

检查 setpoint：

1
2
ros2 topic hz /uav1/setpoint_raw/local
ros2 topic echo /uav1/setpoint_raw/local --once

建议频率：

1
20Hz 以上

如果没有 MAVROS 命名空间，把命令中的 /uav1 改成 /mavros。

十二、不装桨 Offboard 测试

保持桥接节点运行，先不装桨。

手动切 Offboard：

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

查看状态：

1
ros2 topic echo /uav1/state --once

正常应看到：

1
mode: OFFBOARD

如果切不进去，优先检查：

1
2
3
ros2 topic hz /uav1/setpoint_raw/local
ros2 topic echo /uav1/state --once
ros2 topic echo /uav1/local_position/odom --once

PX4 进入 Offboard 前必须已经持续收到 setpoint。

十三、低高度真机测试

低高度测试建议流程：

  1. 手动起飞到 0.8 ~ 1.0m。
  2. 进入 Position / Hold，确认悬停稳定。
  3. 启动 EGO-Planner 和桥接节点。
  4. 确认 /uav1/setpoint_raw/local 连续输出。
  5. 手动切 Offboard。
  6. 发送 1m 内目标点。
  7. 遥控器随时准备切回 Position / Altitude / Stabilized。

第一次不要放障碍物。先确认方向、速度、高度都正确。

十四、参数建议

EGO-Planner 常用参数：

参数 建议值 说明
max_vel 0.3 ~ 0.5 真机第一次低速
max_acc 0.6 ~ 1.0 降低轨迹激进程度
map_resolution 0.15 ~ 0.20 算力紧张就调大
inflation 0.30 ~ 0.40 轨迹太贴障碍物就调大
virtual_ceil 1.5 ~ 2.0 防止规划到过高位置

桥接节点常用参数：

参数 建议值 说明
max_vel_xy 0.3 水平限速
max_vel_z 0.2 垂直限速
max_z 1.2 真机第一次限高
max_position_step 0.5 防止 setpoint 突跳
auto_offboard false 先手动切模式
auto_arm false 不建议早期开启

十五、常见问题

1. EGO-Planner 提示没有 odom

检查：

1
2
ros2 topic hz /Odometry
ros2 topic info /Odometry

确认 EGO 启动参数里 odom_topic:=/Odometry。

2. 有 odom，但没有局部地图

检查：

1
2
ros2 topic hz /cloud_registered
ros2 topic info /cloud_registered

确认 EGO 启动参数里 cloud_topic:=/cloud_registered。

3. 发送目标点后没有规划

检查目标点：

1
ros2 topic echo /move_base_simple/goal --once

再看 EGO 终端是否出现 Triggered!。

4. 没有 /drone_0_planning/pos_cmd

检查：

1
ros2 topic list | grep pos_cmd

如果 drone_id 不是 0，话题名会变成 /drone_1_planning/pos_cmd 等。

5. /uav1/setpoint_raw/local 没有输出

桥接节点需要先收到 /Odometry。

检查：

1
2
ros2 topic hz /Odometry
ros2 topic echo /Odometry --once

6. PX4 切不进 Offboard

检查：

1
2
3
ros2 topic hz /uav1/setpoint_raw/local
ros2 topic echo /uav1/state --once
ros2 topic echo /uav1/local_position/odom --once

重点看：

1
connected: true

以及 setpoint 是否已经持续发布。

7. 飞机方向反了

立刻切回手动稳定模式。

优先检查：

1
2
ros2 topic echo /Odometry --once
ros2 topic echo /uav1/setpoint_raw/local --once

确认 FAST-LIO2、RViz、EGO-Planner、MAVROS 输入都在同一套坐标理解下。

8. 轨迹太贴障碍物

优先增大：

1
2
inflation
optimization/dist0

同时降低：

1
2
max_vel
max_acc

9. 高度异常

检查：

1
2
3
4
FAST-LIO2 /Odometry 的 z
EGO virtual_ceil
桥接节点 max_z
目标点 z

第一次建议 virtual_ceil:=2.0，桥接 max_z:=1.2。

十六、推荐最终启动顺序

1
2
3
4
5
6
7
8
9
10
1. Livox 驱动
2. FAST-LIO2
3. MAVROS
4. external_odom_to_mavros
5. EGO-Planner RViz
6. EGO-Planner
7. ego_to_mavros
8. 发送目标点
9. 不装桨 Offboard 测试
10. 低高度真机测试

参考资料