Make NavigateThroughPoses navigator report waypoint statuses information. (fix #4846)

[zz990099]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#4846
Primary OS tested on	Ubuntu
Robotic platform tested on	gazebo simulation
Does this PR contain AI generated software?	No

---

Description of contribution in a few bullet points

• This PR addresses More info about the state of poses in NavigateThroughPoses #4846.
• Make NavigateThroughPoses navigator report waypoint statuses information.
• More information could be exposed by extending the WaypointState message type.

Description of documentation updates required from your changes

• A parameter called waypoint_statuses_blackboard_id in NavigateThroughPoses should be added in documentation.
• I will submit a PR to docs.nav2.org if this PR could been merged.

Description of how this change was tested

• I test these codes in gazebo simulation with some modifications in nav2_rviz_plugins(which is not included in this PR).
• Test results validate proper code execution.

---

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[codecov]

Codecov Report

Attention: Patch coverage is 94.44444% with 3 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_waypoint_follower/src/waypoint_follower.cpp	57.14%	3 Missing ⚠️

Files with missing lines	Coverage Δ
...lugins/action/remove_in_collision_goals_action.hpp	100.00% <100.00%> (ø)
...tree/plugins/action/remove_passed_goals_action.hpp	100.00% <100.00%> (ø)
...lugins/action/remove_in_collision_goals_action.cpp	90.47% <100.00%> (+3.37%)	⬆️
...tree/plugins/action/remove_passed_goals_action.cpp	100.00% <100.00%> (ø)
...bt_navigator/navigators/navigate_through_poses.hpp	100.00% <ø> (ø)
...avigator/src/navigators/navigate_through_poses.cpp	87.50% <100.00%> (+2.13%)	⬆️
nav2_util/include/nav2_util/geometry_utils.hpp	95.45% <100.00%> (+0.71%)	⬆️
...clude/nav2_waypoint_follower/waypoint_follower.hpp	100.00% <ø> (ø)
nav2_waypoint_follower/src/waypoint_follower.cpp	82.23% <57.14%> (-0.27%)	⬇️

... and 2 files with indirect coverage changes

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[SteveMacenski]

I'm not sure this work for the RemoveInCollision considering it overrides the goals variable with a pruned list rather than maintaining it. When it removes from goals the side of waypoint_statuses and goals no longer match. If this is called repeatedly and different ones are removed, the i that you're setting to SKIPPED would no longer map to the actual waypoint.

I think what we might want to do is actually to update the way that the goal and goals are stored in the blackboard from geometry_msgs/PoseStamped and nav_msgs/Goals (respectively) to being a struct containing those values, the current goal, and the waypoint statuses. Then the BT nodes would modify metadata on these to skip or mark as completed.

This is a bit more complicated than we initially discussed, however! That would also mess with the logic attached to Goal(s)Updated and would need to be based on the goal/goals part of the messages so that the metadata updates don't trigger it.

Edit: Or, perhaps an easier way that might be possible is to find the matching goal's PoseStamped rather than index so even though the indexing is changing the PoseStamped is not. That seems actually alot less effort for the same effect. You'd just want to find the next goal that matches the PoseStamped that is not already skipped/completed. That way looping waypoint sets that have the same location multiple times work properly. You can find the iterator of the first non-completed and non-skipped waypoint, then use https://en.cppreference.com/w/cpp/algorithm/find_first_of to find the next that matches a pose stamped after those skipped waypoints to match which ID it should be long to!

---

Some food for thought: We also have this https://github.com/ros-navigation/navigation2/blob/f2755f66ba550332aa603485ccba267c8a33236a/nav2_msgs/msg/MissedWaypoint.msg message as well. I'm curious if we can combine this message and the one you made or make a new one that combines the use cases for the waypoint follower and this navigate through poses application!

[zz990099]

Thank you for your patient guidance and thorough review! My apologies—I overlooked the possibility of RemoveInCollision being called multiple times in the initial implementation. After carefully reviewing your feedback and revisiting the code, I’d like to discuss a few considerations:

1. ​Impact of goal/goals Type Refactoring:
   Taking NavigateThroughPoses as an example, changing the type of goals in the blackboard would indeed affect the port implementations of multiple BTAction nodes (ComputePathThroughPoses/GoalUpdated...) and potentially the logic of NavigateToPose.

2. PoseStamped Matching Approach:
   Your proposed solution—matching indices based on the PoseStamped value of the current goal with find_first_of method—is practical! I will prioritize implementing this and test it thoroughly to ensure indexing stability across repeated RemoveInCollision calls.

3. ​Combining MissedWaypoint and WaypointStatus:
   Perhaps we could integrate the MissedWaypoint concept into the WaypointStatus message by adding fields like:
   waypoint_status (e.g., MISSED, SKIPPED, ACTIVE, COMPLETED)
goal_index
error_code
error_msg
goal
   This would allow WaypointFollower to directly use WaypointStatus messages to mark the failed points.

Thank you again for your thoughtful review and suggestions! Please let me know if these directions align with your expectations.

[SteveMacenski]

No worries, that's what code reviews are for! I didn't realize that was going to be the case either before looking at the code itself 😉

1. Agreed, I think maybe we should not do this, even though it might be the 'cleanest' solution, due to the massive breaking change it would cause people. Also (2) is not actually that bad of a solution overall. It involves some similar elements of looking over the vector to find the goals, but this one would also impact how the ComputePathThroughPoses looks as the goals parameter which would be problematic for other users with highly customized use-cases of it.

2. Sounds good! I think having a couple of utility functions to do the lookups would be nice so that they can be reused in both BT nodes and other user custom nodes (and easily unit testable).

3. Agreed!

[zz990099]

Hello @SteveMacenski !,

I’ve implemented most of the changes as discussed, but I’m encountering an edge case issue with how RemovePassedGoals handles empty goals. Currently, RemovePassedGoals skips returning empty goals to avoid triggering exceptions in ComputePathThroughPoses(as per link). However, this prevents the last waypoint's status in waypoint_statuses from being updated properly.

Here are potential fixes I’ve considered:

1. Allow empty goals in ComputePathThroughPoses: Return success instead of throwing an error. While simple, this might introduce unintended side effects for other modules and test cases.

2. Add edge-case handling within RemovePassedGoals: This could clutter the existing logic and introduce maintenance overhead.

3. ​Update goalCompleted in ComputeThroughPoses: Handle the last waypoint’s status here. Though straightforward, this may decentralize the waypoint_statuses management logic.

I’d appreciate your guidance on which approach aligns best with the project’s design goals. Happy to refine any of these paths ​or any suggestions you might have! :)

