Planning: added test for GetCentricAccLimit()

modules/planning/conf/planning_config.pb.txt

@@ -132,8 +132,8 @@ em_planner_config {
             boundary_buffer: 0.1
             high_speed_centric_acceleration_limit: 1.2
             low_speed_centric_acceleration_limit: 1.4
-            high_speed_threshold: 20
-            low_speed_threshold: 7
+            high_speed_threshold: 11.18
+            low_speed_threshold: 4.5
             minimal_kappa: 0.00001
             point_extension: 1.0
             lowest_speed: 2.5

modules/planning/proto/st_boundary_config.proto

@@ -4,8 +4,8 @@ package apollo.planning;
 
 message StBoundaryConfig {
   optional double boundary_buffer = 1 [ default = 0.1 ];
-  optional double high_speed_centric_acceleration_limit = 2 [ default = 1.0 ];
-  optional double low_speed_centric_acceleration_limit = 3 [ default = 1.3 ];
+  optional double high_speed_centric_acceleration_limit = 2 [ default = 1.2 ];
+  optional double low_speed_centric_acceleration_limit = 3 [ default = 1.4 ];
   optional double high_speed_threshold = 4 [ default =  20.0 ];
   optional double low_speed_threshold = 5 [ default = 7.0 ];
   optional double minimal_kappa = 6 [ default = 0.00001 ];

modules/planning/tasks/st_graph/st_boundary_mapper.cc

@@ -539,15 +539,15 @@ Status StBoundaryMapper::GetSpeedLimits(
     double curr_speed_limit = 0.0;
     if (FLAGS_enable_nudge_slowdown) {
       curr_speed_limit = std::fmax(
-        st_boundary_config_.lowest_speed(),
-        common::util::MinElement(std::vector<double>{
-            centripetal_acceleration_limit, speed_limit_on_reference_line,
-            nudge_obstacle_speed_limit}));
+          st_boundary_config_.lowest_speed(),
+          common::util::MinElement(std::vector<double>{
+              centripetal_acceleration_limit, speed_limit_on_reference_line,
+              nudge_obstacle_speed_limit}));
     } else {
       curr_speed_limit = std::fmax(
-        st_boundary_config_.lowest_speed(),
-        common::util::MinElement(std::vector<double>{
-            centripetal_acceleration_limit, speed_limit_on_reference_line}));
+          st_boundary_config_.lowest_speed(),
+          common::util::MinElement(std::vector<double>{
+              centripetal_acceleration_limit, speed_limit_on_reference_line}));
     }
 
     speed_limit_data->AppendSpeedLimit(path_s, curr_speed_limit);
@@ -586,7 +586,7 @@ double StBoundaryMapper::GetCentricAccLimit(const double kappa) const {
   const double k1 = (h_v_acc - l_v_acc) / (v_high - v_low);
   const double k2 = h_v_acc - v_high * k1;
 
-  const double v = (k1 + std::sqrt(k1 * k1 + 4 * kappa * k2)) / (2 * kappa);
+  const double v = (k1 + std::sqrt(k1 * k1 + 4.0 * kappa * k2)) / (2.0 * kappa);
   ADEBUG << "v = " << v;
 
   if (v > v_high) {

modules/planning/tasks/st_graph/st_boundary_mapper.h

@@ -76,6 +76,7 @@ class StBoundaryMapper {
   apollo::common::Status MapWithPredictionTrajectory(
       PathObstacle* path_obstacle) const;
 
+  FRIEND_TEST(StBoundaryMapperTest, get_centric_acc_limit);
   double GetCentricAccLimit(const double kappa) const;
 
   void GetAvgKappa(const std::vector<common::PathPoint>& path_points,

modules/planning/tasks/st_graph/st_boundary_mapper_test.cc

@@ -93,5 +93,30 @@ TEST_F(StBoundaryMapperTest, check_overlap_test) {
   EXPECT_TRUE(mapper.CheckOverlap(path_point, box, 0.0));
 }
 
+TEST_F(StBoundaryMapperTest, get_centric_acc_limit) {
+  StBoundaryConfig config;
+  double planning_distance = 250.0;
+  double planning_time = 8.0;
+  SLBoundary adc_sl_boundary;
+  StBoundaryMapper mapper(adc_sl_boundary, config, *reference_line_, path_data_,
+                          planning_distance, planning_time);
+
+  double kappa = 0.0001;
+  while (kappa < 0.2) {
+    const double acc_limit = mapper.GetCentricAccLimit(kappa);
+    const double v = std::sqrt(acc_limit / kappa);
+    if (v > config.high_speed_threshold()) {
+      EXPECT_DOUBLE_EQ(acc_limit,
+                       config.high_speed_centric_acceleration_limit());
+    } else if (v < config.low_speed_threshold()) {
+      EXPECT_DOUBLE_EQ(acc_limit,
+                       config.low_speed_centric_acceleration_limit());
+    } else {
+      EXPECT_LE(acc_limit, config.low_speed_centric_acceleration_limit());
+      EXPECT_GE(acc_limit, config.high_speed_centric_acceleration_limit());
+    }
+    kappa += 0.0001;
+  }
+}
 }  // namespace planning
 }  // namespace apollo