planning: move st graph debug into speed optimizer to collect all ST graph data.

modules/planning/optimizer/BUILD

@@ -50,8 +50,12 @@ cc_library(
         "//modules/common/status",
         "//modules/planning/common:path_decision",
         "//modules/planning/common:planning_data",
+        "//modules/planning/common:planning_gflags",
+        "//modules/planning/common:speed_limit",
         "//modules/planning/common/path:path_data",
         "//modules/planning/common/speed:speed_data",
+        "//modules/planning/optimizer/st_graph:st_graph_boundary",
+        "//modules/planning/optimizer/st_graph:st_graph_point",
         "@eigen//:eigen",
     ],
 )

modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc

@@ -99,6 +99,9 @@ Status DpStSpeedOptimizer::Process(const PathData& path_data,
     AERROR << msg;
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
+
+  RecordSTGraphDebug(boundaries, speed_limit, *speed_data);
+
   return Status::OK();
 }
 

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc

@@ -61,38 +61,6 @@ bool QpSplineStSpeedOptimizer::Init() {
   return true;
 }
 
-void QpSplineStSpeedOptimizer::RecordSTGraphDebug(
-    const std::vector<StGraphBoundary>& boundaries,
-    const SpeedLimit& speed_limits, const SpeedData& speed_data) {
-  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());
-    }
-  }
-
-  for (const auto& point : speed_limits.speed_limit_points()) {
-    common::SpeedPoint speed_point;
-    speed_point.set_s(point.first);
-    speed_point.set_v(point.second);
-    st_graph_debug->add_speed_limit()->CopyFrom(speed_point);
-  }
-
-  st_graph_debug->mutable_speed_profile()->CopyFrom(
-      {speed_data.speed_vector().begin(), speed_data.speed_vector().end()});
-}
-
 Status QpSplineStSpeedOptimizer::Process(const PathData& path_data,
                                          const TrajectoryPoint& init_point,
                                          const ReferenceLine& reference_line,

modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.h

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

modules/planning/optimizer/speed_optimizer.cc

@@ -19,6 +19,8 @@
  **/
 
 #include "modules/planning/optimizer/speed_optimizer.h"
+#include "modules/planning/common/planning_gflags.h"
+#include "modules/planning/common/speed_limit.h"
 
 #include <string>
 
@@ -35,5 +37,38 @@ apollo::common::Status SpeedOptimizer::Optimize(Frame* frame) {
                  planning_data->mutable_speed_data());
 }
 
+void SpeedOptimizer::RecordSTGraphDebug(
+    const std::vector<StGraphBoundary>& boundaries,
+    const SpeedLimit& speed_limits, const SpeedData& speed_data) {
+  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());
+    }
+  }
+
+  for (const auto& point : speed_limits.speed_limit_points()) {
+    common::SpeedPoint speed_point;
+    speed_point.set_s(point.first);
+    speed_point.set_v(point.second);
+    st_graph_debug->add_speed_limit()->CopyFrom(speed_point);
+  }
+
+  st_graph_debug->mutable_speed_profile()->CopyFrom(
+      {speed_data.speed_vector().begin(), speed_data.speed_vector().end()});
+}
+
+
 }  // namespace planning
 }  // namespace apollo

modules/planning/optimizer/speed_optimizer.h

@@ -26,10 +26,14 @@
 #include "modules/planning/optimizer/optimizer.h"
 
 #include "modules/common/status/status.h"
+#include "modules/planning/optimizer/st_graph/st_graph_boundary.h"
 
 namespace apollo {
 namespace planning {
 
+class SpeedLimit;
+class SpeedData;
+
 class SpeedOptimizer : public Optimizer {
  public:
   explicit SpeedOptimizer(const std::string& name);
@@ -41,6 +45,10 @@ class SpeedOptimizer : public Optimizer {
       const PathData& path_data, const common::TrajectoryPoint& init_point,
       const ReferenceLine& reference_line, PathDecision* const path_decision,
       SpeedData* const speed_data) = 0;
+
+  void RecordSTGraphDebug(const std::vector<StGraphBoundary>& boundaries,
+    const SpeedLimit& speed_limits, const SpeedData& speed_data);
+
 };
 
 }  // namespace planning