diff --git a/behavior_trees/images/walkthrough/contextual_recoveries.png b/behavior_trees/images/walkthrough/contextual_recoveries.png index e0b2424c3b..e57e654fb0 100644 Binary files a/behavior_trees/images/walkthrough/contextual_recoveries.png and b/behavior_trees/images/walkthrough/contextual_recoveries.png differ diff --git a/behavior_trees/images/walkthrough/navigation_subtree.png b/behavior_trees/images/walkthrough/navigation_subtree.png index b5a28f6159..70453b76c3 100644 Binary files a/behavior_trees/images/walkthrough/navigation_subtree.png and b/behavior_trees/images/walkthrough/navigation_subtree.png differ diff --git a/behavior_trees/images/walkthrough/overall_bt.png b/behavior_trees/images/walkthrough/overall_bt.png index 68c7e7de81..4dce52d788 100644 Binary files a/behavior_trees/images/walkthrough/overall_bt.png and b/behavior_trees/images/walkthrough/overall_bt.png differ diff --git a/behavior_trees/images/walkthrough/overall_bt_w_breakdown.png b/behavior_trees/images/walkthrough/overall_bt_w_breakdown.png index 8f9a39601d..eabe60361f 100644 Binary files a/behavior_trees/images/walkthrough/overall_bt_w_breakdown.png and b/behavior_trees/images/walkthrough/overall_bt_w_breakdown.png differ diff --git a/behavior_trees/overview/detailed_behavior_tree_walkthrough.rst b/behavior_trees/overview/detailed_behavior_tree_walkthrough.rst index 3dabd4a628..a321a9c47f 100644 --- a/behavior_trees/overview/detailed_behavior_tree_walkthrough.rst +++ b/behavior_trees/overview/detailed_behavior_tree_walkthrough.rst @@ -47,15 +47,28 @@ BTs are primarily defined in XML. The tree shown above is represented in XML as .. code-block:: xml - - + + + + + - + + + + + + + + + + + @@ -63,7 +76,7 @@ BTs are primarily defined in XML. The tree shown above is represented in XML as - + @@ -106,6 +119,8 @@ This can be represented in the following way: The ``Navigation`` subtree mainly involves actual navigation behavior: +- selecting planners / controllers / goal checkers / path handlers / progress checkers plugins + - calculating a path - following a path @@ -143,23 +158,36 @@ The XML of this subtree is as follows: .. code-block:: xml + + + - - - - - - - - - - + + + + + + + + + + + + - - + + + + + + + + + + @@ -203,6 +231,10 @@ The only differences in the BT subtree of ``ComputePathToPose`` and ``FollowPath - The ``ComputePathToPose`` subtree centers around the ``ComputePathToPose`` action. - The ``FollowPath`` subtree centers around the ``FollowPath`` action. +- The use of conditional flow control (``Fallback``): + - The ``ComputePathToPose`` subtree incorporates logic to handle the robot's behavior as it nears the goal. When using feasible planners, re-planning within a small radius (e.g., < 1.0m) can be detrimental due to state estimation drift or path-tracking errors, often resulting in unnecessary "looping" behaviors. + To prevent this, the subtree uses a ``ReactiveSequence`` with the ``IsGoalNearby`` node. If the robot is within a specified proximity threshold and the current path remains valid (i.e., no new obstacles), the subtree will skip the re-planning request. This allows the robot to smoothly transition into its final approach using its current path without unnecessary re-planning. + - The ``FollowPath`` subtree, by contrast, does not typically use this conditional gating. Once a path is available, the controller is invoked directly to produce velocity commands. - The ``RateController`` that decorates the ``ComputePathToPose`` subtree The ``RateController`` decorates the ``ComputePathToPose`` subtree to keep planning at the specified frequency. The default frequency for this BT is 1 hz. This is done to prevent the BT from flooding the planning server with too many useless requests at the tree update rate (100Hz). Consider changing this frequency to something higher or lower depending on the application and the computational cost of @@ -212,7 +244,7 @@ The only differences in the BT subtree of ``ComputePathToPose`` and ``FollowPath - The ``ComputePathToPose`` subtree clears the global costmap. The global costmap is the relevant costmap in the context of the planner - The ``FollowPath`` subtree clears the local costmap. The local costmap is the relevant costmap in the context of the controller -This subtree also utilizes the ``PlannerSelector`` and ``ControllerSelector`` nodes. These nodes ffer flexibility for applications that need to adjust navigation behavior on the fly. +This subtree also utilizes the ``PlannerSelector``, ``ControllerSelector``, ``GoalCheckerSelector``, ``ProgressCheckerSelector``, and ``PathHandlerSelector`` nodes. These nodes offer flexibility for applications that need to adjust navigation behavior on the fly. Recovery Subtree ================ diff --git a/behavior_trees/trees/nav_through_poses_recovery.rst b/behavior_trees/trees/nav_through_poses_recovery.rst index e5c1e4ec3c..b4c92b6c57 100644 --- a/behavior_trees/trees/nav_through_poses_recovery.rst +++ b/behavior_trees/trees/nav_through_poses_recovery.rst @@ -13,7 +13,7 @@ By convention we name these by the style of algorithms that they are (e.g. not ` In this behavior tree, we attempt to retry the entire navigation task 6 times before returning to the caller that the task has failed. This allows the navigation system ample opportunity to try to recovery from failure conditions or wait for transient issues to pass, such as crowding from people or a temporary sensor failure. -In nominal execution, this will replan the path at every 3 seconds and pass that path onto the controller, similar to the behavior tree in :ref:`behavior_trees`. +In nominal execution, this will replan the path at every 3 seconds if not close enough to goal and pass that path onto the controller, similar to the behavior tree in :ref:`behavior_trees`. 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. @@ -38,21 +38,31 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con .. code-block:: xml - - + + - + - - - - + + + + + + + + + + + + + + @@ -60,7 +70,7 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con - + diff --git a/behavior_trees/trees/nav_to_pose_recovery.rst b/behavior_trees/trees/nav_to_pose_recovery.rst index f2a39cefde..2588e3b254 100644 --- a/behavior_trees/trees/nav_to_pose_recovery.rst +++ b/behavior_trees/trees/nav_to_pose_recovery.rst @@ -14,7 +14,7 @@ By convention we name these by the style of algorithms that they are (e.g. not ` In this behavior tree, we attempt to retry the entire navigation task 6 times before returning to the caller that the task has failed. This allows the navigation system ample opportunity to try to recovery from failure conditions or wait for transient issues to pass, such as crowding from people or a temporary sensor failure. -In nominal execution, this will replan the path at every second and pass that path onto the controller, similar to the behavior tree in :ref:`behavior_trees`. +In nominal execution, this will replan the path at every second if not close enough to goal and pass that path onto the controller, similar to the behavior tree in :ref:`behavior_trees`. However, this time, if the planner fails, it will trigger contextually aware recovery behaviors in its subtree, clearing the global costmap. Additional recovery behaviors can be added here for additional context-specific recoveries, such as trying another algorithm. @@ -45,7 +45,17 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con - + + + + + + + + + + + @@ -53,7 +63,7 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con - + diff --git a/behavior_trees/trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.rst b/behavior_trees/trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.rst index 71f649fe47..dd353573c8 100644 --- a/behavior_trees/trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.rst +++ b/behavior_trees/trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.rst @@ -30,32 +30,37 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con .. code-block:: xml - - + + - + - - - - + + + + + + + - + + + - - + + - + @@ -63,8 +68,8 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con - - + + diff --git a/configuration/packages/bt-plugins/actions/TruncatePathLocal.rst b/configuration/packages/bt-plugins/actions/TruncatePathLocal.rst index d2c278aeaa..043aa97bec 100644 --- a/configuration/packages/bt-plugins/actions/TruncatePathLocal.rst +++ b/configuration/packages/bt-plugins/actions/TruncatePathLocal.rst @@ -28,7 +28,7 @@ Input Ports ====== =========== Description - The trimming distance in forward direction. + The trimming distance in forward direction. Set to -1 to search full path forward. :distance_backward: