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 971d8eef46..7d036ea206 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 @@ -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_; 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 475bd0706c..591803cb3c 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(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."); }