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..971d8eef46 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 shorten_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..3b0c48e6d8 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -89,6 +89,9 @@ void DWBLocalPlanner::configure( rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( node, dwb_plugin_name_ + ".prune_distance", + rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, dwb_plugin_name_ + ".shorten_distance", rclcpp::ParameterValue(2.0)); declare_parameter_if_not_declared( node, dwb_plugin_name_ + ".debug_trajectory_details", @@ -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_ + ".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); 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 shorten_dist = shorten_distance_; if (shorten_transformed_plan_) { - transform_end_threshold = std::min(dist_threshold, prune_dist); + transform_end_threshold = std::min(dist_threshold, shorten_dist); } else { transform_end_threshold = dist_threshold; } diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index 2b05867ac4..d816dfb55e 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -119,6 +119,7 @@ controller_server: plugin: "dwb_core::DWBLocalPlanner" debug_trajectory_details: True prune_distance: 1.0 + shorten_distance: 2.0 min_vel_x: 0.0 min_vel_y: 0.0 max_vel_x: 0.26