Execution-Time Obstacle Collision Check Only Triggers Replanning Without Immediate Stop, Leading to Real Obstacle Collision

[DongJiaxin0422]

Summary

During trajectory execution, the safety callback checks whether the currently executing trajectory will collide with an inflated obstacle in the near future. When a collision risk is detected, the FSM only sets avoid_collision_ = true and transitions from EXEC_TRAJ to PLAN_TRAJ for replanning.

However, no immediate stop or emergency braking action is triggered at this point. If the drone is already close to the obstacle, replanning may not complete and publish a new safe trajectory in time. As a result, the drone may continue executing the unsafe trajectory long enough to collide with the obstacle.

Relevant Code

In the execution state, the safety callback handles obstacle collision risk as follows:

if (state_ == EXPL_STATE::EXEC_TRAJ) {
  bool safe = planner_manager_->checkTrajCollision(dist);
  if (!safe) {
    ROS_WARN("Replan: collision detected");
    fd_->avoid_collision_ = true;
    transitState(PLAN_TRAJ, "safetyCallback");
  }
}

The collision check evaluates future points on the currently executing trajectory:

fut_pt = local_data_.position_traj_.evaluateDeBoorT(t_now + fut_t);
if (sdf_map_->getInflateOccupancy(fut_pt) == 1) {
  return false;
}

This means the system has already detected that the future trajectory may enter an inflated obstacle region.

Problem

When checkTrajCollision() returns false, the FSM treats the situation as a replanning request rather than an immediate safety-critical stop condition.

The current behavior is:

future trajectory collision detected
        ↓
set avoid_collision_ = true
        ↓
transition to PLAN_TRAJ
        ↓
attempt replanning

This assumes that replanning will finish and publish a new safe trajectory before the drone reaches the obstacle. However, this assumption may not hold when the drone is already close to the predicted collision point.

Expected Behavior

When a future obstacle collision is detected during EXEC_TRAJ, the system should prevent the drone from continuing along the unsafe trajectory.

A safe behavior would be:

future trajectory collision detected
        ↓
stop immediately or enter an emergency braking behavior
        ↓
then attempt to replan from a safe state

Actual Behavior

The current behavior is:

future trajectory collision detected
        ↓
trigger replanning
        ↓
the drone continues executing the previous trajectory before a new one is available
        ↓
real obstacle collision may occur

Impact

This can lead to real obstacle collisions during trajectory execution, especially when the detected collision point is close and replanning cannot complete in time.

The issue is caused by treating an execution-time obstacle collision warning as a normal replanning trigger, without enforcing an immediate stop or braking behavior before replanning.