Planning: set speed limit to avoid creeping forward.

qxdxx · Aug 27, 2018 · 31735f0 · 31735f0
1 parent 5d19e87
commit 31735f0
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 18 deletions.
diff --git a/modules/planning/common/planning_gflags.cc b/modules/planning/common/planning_gflags.cc
@@ -373,8 +373,8 @@ DEFINE_bool(use_planning_fallback, true,
 DEFINE_double(fallback_total_time, 3.0, "total fallback trajectory time");
 DEFINE_double(fallback_time_unit, 0.02,
               "fallback trajectory unit time in seconds");
-DEFINE_bool(enable_polynomial_speed_fallback, false,
-              "True to enable polynomial speed fallback.");
+DEFINE_double(polynomial_speed_fallback_velocity, 3.5,
+              "velocity to use polynomial speed fallback.");
 
 // navigation mode
 DEFINE_double(navigation_fallback_cruise_time, 8.0,

diff --git a/modules/planning/common/planning_gflags.h b/modules/planning/common/planning_gflags.h
@@ -199,7 +199,7 @@ DECLARE_double(bound_buffer);
 DECLARE_bool(use_planning_fallback);
 DECLARE_double(fallback_total_time);
 DECLARE_double(fallback_time_unit);
-DECLARE_bool(enable_polynomial_speed_fallback);
+DECLARE_double(polynomial_speed_fallback_velocity);
 
 // navigation mode
 DECLARE_double(navigation_fallback_cruise_time);

diff --git a/modules/planning/planner/em/em_planner.cc b/modules/planning/planner/em/em_planner.cc
@@ -216,8 +216,9 @@ Status EMPlanner::PlanOnReferenceLine(
 
   if (!ret.ok() || reference_line_info->speed_data().Empty()) {
     ADEBUG << "Speed fallback.";
-    GenerateFallbackSpeedProfile(reference_line_info,
-                                 reference_line_info->mutable_speed_data());
+
+    *reference_line_info->mutable_speed_data() =
+        GenerateFallbackSpeedProfile(*reference_line_info);
     reference_line_info->AddCost(kSpeedOptimizationFallbackClost);
   }
 
@@ -399,16 +400,14 @@ void EMPlanner::GenerateFallbackPathProfile(
   path_data->SetDiscretizedPath(DiscretizedPath(std::move(path_points)));
 }
 
-void EMPlanner::GenerateFallbackSpeedProfile(
-    const ReferenceLineInfo* reference_line_info, SpeedData* speed_data) {
-  if (FLAGS_enable_polynomial_speed_fallback) {
-    *speed_data = GenerateStopProfileFromPolynomial(
-      reference_line_info->AdcPlanningPoint().v(),
-      reference_line_info->AdcPlanningPoint().a());
+SpeedData EMPlanner::GenerateFallbackSpeedProfile(
+    const ReferenceLineInfo& reference_line_info) {
+  const double init_v = reference_line_info.AdcPlanningPoint().v();
+  const double init_a = reference_line_info.AdcPlanningPoint().a();
+  if (init_v > FLAGS_polynomial_speed_fallback_velocity) {
+    return GenerateStopProfileFromPolynomial(init_v, init_a);
   } else {
-    *speed_data =
-        GenerateStopProfile(reference_line_info->AdcPlanningPoint().v(),
-                            reference_line_info->AdcPlanningPoint().a());
+    return GenerateStopProfile(init_v, init_a);
   }
 }
 
@@ -429,8 +428,7 @@ SpeedData EMPlanner::GenerateStopProfile(const double init_speed,
     double v = 0.0;
     s = std::fmax(pre_s,
                   pre_s + 0.5 * (pre_v + (pre_v + unit_t * acc)) * unit_t);
-    v = std::fmax(0.0,
-                  pre_v + unit_t * acc);
+    v = std::fmax(0.0, pre_v + unit_t * acc);
     speed_data.AppendSpeedPoint(s, t, v, acc, 0.0);
     pre_s = s;
     pre_v = v;

diff --git a/modules/planning/planner/em/em_planner.h b/modules/planning/planner/em/em_planner.h
@@ -97,8 +97,8 @@ class EMPlanner : public Planner {
   void GenerateFallbackPathProfile(const ReferenceLineInfo* reference_line_info,
                                    PathData* path_data);
 
-  void GenerateFallbackSpeedProfile(
-      const ReferenceLineInfo* reference_line_info, SpeedData* speed_data);
+  SpeedData GenerateFallbackSpeedProfile(
+      const ReferenceLineInfo& reference_line_info);
 
   SpeedData GenerateStopProfile(const double init_speed,
                                 const double init_acc) const;