"Resulting plan has 0 poses" when the same path is resent

[BCKSELFDRIVEWORLD]

Required Info:

• Operating System: Ubuntu 22.04
• ROS2 Version: Humble
• DDS implementation: Cyclone DDS
• Controller: MPPI

Problem

A robot follows a path normally (everything is fine :=) ). Then, for any reason, FollowPath is aborted (Failed to make progress). At that moment the robot is on a path pose that is farther than max_robot_pose_search_dist from the start of the path

Now the same path is sent to FollowPath again, and we get the error. Here is why:

1. setPath is called → global_plan is reset to the full plan → the front goes back to pose 0. The already-traveled part comes back.
2. The robot is far along the path (e.g. pose 480 of 483).
3. The closest-point search only looks max_robot_pose_search_dist (5 m ≈ the first ~100 poses) ahead of the front. The robot's real closest pose (pose 480) is far outside this window.
4. The search returns the closest pose inside the window (e.g. pose 99, which is 14.75 m away from the robot). That pose is outside the local costmap -> the transformed plan is empty -> throw "Resulting plan has 0 poses in it." -> abort again.
5. The BT sends the same path again → reset again → empty again → loop._

So : the robot is exactly on the path and the poses it should follow are right in front of it. Only the search cannot find them, because after the front is reset the bounded window cannot reach the robot's real position.

I added a log that compares the bounded (capped) search with an unbounded (full-path) search:

Resulting plan has 0 poses. robot=(2.19, 9.23) in 'map'; global_plan=483 poses,
pruned=384 poses; nearest pose found=(-4.67, -3.83), dist robot->found=14.75 m;
costmap 'odom' origin=(1.00, -1.00) size=10.0x10.0 m [x:1.00..10.97 y:-1.00..8.98].

• bounded = result of the capped search (only looks max_robot_pose_search_dist ahead of the front).
• unbounded = result of searching the whole path (no cap).
• d(unbounded)=0.03 m, idx(unbounded)=480 → the robot is on the path, at pose 480; a full search finds it instantly.
• d(bounded)=14.75 m, idx(bounded)=99 → the capped search is stuck at pose 99, 14.75 m away, because pose 480 is far beyond the window (window_end=100).
• That far pose is outside the local costmap → transformed plan is empty → throw.

Workaround and why it is not enough

Computing a new global plan from the robot's current position fixes it. But:

• The current path is already valid and the robot is right on it
• Re-planning is expensive in large environments and is a waste of time.

So: the path could simply be resumed, but instead the system is forced into a costly replan.

Relevant code

1) setPath: when a new OR the same plan is set, the pruned plan is reset -> front goes back to pose 0

  void PathHandler::setPath(const nav_msgs::msg::Path & plan) {
    global_plan_ = plan;
    global_plan_up_to_inversion_ = global_plan_;   // front = pose 0 (pruning is lost)
  }

2) The closest-point search only looks max_robot_pose_search_dist ahead of the front

  auto closest_pose_upper_bound = nav2_util::geometry_utils::first_after_integrated_distance(
      global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist_);
  auto closest_point = nav2_util::geometry_utils::min_by(
      global_plan_.poses.begin(), closest_pose_upper_bound,
      [&](const auto & ps){ return euclidean_distance(global_pose, ps); });

3) If the window does not cover the robot's real position, the transformed plan is empty -> throw

  if (transformed_plan.poses.empty()) {
    throw nav2_core::InvalidPath("Resulting plan has 0 poses in it.");   
  }

• Happy to open a PR if there is interest.

[SteveMacenski]

Computing a new global plan from the robot's current position fixes it. But:

This was going to be my suggestion

The current path is already valid and the robot is right on it

As would a new plan 😉

Re-planning is expensive in large environments and is a waste of time.

The point you make though is that you're pretty far along the path (near the end, in fact) so planning should be cheap

---

But otherwise, do you have a suggested change for how you think the behavior should function? Resending an old path is not a common operation. One of the main reasons control can fail is because the plan is stale, environment has changed, or is in a different state far away that you couldn't know approaching it without impossibly long-range sensors.

Perhaps one of the following, both with strong merits:

• a fallback case in the pruning that if it is out of bounds by a clearly substantial amount, allow a full search? The problem is that's going to be an unexpected behavior and could cause issues with users with not-so-well tuned systems and a rather subtle thing
• your BT itself when it cancels the control / fails, has a BT node that prunes the path so that you only send the current robot pose's path point to the end. I think we may even have a path pruning BT node that could be used for this

[BCKSELFDRIVEWORLD]

