Pair-Opt Continues Running After Global Exploration Completion or During Return-to-Home

[DongJiaxin0422]

Summary

The pair-opt timer remains active after global exploration has completed or the swarm has entered the return-to-home stage.

As a result, the system continues attempting task reallocations even when there are no remaining frontiers or all UAVs have already entered an idle or finished state. Failed reallocations repeatedly produce warning and error messages and introduce unnecessary computational overhead.

This issue is unrelated to Ghost injection and can occur during normal execution.

Relevant Code

The issue occurs in:

void FastExplorationFSM::optTimerCallback(const ros::TimerEvent& e) {
  if (state_ == INIT) return;

The callback only excludes the INIT state. It does not stop pair-opt when:

• No global frontiers remain.
• The exploration task has completed.
• The UAVs are returning home through fd_->go_back_.
• All UAVs have entered an idle or finished state.

Therefore, the callback can continue selecting UAV pairs and attempting task reallocations:

ROS_WARN("Pair opt %d & %d", getId(), select_id);

When the reallocated assignment has a higher cost, the following branch is executed:

if (cur_app1 + cur_app2 > prev_app1 + prev_app2 + 0.1) {
  ROS_ERROR("Larger cost after reallocation");
  if (state_ != WAIT_TRIGGER) {
    return;
  }
}

Problem

Pair-opt is intended to optimize task assignments during active exploration. However, optTimerCallback() does not verify whether exploration is still active before performing the optimization.

This allows pair-opt to continue running after the task has effectively ended. The timer repeatedly selects UAV pairs, computes new task assignments, and reports failed or higher-cost reallocations despite there being no meaningful exploration task left to optimize.

Failure Flow

Global frontiers are exhausted or exploration is completed
        ↓
The swarm enters return-to-home, idle, or finished states
        ↓
optTimerCallback() continues running because state_ != INIT
        ↓
Pair-opt continues selecting UAV pairs
        ↓
Task reallocation is attempted without active exploration tasks
        ↓
The new assignment has a higher cost
        ↓
"Larger cost after reallocation" is repeatedly reported
        ↓
Unnecessary computation and continuous log output

Actual Behavior

After global exploration has completed or return-to-home has started:

• Pair-opt continues attempting task reallocations.
• Pair opt ... warnings continue to be printed.
• Failed reallocations repeatedly produce Larger cost after reallocation.
• CPU resources are consumed by unnecessary optimization attempts.

Expected Behavior

Pair-opt should stop once task reallocation is no longer meaningful, including when:

• No global frontiers remain.
• The exploration task has completed.
• fd_->go_back_ is set.
• All UAVs are idle or finished.

Suggested Fix

Add termination guards at the beginning of optTimerCallback() so that the callback returns when exploration has completed, return-to-home is active, no frontiers remain, or all UAVs have finished their tasks.

The higher-cost reallocation branch should also terminate the current optimization attempt consistently instead of allowing pair-opt processing to continue in WAIT_TRIGGER.