From ec4994217b76b52edff5172f6aad0977fedfb536 Mon Sep 17 00:00:00 2001 From: zz990099 <771647586@qq.com> Date: Sat, 5 Apr 2025 23:20:09 +0800 Subject: [PATCH 1/9] Update RemoveInCollisionGoals & RemovePassedGoals ports Signed-off-by: zz990099 <771647586@qq.com> --- .../actions/RemoveInCollisionGoals.rst | 24 ++++++++++++++++++- .../bt-plugins/actions/RemovePassedGoals.rst | 24 ++++++++++++++++++- 2 files changed, 46 insertions(+), 2 deletions(-) diff --git a/configuration/packages/bt-plugins/actions/RemoveInCollisionGoals.rst b/configuration/packages/bt-plugins/actions/RemoveInCollisionGoals.rst index d16b96e413..d618a91a08 100644 --- a/configuration/packages/bt-plugins/actions/RemoveInCollisionGoals.rst +++ b/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 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 N/A + =========================================== ======= + + Description + Waypoint_statuses with in-collision waypoints marked. + Example ------- .. code-block:: xml - + diff --git a/configuration/packages/bt-plugins/actions/RemovePassedGoals.rst b/configuration/packages/bt-plugins/actions/RemovePassedGoals.rst index 5f9c96448d..992fa8f605 100644 --- a/configuration/packages/bt-plugins/actions/RemovePassedGoals.rst +++ b/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 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 N/A + =========================================== ======= + + Description + Waypoint_statuses with passed waypoints marked. + Example ------- .. code-block:: xml - + From 2c7384f34868f87d7e8749691e78de13d54b8a46 Mon Sep 17 00:00:00 2001 From: zz990099 <771647586@qq.com> Date: Sat, 5 Apr 2025 23:20:25 +0800 Subject: [PATCH 2/9] Update NavigateThroughPoses parameters Signed-off-by: zz990099 <771647586@qq.com> --- behavior_trees/trees/nav_through_poses_recovery.rst | 3 ++- configuration/packages/configuring-bt-navigator.rst | 12 ++++++++++++ 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/behavior_trees/trees/nav_through_poses_recovery.rst b/behavior_trees/trees/nav_through_poses_recovery.rst index eaf95ea1ca..4e14d546a3 100644 --- a/behavior_trees/trees/nav_through_poses_recovery.rst +++ b/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 - + diff --git a/configuration/packages/configuring-bt-navigator.rst b/configuration/packages/configuring-bt-navigator.rst index 798500ea1b..d502243563 100644 --- a/configuration/packages/configuring-bt-navigator.rst +++ b/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 "::" From 1e7af9f62cbb05fc97df2f11beeeea7cf9bc8706 Mon Sep 17 00:00:00 2001 From: zz990099 <771647586@qq.com> Date: Wed, 9 Apr 2025 11:18:33 +0800 Subject: [PATCH 3/9] Update migration docs - Jazzy.rst Signed-off-by: zz990099 <771647586@qq.com> --- migration/Jazzy.rst | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/migration/Jazzy.rst b/migration/Jazzy.rst index c4aa617ed3..9a67e1a494 100644 --- a/migration/Jazzy.rst +++ b/migration/Jazzy.rst @@ -385,3 +385,31 @@ 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 `_ enhances the ``NavigateThroughPoses`` navigator to provide real-time status feedback for waypoints. +Introduces the ``WaypointStatus`` message type, replacing the deprecated ``MissedWaypoint`` type used in ``WaypointFollower``. +Updates the behavior tree nodes ``RemovePassedGoals`` and ``RemoveInCollisionGoals`` to mark waypoint statuses. +``NavigateThroughPoses`` now retrieves waypoint statuses via the blackboard and organizes them into the action server’s feedback and result messages, exposing them 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. +This ensures ``NavigateThroughPoses`` can retrieve and propagate waypoint statuses via the blackboard. + +The following is an example of the ``RemovePassedGoals`` and ``RemoveInCollisionGoals`` nodes configuration: + +.. code-block:: xml + + + + +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 the ``waypoint_statuses`` array via input ports. 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). From fd1a481768c9655ba33e7f51d8a8e89d2b7b7bd8 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 9 Apr 2025 10:43:48 -0700 Subject: [PATCH 4/9] Update migration/Jazzy.rst Signed-off-by: Steve Macenski --- migration/Jazzy.rst | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/migration/Jazzy.rst b/migration/Jazzy.rst index 9a67e1a494..eb5ff8496f 100644 --- a/migration/Jazzy.rst +++ b/migration/Jazzy.rst @@ -390,7 +390,8 @@ NavigateThroughPoses - Reporting waypoint statuses information ************************************************************** `PR #4994 `_ enhances the ``NavigateThroughPoses`` navigator to provide real-time status feedback for waypoints. -Introduces the ``WaypointStatus`` message type, replacing the deprecated ``MissedWaypoint`` type used in ``WaypointFollower``. +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``. Updates the behavior tree nodes ``RemovePassedGoals`` and ``RemoveInCollisionGoals`` to mark waypoint statuses. ``NavigateThroughPoses`` now retrieves waypoint statuses via the blackboard and organizes them into the action server’s feedback and result messages, exposing them to users. From c11e3e35526d3deaf47a1b776a5db6c4b3f0e1cf Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 9 Apr 2025 10:43:54 -0700 Subject: [PATCH 5/9] Update migration/Jazzy.rst Signed-off-by: Steve Macenski --- migration/Jazzy.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/migration/Jazzy.rst b/migration/Jazzy.rst index eb5ff8496f..646976f5d4 100644 --- a/migration/Jazzy.rst +++ b/migration/Jazzy.rst @@ -392,7 +392,7 @@ NavigateThroughPoses - Reporting waypoint statuses information `PR #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``. -Updates the behavior tree nodes ``RemovePassedGoals`` and ``RemoveInCollisionGoals`` to mark waypoint statuses. +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 and organizes them into the action server’s feedback and result messages, exposing them 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. From fa4d55bd280b0c25e345e4e948ac20a6873da884 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 9 Apr 2025 10:44:04 -0700 Subject: [PATCH 6/9] Update migration/Jazzy.rst Signed-off-by: Steve Macenski --- migration/Jazzy.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/migration/Jazzy.rst b/migration/Jazzy.rst index 646976f5d4..d0bc4055b7 100644 --- a/migration/Jazzy.rst +++ b/migration/Jazzy.rst @@ -393,7 +393,7 @@ NavigateThroughPoses - Reporting waypoint statuses information 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 and organizes them into the action server’s feedback and result messages, exposing them to users. +``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. From dc1d30085536a06a95ce87eb197cf19e801fc8b4 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 9 Apr 2025 10:44:11 -0700 Subject: [PATCH 7/9] Update migration/Jazzy.rst Signed-off-by: Steve Macenski --- migration/Jazzy.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/migration/Jazzy.rst b/migration/Jazzy.rst index d0bc4055b7..3ffae2a66e 100644 --- a/migration/Jazzy.rst +++ b/migration/Jazzy.rst @@ -396,7 +396,7 @@ This also updates the behavior tree nodes ``RemovePassedGoals`` and ``RemoveInCo ``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. +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 following is an example of the ``RemovePassedGoals`` and ``RemoveInCollisionGoals`` nodes configuration: From 397280f596a41be5ec015d214f5fccb9c7d6aeb7 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 9 Apr 2025 10:44:17 -0700 Subject: [PATCH 8/9] Update migration/Jazzy.rst Signed-off-by: Steve Macenski --- migration/Jazzy.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/migration/Jazzy.rst b/migration/Jazzy.rst index 3ffae2a66e..17fa31f099 100644 --- a/migration/Jazzy.rst +++ b/migration/Jazzy.rst @@ -398,6 +398,7 @@ This also updates the behavior tree nodes ``RemovePassedGoals`` and ``RemoveInCo 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: From f4e0333a38cade8bd551d4953e051a69117ddb5f Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 9 Apr 2025 10:44:23 -0700 Subject: [PATCH 9/9] Update migration/Jazzy.rst Signed-off-by: Steve Macenski --- migration/Jazzy.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/migration/Jazzy.rst b/migration/Jazzy.rst index 17fa31f099..e37c854ce6 100644 --- a/migration/Jazzy.rst +++ b/migration/Jazzy.rst @@ -410,7 +410,7 @@ The following is an example of the ``RemovePassedGoals`` and ``RemoveInCollision 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 the ``waypoint_statuses`` array via input ports. Use the utility method ``find_next_matching_goal_in_waypoint_statuses`` (from nav2_utils) to map goals to their status entries. +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.