Planning: added UpdateRoutingResponse in reference line provider.

modules/planning/planning.cc

@@ -111,9 +111,7 @@ Status Planning::Init() {
 
   if (FLAGS_enable_reference_line_provider_thread) {
     ReferenceLineProvider::instance()->Init(
-        pnc_map_.get(),
-        AdapterManager::GetRoutingResponse()->GetLatestObserved(),
-        config_.reference_line_smoother_config());
+        pnc_map_.get(), config_.reference_line_smoother_config());
   }
 
   RegisterPlanners();
@@ -163,6 +161,11 @@ void Planning::PublishPlanningPb(ADCTrajectory* trajectory_pb,
 void Planning::RunOnce() {
   const double start_timestamp = Clock::NowInSecond();
   AdapterManager::Observe();
+  if (FLAGS_enable_reference_line_provider_thread) {
+    ReferenceLineProvider::instance()->UpdateRoutingResponse(
+        AdapterManager::GetRoutingResponse()->GetLatestObserved());
+  }
+
   ADCTrajectory not_ready_pb;
   auto* not_ready = not_ready_pb.mutable_decision()
                         ->mutable_main_decision()

modules/planning/reference_line/reference_line_provider.cc

@@ -42,10 +42,8 @@ ReferenceLineProvider::~ReferenceLineProvider() {
 
 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;
 }
@@ -67,6 +65,13 @@ void ReferenceLineProvider::Stop() {
   }
 }
 
+void ReferenceLineProvider::UpdateRoutingResponse(
+    const routing::RoutingResponse &routing_response) {
+  std::lock_guard<std::mutex> lock(routing_response_mutex_);
+  // TODO: check if routing needs to be updated before assigning.
+  routing_response_ = routing_response;
+}
+
 void ReferenceLineProvider::Generate() {
   while (!is_stop_) {
     const auto &curr_adc_position =
@@ -74,7 +79,14 @@ void ReferenceLineProvider::Generate() {
     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_)) {
+    routing::RoutingResponse routing;
+    {
+      std::lock_guard<std::mutex> lock(routing_response_mutex_);
+      // TODO: check if routing needs to be updated before assigning.
+      routing = routing_response_;
+    }
+
+    if (!CreateReferenceLineFromRouting(adc_point_enu, routing)) {
       AERROR << "Fail to create reference line at position: "
              << curr_adc_position.ShortDebugString();
     };

modules/planning/reference_line/reference_line_provider.h

@@ -53,13 +53,14 @@ class ReferenceLineProvider {
   ~ReferenceLineProvider();
 
   void Init(const hdmap::PncMap* pnc_map_,
-            const routing::RoutingResponse& routing_response,
             const ReferenceLineSmootherConfig& smoother_config);
 
   bool Start();
 
   void Stop();
 
+  void UpdateRoutingResponse(const routing::RoutingResponse& routing_response);
+
   std::vector<ReferenceLine> GetReferenceLines();
 
  private:
@@ -75,7 +76,10 @@ class ReferenceLineProvider {
   std::unique_ptr<std::thread> thread_;
 
   const hdmap::PncMap* pnc_map_ = nullptr;
+
+  std::mutex routing_response_mutex_;
   routing::RoutingResponse routing_response_;
+
   ReferenceLineSmootherConfig smoother_config_;
 
   bool is_stop_ = false;