add parameterized refinement recursion numbers in Smac Planner Smooth…

…er and Simple Smoother (ros-navigation#3284)

* add parameterized refinement recursion numbers

* fix tests

nav2_smac_planner/include/nav2_smac_planner/smoother.hpp

@@ -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_;

nav2_smac_planner/include/nav2_smac_planner/types.hpp

@@ -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_;
@@ -95,6 +98,7 @@ struct SmootherParams
   double w_smooth_;
   bool holonomic_;
   bool do_refinement_;
+  int refinement_num_;
 };
 
 /**

nav2_smac_planner/src/smoother.cpp

@@ -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)
@@ -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;
@@ -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;
@@ -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);
   }

nav2_smoother/include/nav2_smoother/simple_smoother.hpp

@@ -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")};

nav2_smoother/src/simple_smoother.cpp

@@ -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(
@@ -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();
 
@@ -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(
@@ -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);
   }

nav2_smoother/test/test_simple_smoother.cpp

@@ -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
@@ -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;