diff --git a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp index 8c2adc5d29a..d896cd90b5f 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp +++ b/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_; diff --git a/nav2_smac_planner/include/nav2_smac_planner/types.hpp b/nav2_smac_planner/include/nav2_smac_planner/types.hpp index f2cd73d33aa..1df34851afd 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/types.hpp +++ b/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_; }; /** diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp index fd0ac34be2d..a69e14011f3 100644 --- a/nav2_smac_planner/src/smoother.cpp +++ b/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>(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); } diff --git a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp index 95f19dc54c9..2e8fea4a945 100644 --- a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp +++ b/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 costmap_sub_; rclcpp::Logger logger_{rclcpp::get_logger("SimpleSmoother")}; diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index d04d88bc61b..2db6ce3c927 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/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>(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); } diff --git a/nav2_smoother/test/test_simple_smoother.cpp b/nav2_smoother/test/test_simple_smoother.cpp index 7d9b56d457e..fc3dceec971 100644 --- a/nav2_smoother/test/test_simple_smoother.cpp +++ b/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;