[SteveMacenski]

Add edge-case handling within RemovePassedGoals: This could clutter the existing logic and introduce maintenance overhead.

What do you have in mind?

​Update goalCompleted in ComputeThroughPoses: Handle the last waypoint’s status here. Though straightforward, this may decentralize the waypoint_statuses management logic.

I think this is a pretty good option. In the migration / docs updates we'll do for this PR, lets make sure to highlight where this is managed (Navigator to initialize; goal complete for goal. removed / passed goals for state updates on completion or failure) & that users can add more BT nodes that modify it for their applications at their set blackboard ID. Its very understandable that users might have custom BT nodes that may choose to skip or retry waypoints, so its perfectly fine for others to have this updated for their custom nodes and applications.

This is all probably good to explicitly document regardless. Maybe even thow a note about the unique nature of goal / goals / path IDs as well

[zz990099]

Add edge-case handling within RemovePassedGoals: This could clutter the existing logic and introduce maintenance overhead.

To prevent empty goals output while ensuring all waypoint statuses are properly recorded, ​perhaps we could modify the code in RemovePassedGoals as follows:

  // get the `waypoint_statuses` vector
  std::string waypoint_statuses_id;
  getInput("waypoint_statuses_id", waypoint_statuses_id);
  std::vector<nav2_msgs::msg::WaypointStatus> waypoint_statuses;
  waypoint_statuses = config().blackboard->get<decltype(waypoint_statuses)>(waypoint_statuses_id);

  double dist_to_goal;
  while (goal_poses.goals.size() > 0) {
    dist_to_goal = euclidean_distance(goal_poses.goals[0].pose, current_pose.pose);

    if (dist_to_goal > viapoint_achieved_radius_) {
      break;
    }

    // mark waypoint statuses before the goal is erased from goals
    auto cur_waypoint_index =
      find_goal_in_waypoint_statuses(waypoint_statuses, goal_poses.goals[0]);
    if (cur_waypoint_index != -1) {
      waypoint_statuses[cur_waypoint_index].waypoint_status =
        nav2_msgs::msg::WaypointStatus::COMPLETED;
    }
    
    // prevent from removing the last goal
    if (goal_poses.goals.size() == 1) break;

    goal_poses.goals.erase(goal_poses.goals.begin());
  }

  setOutput("output_goals", goal_poses);
  // set `waypoint_statuses` output
  config().blackboard->set<decltype(waypoint_statuses)>(waypoint_statuses_id, waypoint_statuses);

