Make NavigateThroughPoses navigator report waypoint statuses information.

behavior_trees/trees/nav_through_poses_recovery.rst

@@ -17,6 +17,7 @@ In nominal execution, this will replan the path at every 3 seconds and pass that
 The planner though is now ``ComputePathThroughPoses`` taking a vector, ``goals``, rather than a single pose ``goal`` to plan to.
 The ``RemovePassedGoals`` node is used to cull out ``goals`` that the robot has passed on its path.
 In this case, it is set to remove a pose from the poses when the robot is within ``0.5`` of the goal and it is the next goal in the list.
+Additionally, it records the status of each waypoint (e.g. ``PENDING``, ``COMPLETED``, ``SKIPPED`` or ``FAILED``) in the ``waypoint_statuses``.
 This is implemented such that replanning can be computed after the robot has passed by some of the intermediary poses and not continue to try to replan through them in the future.
 This time, if the planner fails, it will trigger contextually aware recoveries in its subtree, clearing the global costmap.
 Additional recoveries can be added here for additional context-specific recoveries, such as trying another algorithm.
@@ -46,7 +47,7 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con
           <RateController hz="0.333">
             <RecoveryNode number_of_retries="1" name="ComputePathThroughPoses">
               <ReactiveSequence>
-                <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
+                <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/>
                 <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
               </ReactiveSequence>
               <Sequence>

configuration/packages/bt-plugins/actions/RemoveInCollisionGoals.rst

@@ -64,6 +64,17 @@ Input Ports
   Description
     Whether to consider unknown cost (255) as obstacle.
 
+:input_waypoint_statuses:
+
+  =========================================== =======
+  Type                                        Default
+  ------------------------------------------- -------
+  std::vector<nav2_msgs::msg::WaypointStatus>   N/A
+  =========================================== =======
+
+  Description
+    Original waypoint_statuses to mark waypoint status from.
+
 Output Ports
 ------------
 
@@ -78,9 +89,20 @@ Output Ports
   Description
     A vector of goals containing only those that are not in collision.
 
+:output_waypoint_statuses:
+
+  =========================================== =======
+  Type                                        Default
+  ------------------------------------------- -------
+  std::vector<nav2_msgs::msg::WaypointStatus>   N/A
+  =========================================== =======
+
+  Description
+    Waypoint_statuses with in-collision waypoints marked.
+
 Example
 -------
 
 .. code-block:: xml
 
-  <RemoveInCollisionGoals input_goals="{goals}" output_goals="{goals}" cost_threshold="254.0" use_footprint="true" service_name="/global_costmap/get_cost_global_costmap" />
+  <RemoveInCollisionGoals input_goals="{goals}" output_goals="{goals}" cost_threshold="254.0" use_footprint="true" service_name="/global_costmap/get_cost_global_costmap" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}" />

configuration/packages/bt-plugins/actions/RemovePassedGoals.rst

@@ -42,6 +42,17 @@ Input Ports
   Description
     A vector of goals to check if it passed any in the current iteration.
 
+:input_waypoint_statuses:
+
+  =========================================== =======
+  Type                                        Default
+  ------------------------------------------- -------
+  std::vector<nav2_msgs::msg::WaypointStatus>   N/A
+  =========================================== =======
+
+  Description
+    Original waypoint_statuses to mark waypoint status from.
+
 Output Ports
 ------------
 
@@ -56,9 +67,20 @@ Output Ports
   Description
     A vector of goals with goals removed in proximity to the robot
 
+:output_waypoint_statuses:
+
+  =========================================== =======
+  Type                                        Default
+  ------------------------------------------- -------
+  std::vector<nav2_msgs::msg::WaypointStatus>   N/A
+  =========================================== =======
+
+  Description
+    Waypoint_statuses with passed waypoints marked.
+
 Example
 -------
 
 .. code-block:: xml
 
-  <RemovePassedGoals radius="0.6" input_goals="{goals}" output_goals="{goals}"/>
+  <RemovePassedGoals radius="0.6" input_goals="{goals}" output_goals="{goals}" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/>

configuration/packages/configuring-bt-navigator.rst

@@ -227,6 +227,17 @@ Parameters
   Description
     Blackboard variable to use to supply the goals to the behavior tree for ``NavigateThroughPoses``. Should match ports of BT XML file.
 
