Missing Emergency Braking After a Dangerous Trajectory Is Rejected

Summary

When delayCheck() detects that a newly generated trajectory would cause a collision, RMADER correctly rejects the unsafe trajectory. However, it does not generate an emergency braking trajectory or immediately stop the UAV.

If the UAV is still moving at high speed, the failure-handling branch performs no action. Meanwhile, the trajectory publishing timer continues retrieving commands from the previous plan, causing the UAV to keep moving along an outdated trajectory and potentially collide with an obstacle or another UAV.

Relevant Code

delayCheck() correctly reports that the candidate trajectory is unsafe:

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

However, when delay_check_result_ == false, RMADER republishes the last trajectory only if the UAV has already been nearly stationary for more than one second:

if (timer_stop_.ElapsedMs() > 1000.0 &&
    state_.vel.norm() < 0.1) {
    publishOwnTraj(pwp_last_, ...);
}

See:

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

If the UAV is still moving faster than 0.1, this branch performs no recovery or braking action.

At the same time, the independent trajectory publishing timer continues retrieving the next target from the old plan:

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

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

As a result, rejecting the new trajectory does not prevent the previous plan_ from continuing to control the UAV.

Failure Flow

A newly generated trajectory is predicted to collide
        ↓
delayCheck() rejects the candidate trajectory
        ↓
No emergency braking trajectory is generated
        ↓
The previous plan_ remains active
        ↓
The publishing timer continues sending targets from the old plan
        ↓
The UAV continues moving at high speed
        ↓
The UAV collides with an obstacle or another UAV

Expected Behavior

Once delayCheck() determines that the candidate trajectory is unsafe, RMADER should immediately prevent the UAV from continuing along an unsafe or outdated plan.

Possible recovery actions include:

• Generating and publishing a dynamically feasible braking trajectory.
• Invalidating or pausing the current plan_.
• Switching to an emergency-stop state.
• Holding the current position after the UAV has safely decelerated.
• Triggering replanning only after the UAV has entered a safe state.

Actual Behavior

The unsafe candidate trajectory is rejected, but no braking or emergency-stop command is issued while the UAV is moving.

The previous plan remains active and continues to provide control targets, allowing the UAV to continue moving toward a potential collision.

Impact

This issue can cause a real UAV collision even though RMADER has already detected that the newly generated trajectory is unsafe.

The problem is particularly dangerous when:

• The UAV is moving at high speed.
• The previous plan is no longer safe under the current environment state.
• The remaining time to collision is shorter than the next successful replanning cycle.
• Repeated trajectory generation attempts continue to fail the delay check.

Suggested Fix

When delay_check_result_ == false, RMADER should immediately enter a dedicated emergency-braking or safe-stop procedure instead of waiting for:

state_.vel.norm() < 0.1

Start in rmader.cpp at delayCheck() around line 1064, then trace the failure branch in rmader_ros.cpp around line 687. Inspect the publishing timer around line 939 and the target retrieval near rmader.cpp line 2343 to understand how the old plan continues running. Done means an unsafe candidate cannot leave the previous plan controlling a moving UAV, with the chosen braking or safe-stop behavior covered by the relevant validation.