From 26ae1720f57ef41ddea233eb7d89074c9a223cd0 Mon Sep 17 00:00:00 2001
From: jmtao <taojm99@gmail.com>
Date: Mon, 20 Jul 2020 18:03:37 -0700
Subject: [PATCH] planning: publish a new topic msg to transfer learning data
 to RL python module

---
 modules/planning/conf/planning_config.pb.txt |  3 ++-
 modules/planning/planning_component.cc       | 23 +++++++++++++++++++-
 modules/planning/planning_component.h        |  3 +++
 modules/planning/proto/planning_config.proto |  2 ++
 4 files changed, 29 insertions(+), 2 deletions(-)

diff --git a/modules/planning/conf/planning_config.pb.txt b/modules/planning/conf/planning_config.pb.txt
index d6e118c51b..f295cbe23d 100644
--- a/modules/planning/conf/planning_config.pb.txt
+++ b/modules/planning/conf/planning_config.pb.txt
@@ -10,8 +10,9 @@ topic_config {
   routing_response_topic: "/apollo/routing_response"
   story_telling_topic: "/apollo/storytelling"
   traffic_light_detection_topic: "/apollo/perception/traffic_light"
+  planning_learning_data_topic: "/apollo/planning/learning_data"
 }
-learning_mode: NO_LEARNING  # NOLEARNING / E2E / HYBRID
+learning_mode: NO_LEARNING  # NO_LEARNING / E2E / HYBRID / RL_TEST
 standard_planning_config {
   planner_type: PUBLIC_ROAD
   planner_public_road_config {
diff --git a/modules/planning/planning_component.cc b/modules/planning/planning_component.cc
index b25bb91108..6980409356 100644
--- a/modules/planning/planning_component.cc
+++ b/modules/planning/planning_component.cc
@@ -109,6 +109,9 @@ bool PlanningComponent::Init() {
   rerouting_writer_ = node_->CreateWriter<RoutingRequest>(
       config_.topic_config().routing_request_topic());
 
+  planning_learning_data_writer_ = node_->CreateWriter<PlanningLearningData>(
+      config_.topic_config().planning_learning_data_topic());
+
   return true;
 }
 
@@ -165,12 +168,30 @@ bool PlanningComponent::Proc(
     message_process_.OnLocalization(*local_view_.localization_estimate);
   }
 
+  // publish learning data frame for RL test
+  if (config_.learning_mode() == PlanningConfig::RL_TEST) {
+    PlanningLearningData planning_learning_data;
+    LearningDataFrame* learning_data_frame =
+        injector_->learning_based_data()->GetLatestLearningDataFrame();
+    if (learning_data_frame) {
+      planning_learning_data.mutable_learning_data()
+                            ->add_learning_data()
+                            ->CopyFrom(*learning_data_frame);
+      common::util::FillHeader(node_->Name(), &planning_learning_data);
+      planning_learning_data_writer_->Write(planning_learning_data);
+    } else {
+      AERROR << "fail to generate learning data frame";
+      return false;
+    }
+    return true;
+  }
+
   ADCTrajectory adc_trajectory_pb;
   planning_base_->RunOnce(local_view_, &adc_trajectory_pb);
-  auto start_time = adc_trajectory_pb.header().timestamp_sec();
   common::util::FillHeader(node_->Name(), &adc_trajectory_pb);
 
   // modify trajectory relative time due to the timestamp change in header
+  auto start_time = adc_trajectory_pb.header().timestamp_sec();
   const double dt = start_time - adc_trajectory_pb.header().timestamp_sec();
   for (auto& p : *adc_trajectory_pb.mutable_trajectory_point()) {
     p.set_relative_time(p.relative_time() + dt);
diff --git a/modules/planning/planning_component.h b/modules/planning/planning_component.h
index 2b2519da97..f648de3a72 100644
--- a/modules/planning/planning_component.h
+++ b/modules/planning/planning_component.h
@@ -27,6 +27,7 @@
 #include "modules/planning/common/message_process.h"
 #include "modules/planning/common/planning_gflags.h"
 #include "modules/planning/planning_base.h"
+#include "modules/planning/proto/learning_data.pb.h"
 #include "modules/planning/proto/pad_msg.pb.h"
 #include "modules/planning/proto/planning.pb.h"
 #include "modules/planning/proto/planning_config.pb.h"
@@ -68,6 +69,8 @@ class PlanningComponent final
 
   std::shared_ptr<cyber::Writer<ADCTrajectory>> planning_writer_;
   std::shared_ptr<cyber::Writer<routing::RoutingRequest>> rerouting_writer_;
+  std::shared_ptr<cyber::Writer<PlanningLearningData>>
+      planning_learning_data_writer_;
 
   std::mutex mutex_;
   perception::TrafficLightDetection traffic_light_;
diff --git a/modules/planning/proto/planning_config.proto b/modules/planning/proto/planning_config.proto
index c12b759584..d431a4c128 100644
--- a/modules/planning/proto/planning_config.proto
+++ b/modules/planning/proto/planning_config.proto
@@ -357,6 +357,7 @@ message TopicConfig {
   optional string routing_response_topic = 9;
   optional string story_telling_topic = 10;
   optional string traffic_light_detection_topic = 11;
+  optional string planning_learning_data_topic =12;
 }
 
 message PlanningConfig {
@@ -364,6 +365,7 @@ message PlanningConfig {
     NO_LEARNING = 0;
     E2E = 1;
     HYBRID = 2;
+    RL_TEST = 3;
   }
 
   optional TopicConfig topic_config = 1;
-- 
GitLab