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

add parameterized refinement recursion numbers in Smac Planner Smoother and Simple Smoother #3284

Merged
merged 2 commits into from
Nov 15, 2022
Merged

add parameterized refinement recursion numbers in Smac Planner Smoother and Simple Smoother #3284

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
831fcb3
add parameterized refinement recursion numbers
SteveMacenski Nov 14, 2022
5fb5175
fix tests
SteveMacenski Nov 15, 2022
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
2 changes: 1 addition & 1 deletion nav2_smac_planner/include/nav2_smac_planner/smoother.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -232,7 +232,7 @@ class Smoother
bool & reversing_segment);

double min_turning_rad_, tolerance_, data_w_, smooth_w_;
int max_its_, refinement_ctr_;
int max_its_, refinement_ctr_, refinement_num_;
bool is_holonomic_, do_refinement_;
MotionModel motion_model_;
ompl::base::StateSpacePtr state_space_;
Expand Down
4 changes: 4 additions & 0 deletions nav2_smac_planner/include/nav2_smac_planner/types.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,6 +87,9 @@ struct SmootherParams
nav2_util::declare_parameter_if_not_declared(
node, local_name + "do_refinement", rclcpp::ParameterValue(true));
node->get_parameter(local_name + "do_refinement", do_refinement_);
nav2_util::declare_parameter_if_not_declared(
node, local_name + "refinement_num", rclcpp::ParameterValue(2));
node->get_parameter(local_name + "refinement_num", refinement_num_);
}

double tolerance_;
Expand All @@ -95,6 +98,7 @@ struct SmootherParams
double w_smooth_;
bool holonomic_;
bool do_refinement_;
int refinement_num_;
};

/**
Expand Down
5 changes: 3 additions & 2 deletions nav2_smac_planner/src/smoother.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@ Smoother::Smoother(const SmootherParams & params)
smooth_w_ = params.w_smooth_;
is_holonomic_ = params.holonomic_;
do_refinement_ = params.do_refinement_;
refinement_num_ = params.refinement_num_;
}

void Smoother::initialize(const double & min_turning_radius)
Expand All @@ -49,7 +50,6 @@ bool Smoother::smooth(
return false;
}

refinement_ctr_ = 0;
steady_clock::time_point start = steady_clock::now();
double time_remaining = max_time;
bool success = true, reversing_segment;
Expand All @@ -69,6 +69,7 @@ bool Smoother::smooth(
// Make sure we're still able to smooth with time remaining
steady_clock::time_point now = steady_clock::now();
time_remaining = max_time - duration_cast<duration<double>>(now - start).count();
refinement_ctr_ = 0;

// Smooth path segment naively
const geometry_msgs::msg::Pose start_pose = curr_path_segment.poses.front().pose;
Expand Down Expand Up @@ -178,7 +179,7 @@ bool Smoother::smoothImpl(

// Lets do additional refinement, it shouldn't take more than a couple milliseconds
// but really puts the path quality over the top.
if (do_refinement_ && refinement_ctr_ < 4) {
if (do_refinement_ && refinement_ctr_ < refinement_num_) {
refinement_ctr_++;
smoothImpl(new_path, reversing_segment, costmap, max_time);
}
Expand Down
2 changes: 1 addition & 1 deletion nav2_smoother/include/nav2_smoother/simple_smoother.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -121,7 +121,7 @@ class SimpleSmoother : public nav2_core::Smoother
const double & value);

double tolerance_, data_w_, smooth_w_;
int max_its_, refinement_ctr_;
int max_its_, refinement_ctr_, refinement_num_;
bool do_refinement_;
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
rclcpp::Logger logger_{rclcpp::get_logger("SimpleSmoother")};
Expand Down
7 changes: 5 additions & 2 deletions nav2_smoother/src/simple_smoother.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,12 +44,15 @@ void SimpleSmoother::configure(
node, name + ".w_smooth", rclcpp::ParameterValue(0.3));
declare_parameter_if_not_declared(
node, name + ".do_refinement", rclcpp::ParameterValue(true));
declare_parameter_if_not_declared(
node, name + ".refinement_num", rclcpp::ParameterValue(2));

node->get_parameter(name + ".tolerance", tolerance_);
node->get_parameter(name + ".max_its", max_its_);
node->get_parameter(name + ".w_data", data_w_);
node->get_parameter(name + ".w_smooth", smooth_w_);
node->get_parameter(name + ".do_refinement", do_refinement_);
node->get_parameter(name + ".refinement_num", refinement_num_);
}

bool SimpleSmoother::smooth(
Expand All @@ -58,7 +61,6 @@ bool SimpleSmoother::smooth(
{
auto costmap = costmap_sub_->getCostmap();

refinement_ctr_ = 0;
steady_clock::time_point start = steady_clock::now();
double time_remaining = max_time.seconds();

Expand All @@ -80,6 +82,7 @@ bool SimpleSmoother::smooth(
// Make sure we're still able to smooth with time remaining
steady_clock::time_point now = steady_clock::now();
time_remaining = max_time.seconds() - duration_cast<duration<double>>(now - start).count();
refinement_ctr_ = 0;

// Smooth path segment naively
success = success && smoothImpl(
Expand Down Expand Up @@ -180,7 +183,7 @@ bool SimpleSmoother::smoothImpl(

// Lets do additional refinement, it shouldn't take more than a couple milliseconds
// but really puts the path quality over the top.
if (do_refinement_ && refinement_ctr_ < 4) {
if (do_refinement_ && refinement_ctr_ < refinement_num_) {
refinement_ctr_++;
smoothImpl(new_path, reversing_segment, costmap, max_time);
}
Expand Down
6 changes: 3 additions & 3 deletions nav2_smoother/test/test_simple_smoother.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -128,7 +128,7 @@ TEST(SmootherTest, test_simple_smoother)
EXPECT_LT(
fabs(
straight_irregular_path.poses[i].pose.position.y -
straight_irregular_path.poses[i + 1].pose.position.y), 0.38);
straight_irregular_path.poses[i + 1].pose.position.y), 0.50);
}

// Test regular path, should see no effective change
Expand Down Expand Up @@ -181,8 +181,8 @@ TEST(SmootherTest, test_simple_smoother)
straight_regular_path.poses[10].pose.position.x = 0.95;
straight_regular_path.poses[10].pose.position.y = 0.5;
EXPECT_TRUE(smoother->smooth(straight_regular_path, max_time));
EXPECT_NEAR(straight_regular_path.poses[5].pose.position.x, 0.637, 0.01);
EXPECT_NEAR(straight_regular_path.poses[5].pose.position.y, 0.353, 0.01);
EXPECT_NEAR(straight_regular_path.poses[5].pose.position.x, 0.607, 0.01);
EXPECT_NEAR(straight_regular_path.poses[5].pose.position.y, 0.387, 0.01);

// Test that collisions are rejected
nav_msgs::msg::Path collision_path;
Expand Down