This should ensure goals are never empty and consistently track all waypoint statuses.
However, if we need to throw an exception when find_goal_in_waypoint_statuses fails to find a suitable result, additional logic may need to be added to this part.

Update goalCompleted in ComputeThroughPoses: Handle the last waypoint’s status here. Though straightforward, this may decentralize the waypoint_statuses management logic.

While modifying the code, I noticed that this approach ensures the waypoint_statuses in the result meet expectations, ​but it does not update the waypoint_statuses in the feedback, leading to inconsistencies between the two. This mismatch might introduce potential hidden issues in user's application.

[zz990099]

Hi! @SteveMacenski

I've completed the code revisions. Could you please review the changes when you have a moment?

Regarding the WaypointFollower component (currently uncommitted), I'm considering :

1. Keep returning missed_waypoints in the result via FollowWaypoint.action, or
2. Align with NavigateThroughPoses by returning a full status collection of all waypoints?

[SteveMacenski]

Nice work! A few things to resolve and I think this is close to merge!

[SteveMacenski]

However, if we need to throw an exception when find_goal_in_waypoint_statuses fails to find a suitable result, additional logic may need to be added to this part.

If we fail, we should use another one of the callbacks (like the on completion callback) that knows that it failed to populate the remaining waypoints as SKIPPED / MISSED, I think. We catch exceptions and return a BT Status of Failure that you can check and if so, any pending goals should be set to skipped / missed.

This is a subtle point and may be worth some automated and/or manual test coverage to make sure the feedback/returns are properly populated in success, nominal failure conditions (not related to this), and failure conditions related to this.

While modifying the code, I noticed that this approach ensures the waypoint_statuses in the result meet expectations, ​but it does not update the waypoint_statuses in the feedback, leading to inconsistencies between the two. This mismatch might introduce potential hidden issues in user's application.

Can you expand on these?

Keep returning missed_waypoints in the result via FollowWaypoint.action, or Align with NavigateThroughPoses by returning a full status collection of all waypoints?

I think keeping it the same as today would be good. The WaypointStatus is a stand-alone waypoint status that we can use with the NavThroughPoses to describe all, or only populate the few failures that exist. In NavigateThroughPoses, we include all, but I think for WPF's missed_waypoints field, it makes sense to only include the failures

[tonynajjar]

Nice to see this PR and thank you for the good work @zz990099 . I implemented something similar (but not as generic) in my fork, I'll try to find the time (maybe weekend) to go over the differences and give my 2 cents

[zz990099]

Thank you so much for your kind words, @tonynajjar ! 🙌 I truly appreciate you taking the time to review this PR and share your perspective. Looking forward to your thoughts whenever it’s convenient for you.

[tonynajjar]

​Impact of goal/goals Type Refactoring:
Taking NavigateThroughPoses as an example, changing the type of goals in the blackboard would indeed affect the port implementations of multiple BTAction nodes (ComputePathThroughPoses/GoalUpdated...) and potentially the logic of NavigateToPose.

This is actually what I went for in my fork; I created a new Waypoint msg that contains a PoseStamped and a status. But yes like mentioned, that's a breaking change and needed to change multiple things. I think on the long-run this would probably be the "cleaner" way but I'm not against, even probably prefer, having a proof of concept that is not breaking, like implemented in this PR.

