From d427267f3398147778a68ffad688668bd6fb6cf9 Mon Sep 17 00:00:00 2001
From: Jiangtao Hu <boris.hu@gmail.com>
Date: Thu, 27 Jul 2017 14:29:57 -0700
Subject: [PATCH] record planning init point into debug.

---
 modules/planning/planning.cc                   | 14 ++++++++++++--
 modules/planning/proto/planning_internal.proto |  5 +++--
 modules/tools/diagnostics/batch_include.py     |  1 -
 3 files changed, 15 insertions(+), 5 deletions(-)

diff --git a/modules/planning/planning.cc b/modules/planning/planning.cc
index c8be5bf9e4..24908cbb5a 100644
--- a/modules/planning/planning.cc
+++ b/modules/planning/planning.cc
@@ -200,6 +200,11 @@ bool Planning::Plan(const bool is_on_auto_mode, const double publish_time,
     auto matched_info =
         ComputeStartingPointFromLastTrajectory(execution_start_time);
     TrajectoryPoint matched_point = matched_info.first;
+    if (FLAGS_enable_record_debug) {
+      trajectory_pb->mutable_debug()->mutable_planning_data()
+          ->mutable_init_point()->CopyFrom(matched_point);
+    }
+
     std::uint32_t matched_index = matched_info.second;
 
     // Compute the position deviation between current vehicle
@@ -239,10 +244,15 @@ bool Planning::Plan(const bool is_on_auto_mode, const double publish_time,
   //    2. we don't have the trajectory from last planning cycle or
   //    3. the position deviation from actual and target is too high
   // then planning from current vehicle state.
-  TrajectoryPoint vehicle_state_point =
+  TrajectoryPoint vehicle_start_point =
       ComputeStartingPointFromVehicleState(planning_cycle_time);
 
-  auto status = planner_->MakePlan(vehicle_state_point, trajectory_pb);
+  if (FLAGS_enable_record_debug) {
+    trajectory_pb->mutable_debug()->mutable_planning_data()
+        ->mutable_init_point()->CopyFrom(vehicle_start_point);
+  }
+
+  auto status = planner_->MakePlan(vehicle_start_point, trajectory_pb);
   if (!status.ok()) {
     last_trajectory_.clear();
     return false;
diff --git a/modules/planning/proto/planning_internal.proto b/modules/planning/proto/planning_internal.proto
index d59a9d4031..25f2bf05e1 100644
--- a/modules/planning/proto/planning_internal.proto
+++ b/modules/planning/proto/planning_internal.proto
@@ -87,7 +87,7 @@ message PathDebug {
   optional apollo.common.Path path = 2;
 }
 
-// next id: 10
+// next id: 11
 message PlanningData {
   optional apollo.common.Header header = 1;
 
@@ -95,13 +95,14 @@ message PlanningData {
   optional apollo.localization.LocalizationEstimate adc_position = 7;
   optional apollo.canbus.Chassis chassis = 8;
   optional apollo.hdmap.RoutingResult routing = 9;
+  optional apollo.common.TrajectoryPoint init_point = 10;
 
-  optional apollo.localization.Pose init_status = 2;  // initial status of adc
   // processed planning_obstacles
   repeated PlanningObstacle planning_obstacle = 4;
 
   repeated PathDebug path = 6;
 
+  optional apollo.localization.Pose init_status = 2 [deprecated = true];  // initial status of adc
   optional apollo.planning.MainDecision main_decision = 3 [deprecated = true];
   optional LightSignal light_signal = 5 [deprecated = true];
 }
diff --git a/modules/tools/diagnostics/batch_include.py b/modules/tools/diagnostics/batch_include.py
index d5fd7064b1..cf06ac3c0a 100644
--- a/modules/tools/diagnostics/batch_include.py
+++ b/modules/tools/diagnostics/batch_include.py
@@ -19,7 +19,6 @@
 from modules.canbus.proto import chassis_detail_pb2
 from modules.canbus.proto import chassis_pb2
 from modules.common.monitor.proto import monitor_pb2
-from modules.common.configs.proto import config_extrinsics_pb2
 from modules.common.configs.proto import vehicle_config_pb2
 from modules.common.proto import geometry_pb2
 from modules.common.proto import header_pb2
-- 
GitLab