Planning: used list in reference line provider.

modules/canbus/vehicle/lincoln/lincoln_controller.cc

@@ -16,6 +16,8 @@
 
 #include "modules/canbus/vehicle/lincoln/lincoln_controller.h"
 
+#include "modules/common/proto/vehicle_signal.pb.h"
+
 #include "modules/canbus/can_comm/can_sender.h"
 #include "modules/canbus/vehicle/lincoln/lincoln_message_manager.h"
 #include "modules/canbus/vehicle/lincoln/protocol/brake_60.h"
@@ -25,7 +27,6 @@
 #include "modules/canbus/vehicle/lincoln/protocol/turnsignal_68.h"
 #include "modules/canbus/vehicle/vehicle_controller.h"
 #include "modules/common/log.h"
-#include "modules/common/proto/vehicle_signal.pb.h"
 #include "modules/common/time/time.h"
 
 namespace apollo {
@@ -498,7 +499,7 @@ bool LincolnController::CheckChassisError() {
   int32_t chassis_error_mask = 0;
   if (!chassis_detail.has_eps()) {
     AERROR_EVERY(100) << "ChassisDetail has NO eps."
-        << chassis_detail.DebugString();
+                      << chassis_detail.DebugString();
     return false;
   }
   bool steer_fault = chassis_detail.eps().watchdog_fault() |
@@ -520,7 +521,7 @@ bool LincolnController::CheckChassisError() {
 
   if (!chassis_detail.has_brake()) {
     AERROR_EVERY(100) << "ChassisDetail has NO brake."
-        << chassis_detail.DebugString();
+                      << chassis_detail.DebugString();
     return false;
   }
   // brake fault
@@ -542,7 +543,7 @@ bool LincolnController::CheckChassisError() {
 
   if (!chassis_detail.has_gas()) {
     AERROR_EVERY(100) << "ChassisDetail has NO gas."
-        << chassis_detail.DebugString();
+                      << chassis_detail.DebugString();
     return false;
   }
   // throttle fault
@@ -562,7 +563,7 @@ bool LincolnController::CheckChassisError() {
 
   if (!chassis_detail.has_gear()) {
     AERROR_EVERY(100) << "ChassisDetail has NO gear."
-        << chassis_detail.DebugString();
+                      << chassis_detail.DebugString();
     return false;
   }
   // gear fault
@@ -575,33 +576,33 @@ bool LincolnController::CheckChassisError() {
 
   if (steer_fault) {
     AERROR_EVERY(100) << "Steering fault detected: "
-        << chassis_detail.eps().watchdog_fault() << ", "
-        << chassis_detail.eps().channel_1_fault() << ", "
-        << chassis_detail.eps().channel_2_fault() << ", "
-        << chassis_detail.eps().calibration_fault() << ", "
-        << chassis_detail.eps().connector_fault();
+                      << chassis_detail.eps().watchdog_fault() << ", "
+                      << chassis_detail.eps().channel_1_fault() << ", "
+                      << chassis_detail.eps().channel_2_fault() << ", "
+                      << chassis_detail.eps().calibration_fault() << ", "
+                      << chassis_detail.eps().connector_fault();
   }
 
   if (brake_fault) {
     AERROR_EVERY(100) << "Brake fault detected: "
-        << chassis_detail.brake().watchdog_fault() << ", "
-        << chassis_detail.brake().channel_1_fault() << ", "
-        << chassis_detail.brake().channel_2_fault() << ", "
-        << chassis_detail.brake().boo_fault() << ", "
-        << chassis_detail.brake().connector_fault();
+                      << chassis_detail.brake().watchdog_fault() << ", "
+                      << chassis_detail.brake().channel_1_fault() << ", "
+                      << chassis_detail.brake().channel_2_fault() << ", "
+                      << chassis_detail.brake().boo_fault() << ", "
+                      << chassis_detail.brake().connector_fault();
   }
 
   if (throttle_fault) {
     AERROR_EVERY(100) << "Throttle fault detected: "
-        << chassis_detail.gas().watchdog_fault() << ", "
-        << chassis_detail.gas().channel_1_fault() << ", "
-        << chassis_detail.gas().channel_2_fault() << ", "
-        << chassis_detail.gas().connector_fault();
+                      << chassis_detail.gas().watchdog_fault() << ", "
+                      << chassis_detail.gas().channel_1_fault() << ", "
+                      << chassis_detail.gas().channel_2_fault() << ", "
+                      << chassis_detail.gas().connector_fault();
   }
 
   if (gear_fault) {
     AERROR_EVERY(100) << "Gear fault detected: "
-        << chassis_detail.gear().canbus_fault();
+                      << chassis_detail.gear().canbus_fault();
   }
 
   if (steer_fault || brake_fault || throttle_fault) {

modules/planning/reference_line/reference_line_provider.cc

@@ -44,7 +44,7 @@ void ReferenceLineProvider::Init(
 
 bool ReferenceLineProvider::Start() {
   if (!is_initialized_) {
-    AERROR << "LincolnController has NOT been initiated.";
+    AERROR << "ReferenceLineProvider has NOT been initiated.";
     return false;
   }
   const auto &func = [this] { Generate(); };
@@ -71,18 +71,19 @@ std::vector<ReferenceLine> ReferenceLineProvider::GetReferenceLines() {
   // 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.
+  while (reference_line_groups_.empty()) {
+    std::this_thread::sleep_for(std::chrono::duration<double, std::milli>(20));
+  }
   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)) {
@@ -93,6 +94,8 @@ bool ReferenceLineProvider::CreateReferenceLineFromRouting(
   ReferenceLineSmoother smoother;
   smoother.Init(smoother_config_);
 
+  std::vector<ReferenceLine> reference_lines;
+  // TODO: Added code to enable partially smoothed reference line here.
   for (const auto &segments : route_segments) {
     hdmap::Path hdmap_path;
     pnc_map_->CreatePathFromLaneSegments(segments, &hdmap_path);
@@ -115,6 +118,11 @@ bool ReferenceLineProvider::CreateReferenceLineFromRouting(
 
   std::lock_guard<std::mutex> lock(reference_line_groups_mutex_);
   reference_line_groups_.push_back(reference_lines);
+  const size_t kMaxStoredReferenceLineGroups = 3;
+  while (reference_line_groups_.size() > kMaxStoredReferenceLineGroups) {
+    reference_line_groups_.pop_front();
+  }
+
   return true;
 }
 

modules/planning/reference_line/reference_line_provider.h

@@ -21,6 +21,7 @@
 #ifndef MODULES_PLANNING_REFERENCE_LINE_REFERENCE_LINE_PROVIDER_H_
 #define MODULES_PLANNING_REFERENCE_LINE_REFERENCE_LINE_PROVIDER_H_
 
+#include <list>
 #include <memory>
 #include <mutex>
 #include <thread>
@@ -76,7 +77,7 @@ class ReferenceLineProvider {
   ReferenceLineSmootherConfig smoother_config_;
 
   std::mutex reference_line_groups_mutex_;
-  std::vector<std::vector<ReferenceLine>> reference_line_groups_;
+  std::list<std::vector<ReferenceLine>> reference_line_groups_;
 };
 
 }  // namespace planning