Skip to content

Commit

Permalink
add parameterized refinement recursion numbers in Smac Planner Smooth…
Browse files Browse the repository at this point in the history
…er and Simple Smoother (ros-navigation#3284)

* add parameterized refinement recursion numbers

* fix tests
  • Loading branch information
SteveMacenski authored and Andrew Lycas committed Feb 23, 2023
1 parent aa74c89 commit 75e7019
Show file tree
Hide file tree
Showing 6 changed files with 17 additions and 9 deletions.
2 changes: 1 addition & 1 deletion nav2_smac_planner/include/nav2_smac_planner/smoother.hpp
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
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
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
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
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
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

0 comments on commit 75e7019

Please sign in to comment.