planning: move speed/path debug info into optimizer and fix unit test.

modules/planning/optimizer/path_optimizer.cc

@@ -28,9 +28,21 @@ PathOptimizer::PathOptimizer(const std::string& name) : Optimizer(name) {}
 apollo::common::Status PathOptimizer::Optimize(Frame* frame) {
   frame_ = frame;
   auto* planning_data = frame->mutable_planning_data();
-  return Process(planning_data->speed_data(), frame->reference_line(),
-                 frame->PlanningStartPoint(), frame->path_decision(),
-                 planning_data->mutable_path_data());
+  auto ret = Process(planning_data->speed_data(), frame->reference_line(),
+                     frame->PlanningStartPoint(), frame->path_decision(),
+                     planning_data->mutable_path_data());
+  RecordDebugInfo(planning_data->path_data());
+
+  return ret;
+}
+
+void PathOptimizer::RecordDebugInfo(const PathData& path_data) {
+  auto debug = frame_->MutableADCTrajectory()->mutable_debug();
+  const auto& path_points = path_data.discretized_path().path_points();
+  auto ptr_optimized_path = debug->mutable_planning_data()->add_path();
+  ptr_optimized_path->set_name(name());
+  ptr_optimized_path->mutable_path_point()->CopyFrom(
+      {path_points.begin(), path_points.end()});
 }
 
 }  // namespace planning

modules/planning/optimizer/path_optimizer.h

@@ -45,6 +45,8 @@ class PathOptimizer : public Optimizer {
       const SpeedData &speed_data, const ReferenceLine &reference_line,
       const common::TrajectoryPoint &init_point,
       PathDecision *const path_decision, PathData *const path_data) = 0;
+
+  void RecordDebugInfo(const PathData &path_data);
 };
 
 }  // namespace planning

modules/planning/optimizer/speed_optimizer.cc

@@ -32,9 +32,20 @@ SpeedOptimizer::SpeedOptimizer(const std::string& name) : Optimizer(name) {}
 apollo::common::Status SpeedOptimizer::Optimize(Frame* frame) {
   frame_ = frame;
   auto* planning_data = frame->mutable_planning_data();
-  return Process(planning_data->path_data(), frame->PlanningStartPoint(),
+  auto ret = Process(planning_data->path_data(), frame->PlanningStartPoint(),
                  frame->reference_line(), frame->path_decision(),
                  planning_data->mutable_speed_data());
+  RecordDebugInfo(planning_data->speed_data());
+  return ret;
+}
+
+void SpeedOptimizer::RecordDebugInfo(const SpeedData& speed_data) {
+  auto debug = frame_->MutableADCTrajectory()->mutable_debug();
+  auto ptr_speed_plan = debug->mutable_planning_data()->add_speed_plan();
+  ptr_speed_plan->set_name(name());
+  ptr_speed_plan->mutable_speed_point()->CopyFrom(
+    {speed_data.speed_vector().begin(),
+     speed_data.speed_vector().end()});
 }
 
 void SpeedOptimizer::RecordSTGraphDebug(

modules/planning/optimizer/speed_optimizer.h

@@ -49,6 +49,8 @@ class SpeedOptimizer : public Optimizer {
   void RecordSTGraphDebug(const std::vector<StGraphBoundary>& boundaries,
     const SpeedLimit& speed_limits, const SpeedData& speed_data);
 
+  void RecordDebugInfo(const SpeedData& speed_data);
+
 };
 
 }  // namespace planning

modules/planning/planner/em/em_planner.cc

@@ -87,7 +87,6 @@ Status EMPlanner::Init(const PlanningConfig& config) {
 }
 
 void EMPlanner::RecordDebugInfo(const std::string& name,
-                                const PlanningData* planning_data,
                                 const double time_diff_ms,
                                 planning_internal::Debug* ptr_debug,
                                 planning::LatencyStats* ptr_latency_stats) {
@@ -97,21 +96,6 @@ void EMPlanner::RecordDebugInfo(const std::string& name,
   }
   OptimizerType type;
   DCHECK(OptimizerType_Parse(name, &type));
-  if (type == DP_POLY_PATH_OPTIMIZER || type == QP_SPLINE_PATH_OPTIMIZER) {
-    const auto& path_points =
-        planning_data->path_data().discretized_path().path_points();
-    auto ptr_optimized_path = ptr_debug->mutable_planning_data()->add_path();
-    ptr_optimized_path->set_name(name);
-    ptr_optimized_path->mutable_path_point()->CopyFrom(
-        {path_points.begin(), path_points.end()});
-  } else if (type == DP_ST_SPEED_OPTIMIZER ||
-             type == QP_SPLINE_ST_SPEED_OPTIMIZER) {
-    const auto& speed_points = planning_data->speed_data().speed_vector();
-    auto ptr_speed_plan = ptr_debug->mutable_planning_data()->add_speed_plan();
-    ptr_speed_plan->set_name(name);
-    ptr_speed_plan->mutable_speed_point()->CopyFrom(
-        {speed_points.begin(), speed_points.end()});
-  }
 
   auto ptr_stats = ptr_latency_stats->add_processor_stats();
   ptr_stats->set_name(name);
@@ -151,7 +135,7 @@ Status EMPlanner::Plan(const TrajectoryPoint& planning_start_point,
 
     if (FLAGS_enable_record_debug && ptr_debug != nullptr &&
         ptr_latency_stats != nullptr) {
-      RecordDebugInfo(optimizer->name(), planning_data, time_diff_ms, ptr_debug,
+      RecordDebugInfo(optimizer->name(), time_diff_ms, ptr_debug,
                       ptr_latency_stats);
     }
   }

modules/planning/planner/em/em_planner.h

@@ -77,7 +77,6 @@ class EMPlanner : public Planner {
   void PopulateDecision(Frame* frame);
 
   void RecordDebugInfo(const std::string& name,
-                       const PlanningData* planning_data,
                        const double time_diff_ms,
                        planning_internal::Debug* ptr_debug,
                        planning::LatencyStats* ptr_latency_stats);