Global Coordinate Frame Requirement

EGO-Planner processes point cloud data exclusively in the world coordinate frame. It reads the raw numerical values from the subscribed topic without applying any additional TF transformations, and all published planning outputs remain in the world frame. FAST-LIO2 provides this through the /cloud_registered topic, which transforms raw Livox scans into global coordinates using the accumulated SLAM pose. The full accumulated map is not published as a ROS topic due to excessive bandwidth requirements—it is instead written directly to PCD files. Therefore, the registered real-time scan remains the only viable point cloud source for the planner.

External IMU Time Synchronization

When utilizing an external IMU such as a flight controller unit via MAVROS rather than Livox's native IMU, the time_sync_en parameter must be set to true within the FAST-LIO2 configuration. Without this adjustment, FAST-LIO2 fails to initialize and the terminal continuously outputs synchronization errors without generating valid odometry data.

Startup Procedure

Execute the following launch sequence:

roslaunch livox_ros_driver livox_lidar_msg.launch
roslaunch mavros px4.launch
roslaunch fast_lio mapping_mid40_custom.launch
roslaunch ego_planner rviz.launch
roslaunch ego_planner planner_livox_integration.launch

The livox_lidar_msg.launch is specifically required because it publishes the custom Livox message format that FAST-LIO2 consumes, as opposed to the standard sensor_msgs/PointCloud2 format used by other launch profiles.

FAST-LIO2 Configuration

Adapted YAML configuration for Livox Mid-40 with MAVROS IMU:

common:
    lid_topic:  "/livox/lidar"
    imu_topic:  "/mavros/imu/data"
    time_sync_en: true

preprocess:
    lidar_type: 1
    scan_line: 6
    blind: 4

mapping:
    acc_cov: 0.1
    gyr_cov: 0.1
    b_acc_cov: 0.0001
    b_gyr_cov: 0.0001
    fov_degree: 90
    det_range: 450.0
    extrinsic_est_en: false
    extrinsic_T: [ 0.04165, 0.02326, -0.0284 ]
    extrinsic_R: [ 1, 0, 0,
                   0, 1, 0,
                   0, 0, 1 ]

publish:
    path_en: false
    scan_publish_en: true
    dense_publish_en: true
    scan_bodyframe_pub_en: true

pcd_save:
    pcd_save_en: true
    interval: -1

Planner Launch Configuration

The planner launch file remaps to FAST-LIO2 output topics:

<launch>
  <arg name="env_extent_x" value="40.0"/>
  <arg name="env_extent_y" value="40.0"/>
  <arg name="env_extent_z" value="3.0"/>

  <arg name="localization_stream" value="/Odometry" />

  <include file="$(find ego_planner)/launch/advanced_param_droneyee.xml">
    <arg name="map_size_x_" value="$(arg env_extent_x)"/>
    <arg name="map_size_y_" value="$(arg env_extent_y)"/>
    <arg name="map_size_z_" value="$(arg env_extent_z)"/>
    <arg name="odometry_topic" value="$(arg localization_stream)"/>

    <arg name="camera_pose_topic" value="/pcl_render_node/camera_pose"/>
    <arg name="depth_topic" value="/camera/depth/image_rect_raw"/>

    <arg name="cloud_topic" value="/cloud_registered"/>

    <arg name="cx" value="321.04638671875"/>
    <arg name="cy" value="243.44969177246094"/>
    <arg name="fx" value="387.229248046875"/>
    <arg name="fy" value="387.229248046875"/>

    <arg name="max_vel" value="0.3"/>
    <arg name="max_acc" value="0.5"/>
    <arg name="planning_horizon" value="7.5"/>

    <arg name="flight_type" value="1"/>

    <arg name="point_num" value="4"/>
    <arg name="point0_x" value="-10.0"/>
    <arg name="point0_y" value="5.0"/>
    <arg name="point0_z" value="1.0"/>
    <arg name="point1_x" value="10.0"/>
    <arg name="point1_y" value="5.0"/>
    <arg name="point1_z" value="1.0"/>
    <arg name="point2_x" value="10.0"/>
    <arg name="point2_y" value="-5.0"/>
    <arg name="point2_z" value="1.0"/>
    <arg name="point3_x" value="-10.0"/>
    <arg name="point3_y" value="-5.0"/>
    <arg name="point3_z" value="1.0"/>
  </include>

  <node pkg="ego_planner" name="traj_server" type="traj_server" output="screen">
    <remap from="/position_cmd" to="planning/pos_cmd"/>
    <remap from="/odom_world" to="$(arg localization_stream)"/>
    <param name="traj_server/time_forward" value="1.0" type="double"/>
  </node>

  <node pkg="waypoint_generator" name="waypoint_generator" type="waypoint_generator" output="screen">
    <remap from="~odom" to="$(arg localization_stream)"/>
    <remap from="~goal" to="/move_base_simple/goal"/>
    <remap from="~traj_start_trigger" to="/traj_start_trigger"/>
    <param name="waypoint_type" value="manual-lonely-waypoint"/>
  </node>