Thanks for the detailed reply let me push back a bit, because I think this belongs in the controller,

The point you make though is that you're pretty far along the path (near the end, in fact) so planning should be cheap

As you mentioned, for the specific use case I described, the computational cost is not a major concern. We could perform this at any point after max_robot_pose_search_dist has been passed :)

a BT node that prunes the path so that you only send the current robot pose's path point to the end

Yes, truncating the path in TruncatePathLocal or generating a new path would definitely solve this issue. However, that approach doesn't feel quite right to me. It feels like I'm not addressing the root cause, but instead asking another component to compensate for it. That's why I opened this issue to hear different opinions and get your feedback.

In my opinion, it should be the controller's responsibility to determine where the robot is on the path and continue from there. The controller already has both the robot's current pose and the complete path. max_robot_pose_search_dist is simply an optimization, and I also believe that keeping this limit is the right choice from a safety perspective. At the same time, there is no documented requirement stating that the incoming path must start at the robot's footprint. Because of that, this feels more like a controller design aspect that could be improved, rather than something that should be solved by adding another BT step.

To be honest, solving these kinds of problems in the BT increases the overall complexity of the system. Every edge case eventually turns into another BT node, and over time the BT becomes something that nobody wants to touch or debug anymore :=)

For now, the only solution that comes to mind is an edge-case-specific approach like the one below.

  if (transformed_plan.poses.empty()) {
    const double saved = max_robot_pose_search_dist_;
    max_robot_pose_search_dist_ = std::numeric_limits<double>::max();
    auto [retry_plan, retry_lb] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose);
    max_robot_pose_search_dist_ = saved;

    prunePlan(global_plan_up_to_inversion_, retry_lb); 
    transformed_plan = retry_plan;                        

    if (transformed_plan.poses.empty()) {
      throw std::runtime_error("Resulting plan has 0 poses in it.");
    }
  }

NOTE: If this kind of edge-case-specific solution doesn't feel right to you, even if it's configurable through a parameter, please feel free to close the issue. I'm looking at this primarily from the perspective of my own use case, so I may not be evaluating it objectively.

[SteveMacenski]

How do you plan to address the edge case that this happens while a robot is located near a path intersection (i.e, the path has a loop in it)? The max search distance was introduced to iteratively find the robot's closest point on the path in a realistic update window to resolve that issue of the "nearest" path point actually being on the wrong portion of a path in a U-turn coverage style pattern or a loop-de-loop.

If we're going to break this out, then we need a way to resolve that. Which, the BT solution would do natively by saving the last state 😉

[BCKSELFDRIVEWORLD]

How do you plan to address the edge case that this happens while a robot is located near a path intersection (i.e, the path has a loop in it)?

I have an idea I've wanted to bring up in a couple of these issues, but held back because it felt a bit over-engineered

In U-turns or, as you mentioned, at intersecting parts of the path we could treat the path points near the robot as an array of candidates. We could filter/clamp that array by heading (a global parameter could be introduced for this), or, without clamping at all, score each candidate the way MPPI does its cost terms by heading, distance, and index. That turns it into a small weighted selection over the candidates score each one and pick the lowest, to recover the correct starting point.

I haven't tested this yet, but I believe I can implement it. If the idea makes sense to you in theory and you think it's worth testing, I can give it a try. It probably won't be this week, but I could test it sometime next week and open a draft PR.

[SteveMacenski]

That would not be a suitable solution as the orientation or other functions like that don't tell you what ordered point you should use (and not always are people using MPPI). I don't think this one is worth testing unfortunately :( There is no substitute for maintaining state, whether in the BT or in the path handler itself

[SteveMacenski]

I don't necessarily think this should be closed, as I think this is a good thing to be documented a solution for in the BT or adjustments to the path handler to maintain that state in some such situations as a fallback.

But if this discussion resolved your concern, we can leave this be for now. Someone in the future may choose to reopen it to address if you are not requiring it

[SteveMacenski]

So of this, do you think we can cleanly retain the state in the path handler and use it as a fallback in some way? Seems very possible rather than using the full path search fallback. We'd just want to check that the global path is identical to the previous global path so we know we just had the same follow path goal reissued (to ensure its not another path that's very far off from the state pose & we use a bogus state from a previous path)

[BCKSELFDRIVEWORLD]

Agreed, i think that's the clean way to do it, and it avoids the full-path search fallback entirely. I'll open a draft PR along this week.

[SteveMacenski]

Any update?

[SteveMacenski]

@BCKSELFDRIVEWORLD any update? I was hoping to have this in this week for Lyrical since it will probably involve ABI breaks