planning: record st graph into debug in case of error.

modules/planning/tasks/dp_st_speed/dp_st_speed_optimizer.cc

@@ -98,6 +98,7 @@ Status DpStSpeedOptimizer::Process(const PathData& path_data,
   if (!st_graph.Search(path_decision, speed_data).ok()) {
     const std::string msg = "Failed to search graph with dynamic programming.";
     AERROR << msg;
+    RecordSTGraphDebug(boundaries, speed_limit, *speed_data);
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
 

modules/planning/tasks/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc

@@ -101,6 +101,7 @@ Status QpSplineStSpeedOptimizer::Process(const PathData& path_data,
   StGraphData st_graph_data(boundaries, init_point, speed_limits,
                             path_data.discretized_path().Length());
   if (st_graph.Search(st_graph_data, path_data, speed_data) != Status::OK()) {
+    RecordSTGraphDebug(boundaries, speed_limits, *speed_data);
     return Status(ErrorCode::PLANNING_ERROR,
                   "Failed to search graph with dynamic programming!");
   }