Planning: moved slowdown profile generation to speed_optimizer.cc

modules/planning/tasks/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc

@@ -110,40 +110,13 @@ Status QpSplineStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
       Status::OK()) {
     std::string msg = common::util::StrCat(
         Name(), ":Failed to search graph with quadratic programming!");
-
-    if (FLAGS_enable_slowdown_profile_generator &&
-        init_point.v() > FLAGS_slowdown_speed_threshold) {
-      GenerateStopProfile(init_point.v(), speed_data);
-    } else {
     RecordSTGraphDebug(boundaries, speed_limits, *speed_data, st_graph_debug);
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
-  }
   // record debug info
   RecordSTGraphDebug(boundaries, speed_limits, *speed_data, st_graph_debug);
   return Status::OK();
 }
 
-void QpSplineStSpeedOptimizer::GenerateStopProfile(
-    const double init_speed, SpeedData* const speed_data) const {
-  AERROR << "Slowing down the car.";
-  *speed_data = SpeedData();
-
-  const double min_acc = FLAGS_slowdown_profile_deceleration;
-  const size_t max_t = 3.0;
-  const double unit_t = 0.02;
-
-  double pre_s = 0.0;
-  double t = 0.0;
-
-  while (t < max_t) {
-    const double s = std::fmax(pre_s, init_speed * t + 0.5 * min_acc * t * t);
-    const double v = std::fmax(0.0, init_speed + min_acc * t);
-    speed_data->AppendSpeedPoint(s, t, v, min_acc, 0.0);
-    pre_s = s;
-    t += unit_t;
-  }
-}
-
 }  // namespace planning
 }  // namespace apollo

modules/planning/tasks/speed_optimizer.cc

@@ -38,10 +38,37 @@ apollo::common::Status SpeedOptimizer::Execute(
       frame->PlanningStartPoint(), reference_line_info->reference_line(),
       reference_line_info->path_decision(),
       reference_line_info->mutable_speed_data());
+
+  if (!ret.ok() && FLAGS_enable_slowdown_profile_generator &&
+      frame->PlanningStartPoint().v() > FLAGS_slowdown_speed_threshold) {
+    *reference_line_info->mutable_speed_data() =
+        GenerateStopProfile(frame->PlanningStartPoint().v());
+  }
   RecordDebugInfo(reference_line_info->speed_data());
   return ret;
 }
 
+SpeedData SpeedOptimizer::GenerateStopProfile(const double init_speed) const {
+  AERROR << "Slowing down the car.";
+  SpeedData speed_data;
+
+  const double min_acc = FLAGS_slowdown_profile_deceleration;
+  const size_t max_t = 3.0;
+  const double unit_t = 0.02;
+
+  double pre_s = 0.0;
+  double t = 0.0;
+
+  while (t < max_t) {
+    const double s = std::fmax(pre_s, init_speed * t + 0.5 * min_acc * t * t);
+    const double v = std::fmax(0.0, init_speed + min_acc * t);
+    speed_data.AppendSpeedPoint(s, t, v, min_acc, 0.0);
+    pre_s = s;
+    t += unit_t;
+  }
+  return speed_data;
+}
+
 void SpeedOptimizer::RecordDebugInfo(const SpeedData& speed_data) {
   auto debug = frame_->MutableADCTrajectory()->mutable_debug();
   auto ptr_speed_plan = debug->mutable_planning_data()->add_speed_plan();

modules/planning/tasks/speed_optimizer.h

@@ -26,8 +26,8 @@
 
 #include "modules/common/status/status.h"
 #include "modules/planning/common/speed/speed_data.h"
-#include "modules/planning/common/speed_limit.h"
 #include "modules/planning/common/speed/st_boundary.h"
+#include "modules/planning/common/speed_limit.h"
 #include "modules/planning/tasks/task.h"
 
 namespace apollo {
@@ -47,8 +47,10 @@ class SpeedOptimizer : public Task {
       const ReferenceLine& reference_line, PathDecision* const path_decision,
       SpeedData* const speed_data) = 0;
 
-  void RecordSTGraphDebug(const std::vector<StBoundary>& boundaries,
-                          const SpeedLimit& speed_limits,
+  SpeedData GenerateStopProfile(const double init_speed) const;
+
+  void RecordSTGraphDebug(
+      const std::vector<StBoundary>& boundaries, const SpeedLimit& speed_limits,
       const SpeedData& speed_data,
       ::apollo::planning_internal::STGraphDebug* stGraphDebug);