move reference line generation to data center

modules/planning/common/BUILD

@@ -151,8 +151,12 @@ cc_library(
         ":object_table",
         ":planning_gflags",
         "//modules/common",
+        "//modules/common/adapters:adapter_manager",
         "//modules/common/status",
+        "//modules/common/vehicle_state",
         "//modules/map/hdmap",
+        "//modules/planning/proto:reference_line_smoother_config_proto",
+        "//modules/planning/reference_line:reference_line_smoother",
         "//modules/planning/state_machine:master_state_machine",
     ],
 )

modules/planning/common/data_center.cc

@@ -22,15 +22,24 @@
 
 #include <fstream>
 #include <utility>
+#include <utility>
+
+#include "modules/map/proto/map_id.pb.h"
 
 #include "google/protobuf/text_format.h"
+#include "modules/common/adapters/adapter_manager.h"
 #include "modules/common/log.h"
+#include "modules/common/util/util.h"
+#include "modules/common/vehicle_state/vehicle_state.h"
 #include "modules/planning/common/planning_gflags.h"
+#include "modules/planning/common/planning_gflags.h"
+#include "modules/planning/reference_line/reference_line_smoother.h"
 
 namespace apollo {
 namespace planning {
 
 using apollo::common::Status;
+using apollo::common::adapter::AdapterManager;
 
 DataCenter::DataCenter() {
   _object_table.reset(new ObjectTable());
@@ -53,9 +62,67 @@ Frame *DataCenter::frame(const uint32_t sequence_num) const {
   return nullptr;
 }
 
-apollo::common::Status DataCenter::init_frame(const uint32_t sequence_num) {
+bool DataCenter::CreateReferenceLineFromMap() {
+  if (AdapterManager::GetRoutingResult()->Empty()) {
+    AERROR << "Routing is empty";
+    return false;
+  }
+  const auto &routing_result =
+      AdapterManager::GetRoutingResult()->GetLatestObserved();
+
+  std::vector<ReferencePoint> ref_points;
+  common::math::Vec2d vehicle_position;
+  hdmap::LaneInfoConstPtr lane_info_ptr = nullptr;
+  ReferenceLineSmoother smoother;
+  if (!smoother.SetConfig(FLAGS_reference_line_smoother_config_file)) {
+    AERROR << "Failed to load file "
+           << FLAGS_reference_line_smoother_config_file;
+    return false;
+  }
+
+  vehicle_position.set_x(common::VehicleState::instance()->x());
+  vehicle_position.set_y(common::VehicleState::instance()->y());
+
+  for (const auto &lane : routing_result.route()) {
+    auto lane_id = common::util::MakeMapId(lane.id());
+    ADEBUG << "Added lane from routing:" << lane.id();
+    lane_info_ptr = map_.get_lane_by_id(lane_id);
+    if (!lane_info_ptr) {
+      AERROR << "failed to find lane " + lane.id() + " from map ";
+      return false;
+    }
+    const auto &points = lane_info_ptr->points();
+    const auto &headings = lane_info_ptr->headings();
+    for (size_t i = 0; i < points.size(); ++i) {
+      ref_points.emplace_back(points[i], headings[i], 0.0, 0.0, -2.0, 2.0);
+    }
+  }
+  if (ref_points.empty()) {
+    AERROR << "Found no reference points from map";
+    return false;
+  }
+
+  std::unique_ptr<ReferenceLine> reference_line(new ReferenceLine(ref_points));
+  std::vector<ReferencePoint> smoothed_ref_points;
+  if (!smoother.smooth(*reference_line, vehicle_position,
+                       &smoothed_ref_points)) {
+    AERROR << "Fail to smooth a reference line from map";
+    return false;
+  }
+  ADEBUG << "smooth reference points num:" << smoothed_ref_points.size();
+  _frame->mutable_planning_data()->set_reference_line(smoothed_ref_points);
+  return true;
+}
+
+bool DataCenter::init_frame(const uint32_t sequence_num) {
   _frame.reset(new Frame(sequence_num));
-  return Status::OK();
+  _frame->set_planning_data(new PlanningData());
+
+  if (!CreateReferenceLineFromMap()) {
+    AERROR << "failed to create reference line";
+    return false;
+  }
+  return true;
 }
 
 Frame *DataCenter::current_frame() const { return _frame.get(); }

modules/planning/common/data_center.h

@@ -28,8 +28,8 @@
 #include "modules/common/macro.h"
 #include "modules/common/status/status.h"
 #include "modules/map/hdmap/hdmap.h"
-#include "modules/planning/common/object_table.h"
 #include "modules/planning/common/frame.h"
+#include "modules/planning/common/object_table.h"
 #include "modules/planning/state_machine/master_state_machine.h"
 
 namespace apollo {
@@ -41,7 +41,7 @@ class DataCenter {
   Frame *frame(const uint32_t sequence_num) const;
 
  public:
-  apollo::common::Status init_frame(const uint32_t sequence_num);
+  bool init_frame(const uint32_t sequence_num);
   Frame *current_frame() const;
   void save_frame();
   const ::apollo::hdmap::HDMap &map() { return map_; }
@@ -50,10 +50,12 @@ class DataCenter {
 
   const Frame *last_frame() const;
 
-  const ObjectTable& object_table() const;
-  ObjectTable* mutable_object_table() const;
+  const ObjectTable &object_table() const;
+  ObjectTable *mutable_object_table() const;
 
  private:
+  bool CreateReferenceLineFromMap();
+
   std::unordered_map<uint32_t, std::unique_ptr<Frame>> _frames;
   std::list<uint32_t> _sequence_queue;
   std::unique_ptr<Frame> _frame = nullptr;

modules/planning/common/planning_data.cc

@@ -22,6 +22,9 @@
 
 #include <utility>
 
+#include "modules/planning/common/path/path_data.h"
+#include "modules/planning/proto/planning.pb.h"
+
 namespace apollo {
 namespace planning {
 
@@ -47,9 +50,10 @@ void PlanningData::set_init_planning_point(
 }
 
 void PlanningData::set_reference_line(
-    std::unique_ptr<ReferenceLine>& reference_line) {
-  reference_line_ = std::move(reference_line);
+    const std::vector<ReferencePoint>& ref_points) {
+  reference_line_.reset(new ReferenceLine(ref_points));
 }
+
 void PlanningData::set_decision_data(
     std::shared_ptr<DecisionData>& decision_data) {
   decision_data_ = decision_data;

modules/planning/common/planning_data.h

@@ -23,6 +23,7 @@
 
 #include <memory>
 #include <string>
+#include <vector>
 
 #include "modules/common/proto/path_point.pb.h"
 
@@ -42,7 +43,7 @@ class PlanningData {
   PlanningData() = default;
 
   // copy reference line to planning data
-  void set_reference_line(std::unique_ptr<ReferenceLine>& reference_line);
+  void set_reference_line(const std::vector<ReferencePoint>& ref_points);
   void set_decision_data(std::shared_ptr<DecisionData>& decision_data);
   void set_init_planning_point(const TrajectoryPoint& init_planning_point);
 

modules/planning/common/planning_gflags.cc

@@ -38,6 +38,10 @@ DEFINE_double(trajectory_resolution, 0.01,
               "The time resolution of "
               "output trajectory.");
 
+DEFINE_string(reference_line_smoother_config_file,
+              "modules/planning/conf/reference_line_smoother_config.pb.txt",
+              "The reference line smoother config file");
+
 DEFINE_double(cycle_duration_in_sec, 0.002, "# of seconds per planning cycle.");
 DEFINE_double(maximal_delay_sec, 0.005, "# of seconds for delay.");
 

modules/planning/common/planning_gflags.h

@@ -200,6 +200,8 @@ DECLARE_double(master_min_speed);
 DECLARE_double(max_deacceleration_for_red_light_stop);
 DECLARE_double(max_deacceleration_for_yellow_light_stop);
 
+DECLARE_string(reference_line_smoother_config_file);
+
 // Optimizers
 DECLARE_string(qp_spline_path_config_file);
 DECLARE_string(dp_poly_path_config_file);

modules/planning/conf/reference_line_smoother_config.pb.txt

+num_spline: 6
+spline_order: 6
+num_evaluated_points: 13
+boundary_bound: 0.2
+derivative_bound: 1.0
+second_derivative_bound: 1.0
+third_derivative_bound: 1.0
+ref_line_weight: 0.0
+derivative_weight: 0.0
+second_derivative_weight: 0.0
+third_derivative_weight: 100

modules/planning/optimizer/dp_poly_path/dp_road_graph.h

@@ -24,6 +24,7 @@
 #include <vector>
 
 #include "modules/common/proto/path_point.pb.h"
+
 #include "modules/common/status/status.h"
 #include "modules/planning/common/decision_data.h"
 #include "modules/planning/common/path/path_data.h"

modules/planning/planner/em/em_planner.cc

@@ -39,11 +39,11 @@
 namespace apollo {
 namespace planning {
 
-using apollo::common::Status;
-using apollo::common::adapter::AdapterManager;
-using apollo::common::ErrorCode;
-using apollo::common::TrajectoryPoint;
-using apollo::common::VehicleState;
+using common::Status;
+using common::adapter::AdapterManager;
+using common::ErrorCode;
+using common::TrajectoryPoint;
+using common::VehicleState;
 
 EMPlanner::EMPlanner() {}
 
@@ -87,9 +87,6 @@ Status EMPlanner::MakePlan(
   DataCenter* data_center = DataCenter::instance();
   Frame* frame = data_center->current_frame();
 
-  GenerateReferenceLineFromRouting();
-
-  frame->set_planning_data(new PlanningData());
   if (data_center->last_frame()) {
     ADEBUG << "last frame:" << data_center->last_frame()->DebugString();
   }
@@ -97,10 +94,6 @@ Status EMPlanner::MakePlan(
   auto planning_data = frame->mutable_planning_data();
   planning_data->set_init_planning_point(start_point);
 
-  if (reference_line_) {
-    ADEBUG << "reference line:" << reference_line_->DebugString();
-  }
-  planning_data->set_reference_line(reference_line_);
   std::shared_ptr<DecisionData> decision_data(new DecisionData());
 
   planning_data->set_decision_data(decision_data);
@@ -171,56 +164,5 @@ std::vector<SpeedPoint> EMPlanner::GenerateInitSpeedProfile(
   return std::move(speed_profile);
 }
 
-Status EMPlanner::GenerateReferenceLineFromRouting() {
-  DataCenter* data_center = DataCenter::instance();
-
-  const auto& routing_result =
-      AdapterManager::GetRoutingResult()->GetLatestObserved();
-
-  const auto& map = data_center->map();
-  std::vector<ReferencePoint> ref_points;
-  common::math::Vec2d vehicle_position;
-  hdmap::LaneInfoConstPtr lane_info_ptr = nullptr;
-  ReferenceLineSmoother smoother;
-  smoother.SetConfig(smoother_config_);  // use the default value in config.
-
-  vehicle_position.set_x(common::VehicleState::instance()->x());
-  vehicle_position.set_y(common::VehicleState::instance()->y());
-
-  for (const auto& lane : routing_result.route()) {
-    hdmap::Id lane_id;
-    lane_id.set_id(lane.id());
-    ADEBUG << "Added lane from routing:" << lane.id();
-    lane_info_ptr = map.get_lane_by_id(lane_id);
-    if (!lane_info_ptr) {
-      std::string msg("failed to find lane " + lane.id() + " from map ");
-      AERROR << msg;
-      return Status(ErrorCode::PLANNING_ERROR, msg);
-    }
-    const auto& points = lane_info_ptr->points();
-    const auto& headings = lane_info_ptr->headings();
-    for (size_t i = 0; i < points.size(); ++i) {
-      ref_points.emplace_back(points[i], headings[i], 0.0, 0.0, -2.0, 2.0);
-    }
-  }
-  if (ref_points.empty()) {
-    std::string msg("Found no reference points from map");
-    AERROR << msg;
-    return Status(ErrorCode::PLANNING_ERROR, msg);
-  }
-
-  std::unique_ptr<ReferenceLine> reference_line(new ReferenceLine(ref_points));
-  std::vector<ReferencePoint> smoothed_ref_points;
-  if (!smoother.smooth(*reference_line, vehicle_position,
-                       &smoothed_ref_points)) {
-    std::string msg("Fail to smooth a reference line from map");
-    AERROR << msg;
-    return Status(ErrorCode::PLANNING_ERROR, msg);
-  }
-  ADEBUG << "smooth reference points num:" << smoothed_ref_points.size();
-  reference_line_.reset(new ReferenceLine(smoothed_ref_points));
-  return Status::OK();
-}
-
 }  // namespace planning
 }  // namespace apollo

modules/planning/planner/em/em_planner.h

@@ -30,7 +30,6 @@
 #include "modules/planning/optimizer/optimizer.h"
 #include "modules/planning/planner/planner.h"
 #include "modules/planning/reference_line/reference_line.h"
-#include "modules/planning/reference_line/reference_line_smoother.h"
 #include "modules/planning/reference_line/reference_point.h"
 
 /**
@@ -75,13 +74,9 @@ class EMPlanner : public Planner {
   std::vector<SpeedPoint> GenerateInitSpeedProfile(const double init_v,
                                                    const double init_a);
 
-  apollo::common::Status GenerateReferenceLineFromRouting();
-
  private:
   apollo::common::util::Factory<OptimizerType, Optimizer> optimizer_factory_;
   std::vector<std::unique_ptr<Optimizer>> optimizers_;
-  ReferenceLineSmootherConfig smoother_config_;
-  std::unique_ptr<ReferenceLine> reference_line_;
 };
 
 }  // namespace planning

modules/planning/planning.cc

@@ -139,8 +139,11 @@ void Planning::RunOnce() {
       apollo::common::time::ToSecond(apollo::common::time::Clock::Now()) +
       planning_cycle_time;
 
-  DataCenter::instance()->init_frame(
-      AdapterManager::GetPlanning()->GetSeqNum() + 1);
+  if (!DataCenter::instance()->init_frame(
+          AdapterManager::GetPlanning()->GetSeqNum() + 1)) {
+    AERROR << "DataCenter init frame failed";
+    return;
+  }
 
   std::vector<TrajectoryPoint> planning_trajectory;
   bool res_planning =