+:waypoint_statuses_blackboard_id:
+
+  ====== ===================
+  Type   Default
+  ------ -------------------
+  string "waypoint_statuses"
+  ====== ===================
+
+  Description
+    Blackboard variable to get the statuses of waypoints from the behavior tree for ``NavigateThroughPoses`` feedback/result. Should match ports of BT XML file.
+
 :use_sim_time:
 
   ==== =======
@@ -300,6 +311,7 @@ Example
         goal_blackboard_id: goal
         goals_blackboard_id: goals
         path_blackboard_id: path
+        waypoint_statuses_blackboard_id: waypoint_statuses
         navigators: ['navigate_to_pose', 'navigate_through_poses']
         navigate_to_pose:
           plugin: "nav2_bt_navigator::NavigateToPoseNavigator" # In Iron and older versions, "/" was used instead of "::"

migration/Jazzy.rst

@@ -385,3 +385,33 @@ MPPI - Publishing Optimal Trajectory
 ************************************
 
 When ``publish_optimal_trajectory`` is enabled, the full optimal trajectory in the form of a ``nav2_msgs/Trajectory`` is published for debugging, visualization, and/or injection by other systems. This provides not just the pose information but velocities and timestamps of the MPC trajectory's viapoints which can be useful for multi-stage control systems, jerk minimization, or collision avoidance systems.
+
+NavigateThroughPoses - Reporting waypoint statuses information
+**************************************************************
+
+`PR #4994 <https://github.com/ros-navigation/navigation2/pull/4994>`_ enhances the ``NavigateThroughPoses`` navigator to provide real-time status feedback for waypoints.
+Introduces the ``WaypointStatus`` message type, which indicates a waypoint's status: pending, complete, skipped, or failed.
+This also replaces the deprecated ``MissedWaypoint`` type used in ``WaypointFollower``.
+This also updates the behavior tree nodes ``RemovePassedGoals`` and ``RemoveInCollisionGoals`` to mark waypoint statuses as the action progresses.
+``NavigateThroughPoses`` now retrieves waypoint statuses via the blackboard (similar to path and goal(s)) and organizes them into the action server’s feedback and result messages, exposing the waypoint execution status to users.
+
+The parameter ``waypoint_statuses_blackboard_id`` was introduced to the ``bt_navigator`` node to allow users to customize the variable name for ``waypoint_statuses`` in the blackboard.
+In the Behavior Tree XML, the ``RemovePassedGoals`` and ``RemoveInCollisionGoals`` nodes must expose corresponding ports to align with the ``waypoint_statuses`` workflow to manage the waypoints' state when removing them due to completion or skipped due to collision.
+This ensures ``NavigateThroughPoses`` can retrieve and propagate waypoint statuses via the blackboard.
+The action also populates the completed waypoints at the end when populating the action result.
+
+The following is an example of the ``RemovePassedGoals`` and ``RemoveInCollisionGoals`` nodes configuration:
+
+.. code-block:: xml
+
+    <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/>
+    <RemoveInCollisionGoals input_goals="{goals}" output_goals="{goals}" cost_threshold="254.0" use_footprint="true" service_name="/global_costmap/get_cost_global_costmap" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}" />
+
+The ``waypoint_statuses`` array matches the length of the original input ``goals`` vector, with each element being a ``WaypointStatus`` message.
+The ``RemovePassedGoals`` and ``RemoveInCollisionGoals`` nodes prune invalid or completed goals from the goals vector and update their corresponding entries in the ``waypoint_statuses`` array.
+
+Custom nodes can access or modify the ``waypoint_statuses`` array as well if other skip, removal, completion, or failures are used when working with multiple ordered goals. Use the utility method ``find_next_matching_goal_in_waypoint_statuses`` (from nav2_utils) to map goals to their status entries.
+Modified statuses should then be propagated through output ports for downstream nodes.
+
+The ``NavigateThroughPoses`` navigator retrieves the ``waypoint_statuses`` instance from the blackboard in its ``onLoop`` callback and writes it into the feedback message.
+During the ``goalCompleted`` callback, it fetches the ``waypoint_statuses`` instance and, based on the BT's final execution status (``final_bt_status``), updates any waypoints still in the ``PENDING`` state to either ``COMPLETED`` (if ``final_bt_status`` is ``SUCCEEDED``) or ``FAILED`` (otherwise).