Update default w_smooth to 2M (#3341)

nav2_constrained_smoother/README.md

@@ -23,7 +23,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)

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<double>()));

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<unsigned char>((nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
         }
@@ -141,7 +141,7 @@ class SmootherTest : public ::testing::Test
       std::shared_ptr<nav2_costmap_2d::FootprintSubscriber>());
     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<Eigen::Vector3d> short_screwed_path =