Planning: plot ST graph when speed optimizor failed on change lane

qxdxx · Sep 20, 2019 · 5170c31 · 5170c31
1 parent 1f4be95
commit 5170c31
Show file tree

Hide file tree

Showing 4 changed files with 29 additions and 0 deletions.
diff --git a/modules/planning/common/frame.cc b/modules/planning/common/frame.cc
@@ -509,6 +509,17 @@ const ReferenceLineInfo *Frame::FindDriveReferenceLineInfo() {
   return drive_reference_line_info_;
 }
 
+const ReferenceLineInfo *Frame::FindFailedReferenceLineInfo() {
+  for (const auto &reference_line_info : reference_line_info_) {
+    // Find the unsuccessful lane-change path
+    if (!reference_line_info.IsDrivable() &&
+        reference_line_info.IsChangeLanePath()) {
+      return &reference_line_info;
+    }
+  }
+  return nullptr;
+}
+
 const ReferenceLineInfo *Frame::DriveReferenceLineInfo() const {
   return drive_reference_line_info_;
 }

diff --git a/modules/planning/common/frame.h b/modules/planning/common/frame.h
@@ -95,6 +95,8 @@ class Frame {
 
   const ReferenceLineInfo *FindDriveReferenceLineInfo();
 
+  const ReferenceLineInfo *FindFailedReferenceLineInfo();
+
   const ReferenceLineInfo *DriveReferenceLineInfo() const;
 
   const std::vector<const Obstacle *> obstacles() const;

diff --git a/modules/planning/on_lane_planning.cc b/modules/planning/on_lane_planning.cc
@@ -510,6 +510,10 @@ Status OnLanePlanning::Plan(
 
     if (FLAGS_export_chart) {
       ExportOnLaneChart(best_ref_info->debug(), ptr_debug);
+      const auto* failed_ref_info = frame_->FindFailedReferenceLineInfo();
+      if (failed_ref_info) {
+        ExportFailedSTChart(failed_ref_info->debug(), ptr_debug);
+      }
     } else {
       ptr_debug->MergeFrom(best_ref_info->debug());
       ExportReferenceLineDebug(ptr_debug);
@@ -667,6 +671,16 @@ void AddSpeedPlan(
   }
 }
 
+void OnLanePlanning::ExportFailedSTChart(
+    const planning_internal::Debug& debug_info,
+    planning_internal::Debug* debug_chart) {
+  const auto& src_data = debug_info.planning_data();
+  auto* dst_data = debug_chart->mutable_planning_data();
+  for (const auto& st_graph : src_data.st_graph()) {
+    AddSTGraph(st_graph, dst_data->add_chart());
+  }
+}
+
 void OnLanePlanning::ExportOnLaneChart(
     const planning_internal::Debug& debug_info,
     planning_internal::Debug* debug_chart) {

diff --git a/modules/planning/on_lane_planning.h b/modules/planning/on_lane_planning.h
@@ -78,6 +78,8 @@ class OnLanePlanning : public PlanningBase {
   void ExportReferenceLineDebug(planning_internal::Debug* debug);
   bool CheckPlanningConfig(const PlanningConfig& config);
   void GenerateStopTrajectory(ADCTrajectory* ptr_trajectory_pb);
+  void ExportFailedSTChart(const planning_internal::Debug& debug_info,
+                           planning_internal::Debug* debug_chart);
   void ExportOnLaneChart(const planning_internal::Debug& debug_info,
                          planning_internal::Debug* debug_chart);
   void ExportOpenSpaceChart(const planning_internal::Debug& debug_info,