Add path handler

behavior_trees/trees/nav_through_poses_recovery.rst

@@ -34,7 +34,7 @@ Next, the recovery subtree will tick the costmap clearing operations, spinning,
 After each of the recoveries in the subtree, the main navigation subtree will be reattempted.
 If it continues to fail, the next recovery in the recovery subtree is ticked.
 
-While this behavior tree does not make use of it, the ``PlannerSelector``, ``ControllerSelector``, and ``GoalCheckerSelector`` behavior tree nodes can also be helpful. Rather than hardcoding the algorithm to use (``GridBased`` and ``FollowPath``), these behavior tree nodes will allow a user to dynamically change the algorithm used in the navigation system via a ROS topic. It may be instead advisable to create different subtree contexts using condition nodes with specified algorithms in their most useful and unique situations. However, the selector nodes can be a useful way to change algorithms from an external application rather than via internal behavior tree control flow logic. It is better to implement changes through behavior tree methods, but we understand that many professional users have external applications to dynamically change settings of their navigators.
+While this behavior tree does not make use of it, the ``PlannerSelector``, ``ControllerSelector``, ``GoalCheckerSelector``, ``ProgressCheckerSelector``, and ``PathHandlerSelector`` behavior tree nodes can also be helpful. Rather than hardcoding the algorithm to use (``GridBased`` and ``FollowPath``), these behavior tree nodes will allow a user to dynamically change the algorithm used in the navigation system via a ROS topic. It may be instead advisable to create different subtree contexts using condition nodes with specified algorithms in their most useful and unique situations. However, the selector nodes can be a useful way to change algorithms from an external application rather than via internal behavior tree control flow logic. It is better to implement changes through behavior tree methods, but we understand that many professional users have external applications to dynamically change settings of their navigators.
 
 .. code-block:: xml
 
@@ -44,6 +44,7 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con
         <PipelineSequence name="NavigateWithReplanning">
           <ProgressCheckerSelector selected_progress_checker="{selected_progress_checker}" default_progress_checker="progress_checker" topic_name="progress_checker_selector"/>
           <GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="general_goal_checker" topic_name="goal_checker_selector"/>
+          <PathHandlerSelector selected_path_handler="{selected_path_handler}" default_path_handler="PathHandler" topic_name="path_handler_selector" />
           <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
           <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
           <RateController hz="0.333">
@@ -59,7 +60,7 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con
             </RecoveryNode>
           </RateController>
           <RecoveryNode number_of_retries="1" name="FollowPath">
-            <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}" goal_checker_id="{selected_goal_checker}" progress_checker_id="{selected_progress_checker}"/>
+            <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}" goal_checker_id="{selected_goal_checker}" progress_checker_id="{selected_progress_checker}" path_handler_id="{selected_path_handler}"/>
             <Sequence>
               <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
               <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>

behavior_trees/trees/nav_to_pose_recovery.rst

@@ -30,14 +30,17 @@ Next, the recovery subtree will the recoveries: costmap clearing operations, spi
 After each of the recoveries in the subtree, the main navigation subtree will be reattempted.
 If it continues to fail, the next recovery in the recovery subtree is ticked.
 
-While this behavior tree does not make use of it, the ``PlannerSelector``, ``ControllerSelector``, and ``GoalCheckerSelector`` behavior tree nodes can also be helpful. Rather than hardcoding the algorithm to use (``GridBased`` and ``FollowPath``), these behavior tree nodes will allow a user to dynamically change the algorithm used in the navigation system via a ROS topic. It may be instead advisable to create different subtree contexts using condition nodes with specified algorithms in their most useful and unique situations. However, the selector nodes can be a useful way to change algorithms from an external application rather than via internal behavior tree control flow logic. It is better to implement changes through behavior tree methods, but we understand that many professional users have external applications to dynamically change settings of their navigators.
+While this behavior tree does not make use of it, the ``PlannerSelector``, ``ControllerSelector``, ``GoalCheckerSelector``, ``ProgressCheckerSelector``, and ``PathHandlerSelector`` behavior tree nodes can also be helpful. Rather than hardcoding the algorithm to use (``GridBased`` and ``FollowPath``), these behavior tree nodes will allow a user to dynamically change the algorithm used in the navigation system via a ROS topic. It may be instead advisable to create different subtree contexts using condition nodes with specified algorithms in their most useful and unique situations. However, the selector nodes can be a useful way to change algorithms from an external application rather than via internal behavior tree control flow logic. It is better to implement changes through behavior tree methods, but we understand that many professional users have external applications to dynamically change settings of their navigators.
 
 .. code-block:: xml
 
-  <root BTCPP_format="4" main_tree_to_execute="MainTree">
-    <BehaviorTree ID="MainTree">
+  <root BTCPP_format="4" main_tree_to_execute="NavigateToPoseWReplanningAndRecovery">
+    <BehaviorTree ID="NavigateToPoseWReplanningAndRecovery">
       <RecoveryNode number_of_retries="6" name="NavigateRecovery">
         <PipelineSequence name="NavigateWithReplanning">
