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