nav2_dwb_controller: add forward_prune_distance parameter (#3374)

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.

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"

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;
   }