uint8 PENDING = 0
uint8 VISITED = 1
uint8 SKIPPED = 2
uint8 FAILED = 3

These are the statuses I came up with. We match on PENDING. I have COMPLETED instead of VISITED; I don't have a strong preference or really any argument for or against either.
I have SKIPPED and FAILED which are combined in this PR into MISSED: I think it would be useful to make a differentiation between waypoints we missed "on purpose" (SKIPPED) and those we missed because of some kind of error. E.g. I have RemoveGoalsInCollision mark waypoints as SKIPPED whereas the navigator could mark a waypoint as FAILED if the BT fails or ComputePathThroughPoses could mark it as FAILED if it fails to generate a plan

Otherwise design looks good!

[SteveMacenski]

Otherwise, LGTM!

[mergify]

This pull request is in conflict. Could you fix it @zz990099?

[zz990099]

Hi!

I’ve submitted the PR for the documentation updates. ros-navigation/docs.nav2.org#669

I think our migration guide also needs to mention that NavigateThroughPoses navigator now has this waypoint statuses struct in the feedback/result. Also mention how it is managed and what its role is (notifying users about the status of each goal set in the request)

I’m not entirely clear on where to make these changes. Could you kindly point me to the specific sections of the documentation that need adjustments? Thank you for your guidance!

[SteveMacenski]

I followed up on that point in the docs PR!

[SteveMacenski]

Just a few small changes for the docs PR and happy to merge the pair!

[SteveMacenski]

Thanks for this! This is a really wonderful new feature! Let me know if you're interested in contributing to another feature!

[tonynajjar]

Hey guys, I started porting my custom version of this feature to this version and I think I'm only missing one last piece of the puzzle: the ability to pause and resume without losing the waypoint statuses and therefore continuing from the next PENDING pose

Here is roughly how I have it implemented:

• The navigator client (some kind of mission manager) is responsible for keeping track of the status of all the waypoints by subscribing to the feedback of the navigator.
• My NavigateThroughPoses goal sends WaypointStatus[] instead of nav_msgs/Goals so that we can pass the statuses
• The navigator's internal waypoint_statuses is initialized with the ones provided from the client and only the goals that are PENDING are set on the blackboard

Do you see this as a generally useful feature? Am I missing an already existing way to pause and resume without losing the state?

[SteveMacenski]

I definitely see the value. There's been a related discussion in #5070 (comment) that you might be interested in (and possibly helping bring to fruition?).

How is the pause / resume implemented - as an outside action (i.e. something cancels, stores info, and resubmits) or internal (i.e. BT has a pause variable that it waits on before continuing)? It sounds like external. What was the rationale against having it be internal in the behavior tree so that all the information stays hot?

Some other thoughts for a hybrid approach:

• We could have a "ResumeNavigateThroughPoses" and "ResumeNavigateToPose" (with complimentary "Pause" ones) so that we have different semantically meaningful APIs for Start/Cancel/Pause/Resume that each have their contextual information. No need to change the main Action APIs, just have new ones that have that information. I think sharper messages are good to have rather than trying to cram everything into one.
• The Pause can return the state and the Resume can send it back.
• Or, the Pause can store that in the navigator itself and the Resume can load it back up in a new tree, so that the application doesn't have to care (but can still return it anyway in case they care or want to persist between reboots. If Resume is given values, it uses those instead of its internally stored one).
• Or, the Pause can simply stop ticking the tree to cold store it and the Resume can continue the ticking loop (TBD: some nodes may care about timing or have progress checking, that would need to be considered. Maybe we just halt the tree but keep its blackboard active to resume in a fresh new-tick state? That's essentially what your current implementation and the other ideas above do anyway by resume being a re-goal-submission)

What to open a new ticket to discuss?

[tonynajjar]

It sounds like external. What was the rationale against having it be internal in the behavior tree so that all the information stays hot?

Yes it's external - just historical reasons, I never really thought about the tradeoffs

What to open a new ticket to discuss?

I created the issue: #5213

(and possibly helping bring to fruition?).

I'm not sure I can get the time for it right now since we have a working version.