From 5dca75aaba54241308f13c12dc2f07b3772ba35e Mon Sep 17 00:00:00 2001
From: Zhang Liangliang <zhangliangliang@baidu.com>
Date: Wed, 6 Sep 2017 15:32:07 -0700
Subject: [PATCH] Planning: added more functions into reference_line_provider.

---
 modules/planning/reference_line/BUILD         |  1 +
 .../reference_line/reference_line_provider.cc | 71 ++++++++++++++++++-
 .../reference_line/reference_line_provider.h  | 22 +++++-
 3 files changed, 88 insertions(+), 6 deletions(-)

diff --git a/modules/planning/reference_line/BUILD b/modules/planning/reference_line/BUILD
index 7cf529c36b..b8c63b8fde 100644
--- a/modules/planning/reference_line/BUILD
+++ b/modules/planning/reference_line/BUILD
@@ -58,6 +58,7 @@ cc_library(
     deps = [
         "reference_line",
         "reference_line_smoother",
+        "//modules/map/pnc_map",
     ],
 )
 
diff --git a/modules/planning/reference_line/reference_line_provider.cc b/modules/planning/reference_line/reference_line_provider.cc
index 301fb335bc..97d4786059 100644
--- a/modules/planning/reference_line/reference_line_provider.cc
+++ b/modules/planning/reference_line/reference_line_provider.cc
@@ -20,6 +20,7 @@
  * @brief Implementation of the class ReferenceLineProvider.
  */
 
+#include "modules/planning/common/planning_gflags.h"
 #include "modules/planning/reference_line/reference_line_provider.h"
 
 /**
@@ -29,8 +30,15 @@
 namespace apollo {
 namespace planning {
 
-void ReferenceLineProvider::Init() {
-  // TODO: implement this function.
+using apollo::common::VehicleState;
+
+void ReferenceLineProvider::Init(
+    const hdmap::PncMap *pnc_map,
+    const routing::RoutingResponse &routing_response,
+    const ReferenceLineSmootherConfig &smoother_config) {
+  pnc_map_ = pnc_map;
+  routing_response_ = routing_response;
+  smoother_config_ = smoother_config;
   is_initialized_ = true;
 }
 
@@ -46,12 +54,69 @@ bool ReferenceLineProvider::Start() {
 
 void ReferenceLineProvider::Generate() {
   // TODO: implement this function.
+  const auto &curr_adc_position =
+      common::VehicleState::instance()->pose().position();
+  const auto adc_point_enu = common::util::MakePointENU(
+      curr_adc_position.x(), curr_adc_position.y(), curr_adc_position.z());
+
+  if (!CreateReferenceLineFromRouting(adc_point_enu, routing_response_)) {
+    AERROR << "Fail to create reference line at position: "
+           << curr_adc_position.ShortDebugString();
+  };
 }
 
 std::vector<ReferenceLine> ReferenceLineProvider::GetReferenceLines() {
-  // TODO: implement this function.
+  // TODO: implement this function using the current adc position and the
+  // existing reference lines. It is required that the current reference lines
+  // can cover thoroughly the current adc position so that planning can be make
+  // with a minimum planning distance of 100 meters ahead and 10 meters
+  // backward.
+  std::lock_guard<std::mutex> lock(reference_line_groups_mutex_);
   return reference_line_groups_.back();
 }
 
+bool ReferenceLineProvider::CreateReferenceLineFromRouting(
+    const common::PointENU &position, const routing::RoutingResponse &routing) {
+  std::vector<ReferenceLine> reference_lines;
+  std::vector<std::vector<hdmap::LaneSegment>> route_segments;
+
+  const double kBackwardDistance = 20;
+  const double kForwardDistance = 100;
+
+  if (!pnc_map_->GetLaneSegmentsFromRouting(routing, position,
+                                            kBackwardDistance, kForwardDistance,
+                                            &route_segments)) {
+    AERROR << "Failed to extract segments from routing";
+    return false;
+  }
+
+  ReferenceLineSmoother smoother;
+  smoother.Init(smoother_config_);
+
+  for (const auto &segments : route_segments) {
+    hdmap::Path hdmap_path;
+    pnc_map_->CreatePathFromLaneSegments(segments, &hdmap_path);
+    if (FLAGS_enable_smooth_reference_line) {
+      ReferenceLine reference_line;
+      if (!smoother.Smooth(ReferenceLine(hdmap_path), &reference_line)) {
+        AERROR << "Failed to smooth reference line";
+        continue;
+      }
+      reference_lines.push_back(std::move(reference_line));
+    } else {
+      reference_lines.emplace_back(hdmap_path);
+    }
+  }
+
+  if (reference_lines.empty()) {
+    AERROR << "No smooth reference lines available";
+    return false;
+  }
+
+  std::lock_guard<std::mutex> lock(reference_line_groups_mutex_);
+  reference_line_groups_.push_back(reference_lines);
+  return true;
+}
+
 }  // namespace planning
 }  // namespace apollo
diff --git a/modules/planning/reference_line/reference_line_provider.h b/modules/planning/reference_line/reference_line_provider.h
index 982f66dc57..9bc0f529b3 100644
--- a/modules/planning/reference_line/reference_line_provider.h
+++ b/modules/planning/reference_line/reference_line_provider.h
@@ -22,9 +22,13 @@
 #define MODULES_PLANNING_REFERENCE_LINE_REFERENCE_LINE_PROVIDER_H_
 
 #include <memory>
+#include <mutex>
 #include <thread>
 #include <vector>
 
+#include "modules/common/util/util.h"
+#include "modules/common/vehicle_state/vehicle_state.h"
+#include "modules/map/pnc_map/pnc_map.h"
 #include "modules/planning/reference_line/reference_line.h"
 #include "modules/planning/reference_line/reference_line_smoother.h"
 
@@ -47,19 +51,31 @@ class ReferenceLineProvider {
    */
   virtual ~ReferenceLineProvider() = default;
 
-  void Init();
+  void Init(const hdmap::PncMap* pnc_map_,
+            const routing::RoutingResponse& routing_response,
+            const ReferenceLineSmootherConfig& smoother_config);
 
   bool Start();
 
   std::vector<ReferenceLine> GetReferenceLines();
 
  private:
-  DECLARE_SINGLETON(ReferenceLineProvider);
-
   void Generate();
+  void IsValidReferenceLine();
+  bool CreateReferenceLineFromRouting(const common::PointENU& position,
+                                      const routing::RoutingResponse& routing);
+
+ private:
+  DECLARE_SINGLETON(ReferenceLineProvider);
 
   bool is_initialized_ = false;
   std::unique_ptr<std::thread> thread_;
+
+  const hdmap::PncMap* pnc_map_ = nullptr;
+  routing::RoutingResponse routing_response_;
+  ReferenceLineSmootherConfig smoother_config_;
+
+  std::mutex reference_line_groups_mutex_;
   std::vector<std::vector<ReferenceLine>> reference_line_groups_;
 };
 
-- 
GitLab