Planning application development

EGO-Planner is a local trajectory planner for quadrotors. In unknown or partially known environments, it uses a local map built from onboard perception to generate smooth, flyable, collision-free trajectories in real time and adapts to environmental changes through receding-horizon replanning.

Before modifying the code, if you cannot confirm that the changes will not have a significant impact on the navigation module, complete testing in a simulation environment before conducting tests on the actual aircraft. In this project, we have completed:

• ROS 2 porting
• AirSim simulation integration (LiDAR/depth point clouds)
• Octomap / depth point cloud mapping
• Prometheus flight controller interface

1: What problems does EGO-Planner solve?

EGO-Planner's approach

• It does not depend on a complete global map and instead uses a local perception map + a receding local target
• Generate quickly first, then refine: polynomial initial trajectory → A* obstacle avoidance → gradient optimization
• Replan at a high frequency and continuously update the trajectory during flight

2: Module breakdown

This module is located in the Modules/ego_planner_swarm directory of the Prometheus project. swarm in the name indicates that the framework supports multi-UAV planning tasks. Run the following command to enter the directory:

cd /home/amov/Prometheus/Modules/ego_planner_swarm

2.1 Module breakdown

The code structure is as follows:

2.2 Runtime logic

EGO uses a finite-state machine to drive the entire planning process. The corresponding code is in theplanner_manager folder in ego_replan_fsm.cpp:

A 10 Hz timer executes the execFSMCallback function. The main execution logic is shown in the following figure.

The following table describes each part in detail.

Each time execFSMCallback() runs:

1. It publishes a heartbeat (used by traj_server to check whether the planner is alive)
2. It executes the corresponding logic for the current FSM state
3. It publishes debugging data

Replanning decisions in EXEC_TRAJ:

• Trajectory execution time > fsm.thresh_replan_time (default: 1s) → enter REPLAN_TRAJ
• Distance to the end of the current trajectory < fsm.emergency_time and the final goal has not been reached → replan in advance
• The final goal is reached (the trajectory ends or the odom distance < goal_reach_dist) → WAIT_TARGET

2.3 Main planning flow

The main planning function calls are shown below. After the /uav1/prometheus/motion_planning/goalmotion/plan topic is received, the process runs in the following order.

• waypointCallback: Topic callback function that records information
• planNextWaypoint: Uses the current odometry as the start point and the specified target point as the end point, generates a global polynomial trajectory using MINCO, allocates the duration of each segment, and writes it to traj_.global_traj for subsequent local planning and tracking. This stage does not query the map or perform obstacle avoidance.

• planFromGlobalTraj (used for the initial planning): Uses odom_pos_ / odom_vel_ as the start state and repeatedly calls callReboundReplan to generate the first flyable local trajectory.

• callReboundReplan: Entry point for local planning that links the following two steps:

  • getLocalTarget: Truncates a local target point local_target_pt_ from traj_.global_traj according to planning_horizon_ (it still tracks the global reference path and does not itself perform obstacle avoidance).
  • reboundReplan: The function that actually performs obstacle avoidance. It first initializes a trajectory using MINCO, and then PolyTrajOptimizer performs rebound optimization using grid_map_. The result is written to traj_.local_traj.
  • Publish trajectory: polyTraj2ROSMsg → publish planning/trajectory.
  • FSM → EXEC_TRAJ: Execute the local trajectory; enter REPLAN_TRAJ / GEN_NEW_TRAJ as required during execution.

Summary: After receiving a target point, the system first uses MINCO to generate a global reference path without obstacle avoidance (planNextWaypoint), then the FSM triggers local rebound optimization to generate a flyable trajectory (callReboundReplan), and finally publishes the trajectory and enters EXEC_TRAJ for execution. Replanning is performed as required during execution.

2.4 Core planning method

The core function, reboundReplan, implements the core obstacle-avoidance logic. After receiving a straight-line MINCO trajectory, it performs collision detection, optimization, and other operations. It incorporates many techniques, including initialization from the previous time step, polynomial initialization, random initialization, and decomposition of the path into multiple polynomial segments.

reboundReplan
│
├─ STEP 1: INIT (initial trajectory generation)
│     computeInitState()           → MINCO initial trajectory
│     finelyCheckAndSetConstraintPoints() → checks for obstacles; if a collision is found, performs A* segmentation
│
├─ STEP 2: OPTIMIZE (gradient optimization)
│     optimizeTrajectory()       → smoothing + obstacle avoidance + dynamic constraints
│
└─ STEP 3: STORE (storage)
setLocalTrajFromOpt()        → writes to traj_.local_traj for use

• STEP1: Initial trajectory generation

  • Determines the segment length according to polyTraj_piece_length / max_vel and generates the MINCO initial trajectory.
  • finelyCheckAndSetConstraintPoints samples along the initial trajectory and uses grid_map_->getInflateOccupancy() to check for obstacles.
  • No collision: Directly use the control points of the initial trajectory for optimization.
  • Collision: PolyTrajOptimizer internally invokes A* (path_searching) to segment the path around obstacles and resets the control points.

• STEP2: Gradient optimization

  • optimizeTrajectory(headState, tailState, innerPts, durations, cost) iterates over the control points.
  • The cost generally includes smoothness (jerk), obstacle distance, and dynamic feasibility (velocity/acceleration limits).
  • Success → best_MJO; failure → visualize the failed list and return false.

3 Data flow and ROS interfaces

3.1 Input topics

3.2 Output topics

4 Common planning application development scenarios

Recommendations

1. Tune parameters before modifying the algorithm: Most cases of poor flight performance are caused by the horizon, velocity, inflation, or replanning period.
2. Identify the failure stage: For INIT failures, check the initial trajectory/A*; for OPT failures, check the weights and constraints.
3. Preserve warm starts: During execution, do not change replanning back to a straight-line initial trajectory by default.
4. Random initialization is a fallback: Use it to retry after failures, not as the default strategy.
5. Add new constraints to the optimizer: Do not pile them onto the reboundReplan entry point.
6. Change only one layer at a time: Test parameters / optimizer / initial trajectory / FSM separately.
7. Run simulation stress tests before testing on the actual aircraft: This is especially important when changing A*, random initialization, or EMERGENCY-related logic.
8. Local path planning has poor global optimality: It is not suitable for narrow scenarios with only one corridor; it often fails to find a path and resorts to haphazard A* searching.
9. Terminal output is very important when a problem occurs: Record it, and then investigate it yourself, use AI, or ask our technical staff.