From 3fd5a554e50e4a44f88e90731b7fd908597a61e7 Mon Sep 17 00:00:00 2001
From: Zhang Liangliang <zhangliangliang@baidu.com>
Date: Thu, 21 Dec 2017 09:06:15 -0800
Subject: [PATCH] Planning: use configurable params for nudge speed.

---
 modules/planning/conf/planning_config.pb.txt       |  4 ++++
 modules/planning/proto/st_boundary_config.proto    |  2 ++
 .../planning/tasks/st_graph/st_boundary_mapper.cc  | 14 ++++++++++++--
 3 files changed, 18 insertions(+), 2 deletions(-)

diff --git a/modules/planning/conf/planning_config.pb.txt b/modules/planning/conf/planning_config.pb.txt
index 7e5da507f4..c2ea457c67 100644
--- a/modules/planning/conf/planning_config.pb.txt
+++ b/modules/planning/conf/planning_config.pb.txt
@@ -72,6 +72,8 @@ em_planner_config {
             minimal_kappa: 0.00001
             point_extension: 1.0
             lowest_speed: 2.5
+            static_obs_nudge_speed_ratio: 0.6
+            dynamic_obs_nudge_speed_ratio: 0.8
         }
     }
     qp_spline_path_config {
@@ -136,6 +138,8 @@ em_planner_config {
             point_extension: 1.0
             lowest_speed: 2.5
             num_points_to_avg_kappa: 2
+            static_obs_nudge_speed_ratio: 0.6
+            dynamic_obs_nudge_speed_ratio: 0.8
         }
     }
 }
diff --git a/modules/planning/proto/st_boundary_config.proto b/modules/planning/proto/st_boundary_config.proto
index 1f79de6275..fbdfa5c0d5 100644
--- a/modules/planning/proto/st_boundary_config.proto
+++ b/modules/planning/proto/st_boundary_config.proto
@@ -12,4 +12,6 @@ message StBoundaryConfig {
   optional double point_extension = 7 [ default = 1.0 ];
   optional double lowest_speed = 8 [ default = 2.5 ];
   optional uint32 num_points_to_avg_kappa = 9 [ default = 4 ];
+  optional double static_obs_nudge_speed_ratio = 10;
+  optional double dynamic_obs_nudge_speed_ratio = 11;
 }
diff --git a/modules/planning/tasks/st_graph/st_boundary_mapper.cc b/modules/planning/tasks/st_graph/st_boundary_mapper.cc
index 16e3a767cb..0d20e382a5 100644
--- a/modules/planning/tasks/st_graph/st_boundary_mapper.cc
+++ b/modules/planning/tasks/st_graph/st_boundary_mapper.cc
@@ -503,6 +503,9 @@ Status StBoundaryMapper::GetSpeedLimits(
     // speed limit from nudge obstacles
     double nudge_obstacle_speed_limit = std::numeric_limits<double>::max();
     for (const auto* const_path_obstacle : path_obstacles.Items()) {
+      if (const_path_obstacle->obstacle()->IsVirtual()) {
+        continue;
+      }
       if (!const_path_obstacle->LateralDecision().has_nudge()) {
         continue;
       }
@@ -519,9 +522,16 @@ Status StBoundaryMapper::GetSpeedLimits(
           (nudge.type() == ObjectNudge::RIGHT_NUDGE) &&
           (const_path_obstacle->perception_sl_boundary().start_l() < kRange);
       if (is_close_on_left || is_close_on_right) {
-        constexpr double kNudgeSpeedRatio = 0.6;
+        double nudge_speed_ratio = 1.0;
+        if (const_path_obstacle->obstacle()->IsStatic()) {
+          nudge_speed_ratio =
+              st_boundary_config_.static_obs_nudge_speed_ratio();
+        } else {
+          nudge_speed_ratio =
+              st_boundary_config_.dynamic_obs_nudge_speed_ratio();
+        }
         nudge_obstacle_speed_limit =
-            kNudgeSpeedRatio * speed_limit_on_reference_line;
+            nudge_speed_ratio * speed_limit_on_reference_line;
         break;
       }
     }
-- 
GitLab