Failed Delay Check Can Cause an Infinite Replanning Loop

[DongJiaxin0422]

Summary

After Gurobi generates an optimal candidate trajectory, the delay check may reject it because the trajectory collides with an obstacle.

When this happens, the new trajectory is not accepted, and the system continues publishing pwp_last_. The UAV therefore remains at its previous position. In the next planning cycle, the planner may generate an approximately identical candidate trajectory, which is rejected again by the same obstacle.

Without an alternative recovery strategy or retry limit, the system can remain indefinitely in this replanning loop.

Relevant Code

The issue occurs in:

[rmader_ros.cpp (line 675)](/home/dongjiaxin/workspace/RMADER_ws/src/rmader/rmader/src/rmader_ros.cpp:675)

The delay check rejects the newly optimized trajectory when a collision is detected. However, the rejected trajectory is not replaced with an alternative trajectory, and the system continues using the previous trajectory pwp_last_.

Problem Description

The planner treats a failed delay check as a rejection of the current candidate trajectory, but it does not modify the planning conditions that produced the rejected trajectory.

As a result:

• The new trajectory is discarded.
• pwp_last_ continues to be published.
• The UAV remains at its previous position.
• The next planning cycle starts from nearly the same state.
• Gurobi may generate nearly the same candidate trajectory.
• The delay check rejects it again because of the same obstacle.

This creates a livelock in which the planner continues running but makes no progress.

Failure Flow

Gurobi finds an optimal candidate trajectory
        ↓
The delay check detects a collision with an obstacle
        ↓
The candidate trajectory is rejected
        ↓
The new trajectory is not committed
        ↓
The system continues publishing pwp_last_
        ↓
The UAV remains at its previous position
        ↓
The next planning cycle starts from nearly the same state
        ↓
Gurobi generates an approximately identical trajectory
        ↓
The delay check rejects it because of the same obstacle
        ↓
The process repeats indefinitely

Actual Behavior

When the delay check repeatedly rejects the optimized trajectory:

• The UAV remains stationary.
• The previous trajectory pwp_last_ continues to be published.
• The planner repeatedly generates similar candidate trajectories.
• The same obstacle repeatedly causes the delay check to fail.
• The system remains active but cannot make progress.

Expected Behavior

After a candidate trajectory fails the delay check, the planner should enter a recovery path that changes the conditions of the next planning attempt.

For example, the system should avoid repeatedly generating and checking the same invalid trajectory and should eventually either:

• generate an alternative feasible trajectory,
• modify the replanning constraints or initial conditions,
• enter a defined waiting or recovery state, or
• terminate the repeated attempts after a bounded number of failures.

Suggested Fix

Handle repeated delay-check failures explicitly.

Possible fixes include:

• Recording consecutive delay-check failures.
• Preventing the next planning cycle from reproducing the same rejected trajectory.
• Triggering an alternative replanning or recovery strategy after repeated failures.
• Adding a retry limit or timeout.
• Avoiding indefinite publication of pwp_last_ when it prevents the UAV from making progress.