Planning: (1) changed the way to add init point constraint; (2) muted

unnecessary error msg in reference line provider.

modules/planning/reference_line/reference_line_provider.cc

@@ -72,6 +72,7 @@ void ReferenceLineProvider::UpdateRoutingResponse(
   std::lock_guard<std::mutex> lock(routing_response_mutex_);
   // TODO(all): check if routing needs to be updated before assigning.
   routing_response_ = routing_response;
+  has_routing_ = true;
 }
 
 void ReferenceLineProvider::Generate() {
@@ -80,6 +81,19 @@ void ReferenceLineProvider::Generate() {
         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());
+    const int32_t kReferenceLineProviderSleepTime = 1000;
+    if (!has_routing_) {
+      AERROR << "Routing is not ready.";
+      std::this_thread::sleep_for(std::chrono::duration<double, std::milli>(
+          kReferenceLineProviderSleepTime));
+      continue;
+    }
+
+    if (!curr_adc_position.has_x() || !curr_adc_position.has_y()) {
+      AERROR << "VehicleState is not ready.";
+      std::this_thread::sleep_for(std::chrono::duration<double, std::milli>(
+          kReferenceLineProviderSleepTime));
+    }
 
     routing::RoutingResponse routing;
     {

modules/planning/reference_line/reference_line_provider.h

@@ -77,6 +77,7 @@ class ReferenceLineProvider {
 
   const hdmap::PncMap* pnc_map_ = nullptr;
 
+  bool has_routing_ = false;
   std::mutex routing_response_mutex_;
   routing::RoutingResponse routing_response_;
 

modules/planning/tasks/qp_spline_st_speed/qp_spline_st_graph.cc

@@ -246,9 +246,22 @@ Status QpSplineStGraph::ApplyConstraint(
   } else {
     constexpr double kInitPointAccelRelaxedSpeed = 1.0;
     constexpr double kInitPointAccelRelaxedRange = 0.5;
-    if (init_point_.v() > kInitPointAccelRelaxedSpeed) {
-      accel_lower_bound.front() = init_point_.a() - kInitPointAccelRelaxedRange;
-      accel_upper_bound.front() = init_point_.a() + kInitPointAccelRelaxedRange;
+    if (init_point_.v() > kInitPointAccelRelaxedSpeed &&
+        (init_point_.a() > accel_bound.second - kInitPointAccelRelaxedRange ||
+         init_point_.a() < accel_bound.first + kInitPointAccelRelaxedRange)) {
+      accel_lower_bound.front() =
+          std::max(accel_lower_bound.front(),
+                   init_point_.a() - kInitPointAccelRelaxedRange);
+      accel_upper_bound.front() =
+          std::min(accel_upper_bound.front(),
+                   init_point_.a() + kInitPointAccelRelaxedRange);
+    } else {
+      if (!constraint->AddPointSecondDerivativeConstraint(0.0,
+                                                          init_point_.a())) {
+        const std::string msg =
+            "Fail to apply init point acceleration constraints.";
+        return Status(ErrorCode::PLANNING_ERROR, msg);
+      }
     }
   }