Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

nav2_dwb_controller: add forward_prune_distance parameter #3374

Merged
merged 1 commit into from
Feb 21, 2023
Merged

nav2_dwb_controller: add forward_prune_distance parameter #3374

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
14 changes: 9 additions & 5 deletions nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand All @@ -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(
Expand Down Expand Up @@ -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_;
Expand All @@ -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;
}
Expand Down