Zero-Segment Trajectory Causes Eigen Assertion Failure During Collision Replanning

[DongJiaxin0422]

Summary

When collision avoidance or trajectory replanning produces a goal that is too close to the current position, the resulting A* path may degenerate into a single waypoint.

The replanning pipeline does not validate the minimum path length before passing the path to the polynomial trajectory generator. Consequently, planExploreTraj() constructs a trajectory with zero segments, and PolynomialTraj::waypointsTraj() accesses an empty Eigen matrix, triggering an assertion failure and terminating the process.

This failure can occur during safety-triggered replanning because the system reuses the previous next_pos_ without checking whether it is sufficiently far from the current replanning start point.

Failure Flow

The safety callback detects an unsafe trajectory
        ↓
The FSM transitions to PLAN_TRAJ and sets avoid_collision_
        ↓
The planner replans toward the existing next_pos_
        ↓
The replanning start position is already at or very close to next_pos_
        ↓
A* returns a path whose start and end positions nearly coincide
        ↓
shortenPath() reduces the path to a single waypoint
        ↓
planExploreTraj() receives tour.size() == 1
        ↓
The trajectory duration vector contains zero segments
        ↓
PolynomialTraj::waypointsTraj() creates a zero-row matrix
        ↓
Ct(0, 0) triggers an Eigen assertion failure
        ↓
The planning process crashes without returning a recoverable failure

Relevant Code

1. The safety callback only triggers replanning

File:

swarm_exploration/exploration_manager/src/fast_exploration_fsm.cpp

Around line 619:

if (!safe) {
  fd_->avoid_collision_ = true;
  transitState(PLAN_TRAJ, "safetyCallback");
}

When the current trajectory becomes unsafe, the callback only switches the FSM to PLAN_TRAJ. It does not stop the UAV immediately or validate whether the existing planning target remains suitable for replanning.

2. Collision replanning reuses the previous next_pos_

File:

fast_exploration_fsm.cpp around line 268

if (fd_->avoid_collision_ || fd_->go_back_) {  // Only replan trajectory
  res = expl_manager_->planTrajToView(
      fd_->start_pt_,
      fd_->start_vel_,
      fd_->start_acc_,
      fd_->start_yaw_,
      expl_manager_->ed_->next_pos_,
      expl_manager_->ed_->next_yaw_);

  fd_->avoid_collision_ = false;
}

When avoid_collision_ is set, the planner directly replans toward the previously selected next_pos_.

There is no minimum-distance check between fd_->start_pt_ and ed_->next_pos_. If the UAV is already at or very close to that position, the generated path can contain only one effective waypoint.

3. The A* path is shortened before trajectory generation

File:

fast_exploration_manager.cpp around line 315

ed_->path_next_goal_ = planner_manager_->path_finder_->getPath();
shortenPath(ed_->path_next_goal_);

After A* returns a path, shortenPath() removes unnecessary intermediate points.

4. shortenPath() handles two-point paths but not one-point paths

File:

swarm_exploration/exploration_manager/src/fast_exploration_fsm.cpp

Around line 490:

if ((path.back() - short_tour.back()).norm() > 1e-3)
  short_tour.push_back(path.back());

// Ensure at least three points in the path
if (short_tour.size() == 2)
  short_tour.insert(
      short_tour.begin() + 1,
      0.5 * (short_tour[0] + short_tour[1]));

path = short_tour;

The function expands a two-point path into three points, but it does not handle the case where short_tour.size() == 1.

If path.front() and path.back() are nearly identical, the final point is not inserted:

(path.back() - short_tour.back()).norm() <= 1e-3

As a result, path may contain only one waypoint after shortening.

5. planExploreTraj() does not reject paths with fewer than two points

File:

planner_manager.cpp around line 210

const int pt_num = tour.size();
Eigen::VectorXd times(pt_num - 1);

PolynomialTraj::waypointsTraj(
    ...,
    times,
    ...);

When tour.size() == 1:

pt_num == 1
times.size() == 0

The function still calls the polynomial trajectory generator instead of returning a planning failure.

6. PolynomialTraj::waypointsTraj() does not support zero segments

File:

polynomial_traj.cpp around line 9

const int seg_num = times.size();

...

Ct = Eigen::MatrixXd::Zero(num_d, num_f + num_p);
Ct(0, 0) = 1;

When seg_num == 0, the matrix dimensions derived from the segment count are invalid for the subsequent indexing operations.

Ct becomes a matrix with zero rows, but the implementation immediately accesses:

Ct(0, 0)

This triggers an Eigen bounds assertion and terminates the process.

Root Cause

The replanning pipeline assumes that every valid A* result contains at least two distinct waypoints and therefore represents at least one trajectory segment.

This assumption is not enforced at any layer:

1. Collision replanning does not check whether the target is distinct from the current position.
2. shortenPath() does not guarantee a minimum path size when the path contains only one effective point.
3. planExploreTraj() does not reject tour.size() < 2.
4. PolynomialTraj::waypointsTraj() does not reject times.size() == 0.

The combination of these missing validations allows a degenerate path to reach code that requires at least one trajectory segment.

Impact

A routine safety-triggered replanning operation can terminate the entire planning process instead of returning a recoverable planning failure.

Potential consequences include:

• loss of trajectory generation;
• unexpected termination of the exploration node;
• interruption of the UAV mission;
• loss of collision-avoidance functionality;
• a UAV continuing with stale control commands, depending on the behavior of the remaining control stack;
• failure of the swarm mission when one UAV planner crashes.

Because the crash occurs in the collision-recovery path, it may happen precisely when the UAV is already in an unsafe or constrained configuration.

Expected Behavior

The planner should detect degenerate paths before invoking the polynomial trajectory generator.

When the replanning target is too close to the current position or the shortened path contains fewer than two distinct points, the planner should return a controlled failure or treat the target as already reached.

The trajectory generator should never receive a zero-segment trajectory.