+          <ProgressCheckerSelector selected_progress_checker="{selected_progress_checker}" default_progress_checker="progress_checker" topic_name="progress_checker_selector"/>
+          <GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="general_goal_checker" topic_name="goal_checker_selector"/>
+          <PathHandlerSelector selected_path_handler="{selected_path_handler}" default_path_handler="PathHandler" topic_name="path_handler_selector"/>
           <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
           <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
           <RateController hz="1.0">

commander_api/index.rst

@@ -71,7 +71,9 @@ New as of September 2023: the simple navigator constructor will accept a `namesp
 |                                       | This will execute the chosen ``TaskExecutor`` plugin at each pose.         |
 +---------------------------------------+----------------------------------------------------------------------------+
 | followPath(path, controller_id='',    | Requests the robot to follow a path from a starting to a goal              |
-| goal_checker_id='')                   | ``PoseStamped``, ``nav_msgs/Path``.                                        |
+| goal_checker_id='',                   | ``PoseStamped``, ``nav_msgs/Path``.                                        |
+| progress_checker_id='',               |                                                                            |
+| path_handler_id='')                   |                                                                            |
 +---------------------------------------+----------------------------------------------------------------------------+
 | spin(spin_dist=1.57,                  | Requests the robot to performs an in-place rotation by a given angle.      |
 | time_allowance=10,                    |                                                                            |

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

@@ -53,6 +53,17 @@ Input Ports
   Description
       Mapped name of the progress checker plugin type to use, e.g. SimpleProgressChecker.
 
+:path_handler_id:
+
+  ====== =======
+  Type   Default
+  ------ -------
+  string N/A
+  ====== =======
+
+  Description
+      Mapped name of the path handler plugin type to use, e.g. FeasiblePathHandler.
+
 :server_name:
 
   ====== =======
@@ -108,4 +119,4 @@ Example
 
 .. code-block:: xml
 
-    <FollowPath path="{path}" controller_id="FollowPath" goal_checker_id="precise_goal_checker" server_name="FollowPath" server_timeout="10" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
+    <FollowPath path="{path}" controller_id="FollowPath" goal_checker_id="precise_goal_checker" path_handler_id="PathHandler" server_name="FollowPath" server_timeout="10" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>

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

@@ -0,0 +1,58 @@
+.. _bt_path_handler_selector_node:
+
+PathHandlerSelector
+===================
+
+It is used to select the PathHandler that will be used by the controller server. It subscribes to the ``path_handler_selector`` topic to receive command messages with the name of the PathHandler to be used. It is commonly used before of the FollowPathAction. The ``selected_path_handler`` output port is passed to ``path_handler_id`` input port of the FollowPathAction. If none is provided on the topic, the ``default_path_handler`` is used.
+
+Any publisher to this topic needs to be configured with some QoS defined as ``reliable`` and ``transient local``.
+
+.. _bt_navigator: https://github.com/ros-navigation/navigation2/tree/main/nav2_bt_navigator
+
+Input Ports
+-----------
+
+:topic_name:
+
+  ====== =====================
+  Type   Default
+  ------ ---------------------
+  string path_handler_selector
+  ====== =====================
+
+  Description
+      The name of the topic used to received select command messages. This is used to support multiple PathHandlerSelector nodes.
+
+:default_path_handler:
+
+  ====== =======
+  Type   Default
+  ------ -------
+  string N/A
+  ====== =======
+
+  Description
+      The default value for the selected PathHandler if no message is received from the input topic.
+
+
+Output Ports
+------------
+
+:selected_path_handler:
+
+  ====== =======
+  Type   Default
+  ------ -------
+  string N/A
+  ====== =======
+
+  Description
+      The output selected PathHandler id. This selected_path_handler string is usually passed to the FollowPath behavior via the path_handler_id input port.
+
+
+Example
+-------
+
+.. code-block:: xml
+
+  <PathHandlerSelector selected_path_handler="{selected_path_handler}" default_path_handler="PathHandler" topic_name="path_handler_selector"/>

configuration/packages/configuring-bt-xml.rst

@@ -41,6 +41,7 @@ Action Plugins
   bt-plugins/actions/SmootherSelector.rst
   bt-plugins/actions/GoalCheckerSelector.rst
   bt-plugins/actions/ProgressCheckerSelector.rst
+  bt-plugins/actions/PathHandlerSelector.rst
   bt-plugins/actions/NavigateThroughPoses.rst
   bt-plugins/actions/ComputePathThroughPoses.rst
   bt-plugins/actions/ComputeCoveragePath.rst

configuration/packages/configuring-controller-server.rst

@@ -132,6 +132,31 @@ Parameters
             goal_checker:
               plugin: "nav2_controller::SimpleGoalChecker"
 
+:path_handler_plugins:
+
+  ============== ================
+  Type           Default
+  -------------- ----------------
+  vector<string> ["PathHandler"]
+  ============== ================
+
+  Description
+    Mapped name for path handler plugin for processing path from the planner. When the number of the plugins is more than 2, each :code:`FollowPath` action needs to specify the path handler plugin name with its :code:`path_handler_id` field.
+
+  Note
+    The plugin namespace defined needs to have a :code:`plugin` parameter defining the type of plugin to be loaded in the namespace.
+
+    Example:
+
+    .. code-block:: yaml
+
+        controller_server:
+          ros__parameters:
+            path_handler_plugins: ["PathHandler"]
+            path_handler:
+              plugin: "nav2_controller::FeasiblePathHandler"
+
+
 :min_x_velocity_threshold:
 
   ============== =============================
@@ -282,11 +307,12 @@ Provided Plugins
   nav2_controller-plugins/simple_goal_checker.rst
   nav2_controller-plugins/stopped_goal_checker.rst
   nav2_controller-plugins/position_goal_checker.rst
+  nav2_controller-plugins/feasible_path_handler.rst
 
 Default Plugins
 ***************
 
-When the :code:`progress_checker_plugins`, :code:`goal_checker_plugin` or :code:`controller_plugins` parameters are not overridden, the following default plugins are loaded:
+When the :code:`progress_checker_plugins`, :code:`goal_checker_plugin`, :code:`path_handler_plugin` or :code:`controller_plugins` parameters are not overridden, the following default plugins are loaded:
 
   ================== =====================================================
   Namespace          Plugin
@@ -295,6 +321,8 @@ When the :code:`progress_checker_plugins`, :code:`goal_checker_plugin` or :code:
   ------------------ -----------------------------------------------------
   "goal_checker"     "nav2_controller::SimpleGoalChecker"
   ------------------ -----------------------------------------------------
+  "path_handler"     "nav2_controller::FeasiblePathHandler"
+  ------------------ -----------------------------------------------------
   "FollowPath"       "dwb_core::DWBLocalPlanner"
   ================== =====================================================
 
@@ -314,6 +342,7 @@ Example
         odom_duration: 0.3
         progress_checker_plugins: ["progress_checker"] # progress_checker_plugin: "progress_checker" For Humble and older
         goal_checker_plugins: ["goal_checker"] # goal_checker_plugin: "goal_checker" For Galactic and older
+        path_handler_plguins: ["PathHandler"]
         controller_plugins: ["FollowPath"]
         progress_checker:
           plugin: "nav2_controller::SimpleProgressChecker"
@@ -323,6 +352,16 @@ Example
           plugin: "nav2_controller::SimpleGoalChecker"
           xy_goal_tolerance: 0.25
           yaw_goal_tolerance: 0.25
+          path_length_tolerance: 1.0
           stateful: True
+        PathHandler:
+          plugin: "nav2_controller::FeasiblePathHandler"
+          prune_distance: 2.0
+          enforce_path_inversion: True
+          enforce_path_rotation: False
+          inversion_xy_tolerance: 0.2
+          inversion_yaw_tolerance: 0.4
+          minimum_rotation_angle: 0.785
+          reject_unit_path: False
         FollowPath:
           plugin: "dwb_core::DWBLocalPlanner"

configuration/packages/configuring-dwb-controller.rst

@@ -99,8 +99,8 @@ Example
           sim_time: 1.7
           linear_granularity: 0.05
           angular_granularity: 0.025
-          transform_tolerance: 0.2
           xy_goal_tolerance: 0.25
+          path_length_tolerance: 1.0
           trans_stopped_velocity: 0.25
           short_circuit_trajectory_evaluation: True
           limit_vel_cmd_in_traj: False

configuration/packages/configuring-graceful-motion-controller.rst

@@ -14,17 +14,6 @@ See the package's ``README`` for more complete information.
 Graceful Controller Parameters
 ******************************
 
-:transform_tolerance:
-
-  ============== ===========================
-  Type           Default
-  -------------- ---------------------------
-  double         0.1
-  ============== ===========================
-
-  Description
-    The TF transform tolerance (s).
-
 :max_lookahead:
 
   ============== =============================
@@ -47,17 +36,6 @@ Graceful Controller Parameters
   Description
     The minimum lookahead distance (m) to use when selecting a target pose for the underlying control law. This parameter avoids instability when an unexpected obstacle appears in the path of the robot by returning failure, which typically triggers replanning.
 
-:max_robot_pose_search_dist:
-
-  ============== =================================================
-  Type           Default
-  -------------- -------------------------------------------------
-  double         Local costmap max extent (max(width, height) / 2)
-  ============== =================================================
-
-  Description
-    Upper bound on integrated distance along the global plan to search for the closest pose to the robot pose. This should be left as the default unless there are paths with loops and intersections that do not leave the local costmap, in which case making this value smaller is necessary to prevent shortcutting. If set to ``-1``, it will use the maximum distance possible to search every point on the path for the nearest path point.
-
 :k_phi:
 
   ============== =============================
@@ -270,7 +248,6 @@ Example
         stateful: True
       FollowPath:
         plugin: nav2_graceful_controller::GracefulController
-        transform_tolerance: 0.1
         min_lookahead: 0.25
         max_lookahead: 1.0
         initial_rotation: true