integrate reference line smoother into em planner.

modules/planning/planner/em/BUILD

@@ -18,6 +18,7 @@ cc_library(
         "//modules/common/util",
         "//modules/common/util:factory",
         "//modules/common/vehicle_state",
+        "//modules/map/hdmap",
         "//modules/planning/common:planning_common",
         "//modules/planning/math/curve1d:quartic_polynomial_curve1d",
         "//modules/planning/optimizer",
@@ -28,6 +29,9 @@ cc_library(
         "//modules/planning/optimizer/qp_spline_path",
         "//modules/planning/planner",
         "//modules/planning/proto:planning_proto",
+        "//modules/planning/proxy:routing_proxy",
+        "//modules/planning/reference_line",
+        "//modules/planning/reference_line:reference_line_smoother",
         "@eigen//:eigen",
     ],
 )

modules/planning/planner/em/em_planner.cc

@@ -22,6 +22,8 @@
 
 #include "modules/common/log.h"
 #include "modules/common/util/string_tokenizer.h"
+#include "modules/map/hdmap/hdmap.h"
+#include "modules/map/hdmap/hdmap_common.h"
 #include "modules/planning/common/data_center.h"
 #include "modules/planning/common/frame.h"
 #include "modules/planning/common/planning_gflags.h"
@@ -62,6 +64,10 @@ Status EMPlanner::Init(const PlanningConfig& config) {
     optimizers_.emplace_back(optimizer_factory_.CreateObject(
         config.em_planner_config().optimizer(i)));
   }
+  routing_proxy_.Init();
+  smoother_.SetConfig(smoother_config_);  // use the default value in config.
+  // FIXME(all): switch to real routing when it is ready
+  GenerateReferenceLineFromRouting(routing_proxy_);
   return Status::OK();
 }
 
@@ -77,7 +83,10 @@ Status EMPlanner::MakePlan(const TrajectoryPoint& start_point,
   ADEBUG << "start point:" << start_point.DebugString();
   frame->mutable_planning_data()->set_init_planning_point(start_point);
 
-  //  frame->mutable_planning_data()->set_reference_line(reference_line);
+  if (reference_line_) {
+    ADEBUG << "reference line:" << reference_line_->DebugString();
+  }
+  frame->mutable_planning_data()->set_reference_line(reference_line_);
   //  frame->mutable_planning_data()->set_decision_data(decision_data);
   for (auto& optimizer : optimizers_) {
     optimizer->Optimize(frame->mutable_planning_data());
@@ -135,5 +144,50 @@ std::vector<SpeedPoint> EMPlanner::GenerateInitSpeedProfile(
   return std::move(speed_profile);
 }
 
+Status EMPlanner::GenerateReferenceLineFromRouting(
+    const RoutingProxy& routing_proxy) {
+  DataCenter* data_center = DataCenter::instance();
+
+  const auto& routing_result = routing_proxy.routing();
+  const auto& map = data_center->map();
+  std::vector<ReferencePoint> ref_points;
+  common::math::Vec2d vehicle_position;
+  hdmap::LaneInfoConstPtr lane_info_ptr = nullptr;
+
+  for (const auto& lane : routing_result.route()) {
+    hdmap::Id lane_id;
+    lane_id.set_id(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);
+    }
+    // FIXME(all): need vehicle position to smooth?
+    vehicle_position = points[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);
+  }
+  reference_line_.reset(new ReferenceLine(smoothed_ref_points));
+  return Status::OK();
+}
+
+
 }  // namespace planning
 }  // namespace apollo

modules/planning/planner/em/em_planner.h

@@ -28,7 +28,10 @@
 #include "modules/planning/planner/planner.h"
 #include "modules/planning/proto/planning.pb.h"
 #include "modules/planning/proto/planning_config.pb.h"
-
+#include "modules/planning/proxy/routing_proxy.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"
 /**
  * @namespace apollo::planning
  * @brief apollo::planning
@@ -71,10 +74,18 @@ class EMPlanner : public Planner {
   std::vector<SpeedPoint> GenerateInitSpeedProfile(const double init_v,
                                                    const double init_a);
 
+  apollo::common::Status GenerateReferenceLineFromRouting(
+      const RoutingProxy& routing_proxy);
 
  private:
   apollo::common::util::Factory<OptimizerType, Optimizer> optimizer_factory_;
   std::vector<std::unique_ptr<Optimizer>> optimizers_;
+  // FIXME(all): replace RoutingProxy with RoutingAdapter when
+  // routing is ready.
+  RoutingProxy routing_proxy_;
+  ReferenceLineSmootherConfig smoother_config_;
+  ReferenceLineSmoother smoother_;
+  std::unique_ptr<ReferenceLine> reference_line_;
 };
 
 }  // namespace planning

modules/planning/reference_line/reference_line.cc

@@ -20,6 +20,7 @@
 
 #include "modules/planning/reference_line/reference_line.h"
 
+#include <sstream>
 #include <string>
 #include <utility>
 #include <vector>
@@ -225,5 +226,11 @@ bool ReferenceLine::is_on_road(const common::SLPoint& sl_point) const {
   return true;
 }
 
+std::string ReferenceLine::DebugString() const {
+  std::ostringstream ss;
+  ss << "point num:" << reference_points_.size();
+  return ss.str();
+}
+
 }  // namespace planning
 }  // namespace apollo

modules/planning/reference_line/reference_line.h

@@ -57,6 +57,8 @@ class ReferenceLine {
   double get_lane_width(const double s) const;
   bool is_on_road(const common::SLPoint& sl_point) const;
 
+  std::string DebugString() const;
+
  private:
   static ReferencePoint interpolate(const ReferencePoint& p0, const double s0,
                                     const ReferencePoint& p1, const double s1,