diff --git a/behavior_trees/trees/nav_through_poses_recovery.rst b/behavior_trees/trees/nav_through_poses_recovery.rst index ab2976d98..e5c1e4ec3 100644 --- a/behavior_trees/trees/nav_through_poses_recovery.rst +++ b/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 + @@ -59,7 +60,7 @@ While this behavior tree does not make use of it, the ``PlannerSelector``, ``Con - + diff --git a/commander_api/index.rst b/commander_api/index.rst index ea7353914..ad6d5635f 100644 --- a/commander_api/index.rst +++ b/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, | | diff --git a/configuration/packages/bt-plugins/actions/FollowPath.rst b/configuration/packages/bt-plugins/actions/FollowPath.rst index 3e784e7cf..511d818b3 100644 --- a/configuration/packages/bt-plugins/actions/FollowPath.rst +++ b/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 - + diff --git a/configuration/packages/bt-plugins/actions/PathHandlerSelector.rst b/configuration/packages/bt-plugins/actions/PathHandlerSelector.rst new file mode 100644 index 000000000..c7dc0c6f0 --- /dev/null +++ b/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 + + diff --git a/configuration/packages/configuring-bt-xml.rst b/configuration/packages/configuring-bt-xml.rst index 38521a967..796d9d154 100644 --- a/configuration/packages/configuring-bt-xml.rst +++ b/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 diff --git a/configuration/packages/configuring-controller-server.rst b/configuration/packages/configuring-controller-server.rst index bab4cedb1..2170c1573 100644 --- a/configuration/packages/configuring-controller-server.rst +++ b/configuration/packages/configuring-controller-server.rst @@ -132,6 +132,31 @@ Parameters goal_checker: plugin: "nav2_controller::SimpleGoalChecker" +:path_handler_plugins: + + ============== ================ + Type Default + -------------- ---------------- + vector ["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: 1.4 + 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" diff --git a/configuration/packages/configuring-dwb-controller.rst b/configuration/packages/configuring-dwb-controller.rst index b47315820..066f565b8 100644 --- a/configuration/packages/configuring-dwb-controller.rst +++ b/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 diff --git a/configuration/packages/configuring-graceful-motion-controller.rst b/configuration/packages/configuring-graceful-motion-controller.rst index 8128f1a6b..adcf50194 100644 --- a/configuration/packages/configuring-graceful-motion-controller.rst +++ b/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 diff --git a/configuration/packages/configuring-mppic.rst b/configuration/packages/configuring-mppic.rst index 7945da18b..26b10217f 100644 --- a/configuration/packages/configuring-mppic.rst +++ b/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" diff --git a/configuration/packages/configuring-regulated-pp.rst b/configuration/packages/configuring-regulated-pp.rst index a5e46556c..d7eb2bacd 100644 --- a/configuration/packages/configuring-regulated-pp.rst +++ b/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 diff --git a/configuration/packages/configuring-rotation-shim-controller.rst b/configuration/packages/configuring-rotation-shim-controller.rst index 216b0b224..21ec3ff8e 100644 --- a/configuration/packages/configuring-rotation-shim-controller.rst +++ b/configuration/packages/configuring-rotation-shim-controller.rst @@ -61,7 +61,7 @@ Rotation Shim Controller Parameters ============== ============================= Description - Forward distance, in meters, along path to select a sampling point to use to approximate path heading + Forward distance, in meters, along path to select a sampling point to use to approximate path heading. This distance should not be larger than the path handler's prune distance. :rotate_to_heading_angular_vel: diff --git a/configuration/packages/dwb-params/controller.rst b/configuration/packages/dwb-params/controller.rst index 6b46e0d54..4e6d9ed1a 100644 --- a/configuration/packages/dwb-params/controller.rst +++ b/configuration/packages/dwb-params/controller.rst @@ -30,50 +30,6 @@ Parameters Description Namespaces to load critics in. -:````.prune_plan: - - ==== ======= - Type Default - ---- ------- - bool true - ==== ======= - - Description - Whether to prune the path of old, passed points. - -:````.shorten_transformed_plan: - - ==== ======= - Type Default - ---- ------- - bool true - ==== ======= - - Description - Determines whether we will pass the full plan on to the critics. - -:````.prune_distance: - - ====== ======= - Type Default - ------ ------- - double 2.0 - ====== ======= - - Description - Distance (m) to prune backward until. - -:````.forward_prune_distance: - - ====== ======= - Type Default - ------ ------- - double 2.0 - ====== ======= - - Description - Distance (m) to prune forward until. If set to ``-1``, it will search the full path for the closest point, in the case of no replanning. - :````.debug_trajectory_details: ==== ======= @@ -107,17 +63,6 @@ Parameters Description Goal checker plugin name. -:````.transform_tolerance: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 0.1 - ============== ============================= - - Description - TF transform tolerance (s). - :````.short_circuit_trajectory_evaluation: ============== ============================= diff --git a/configuration/packages/dwb-params/visualization.rst b/configuration/packages/dwb-params/visualization.rst index 9fb847a70..90bc15855 100644 --- a/configuration/packages/dwb-params/visualization.rst +++ b/configuration/packages/dwb-params/visualization.rst @@ -19,28 +19,6 @@ Parameters Description Whether to publish the local plan evaluation. -:````.publish_global_plan: - - ==== ======= - Type Default - ---- ------- - bool true - ==== ======= - - Description - Whether to publish the global plan. - -:````.publish_transformed_plan: - - ==== ======= - Type Default - ---- ------- - bool true - ==== ======= - - Description - Whether to publish the global plan in the odometry frame. - :````.publish_local_plan: ==== ======= diff --git a/configuration/packages/nav2_controller-plugins/feasible_path_handler.rst b/configuration/packages/nav2_controller-plugins/feasible_path_handler.rst new file mode 100644 index 000000000..d1720f535 --- /dev/null +++ b/configuration/packages/nav2_controller-plugins/feasible_path_handler.rst @@ -0,0 +1,115 @@ +.. _configuring_nav2_controller_feasible_path_handler_plugin: + +FeasiblePathHandler +=================== + +Transforms the global plan into the local costmap frame, prunes it to the relevant portion +within the costmap bounds, and handles in-place rotation and cusp pruning. + +Parameters +********** + +````: nav2_controller plugin name defined in the **path_handler_plugin_id** parameter in :ref:`configuring_controller_server`. + +:````.reject_unit_path: + + ============== =========================== + Type Default + -------------- --------------------------- + bool false + ============== =========================== + + Description + If enabled, the path handler will reject a path that contains only a single pose. + +:````.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. + +:````.enforce_path_rotation: + + ============== =========================== + Type Default + -------------- --------------------------- + bool false + ============== =========================== + + Description + If true, the controller will detect in-place rotation segments (where translation is near zero) and prune the remaining poses after the rotation point. This forces the robot to explicitly perform the rotation before proceeding along the rest of the path. + This is particularly useful for feasible planners (e.g., Smac Planner) where direction changes are intentionally introduced and must be respected during execution. + +:````.minimum_rotation_angle: + + ============== =========================== + Type Default + -------------- --------------------------- + double 0.785 + ============== =========================== + + Description + The minimum accumulated rotation (in radians) required to classify a segment as an in-place rotation. 0.785 rad = 45 deg. + +:````.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. + +Example +******* +.. code-block:: yaml + + PathHandler: + plugin: "nav2_controller::FeasiblePathHandler" + prune_distance: 1.4 + 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 diff --git a/configuration/packages/nav2_controller-plugins/position_goal_checker.rst b/configuration/packages/nav2_controller-plugins/position_goal_checker.rst index f607a47d2..e6d39a3c0 100644 --- a/configuration/packages/nav2_controller-plugins/position_goal_checker.rst +++ b/configuration/packages/nav2_controller-plugins/position_goal_checker.rst @@ -21,6 +21,17 @@ Parameters Description Tolerance to meet goal completion criteria (m). +:````.path_length_tolerance: + + ====== ======= + Type Default + ------ ------- + double 1.0 + ====== ======= + + Description + Tolerance to meet goal completion criteria (m). + :````.stateful: ==== ======= diff --git a/configuration/packages/nav2_controller-plugins/simple_goal_checker.rst b/configuration/packages/nav2_controller-plugins/simple_goal_checker.rst index 531f69cc7..c5bca8abd 100644 --- a/configuration/packages/nav2_controller-plugins/simple_goal_checker.rst +++ b/configuration/packages/nav2_controller-plugins/simple_goal_checker.rst @@ -32,6 +32,17 @@ Parameters Description Tolerance to meet goal completion criteria (rad). +:````.path_length_tolerance: + + ====== ======= + Type Default + ------ ------- + double 1.0 + ====== ======= + + Description + Tolerance to meet goal completion criteria (m). + :````.stateful: ==== ======= diff --git a/configuration/packages/nav2_controller-plugins/stopped_goal_checker.rst b/configuration/packages/nav2_controller-plugins/stopped_goal_checker.rst index eea4cf73c..3e56d5dfb 100644 --- a/configuration/packages/nav2_controller-plugins/stopped_goal_checker.rst +++ b/configuration/packages/nav2_controller-plugins/stopped_goal_checker.rst @@ -54,6 +54,17 @@ Parameters Description Tolerance to meet goal completion criteria (rad). +:````.path_length_tolerance: + + ====== ======= + Type Default + ------ ------- + double 1.0 + ====== ======= + + Description + Tolerance to meet goal completion criteria (m). + :````.stateful: ==== ======= diff --git a/configuration/packages/trajectory_critics/rotate_to_goal.rst b/configuration/packages/trajectory_critics/rotate_to_goal.rst index dcaf31791..666fd9e8c 100644 --- a/configuration/packages/trajectory_critics/rotate_to_goal.rst +++ b/configuration/packages/trajectory_critics/rotate_to_goal.rst @@ -24,6 +24,17 @@ Parameters Description Tolerance to meet goal completion criteria (m). +:````.path_length_tolerance: + + ====== ======= + Type Default + ------ ------- + double 1.0 + ====== ======= + + Description + Tolerance to meet goal completion criteria (m). + :````.trans_stopped_velocity: ====== ======= diff --git a/migration/Kilted.rst b/migration/Kilted.rst index 458f24ec2..8e011528c 100644 --- a/migration/Kilted.rst +++ b/migration/Kilted.rst @@ -492,3 +492,145 @@ Options to build with isolated tests In `PR #5516 `_, we added an option to build with isolated tests. This allows users of ``rmw_zenoh_cpp`` to run the tests without needing to start a Zenoh router in a separate terminal. You can enable this by building with ``--cmake-args -DUSE_ISOLATED_TESTS=ON``. + +Centralize Path Handler logic in Controller Server +-------------------------------------------------- + +`PR #5446 `_ centralizes +path handling logic inside the controller server. Previously, each controller +plugin implemented its own version of this logic. + +With this change, users can now configure a path handler in the controller server: + +.. code-block:: yaml + + PathHandler: + plugin: "nav2_controller::FeasiblePathHandler" + prune_distance: 1.4 + 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 + + +For more details, refer to the *Path Handler* section in the Controller Server +documentation. + +This update resolves the issue where *Navigate Through Poses* could terminate +prematurely when passing near intermediate goal poses. We also introduce the following API changes as part of this fix. + +GoalChecker +^^^^^^^^^^^^ + +Previously: + +.. code-block:: c++ + + virtual bool isGoalReached( + const geometry_msgs::msg::Pose & query_pose, + const geometry_msgs::msg::Pose & goal_pose, + const geometry_msgs::msg::Twist & velocity) = 0; + +Now: + +.. code-block:: c++ + + virtual bool isGoalReached( + const geometry_msgs::msg::Pose & query_pose, + const geometry_msgs::msg::Pose & goal_pose, + const geometry_msgs::msg::Twist & velocity, + const nav_msgs::msg::Path & transformed_global_plan) = 0; + +A new argument is added: the **transformed and pruned global plan** from the path handler. + +Controller Plugins +^^^^^^^^^^^^^^^^^^ + +Previously: + +.. code-block:: c++ + + virtual void setPlan(const nav_msgs::msg::Path & path) = 0; + +Now: + +.. code-block:: c++ + + virtual void newPathReceived(const nav_msgs::msg::Path & raw_global_path) = 0; + +This callback should now only perform lightweight tasks (e.g. resetting internal +state). The controller will be provided the processed plan during computeVelocityCommands(). + +Previously: + +.. code-block:: c++ + + virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped & pose, + const geometry_msgs::msg::Twist & velocity, + nav2_core::GoalChecker * goal_checker) = 0; + +Now: + +.. code-block:: c++ + + virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped & pose, + const geometry_msgs::msg::Twist & velocity, + nav2_core::GoalChecker * goal_checker, + const nav_msgs::msg::Path & transformed_global_plan, + const geometry_msgs::msg::PoseStamped & global_goal) = 0; + +Two new arguments are provided: + +- the **transformed and pruned global plan** from the path handler. +- The last pose from the global plan. + +Moreover, several parameters have also been added to / removed from individual controller plugin configurations as part of centralizing path-handling logic in the controller server. + +DWB +^^^ + +**Removed parameters** + +- ``prune_plan`` +- ``shorten_transformed_plan`` +- ``prune_distance`` +- ``forward_prune_distance`` +- ``transform_tolerance`` +- ``publish_global_plan`` +- ``publish_transformed_plan`` + +**Added parameters** + +- ``path_length_tolerance`` + +RPP +^^^ + +**Removed parameters** + +- ``transform_tolerance`` +- ``max_robot_pose_search_dist`` + +Graceful +^^^^^^^^ + +**Removed parameters** + +- ``transform_tolerance`` +- ``max_robot_pose_search_dist`` + +MPPI +^^^^ + +**Removed parameters** + +- ``transform_tolerance`` +- ``prune_distance`` +- ``max_robot_pose_search_dist`` +- ``enforce_path_inversion`` +- ``inversion_xy_tolerance`` +- ``inversion_yaw_tolerance`` diff --git a/plugin_tutorials/docs/writing_new_nav2controller_plugin.rst b/plugin_tutorials/docs/writing_new_nav2controller_plugin.rst index e1d3010b8..2be1503da 100644 --- a/plugin_tutorials/docs/writing_new_nav2controller_plugin.rst +++ b/plugin_tutorials/docs/writing_new_nav2controller_plugin.rst @@ -64,14 +64,17 @@ The list of methods, their descriptions, and necessity are presented in the tabl | cleanup() | Method is called when controller server goes to on_cleanup state. Ideally this method | Yes | | | should clean up resources which are created for the controller. | | +---------------------------+---------------------------------------------------------------------------------------+------------------------+ -| setPlan() | Method is called when the global plan is updated. Ideally this method should perform | Yes | -| | operations that transform the global plan and store it. | | +| newPathReceived() | Method is called when the global plan is updated. Ideally this method should only | Yes | +| | perform minimal work, such as extracting global information that may be of interest | | +| | (e.g. reset internal states when new path received). | | +---------------------------+---------------------------------------------------------------------------------------+------------------------+ | computeVelocityCommands() | Method is called when a new velocity command is demanded by the controller server | Yes | | | in-order for the robot to follow the global path. This method returns a | | | | `geometry_msgs\:\:msg\:\:TwistStamped` which represents the velocity command for the | | -| | robot to drive. This method passes 3 parameters: reference to the current robot | | -| | pose, its current velocity, and a pointer to the `nav2_core::GoalChecker`. | | +| | robot to drive. This method passes 5 parameters: reference to the current robot | | +| | pose, its current velocity, a pointer to the `nav2_core::GoalChecker`, the | | +| | transformed_global_plan output by the path handler plugin, and the last pose of the | | +| | global plan. | | +---------------------------+---------------------------------------------------------------------------------------+------------------------+ | cancel() | Method is called when the controller server receives a cancel request. If this method | No | | | is unimplemented, the controller will immediately stop when receiving a cancel | | @@ -85,7 +88,7 @@ The list of methods, their descriptions, and necessity are presented in the tabl | | behavior untouched. | | +---------------------------+---------------------------------------------------------------------------------------+------------------------+ -In this tutorial, we will use the methods ``PurePursuitController::configure``, ``PurePursuitController::setPlan`` and +In this tutorial, we will use the methods ``PurePursuitController::configure``, ``PurePursuitController::newPathReceived`` and ``PurePursuitController::computeVelocityCommands``. In controllers, ``configure()`` method must set member variables from ROS parameters and perform any initialization required. @@ -136,19 +139,10 @@ We will see more on this when we discuss the parameters file (or params file). The passed-in arguments are stored in member variables so that they can be used at a later stage if needed. -In ``setPlan()`` method, we receive the updated global path for the robot to follow. In our example, we transform the received global path into -the frame of the robot and then store this transformed global path for later use. - -.. code-block:: c++ - - void PurePursuitController::setPlan(const nav_msgs::msg::Path & path) - { - // Transform global path into the robot's frame - global_plan_ = transformGlobalPlan(path); - } - The computation for the desired velocity happens in the ``computeVelocityCommands()`` method. It is used to calculate the desired velocity command given the current velocity and pose. The third argument - is a pointer to the ``nav2_core::GoalChecker``, that checks whether a goal has been reached. In our example, this won't be used. +The fourth argument - is the global plan that has already been transformed into the local costmap frame and pruned to only the relevant portion within the costmap bounds. In our example, we will transform this local plan from costmap global frame to robot base frame. This is the path that pure pursuit will track. +The fifth argument - is the last pose of the global plan. In our example, this won't be used. In the case of pure pursuit, the algorithm computes velocity commands such that the robot tries to follow the global path as closely as possible. This algorithm assumes a constant linear velocity and computes the angular velocity based on the curvature of the global path. @@ -157,8 +151,20 @@ This algorithm assumes a constant linear velocity and computes the angular veloc geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, - nav2_core::GoalChecker * /*goal_checker*/) + nav2_core::GoalChecker * /*goal_checker*/, + const nav_msgs::msg::Path & transformed_global_plan, + const geometry_msgs::msg::PoseStamped & /*global_goal*/) { + // Transform the plan from costmap's global frame to robot base frame + nav_msgs::msg::Path transformed_plan; + if (!nav2_util::transformPathInTargetFrame( + transformed_global_plan, transformed_plan, *tf_, + costmap_ros_->getBaseFrameID(), costmap_ros_->getTransformTolerance())) + { + throw nav2_core::ControllerTFError( + "Unable to transform plan pose into local frame"); + } + // Find the first pose which is at a distance greater than the specified lookahead distance auto goal_pose = std::find_if( global_plan_.poses.begin(), global_plan_.poses.end(), diff --git a/plugins/index.rst b/plugins/index.rst index 5366840ac..5edf03b77 100644 --- a/plugins/index.rst +++ b/plugins/index.rst @@ -341,6 +341,22 @@ Progress Checkers .. _SimpleProgressChecker: https://github.com/ros-navigation/navigation2/blob/main/nav2_controller/plugins/simple_progress_checker.cpp .. _PoseProgressChecker: https://github.com/ros-navigation/navigation2/blob/main/nav2_controller/plugins/pose_progress_checker.cpp +Path Handlers +============= + ++---------------------------------+------------------------+----------------------------------+ +| Plugin Name | Creator | Description | ++=================================+========================+==================================+ +| `FeasiblePathHandler`_ | Maurice Alexander | A plugin that transforms global | +| | Purnawan | plan to the local costmap frame, | +| | | prunes it to the relevant portion| +| | | within the costmap bounds, and | +| | | handles in-place rotation and | +| | | cusp pruning. | ++---------------------------------+------------------------+----------------------------------+ + +.. _FeasiblePathHandler: + Behavior Tree Nodes =================== @@ -400,6 +416,10 @@ Behavior Tree Nodes | | | topic input, otherwises uses a default | | | | progress checker id | +---------------------------------------------+---------------------+------------------------------------------+ +| `Path Handler Selector`_ | Maurice Alexander | Selects the path handler based on a | +| | Purnawan | topic input, otherwises uses a default | +| | | path handler id | ++---------------------------------------------+---------------------+------------------------------------------+ | `Navigate Through Poses`_ | Steve Macenski | BT Node for other BehaviorTree.CPP BTs | | | | to call Nav2's NavThroughPoses action | | | | | @@ -480,6 +500,7 @@ Behavior Tree Nodes .. _Goal Checker Selector: https://github.com/ros-navigation/navigation2/tree/main/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp .. _Smoother Selector: https://github.com/ros-navigation/navigation2/tree/main/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp .. _Progress Checker Selector: https://github.com/ros-navigation/navigation2/tree/main/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp +.. _Path Handler Selector: https://github.com/ros-navigation/navigation2/tree/main/nav2_behavior_tree/plugins/action/path_handler_selector_node.cpp .. _Navigate Through Poses: https://github.com/ros-navigation/navigation2/tree/main/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp .. _Remove Passed Goals: https://github.com/ros-navigation/navigation2/tree/main/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp .. _Remove In Collision Goals: https://github.com/ros-navigation/navigation2/tree/main/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp