add st graph debug into debug pb.

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc

@@ -59,6 +59,32 @@ bool QpSplineStSpeedOptimizer::Init() {
   return true;
 }
 
+void QpSplineStSpeedOptimizer::RecordSTGraphDebug(
+    const std::vector<StGraphBoundary>& boundaries,
+    const SpeedLimit& speed_limits) {
+  if (!FLAGS_enable_record_debug) {
+    ADEBUG << "Skip record debug info";
+    return;
+  }
+
+  auto debug = frame_->MutableADCTrajectory()->mutable_debug();
+  auto st_graph_debug = debug->mutable_planning_data()->add_st_graph();
+  st_graph_debug->set_name(name());
+  for (const auto& boundary : boundaries) {
+    auto boundary_debug = st_graph_debug->add_boundary();
+    boundary_debug->set_name(boundary.id());
+    for (const auto& point : boundary.points()) {
+      auto point_debug = boundary_debug->add_point();
+      point_debug->set_t(point.x());
+      point_debug->set_s(point.y());
+    }
+  }
+
+  st_graph_debug->mutable_speed_limit()->CopyFrom(
+      {speed_limits.speed_points().begin(),
+       speed_limits.speed_points().end()});
+}
+
 Status QpSplineStSpeedOptimizer::Process(const PathData& path_data,
                                          const TrajectoryPoint& init_point,
                                          const ReferenceLine& reference_line,
@@ -88,6 +114,7 @@ Status QpSplineStSpeedOptimizer::Process(const PathData& path_data,
     return Status(ErrorCode::PLANNING_ERROR,
                   "Mapping obstacle for dp st speed optimizer failed!");
   }
+  RecordSTGraphDebug(boundaries, speed_limits);
 
   // step 2 perform graph search
   const auto& veh_param =

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.h

@@ -45,6 +45,10 @@ class QpSplineStSpeedOptimizer : public SpeedOptimizer {
                          const ReferenceLine& reference_line,
                          DecisionData* const decision_data,
                          SpeedData* const speed_data) override;
+
+  void RecordSTGraphDebug(const std::vector<StGraphBoundary>& boundaries,
+                          const SpeedLimit& speed_limits);
+
   QpSplineStSpeedConfig qp_spline_st_speed_config_;
   StBoundaryConfig st_boundary_config_;
 };

modules/planning/planner/em/em_planner.cc

@@ -88,6 +88,10 @@ void EMPlanner::RecordDebugInfo(const std::string& name,
                                 const double time_diff_ms,
                                 planning_internal::Debug* ptr_debug,
                                 planning::LatencyStats* ptr_latency_stats) {
+  if (!FLAGS_enable_record_debug) {
+    ADEBUG << "Skip record debug info";
+    return;
+  }
   OptimizerType type;
   DCHECK(OptimizerType_Parse(name, &type));
   if (type == DP_POLY_PATH_OPTIMIZER || type == QP_SPLINE_PATH_OPTIMIZER) {

modules/planning/proto/planning_internal.proto

@@ -92,14 +92,10 @@ message StGraphBoundaryDebug {
   repeated apollo.common.SpeedPoint point = 2;
 }
 
-message SpeedLimitDebug {
-  repeated apollo.common.SpeedPoint point = 1;
-}
-
 message STGraphDebug {
   optional string name = 1;
   repeated StGraphBoundaryDebug boundary = 2;
-  optional SpeedLimitDebug speed_limit = 3;
+  repeated apollo.common.SpeedPoint speed_limit = 3;
 }
 
 // next id: 15