Add EMPlanningData. (#100)

modules/common/adapters/adapter.h

@@ -232,6 +232,10 @@ class Adapter {
     header->set_sequence_num(++seq_num_);
   }
 
+  uint32_t GetSeqNum() const {
+    return seq_num_;
+  }
+
  private:
   // HasSequenceNumber returns false for non-proto-message data types.
   template <typename InputMessageType>

modules/common/adapters/adapter_manager.cc

@@ -84,8 +84,8 @@ void AdapterManager::Init(const AdapterManagerConfig &configs) {
                              config.message_history_limit());
         break;
       case AdapterConfig::PLANNING_TRAJECTORY:
-        EnablePlanningTrajectory(FLAGS_planning_trajectory_topic, config.mode(),
-                                 config.message_history_limit());
+        EnablePlanning(FLAGS_planning_trajectory_topic, config.mode(),
+                       config.message_history_limit());
         break;
       case AdapterConfig::PREDICTION:
         EnablePrediction(FLAGS_prediction_topic, config.mode(),

modules/common/adapters/adapter_manager.h

@@ -187,7 +187,7 @@ class AdapterManager {
   REGISTER_ADAPTER(Monitor);
   REGISTER_ADAPTER(Pad);
   REGISTER_ADAPTER(PerceptionObstacles);
-  REGISTER_ADAPTER(PlanningTrajectory);
+  REGISTER_ADAPTER(Planning);
   REGISTER_ADAPTER(Prediction);
   REGISTER_ADAPTER(TrafficLightDetection);
 

modules/common/adapters/message_adapters.h

@@ -55,7 +55,7 @@ using MonitorAdapter = Adapter<apollo::common::monitor::MonitorMessage>;
 using PadAdapter = Adapter<::apollo::control::PadMessage>;
 using PerceptionObstaclesAdapter =
     Adapter<::apollo::perception::PerceptionObstacles>;
-using PlanningTrajectoryAdapter = Adapter<::apollo::planning::ADCTrajectory>;
+using PlanningAdapter = Adapter<::apollo::planning::ADCTrajectory>;
 using PredictionAdapter = Adapter<::apollo::prediction::PredictionObstacles>;
 using TrafficLightDetectionAdapter =
     Adapter<::apollo::perception::TrafficLightDetection>;

modules/common/log.h

@@ -22,6 +22,7 @@
 #define MODULES_COMMON_LOG_H_
 
 #include "glog/logging.h"
+#include "glog/raw_logging.h"
 
 #define ADEBUG VLOG(4) << "[DEBUG] "
 #define AINFO VLOG(3) << "[INFO] "
@@ -29,4 +30,13 @@
 #define AERROR LOG(ERROR)
 #define AFATAL LOG(FATAL)
 
+// quit if condition is met
+#define QUIT_IF(CONDITION, RET, LEVEL, MSG, ...) \
+do { \
+    if (CONDITION) { \
+        RAW_LOG(LEVEL, MSG, ##__VA_ARGS__); \
+        return RET; \
+    } \
+} while (0);
+
 #endif  // MODULES_COMMON_LOG_H_

modules/control/control.cc

@@ -75,8 +75,8 @@ Status Control::Init() {
 
   CHECK(AdapterManager::GetChassis()) << "Chassis is not initialized.";
 
-  CHECK(AdapterManager::GetPlanningTrajectory())
-      << "PlanningTrajectory is not initialized.";
+  CHECK(AdapterManager::GetPlanning())
+      << "Planning is not initialized.";
 
   CHECK(AdapterManager::GetPad()) << "Pad is not initialized.";
 
@@ -244,7 +244,7 @@ Status Control::CheckInput() {
   chassis_ = chassis_adapter->GetLatestObserved();
   ADEBUG << "Received chassis:" << chassis_.ShortDebugString();
 
-  auto trajectory_adapter = AdapterManager::GetPlanningTrajectory();
+  auto trajectory_adapter = AdapterManager::GetPlanning();
   if (trajectory_adapter->Empty()) {
     AINFO << "No planning msg yet. ";
     return Status(ErrorCode::CONTROL_COMPUTE_ERROR, "No planning msg");

modules/control/integration_tests/control_test_base.cc

@@ -76,8 +76,8 @@ bool ControlTestBase::test_control() {
                                               FLAGS_test_localization_file);
   }
   if (!FLAGS_test_planning_file.empty()) {
-    AdapterManager::FeedPlanningTrajectoryProtoFile(FLAGS_test_data_dir +
-                                                    FLAGS_test_planning_file);
+    AdapterManager::FeedPlanningProtoFile(FLAGS_test_data_dir +
+                                          FLAGS_test_planning_file);
   }
   if (!FLAGS_test_chassis_file.empty()) {
     AdapterManager::FeedChassisProtoFile(FLAGS_test_data_dir +

modules/control/tools/control_tester.cc

@@ -67,7 +67,7 @@ int main(int argc, char **argv) {
     AdapterManager::FeedChassisProtoFile(FLAGS_chassis_test_file);
     AdapterManager::FeedLocalizationProtoFile(FLAGS_l10n_test_file);
     AdapterManager::FeedPadProtoFile(FLAGS_pad_msg_test_file);
-    AdapterManager::FeedPlanningTrajectoryProtoFile(FLAGS_planning_test_file);
+    AdapterManager::FeedPlanningProtoFile(FLAGS_planning_test_file);
     sleep_for(std::chrono::milliseconds(1000 / FLAGS_feed_frequency));
   }
   AINFO << "Successfully fed proto files.";

modules/dreamview/backend/simulation_world_service.cc

@@ -37,7 +37,7 @@ using apollo::common::adapter::AdapterManager;
 using apollo::common::adapter::MonitorAdapter;
 using apollo::common::adapter::LocalizationAdapter;
 using apollo::common::adapter::ChassisAdapter;
-using apollo::common::adapter::PlanningTrajectoryAdapter;
+using apollo::common::adapter::PlanningAdapter;
 using apollo::common::config::VehicleConfigHelper;
 using apollo::common::monitor::MonitorMessage;
 using apollo::common::monitor::MonitorMessageItem;
@@ -252,7 +252,7 @@ void UpdateSimulationWorld<ChassisAdapter>(const Chassis &chassis,
 }
 
 template <>
-void UpdateSimulationWorld<PlanningTrajectoryAdapter>(
+void UpdateSimulationWorld<PlanningAdapter>(
     const ADCTrajectory &trajectory, SimulationWorld *world) {
   const double cutoff_time = world->auto_driving_car().timestamp_sec();
   const double header_time = trajectory.header().timestamp_sec();
@@ -329,7 +329,7 @@ const SimulationWorld &SimulationWorldService::Update() {
   UpdateWithLatestObserved("Chassis", AdapterManager::GetChassis(), &world_);
   UpdateWithLatestObserved("Localization", AdapterManager::GetLocalization(),
                            &world_);
-  UpdateWithLatestObserved("Planning", AdapterManager::GetPlanningTrajectory(),
+  UpdateWithLatestObserved("Planning", AdapterManager::GetPlanning(),
                            &world_);
   UpdateWithLatestObserved("PerceptionObstacles",
                            AdapterManager::GetPerceptionObstacles(), &world_);

modules/dreamview/backend/simulation_world_service.h

@@ -60,7 +60,7 @@ void UpdateSimulationWorld<apollo::common::adapter::ChassisAdapter>(
     const apollo::canbus::Chassis &chassis, SimulationWorld *world);
 
 template <>
-void UpdateSimulationWorld<apollo::common::adapter::PlanningTrajectoryAdapter>(
+void UpdateSimulationWorld<apollo::common::adapter::PlanningAdapter>(
     const apollo::planning::ADCTrajectory &trajectory, SimulationWorld *world);
 
 template<>

modules/map/pnc_map/path.cc

@@ -155,7 +155,7 @@ void Path::init() {
   init_lane_segments();
   init_point_index();
   init_width();
-  init_overlaps();
+  //  init_overlaps();
 }
 
 void Path::init_points() {

modules/map/pnc_map/path.h

@@ -284,7 +284,7 @@ class Path {
   void init_lane_segments();
   void init_width();
   void init_point_index();
-  void init_overlaps();
+  //  void init_overlaps();
 
   double get_sample(const std::vector<double>& samples, const double s) const;
 

modules/planning/BUILD

@@ -19,6 +19,7 @@ cc_library(
         "//modules/common/vehicle_state",
         "//modules/decision/proto:decision_proto",
         "//modules/perception/proto:perception_proto",
+        "//modules/planning/common:data_center",
         "//modules/planning/common:planning_common",
         "//modules/planning/planner/em:em_planner",
         "//modules/planning/planner/rtk:rtk_planner",

modules/planning/common/BUILD

@@ -111,12 +111,29 @@ cc_library(
         ":decision_data",
         "//modules/common/proto:path_point_proto",
         "//modules/planning/common/trajectory:publishable_trajectory",
-        "//modules/planning/reference_line:lib_reference_line",
+        "//modules/planning/reference_line",
         "//modules/map/proto:map_proto",
         "@eigen//:eigen",
     ],
 )
 
+cc_library(
+    name = "em_planning_data",
+    srcs = [
+        "em_planning_data.cc",
+    ],
+    hdrs = [
+        "em_planning_data.h",
+    ],
+    deps = [
+        ":planning_data",
+        "//modules/common/proto:path_point_proto",
+        "//modules/planning/common/path:path_data",
+        "//modules/planning/common/speed",
+        "//modules/planning/math:double",
+    ]
+)
+
 cc_library(
     name = "environment",
     srcs = [

modules/planning/common/em_planning_data.cc

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file
+ **/
+
+#include "modules/planning/common/em_planning_data.h"
+#include "glog/logging.h"
+
+#include "modules/common/proto/path_point.pb.h"
+#include "modules/planning/math/double.h"
+
+namespace apollo {
+namespace planning {
+
+std::string EMPlanningData::type() const {
+  return "EMPlanningData";
+}
+
+void EMPlanningData::init(const std::size_t num_iter) {
+  _num_iter = num_iter;
+  _path_data_vec = std::vector < PathData > (num_iter, PathData());
+  _speed_data_vec = std::vector < SpeedData > (num_iter + 1, SpeedData());
+}
+
+std::size_t EMPlanningData::num_iter() const {
+  return _num_iter;
+}
+
+const PathData& EMPlanningData::path_data(const std::size_t index) const {
+  return _path_data_vec[index];
+}
+
+const SpeedData& EMPlanningData::speed_data(const std::size_t index) const {
+  return _speed_data_vec[index];
+}
+
+PathData* EMPlanningData::mutable_path_data(const std::size_t index) {
+  if (index >= _path_data_vec.size()) {
+    return nullptr;
+  }
+  return &_path_data_vec[index];
+}
+
+SpeedData* EMPlanningData::mutable_speed_data(const std::size_t index) {
+  if (index >= _speed_data_vec.size()) {
+    return nullptr;
+  }
+  return &_speed_data_vec[index];
+}
+
+bool EMPlanningData::aggregate(const double time_resolution) {
+  CHECK(time_resolution > 0.0);
+  CHECK(_computed_trajectory.num_of_points() == 0);
+
+  const SpeedData& speed_data = _speed_data_vec.back();
+  const PathData& path_data = _path_data_vec.back();
+  for (double cur_rel_time = 0.0; cur_rel_time < speed_data.total_time();
+      cur_rel_time += time_resolution) {
+
+    SpeedPoint speed_point;
+    QUIT_IF(!speed_data.get_speed_point_with_time(cur_rel_time, &speed_point), false,
+        ERROR, "Fail to get speed point with relative time %f", cur_rel_time);
+
+    apollo::common::PathPoint path_point;
+    // TODO temp fix speed point s out of path point bound, need further refine later
+    if (speed_point.s() > path_data.path().param_length()) {
+        break;
+    }
+    QUIT_IF(!path_data.get_path_point_with_path_s(speed_point.s(), &path_point), false,
+        ERROR, "Fail to get path data with s %f, path total length %f",
+        speed_point.s(), path_data.path().param_length());
+
+    apollo::common::TrajectoryPoint trajectory_point;
+    trajectory_point.mutable_path_point()->CopyFrom(path_point);
+    trajectory_point.set_v(speed_point.v());
+    trajectory_point.set_a(speed_point.a());
+    trajectory_point.set_relative_time(
+        _init_planning_point.relative_time() + speed_point.t());
+    _computed_trajectory.add_trajectory_point(trajectory_point);
+  }
+  return true;
+}
+}  // namespace planning
+}  // namespace apollo

modules/planning/common/em_planning_data.h

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file
+ **/
+
+#ifndef MODULES_PLANNING_COMMON_EM_PLANNING_DATA_H
+#define MODULES_PLANNING_COMMON_EM_PLANNING_DATA_H
+
+#include "modules/planning/common/planning_data.h"
+#include "modules/planning/common/path/path_data.h"
+#include "modules/planning/common/speed/speed_data.h"
+
+namespace apollo {
+namespace planning {
+
+class EMPlanningData : public PlanningData {
+ public:
+  EMPlanningData() = default;
+  virtual std::string type() const;
+
+  void init(const std::size_t num_iter);
+
+  std::size_t num_iter() const;
+  const PathData& path_data(const std::size_t index) const;
+  const SpeedData& speed_data(const std::size_t index) const;
+
+  PathData* mutable_path_data(const std::size_t index);
+  SpeedData* mutable_speed_data(const std::size_t index);
+
+  // aggregate final result together by some configuration
+  bool aggregate(const double time_resolution);
+
+ private:
+  std::size_t _num_iter = 0;
+  std::vector<PathData> _path_data_vec;
+  std::vector<SpeedData> _speed_data_vec;
+};
+
+}  // namespace planning
+}  // namespace apollo
+
+#endif // MODULES_PLANNING_COMMON_EM_PLANNING_DATA_H

modules/planning/common/planning_data.h

@@ -15,13 +15,14 @@
  *****************************************************************************/
 
 /**
- * @file planning_data.h
+ * @file
  **/
 
 #ifndef MODULES_PLANNING_COMMON_PLANNING_DATA_H_
 #define MODULES_PLANNING_COMMON_PLANNING_DATA_H_
 
 #include "modules/common/proto/path_point.pb.h"
+#include "modules/planning/reference_line/reference_line.h"
 #include "modules/planning/common/decision_data.h"
 #include "modules/planning/common/trajectory/publishable_trajectory.h"
 #include "modules/planning/reference_line/reference_line.h"

modules/planning/optimizer/dp_poly_path/BUILD

@@ -33,6 +33,6 @@ cc_library(
         "//modules/planning/math:sl_analytic_transformation",
         "//modules/planning/math/curve1d:polynomial_curve1d",
         "//modules/planning/math/curve1d:quintic_polynomial_curve1d",
-        "//modules/planning/reference_line:lib_reference_line",
+        "//modules/planning/reference_line",
     ],
 )

modules/planning/planner/em/em_planner.cc

@@ -22,6 +22,7 @@
 #include "modules/common/log.h"
 #include "modules/common/util/string_tokenizer.h"
 #include "modules/planning/common/data_center.h"
+#include "modules/planning/common/frame.h"
 #include "modules/planning/common/planning_gflags.h"
 #include "modules/planning/math/curve1d/quartic_polynomial_curve1d.h"
 
@@ -31,10 +32,20 @@ namespace planning {
 using apollo::common::TrajectoryPoint;
 using apollo::common::vehicle_state::VehicleState;
 
-EMPlanner::EMPlanner() {}
+EMPlanner::EMPlanner() {
+}
 
 bool EMPlanner::MakePlan(const TrajectoryPoint& start_point,
-                         std::vector<TrajectoryPoint>* discretized_trajectory) {
+                     std::vector<TrajectoryPoint>* discretized_trajectory) {
+  DataCenter* data_center = DataCenter::instance();
+  Frame* frame = data_center->current_frame();
+
+//  frame->set_planning_data(task->create_planning_data_instance());
+//  frame->mutable_planning_data()->set_reference_line(reference_line);
+//  frame->mutable_planning_data()->set_decision_data(decision_data);
+//  frame->mutable_planning_data()->set_init_planning_point(
+//      frame->environment().vehicle_state_proxy().init_point(data_center->last_frame()));
+
   return true;
 }
 
@@ -65,8 +76,8 @@ std::vector<SpeedPoint> EMPlanner::GenerateInitSpeedProfile(
 
   // assume the time resolution is 0.1
   std::size_t num_time_steps =
-      static_cast<std::size_t>(FLAGS_trajectory_time_length /
-                               FLAGS_trajectory_time_resolution) + 1;
+      static_cast<std::size_t>(FLAGS_trajectory_time_length
+          / FLAGS_trajectory_time_resolution) + 1;
 
   std::vector<SpeedPoint> speed_profile;
   speed_profile.reserve(num_time_steps);

modules/planning/planning.cc

@@ -18,6 +18,7 @@
 
 #include "modules/common/adapters/adapter_manager.h"
 #include "modules/common/time/time.h"
+#include "modules/planning/common/data_center.h"
 #include "modules/planning/common/planning_gflags.h"
 #include "modules/planning/planner/em/em_planner.h"
 #include "modules/planning/planner/rtk/rtk_replay_planner.h"
@@ -115,13 +116,15 @@ void Planning::RunOnce() {
       apollo::common::time::ToSecond(apollo::common::time::Clock::Now()) +
       planning_cycle_time;
 
+  DataCenter::instance()->init_frame(AdapterManager::GetPlanning()->GetSeqNum() + 1);
+
   std::vector<TrajectoryPoint> planning_trajectory;
   bool res_planning = Plan(vehicle_state, is_on_auto_mode, execution_start_time,
                            &planning_trajectory);
   if (res_planning) {
     ADCTrajectory trajectory_pb =
         ToADCTrajectory(execution_start_time, planning_trajectory);
-    AdapterManager::PublishPlanningTrajectory(trajectory_pb);
+    AdapterManager::PublishPlanning(trajectory_pb);
     AINFO << "Planning succeeded";
   } else {
     AINFO << "Planning failed";
@@ -269,8 +272,8 @@ ADCTrajectory Planning::ToADCTrajectory(
     const double header_time,
     const std::vector<TrajectoryPoint>& discretized_trajectory) {
   ADCTrajectory trajectory_pb;
-  AdapterManager::FillPlanningTrajectoryHeader("planning",
-                                               trajectory_pb.mutable_header());
+  AdapterManager::FillPlanningHeader("planning",
+                                     trajectory_pb.mutable_header());
 
   trajectory_pb.mutable_header()->set_timestamp_sec(header_time);
 

modules/planning/reference_line/BUILD

 package(default_visibility = ["//visibility:public"])
 
 cc_library(
-    name = "lib_reference_line",
+    name = "reference_line",
     srcs = [
         "reference_point.cc",
         "reference_line.cc",