From 7ea0fb4942a22c25c24b78ece65146af48293f9e Mon Sep 17 00:00:00 2001 From: Zhang Liangliang Date: Sat, 23 Dec 2017 11:44:03 -0800 Subject: [PATCH] Planning: added test for GetCentricAccLimit() --- modules/planning/conf/planning_config.pb.txt | 4 +-- .../planning/proto/st_boundary_config.proto | 4 +-- .../tasks/st_graph/st_boundary_mapper.cc | 16 ++++++------ .../tasks/st_graph/st_boundary_mapper.h | 1 + .../tasks/st_graph/st_boundary_mapper_test.cc | 25 +++++++++++++++++++ 5 files changed, 38 insertions(+), 12 deletions(-) diff --git a/modules/planning/conf/planning_config.pb.txt b/modules/planning/conf/planning_config.pb.txt index 8f414df4a6..0c773760e4 100644 --- a/modules/planning/conf/planning_config.pb.txt +++ b/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 diff --git a/modules/planning/proto/st_boundary_config.proto b/modules/planning/proto/st_boundary_config.proto index fbdfa5c0d5..30a6b7b8e0 100644 --- a/modules/planning/proto/st_boundary_config.proto +++ b/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 ]; diff --git a/modules/planning/tasks/st_graph/st_boundary_mapper.cc b/modules/planning/tasks/st_graph/st_boundary_mapper.cc index 0d20e382a5..80e4cd7d41 100644 --- a/modules/planning/tasks/st_graph/st_boundary_mapper.cc +++ b/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{ - centripetal_acceleration_limit, speed_limit_on_reference_line, - nudge_obstacle_speed_limit})); + st_boundary_config_.lowest_speed(), + common::util::MinElement(std::vector{ + 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{ - centripetal_acceleration_limit, speed_limit_on_reference_line})); + st_boundary_config_.lowest_speed(), + common::util::MinElement(std::vector{ + 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) { diff --git a/modules/planning/tasks/st_graph/st_boundary_mapper.h b/modules/planning/tasks/st_graph/st_boundary_mapper.h index 4af9b10e41..7f501027f5 100644 --- a/modules/planning/tasks/st_graph/st_boundary_mapper.h +++ b/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& path_points, diff --git a/modules/planning/tasks/st_graph/st_boundary_mapper_test.cc b/modules/planning/tasks/st_graph/st_boundary_mapper_test.cc index df115371c7..25d3cf3aaf 100644 --- a/modules/planning/tasks/st_graph/st_boundary_mapper_test.cc +++ b/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 -- GitLab