From ac502568806575677de98f03b25fe88f584d7bc9 Mon Sep 17 00:00:00 2001
From: Jiangtao Hu <boris.hu@gmail.com>
Date: Thu, 27 Jul 2017 13:44:51 -0700
Subject: [PATCH] record planning input into debug pb.

---
 modules/planning/common/planning_gflags.cc    |  2 ++
 modules/planning/common/planning_gflags.h     |  2 ++
 modules/planning/planning.cc                  | 26 ++++++++++++++++++-
 modules/planning/planning.h                   |  1 +
 modules/planning/proto/BUILD                  |  1 +
 .../planning/proto/planning_internal.proto    | 10 ++++++-
 6 files changed, 40 insertions(+), 2 deletions(-)

diff --git a/modules/planning/common/planning_gflags.cc b/modules/planning/common/planning_gflags.cc
index afd2ba0bec..1604ac0364 100644
--- a/modules/planning/common/planning_gflags.cc
+++ b/modules/planning/common/planning_gflags.cc
@@ -311,3 +311,5 @@ DEFINE_double(backward_routing_distance, 100.0,
               "The backward routing distance.");
 DEFINE_double(decision_valid_stop_range, 0.5,
               "The valid stop range in decision.");
+DEFINE_bool(enable_record_debug, true,
+            "True to enable record debug into debug protobuf.");
diff --git a/modules/planning/common/planning_gflags.h b/modules/planning/common/planning_gflags.h
index 4adea59f78..3bde74e9ca 100644
--- a/modules/planning/common/planning_gflags.h
+++ b/modules/planning/common/planning_gflags.h
@@ -214,4 +214,6 @@ DECLARE_string(offline_routing_file);
 DECLARE_double(backward_routing_distance);
 DECLARE_double(decision_valid_stop_range);
 
+DECLARE_bool(enable_record_debug);
+
 #endif  // MODULES_PLANNING_COMMON_PLANNING_GFLAGS_H_
diff --git a/modules/planning/planning.cc b/modules/planning/planning.cc
index 64926df052..c8be5bf9e4 100644
--- a/modules/planning/planning.cc
+++ b/modules/planning/planning.cc
@@ -105,6 +105,28 @@ Status Planning::Start() {
   return Status::OK();
 }
 
+void Planning:: RecordInput(ADCTrajectory* trajectory_pb) {
+  if (!FLAGS_enable_record_debug) {
+    ADEBUG << "Skip record input into debug";
+    return;
+  }
+  auto planning_data = trajectory_pb->mutable_debug()->mutable_planning_data();
+  auto adc_position = planning_data->mutable_adc_position();
+  const auto& localization =
+      AdapterManager::GetLocalization()->GetLatestObserved();
+  adc_position->CopyFrom(localization);
+
+  const auto& chassis = AdapterManager::GetChassis()->GetLatestObserved();
+  auto debug_chassis= planning_data->mutable_chassis();
+  debug_chassis->CopyFrom(chassis);
+
+  const auto &routing_result =
+      AdapterManager::GetRoutingResult()->GetLatestObserved();
+
+  auto debug_routing = planning_data->mutable_routing();
+  debug_routing->CopyFrom(routing_result);
+}
+
 void Planning::RunOnce() {
   AdapterManager::Observe();
   if (AdapterManager::GetLocalization()->Empty()) {
@@ -139,13 +161,15 @@ void Planning::RunOnce() {
       apollo::common::time::ToSecond(apollo::common::time::Clock::Now()) +
       planning_cycle_time;
 
+  ADCTrajectory trajectory_pb;
+  RecordInput(&trajectory_pb);
+
   if (!DataCenter::instance()->init_current_frame(
           AdapterManager::GetPlanning()->GetSeqNum() + 1)) {
     AERROR << "DataCenter init frame failed";
     return;
   }
 
-  ADCTrajectory trajectory_pb;
   bool is_auto_mode = chassis.driving_mode() == chassis.COMPLETE_AUTO_DRIVE;
   bool res_planning = Plan(is_auto_mode, execution_start_time, &trajectory_pb);
   if (res_planning) {
diff --git a/modules/planning/planning.h b/modules/planning/planning.h
index 32c5b74b3c..5c0b7e4c2c 100644
--- a/modules/planning/planning.h
+++ b/modules/planning/planning.h
@@ -86,6 +86,7 @@ class Planning : public apollo::common::ApolloApp {
  private:
   void RegisterPlanners();
   void RunOnce();
+  void RecordInput(ADCTrajectory* trajectory_pb);
 
   std::pair<common::TrajectoryPoint, std::uint32_t>
   ComputeStartingPointFromLastTrajectory(const double curr_time) const;
diff --git a/modules/planning/proto/BUILD b/modules/planning/proto/BUILD
index 26da0b1839..20df34f777 100644
--- a/modules/planning/proto/BUILD
+++ b/modules/planning/proto/BUILD
@@ -21,6 +21,7 @@ cc_proto_library(
         "//modules/common/proto:path_point_proto",
         "//modules/decision/proto:decision_proto",
         "//modules/localization/proto:localization_proto",
+        "//modules/map/proto:map_proto",
         "//modules/perception/proto:perception_proto",
         "//modules/prediction/proto:prediction_proto",
     ],
diff --git a/modules/planning/proto/planning_internal.proto b/modules/planning/proto/planning_internal.proto
index 368dc39b46..d59a9d4031 100644
--- a/modules/planning/proto/planning_internal.proto
+++ b/modules/planning/proto/planning_internal.proto
@@ -2,11 +2,13 @@ syntax = "proto2";
 
 package apollo.planning_internal;
 
+import "modules/canbus/proto/chassis.proto";
 import "modules/common/proto/header.proto";
 import "modules/common/proto/error_code.proto";
 import "modules/common/proto/path_point.proto";
 import "modules/localization/proto/localization.proto";
 import "modules/localization/proto/pose.proto";
+import "modules/map/proto/routing.proto";
 import "modules/perception/proto/perception_obstacle.proto";
 import "modules/planning/proto/decision.proto";
 import "modules/prediction/proto/prediction_obstacle.proto";
@@ -85,9 +87,15 @@ message PathDebug {
   optional apollo.common.Path path = 2;
 }
 
-// next id: 7
+// next id: 10
 message PlanningData {
   optional apollo.common.Header header = 1;
+
+  // input
+  optional apollo.localization.LocalizationEstimate adc_position = 7;
+  optional apollo.canbus.Chassis chassis = 8;
+  optional apollo.hdmap.RoutingResult routing = 9;
+
   optional apollo.localization.Pose init_status = 2;  // initial status of adc
   // processed planning_obstacles
   repeated PlanningObstacle planning_obstacle = 4;
-- 
GitLab