Add path handler

behavior_trees/trees/nav_through_poses_recovery.rst

@@ -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"/>

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='',               |                                                                            |
+| goal_checker_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="path_handler" 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,9 @@ 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"
         FollowPath:
           plugin: "dwb_core::DWBLocalPlanner"

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

configuration/packages/configuring-mppic.rst

@@ -363,75 +363,6 @@ Trajectory Visualization
   Description
     The step between points on trajectories to visualize to downsample trajectory density.
 
-Path Handler
-------------
-
-:transform_tolerance:
-
-  ============== ===========================
-  Type           Default
-  -------------- ---------------------------
-  double         0.1
-  ============== ===========================
-
-  Description
-    Time tolerance for data transformations with TF (s).
-
-:prune_distance:
-
-  ============== ===========================
-  Type           Default
-  -------------- ---------------------------
-  double         1.5
-  ============== ===========================
-
-  Description
-    Distance ahead of nearest point on path to robot to prune path to (m). This distance should be at least as great as the furthest distance of interest by a critic (i.e. for maximum velocity projection forward, threshold to consider).
-
-:max_robot_pose_search_dist:
-
-  ============== ===========================
-  Type           Default
-  -------------- ---------------------------
-  double         Costmap size / 2
-  ============== ===========================
-
-  Description
-    Max integrated distance ahead of robot pose to search for nearest path point in case of path looping.
-
-:enforce_path_inversion:
-
-  ============== ===========================
-  Type           Default
-  -------------- ---------------------------
-  bool           false
-  ============== ===========================
-
-  Description
-    If true, it will prune paths containing cusping points for segments changing directions (e.g. path inversions) such that the controller will be forced to change directions at or very near the planner's requested inversion point. In addition, these cusping points will also be treated by the critics as local goals that the robot will attempt to reach. This is targeting Smac Planner users with feasible paths who need their robots to switch directions where specifically requested.
-
-:inversion_xy_tolerance:
-
-  ============== ===========================
-  Type           Default
-  -------------- ---------------------------
-  double         0.2
-  ============== ===========================
-
-  Description
-    Cartesian proximity (m) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion.
-
-:inversion_yaw_tolerance:
-
-  ============== ===========================
-  Type           Default
-  -------------- ---------------------------
-  double         0.4
-  ============== ===========================
-
-  Description
-    Angular proximity (radians) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion. 0.4 rad = 23 deg.
-
 :allow_parameter_qos_overrides:
 
   ============== =============================
@@ -1141,8 +1072,6 @@ Example
           ay_max: 3.0
           az_max: 3.5
           iteration_count: 1
-          prune_distance: 1.7
-          transform_tolerance: 0.1
           temperature: 0.3
           gamma: 0.015
           motion_model: "DiffDrive"

configuration/packages/configuring-regulated-pp.rst

@@ -88,17 +88,6 @@ Regulated Pure Pursuit Parameters
   Description
     If ``use_rotate_to_heading`` is ``true``, this is the angular velocity to use.
 
-:transform_tolerance:
-
-  ============== =============================
-  Type           Default
-  -------------- -----------------------------
-  double         0.1
-  ============== =============================
-
-  Description
-    The TF transform tolerance (s).
-
 :use_velocity_scaled_lookahead_dist:
 
   ============== =============================
@@ -310,17 +299,6 @@ Regulated Pure Pursuit Parameters
   Description
     Linear deceleration (m/s/s) to apply when the goal is canceled.
 
-: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.
-
 :interpolate_curvature_after_goal:
 
   ============== =============================
@@ -398,7 +376,6 @@ Example
         max_lookahead_dist: 0.9
         lookahead_time: 1.5
         rotate_to_heading_angular_vel: 1.8
-        transform_tolerance: 0.1
         use_velocity_scaled_lookahead_dist: false
         min_approach_linear_velocity: 0.05
         approach_velocity_scaling_dist: 0.6
@@ -416,6 +393,5 @@ Example
         allow_reversing: false
         rotate_to_heading_min_angle: 0.785
         max_angular_accel: 3.2
-        max_robot_pose_search_dist: 10.0
         min_distance_to_obstacle: 0.0
         stateful: true