Stalled Drones Keep Assigned Grids and Cause Premature Exploration Termination

[peter-ao209]

Problem

RACER does not properly reassign exploration tasks when a drone becomes unable to continue planning.

If a drone repeatedly fails to generate a valid trajectory, its assigned grids and frontiers may remain reserved. Other drones only check their own grid lists, so they may enter IDLE even when unexplored frontiers still exist globally.

As a result, the swarm may return home before exploration is complete.

Code

The next exploration task is selected from the local grid list:

findGridAndFrontierPath(..., grid_ids, frontier_ids);

if (grid_ids.empty()) {
  ROS_WARN("No grid");
  return NO_GRID;
}

An empty local grid_ids list does not necessarily mean that global exploration is complete. Remaining frontiers may still be assigned to drones that are stuck in repeated planning failures.

The current logic also lacks a failure threshold for releasing tasks after prolonged replanning failure.

Observed Behavior

Drone 3 stopped publishing valid trajectories after:

10.643935, drone3, BSPLINE, traj_id=12

It then repeatedly failed from the same start state:

start pos: 5.71087 -3.04046 0.862643, vel: 0 0 0
open set empty, no path!
use node num: 1
[Kino replan]: Can't find path.

The same failure was repeated 7,149 times. Odometry showed that the drone remained stationary for more than 100 seconds:

position = 5.7108657, -3.0404613, 0.8626432
velocity = 0, 0, 0

Drone 2 also remained in repeated kinodynamic replanning while still holding multiple grids:

Allocated grid: 30, 18, 19, 31, 56, 45
Next view: -2.30007 -13.1146 0.918349, 2.19624
open set empty, no path!
use node num: 1
[Kino replan]: Can't find path.

Meanwhile, drones 1 and 4 completed their local grid lists, entered IDLE, and later returned home.

The run terminated with unexplored frontiers still present:

exploration_completeness: 0.6571540616246498
finish_reason: FINISH_RETURN_HOME
remaining_frontier_count: 18
no_frontier_seen: false

Expected Behavior

A drone that remains unable to plan or move should not indefinitely retain its assigned grids.

Remaining tasks should be reassigned to available drones, and exploration should only finish when no global frontier remains.

Fix

• Detect prolonged planning failure or lack of movement.
• Release the stalled drone's unfinished grids.
• Reassign them to available IDLE drones.
• Do not treat an empty local grid list as global exploration completion.