From e8d696718dca506dd00dd9784ca00bd1c31006f8 Mon Sep 17 00:00:00 2001 From: Dylan De Coeyer Date: Sat, 14 Jan 2023 21:16:35 +0100 Subject: [PATCH] nav2_dwb_controller: add forward_prune_distance parameter Until now, the prune_distance was used as distance threshold to shorten the upcoming path when shorten_transformed_plan was enabled. However, the prune and shortening mechanisms are de-correlated mechanisms. One could wish to use a different shortening distance for upcoming points, than the prune distance used for passed points. For this reason, a new parameter "forward_prune_distance" was added. --- .../include/dwb_core/dwb_local_planner.hpp | 1 + .../dwb_core/src/dwb_local_planner.cpp | 14 +++++++++----- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp index ba2c87d67b..49290e8d94 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp @@ -201,6 +201,7 @@ class DWBLocalPlanner : public nav2_core::Controller bool debug_trajectory_details_; rclcpp::Duration transform_tolerance_{0, 0}; bool shorten_transformed_plan_; + double forward_prune_distance_; /** * @brief try to resolve a possibly shortened critic name with the default namespaces and the suffix "Critic" diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index da8110a1a5..63ee4ce256 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -90,6 +90,9 @@ void DWBLocalPlanner::configure( declare_parameter_if_not_declared( node, dwb_plugin_name_ + ".prune_distance", rclcpp::ParameterValue(2.0)); + declare_parameter_if_not_declared( + node, dwb_plugin_name_ + ".forward_prune_distance", + rclcpp::ParameterValue(2.0)); declare_parameter_if_not_declared( node, dwb_plugin_name_ + ".debug_trajectory_details", rclcpp::ParameterValue(false)); @@ -115,6 +118,7 @@ void DWBLocalPlanner::configure( node->get_parameter(dwb_plugin_name_ + ".prune_plan", prune_plan_); node->get_parameter(dwb_plugin_name_ + ".prune_distance", prune_distance_); + node->get_parameter(dwb_plugin_name_ + ".forward_prune_distance", forward_prune_distance_); node->get_parameter(dwb_plugin_name_ + ".debug_trajectory_details", debug_trajectory_details_); node->get_parameter(dwb_plugin_name_ + ".trajectory_generator_name", traj_generator_name); node->get_parameter( @@ -474,7 +478,6 @@ DWBLocalPlanner::transformGlobalPlan( double dist_threshold = std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()) * costmap->getResolution() / 2.0; - // If prune_plan is enabled (it is by default) then we want to restrict the // plan to distances within that range as well. double prune_dist = prune_distance_; @@ -489,12 +492,13 @@ DWBLocalPlanner::transformGlobalPlan( transform_start_threshold = dist_threshold; } - // Set the maximum distance we'll include points after the part of the part of - // the plan near the robot (the end of the plan). This determines the amount - // of the plan passed on to the critics + // Set the maximum distance we'll include points after the part of the plan + // near the robot (the end of the plan). This determines the amount of the + // plan passed on to the critics double transform_end_threshold; + double forward_prune_dist = forward_prune_distance_; if (shorten_transformed_plan_) { - transform_end_threshold = std::min(dist_threshold, prune_dist); + transform_end_threshold = std::min(dist_threshold, forward_prune_dist); } else { transform_end_threshold = dist_threshold; }