No stationary trajectory is broadcast in IDLE state, making an idle drone invisible to swarm trajectory coordination

[DongJiaxin0422]

Description

When a drone enters the IDLE state, swarmTrajTimerCallback() stops publishing its trajectory to the other drones.

The callback currently publishes:

• fd_->newest_traj_ while the FSM is in EXEC_TRAJ;
• a virtual stationary trajectory at the current position while the FSM is in WAIT_TRIGGER.

However, there is no corresponding branch for the IDLE state.

As a result, although the drone may remain physically stationary in the shared environment, it no longer continuously broadcasts a trajectory representing its occupied position.

Relevant Code

File:

swarm_exploration/exploration_manager/src/fast_exploration_fsm.cpp

Around line 1010:

void FastExplorationFSM::swarmTrajTimerCallback(const ros::TimerEvent& e) {
  // Broadcast newest traj of this drone to others
  if (state_ == EXEC_TRAJ) {
    swarm_traj_pub_.publish(fd_->newest_traj_);

  } else if (state_ == WAIT_TRIGGER) {
    // Publish a virtual traj at current pose, to avoid collision
    ...
    swarm_traj_pub_.publish(bspline);
  }
}

Current behavior:

EXEC_TRAJ
    → Broadcast fd_->newest_traj_

WAIT_TRIGGER
    → Broadcast a virtual stationary trajectory at the current pose

IDLE
    → No trajectory is broadcast

Problem

A drone in IDLE may still occupy a position inside the exploration environment. Other drones therefore still need to consider it during trajectory planning and collision checking.

Because no trajectory is published in this state, the other drones may only have stale trajectory information for the idle drone, or may eventually treat the drone as having no valid trajectory, depending on how received trajectories are expired or cached.

In either case, the idle drone's current occupied position is not continuously represented through the swarm trajectory communication mechanism.

This is inconsistent with the handling of WAIT_TRIGGER, where a virtual stationary trajectory is explicitly published “to avoid collision.”

Failure Flow

A drone enters IDLE
        ↓
The drone remains physically stationary in the environment
        ↓
swarmTrajTimerCallback() matches neither EXEC_TRAJ nor WAIT_TRIGGER
        ↓
No stationary trajectory is published
        ↓
Other drones do not receive refreshed occupancy information for this drone
        ↓
They may use stale trajectory data or stop considering the drone's current position
        ↓
Potential unsafe trajectory planning near the idle drone

Expected Behavior

While a drone remains physically present in the shared environment, it should continue broadcasting a valid trajectory representation of its occupied position.

For the IDLE state, the FSM should publish a virtual stationary trajectory at the drone's current pose, similar to the existing behavior in WAIT_TRIGGER.

Suggested Fix

Handle IDLE together with WAIT_TRIGGER when constructing and publishing the virtual stationary trajectory:

if (state_ == EXEC_TRAJ) {
  swarm_traj_pub_.publish(fd_->newest_traj_);

} else if (state_ == WAIT_TRIGGER || state_ == IDLE) {
  // Publish a virtual stationary trajectory at the current pose
  ...
  swarm_traj_pub_.publish(bspline);
}

Alternatively, the virtual stationary trajectory generation could be extracted into a shared helper function and used for every non-executing state in which the drone remains physically present.

Impact

This issue can cause inconsistent swarm state information. Other drones may plan trajectories without an up-to-date representation of the idle drone's occupied position, increasing the risk of unsafe proximity or collision.

The exact behavior depends on how the receiving drones cache and expire previously received trajectories.