From ca864525f75a30bce25ec418909cbcce5de1f49b Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Tue, 27 Dec 2022 17:41:08 +0300 Subject: [PATCH] Update default w_smooth to 2M --- nav2_constrained_smoother/README.md | 2 +- .../nav2_constrained_smoother/options.hpp | 2 +- .../test/test_constrained_smoother.cpp | 25 ++++++++++--------- 3 files changed, 15 insertions(+), 14 deletions(-) diff --git a/nav2_constrained_smoother/README.md b/nav2_constrained_smoother/README.md index d34274cda29..55482d5dcf9 100644 --- a/nav2_constrained_smoother/README.md +++ b/nav2_constrained_smoother/README.md @@ -22,7 +22,7 @@ smoother_server: minimum_turning_radius: 0.40 # minimum turning radius the robot can perform. Can be set to 0.0 (or w_curve can be set to 0.0 with the same effect) for diff-drive/holonomic robots w_curve: 30.0 # weight to enforce minimum_turning_radius w_dist: 0.0 # weight to bind path to original as optional replacement for cost weight - w_smooth: 15000.0 # weight to maximize smoothness of path + w_smooth: 2000000.0 # weight to maximize smoothness of path w_cost: 0.015 # weight to steer robot away from collision and cost # Parameters used to improve obstacle avoidance near cusps (forward/reverse movement changes) diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/options.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/options.hpp index 36c313cb092..637443ebdd0 100644 --- a/nav2_constrained_smoother/include/nav2_constrained_smoother/options.hpp +++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/options.hpp @@ -72,7 +72,7 @@ struct SmootherParams node, local_name + "w_dist", rclcpp::ParameterValue(0.0)); node->get_parameter(local_name + "w_dist", distance_weight); nav2_util::declare_parameter_if_not_declared( - node, local_name + "w_smooth", rclcpp::ParameterValue(15000.0)); + node, local_name + "w_smooth", rclcpp::ParameterValue(2000000.0)); node->get_parameter(local_name + "w_smooth", smooth_weight); nav2_util::declare_parameter_if_not_declared( node, local_name + "cost_check_points", rclcpp::ParameterValue(std::vector())); diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp index f50d2fb780b..463af33fd08 100644 --- a/nav2_constrained_smoother/test/test_constrained_smoother.cpp +++ b/nav2_constrained_smoother/test/test_constrained_smoother.cpp @@ -59,7 +59,7 @@ class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber for (int j = 40; j < 100; ++j) { int dist_x = std::max(0, std::max(60 - j, j - 80)); int dist_y = std::max(0, std::max(30 - i, i - 40)); - double dist = sqrt(dist_x * dist_x + dist_y * dist_y); + double dist = sqrt(dist_x * dist_x + dist_y * dist_y) * costmap->metadata.resolution; unsigned char cost; if (dist == 0) { cost = nav2_costmap_2d::LETHAL_OBSTACLE; @@ -68,7 +68,7 @@ class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber } else { double factor = exp( - -1.0 * cost_scaling_factor * (dist * costmap->metadata.resolution - inscribed_radius)); + -1.0 * cost_scaling_factor * (dist - inscribed_radius)); cost = static_cast((nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1) * factor); } @@ -141,7 +141,7 @@ class SmootherTest : public ::testing::Test std::shared_ptr()); smoother_->activate(); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_smooth", 15000.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_smooth", 2000000.0)); node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.minimum_turning_radius", 0.4)); node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_curve", 30.0)); node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_dist", 0.0)); @@ -614,7 +614,7 @@ TEST_F(SmootherTest, testingObstacleAvoidance) footprint.push_back(pointMsg(-0.4, -0.25)); footprint.push_back(pointMsg(0.4, -0.25)); - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_smooth", 15000.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_smooth", 2000000.0)); node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_cost", 0.015)); reloadParams(); @@ -644,7 +644,7 @@ TEST_F(SmootherTest, testingObstacleAvoidance) straight_near_obstacle, smoothed_path, cost_avoidance_criterion); EXPECT_GT(cost_avoidance_improvement, 0.0); - EXPECT_NEAR(cost_avoidance_improvement, 12.9, 1.0); + EXPECT_NEAR(cost_avoidance_improvement, 9.4, 1.0); } TEST_F(SmootherTest, testingObstacleAvoidanceNearCusps) @@ -723,6 +723,7 @@ TEST_F(SmootherTest, testingObstacleAvoidanceNearCusps) footprint.push_back(pointMsg(0.4, -0.2)); // first smooth with homogeneous w_cost to compare + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_smooth", 15000.0)); node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_cost", 0.015)); // higher w_curve significantly decreases convergence speed here // path feasibility can be restored by subsequent resmoothing with higher w_curve @@ -809,7 +810,7 @@ TEST_F(SmootherTest, testingObstacleAvoidanceNearCusps) footprint.push_back(pointMsg(0.15, -0.2)); // reset parameters back to homogeneous and shift cost check point to the center of the footprint - node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_smooth", 15000)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_smooth", 15000.0)); node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_curve", 1.0)); node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_cost", 0.015)); node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.cusp_zone_length", -1.0)); @@ -995,7 +996,7 @@ TEST_F(SmootherTest, testingDownsamplingUpsampling) &mvmt_smoothness_criterion_out); // more poses -> smoother path EXPECT_GT(smoothness_improvement, 0.0); - EXPECT_NEAR(smoothness_improvement, 65.7, 1.0); + EXPECT_NEAR(smoothness_improvement, 63.9, 1.0); // upsample above original size node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.path_upsampling_factor", 2)); @@ -1010,7 +1011,7 @@ TEST_F(SmootherTest, testingDownsamplingUpsampling) &mvmt_smoothness_criterion_out); // even more poses -> even smoother path EXPECT_GT(smoothness_improvement, 0.0); - EXPECT_NEAR(smoothness_improvement, 83.7, 1.0); + EXPECT_NEAR(smoothness_improvement, 82.2, 1.0); } TEST_F(SmootherTest, testingStartGoalOrientations) @@ -1032,7 +1033,7 @@ TEST_F(SmootherTest, testingStartGoalOrientations) double mvmt_smoothness_improvement = assessPathImprovement(sharp_turn_90, smoothed_path, mvmt_smoothness_criterion_); EXPECT_GT(mvmt_smoothness_improvement, 0.0); - EXPECT_NEAR(mvmt_smoothness_improvement, 53.3, 1.0); + EXPECT_NEAR(mvmt_smoothness_improvement, 55.2, 1.0); // no change in orientations EXPECT_NEAR(smoothed_path.front()[2], 0, 0.001); EXPECT_NEAR(smoothed_path.back()[2], M_PI / 2, 0.001); @@ -1049,10 +1050,10 @@ TEST_F(SmootherTest, testingStartGoalOrientations) mvmt_smoothness_improvement = assessPathImprovement(smoothed_path, smoothed_path_sg_overwritten, mvmt_smoothness_criterion_); EXPECT_GT(mvmt_smoothness_improvement, 0.0); - EXPECT_NEAR(mvmt_smoothness_improvement, 98.3, 1.0); + EXPECT_NEAR(mvmt_smoothness_improvement, 58.9, 1.0); // orientations adjusted to follow the path - EXPECT_NEAR(smoothed_path_sg_overwritten.front()[2], M_PI / 4, 0.1); - EXPECT_NEAR(smoothed_path_sg_overwritten.back()[2], M_PI / 4, 0.1); + EXPECT_NEAR(smoothed_path_sg_overwritten.front()[2], M_PI / 8, 0.1); + EXPECT_NEAR(smoothed_path_sg_overwritten.back()[2], 3 * M_PI / 8, 0.1); // test short paths std::vector short_screwed_path =