nav2_dwb_controller: add max_deviation parameter

Until now, the maximum authorized deviation from the path was
implicitly set to the local costmap half-width. This new parameter
allows to define a custom deviation tolerance. When the threshold is
reached, a nav2_core::PlannerException is thrown.

nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp

@@ -198,6 +198,7 @@ class DWBLocalPlanner : public nav2_core::Controller
  nav_2d_msgs::msg::Path2D global_plan_; ///< Saved Global Plan
  bool prune_plan_;
  double prune_distance_;
+ double max_deviation_;
  bool debug_trajectory_details_;
  rclcpp::Duration transform_tolerance_{0, 0};
  bool shorten_transformed_plan_;

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(1.0));
+ declare_parameter_if_not_declared(
+ node, dwb_plugin_name_ + ".max_deviation",
+ rclcpp::ParameterValue(1.0));
  declare_parameter_if_not_declared(
  node, dwb_plugin_name_ + ".shorten_distance",
  rclcpp::ParameterValue(2.0));
@@ -118,6 +121,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_ + ".max_deviation", max_deviation_);
  node->get_parameter(dwb_plugin_name_ + ".shorten_distance", shorten_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);
@@ -503,7 +507,7 @@ DWBLocalPlanner::transformGlobalPlan(
  }
  }
 
- if (smallest_distance == dist_threshold)
+ if (smallest_distance == dist_threshold || smallest_distance > max_deviation_)
  {
  throw nav2_core::ControllerException("The robot is too far from the path.");
  }