</launch>

The referenced XML configuration file contains the core planner parameters:

<launch>
  <arg name="map_size_x_"/>
  <arg name="map_size_y_"/>
  <arg name="map_size_z_"/>
  <arg name="odometry_topic"/>
  <arg name="camera_pose_topic"/>
  <arg name="depth_topic"/>
  <arg name="cloud_topic"/>
  <arg name="cx"/>
  <arg name="cy"/>
  <arg name="fx"/>
  <arg name="fy"/>
  <arg name="max_vel"/>
  <arg name="max_acc"/>
  <arg name="planning_horizon"/>
  <arg name="point_num"/>
  <arg name="point0_x"/>
  <arg name="point0_y"/>
  <arg name="point0_z"/>
  <arg name="point1_x"/>
  <arg name="point1_y"/>
  <arg name="point1_z"/>
  <arg name="point2_x"/>
  <arg name="point2_y"/>
  <arg name="point2_z"/>
  <arg name="point3_x"/>
  <arg name="point3_y"/>
  <arg name="point3_z"/>
  <arg name="flight_type"/>

  <node pkg="ego_planner" name="ego_planner_node" type="ego_planner_node" output="screen">
    <remap from="/odom_world" to="$(arg odometry_topic)"/>
    <remap from="/grid_map/odom" to="$(arg odometry_topic)"/>
    <remap from="/grid_map/cloud" to="$(arg cloud_topic)"/>
    <remap from="/grid_map/pose" to="$(arg camera_pose_topic)"/>
    <remap from="/grid_map/depth" to="$(arg depth_topic)"/>

    <param name="fsm/flight_type" value="$(arg flight_type)" type="int"/>
    <param name="fsm/thresh_replan" value="1.5" type="double"/>
    <param name="fsm/thresh_no_replan" value="2.0" type="double"/>
    <param name="fsm/planning_horizon" value="$(arg planning_horizon)" type="double"/>
    <param name="fsm/planning_horizen_time" value="3" type="double"/>
    <param name="fsm/emergency_time_" value="1.0" type="double"/>

    <param name="fsm/waypoint_num" value="$(arg point_num)" type="int"/>
    <param name="fsm/waypoint0_x" value="$(arg point0_x)" type="double"/>
    <param name="fsm/waypoint0_y" value="$(arg point0_y)" type="double"/>
    <param name="fsm/waypoint0_z" value="$(arg point0_z)" type="double"/>
    <param name="fsm/waypoint1_x" value="$(arg point1_x)" type="double"/>
    <param name="fsm/waypoint1_y" value="$(arg point1_y)" type="double"/>
    <param name="fsm/waypoint1_z" value="$(arg point1_z)" type="double"/>
    <param name="fsm/waypoint2_x" value="$(arg point2_x)" type="double"/>
    <param name="fsm/waypoint2_y" value="$(arg point2_y)" type="double"/>
    <param name="fsm/waypoint2_z" value="$(arg point2_z)" type="double"/>
    <param name="fsm/waypoint3_x" value="$(arg point3_x)" type="double"/>
    <param name="fsm/waypoint3_y" value="$(arg point3_y)" type="double"/>
    <param name="fsm/waypoint3_z" value="$(arg point3_z)" type="double"/>

    <param name="grid_map/resolution" value="0.1"/>
    <param name="grid_map/map_size_x" value="$(arg map_size_x_)"/>
    <param name="grid_map/map_size_y" value="$(arg map_size_y_)"/>
    <param name="grid_map/map_size_z" value="$(arg map_size_z_)"/>
    <param name="grid_map/local_update_range_x" value="5.5"/>
    <param name="grid_map/local_update_range_y" value="5.5"/>
    <param name="grid_map/local_update_range_z" value="4.5"/>
    <param name="grid_map/obstacles_inflation" value="0.099"/>
    <param name="grid_map/local_map_margin" value="30"/>
    <param name="grid_map/ground_height" value="-0.01"/>
    <param name="grid_map/cx" value="$(arg cx)"/>
    <param name="grid_map/cy" value="$(arg cy)"/>
    <param name="grid_map/fx" value="$(arg fx)"/>
    <param name="grid_map/fy" value="$(arg fy)"/>
    <param name="grid_map/use_depth_filter" value="true"/>
    <param name="grid_map/depth_filter_tolerance" value="0.15"/>
    <param name="grid_map/depth_filter_maxdist" value="5.0"/>
    <param name="grid_map/depth_filter_mindist" value="0.2"/>
    <param name="grid_map/depth_filter_margin" value="1"/>
    <param name="grid_map/k_depth_scaling_factor" value="1000.0"/>
    <param name="grid_map/skip_pixel" value="2"/>
    <param name="grid_map/p_hit" value="0.65"/>
    <param name="grid_map/p_miss" value="0.35"/>
    <param name="grid_map/p_min" value="0.12"/>
    <param name="grid_map/p_max" value="0.90"/>
    <param name="grid_map/p_occ" value="0.80"/>
    <param name="grid_map/min_ray_length" value="0.1"/>
    <param name="grid_map/max_ray_length" value="4.5"/>
    <param name="grid_map/virtual_ceil_height" value="2.5"/>
    <param name="grid_map/visualization_truncate_height" value="2.4"/>
    <param name="grid_map/show_occ_time" value="false"/>
    <param name="grid_map/pose_type" value="2"/>
    <param name="grid_map/frame_id" value="world"/>

    <param name="manager/max_vel" value="$(arg max_vel)" type="double"/>
    <param name="manager/max_acc" value="$(arg max_acc)" type="double"/>
    <param name="manager/max_jerk" value="4" type="double"/>
    <param name="manager/control_points_distance" value="0.4" type="double"/>
    <param name="manager/feasibility_tolerance" value="0.05" type="double"/>
    <param name="manager/planning_horizon" value="$(arg planning_horizon)" type="double"/>

    <param name="optimization/lambda_smooth" value="1.0" type="double"/>
    <param name="optimization/lambda_collision" value="0.5" type="double"/>
    <param name="optimization/lambda_feasibility" value="0.1" type="double"/>
    <param name="optimization/lambda_fitness" value="1.0" type="double"/>
    <param name="optimization/dist0" value="0.5" type="double"/>
    <param name="optimization/max_vel" value="$(arg max_vel)" type="double"/>
    <param name="optimization/max_acc" value="$(arg max_acc)" type="double"/>

    <param name="bspline/limit_vel" value="$(arg max_vel)" type="double"/>
    <param name="bspline/limit_acc" value="$(arg max_acc)" type="double"/>
    <param name="bspline/limit_ratio" value="1.1" type="double"/>
  </node>
