Unbounded `PLAN_TRAJ` retry loop when `next_viewpoint` becomes unreachable

[DongJiaxin0422]

Summary

When the currently selected next_viewpoint becomes unreachable, the exploration FSM may remain indefinitely in the PLAN_TRAJ state.

After trajectory planning returns FAIL, the FSM only sets static_state_ = true. It does not limit the number of retries, invalidate the current target, select another viewpoint/frontier, or transition to another recovery state. As a result, the planner repeatedly attempts to reach the same unreachable next_pos_.

This behavior can be triggered by a temporary local map disturbance, such as an obstacle appearing in the local sensing point cloud.

Relevant Code

The PLAN_TRAJ failure branch keeps the FSM in the same state:

int res = callExplorationPlanner();

if (res == SUCCEED) {
  transitState(PUB_TRAJ, "FSM");
} else if (res == FAIL) {  // Keep trying to replan
  fd_->static_state_ = true;
  ROS_WARN("Plan fail");
}

When planning fails, the code only updates static_state_. There is no:

• retry limit;
• invalidation of the current viewpoint;
• unreachable-target handling;
• frontier/viewpoint reselection;
• transition to IDLE;
• task reassignment or other fallback behavior.

The planning failure can originate from the A* search:

if (path_finder_->search(pos, next_pos, optimistic) != Astar::REACH_END) {
  ROS_ERROR("No path to next viewpoint");
  return FAIL;
}

Additionally, when replanning is caused by collision avoidance or return behavior, the planner continues using the existing target:

if (fd_->avoid_collision_ || fd_->go_back_) {
  res = expl_manager_->planTrajToView(
      ...,
      ed_->next_pos_,
      ed_->next_yaw_);
}

Therefore, the replanning process attempts to generate another trajectory toward the same next_pos_, rather than performing a complete exploration-planning cycle and selecting a different target.

Failure Sequence

1. A local map disturbance affects the currently planned path.
2. Collision checking triggers replanning, or the trajectory planner directly returns FAIL.
3. A* cannot find a path to the existing next_pos_ and prints:

No path to next viewpoint

4. planTrajToView() returns FAIL.
5. The FSM prints:

Plan fail

6. The FSM remains in PLAN_TRAJ and retries the same target during the next cycle.
7. The following messages are then repeated indefinitely:

No path to next viewpoint
Plan fail
No path to next viewpoint
Plan fail
...

Observed Behavior

In one test run, the log began repeatedly reporting:

No path to next viewpoint
Plan fail

The run eventually timed out and took approximately 1.936× the normal execution time.

These observations indicate that the planner can enter a long-running retry loop when the selected viewpoint becomes unreachable.

Impact

Once the FSM enters this condition, the affected drone may:

• stop making exploration progress;
• repeatedly consume computation in unsuccessful A* and trajectory-planning calls;
• continuously produce warning and error logs;
• fail to select another reachable frontier;
• remain stuck until the experiment or mission times out.

In a multi-drone exploration task, this may also reduce overall exploration efficiency because one drone remains assigned to an unreachable or temporarily blocked target.

Suggested Fix

Introduce a bounded recovery mechanism for repeated failures involving the same next_pos_.

For example:

1. Maintain a consecutive failure counter for the current next_pos_.
2. Reset the counter when the target changes or trajectory planning succeeds.
3. Once the failure count exceeds a threshold, mark the current viewpoint or frontier as temporarily unreachable.
4. Clear or downgrade the avoid_collision_ / go_back_ recovery state.
5. Force a complete planExploreMotion() cycle so that another frontier or viewpoint can be selected.
6. If no reachable target is available, transition to an explicit fallback state such as IDLE or trigger task reassignment.

A cooldown or expiration mechanism could also be used so that temporarily unreachable viewpoints may be reconsidered after the local map changes.