</launch>

Online Extrinsic Estimation Behavior

Enabling extrinsic_est_en (online IMU-LiDAR extrinsic estimation) produced noticeably worse pose accuracy. Upon activation, the initial Z-axis estimate immediately deviated negative by several centimeters, and after a 90-degree yaw rotation followed by a return, positional drift reached several meters. With the parameter disabled, drift remained minimal. The recommendation is to keep extrinsic_est_en set to false unless precise initial extrinsic calibration data is available.

Path Planning Failure Conditions

Trajectory generation fails when the estimated Z position drops below zero. This occurs because the A* search cannot construct a valid path when the origin height is negative relative to the planning grid. Ensuring the Z estimate remains positive is essential for consistent planner operation.

Close-Range Divergence

FAST-LIO2 odometry can diverge when the sensor faces a wall at very close range, likely attributable to uncalibrated LiDAR-IMU extrinsic parameters. However, running FAST-LIO2 alone without the planner stack reduces this divergence tendency, suggesting the combined computational load on the NX platform may also contribute to instability during rapid motion.

Performance Metrics

FAST-LIO2 odometry publishes at 10 Hz. On the Jetson NX platform, the combined FAST-LIO2 and EGO-Planner CPU utilization remains manageable even with concurrent NoMachine remote desktop sessions. Rapid physical movements can induce system freezes on the NX, indicating computational limits under aggressive dynamics.

Data Recording

Record datasets using:

rosbag record -O livox_mid40_mavros_imu /livox/lidar /mavros/imu/data
rosbag record -O livox_mid40_fastlio_ego_full -a