Planning: added flag for trajectory checker. default = true.

modules/planning/common/planning_gflags.cc

@@ -64,6 +64,9 @@ DEFINE_double(trajectory_time_resolution, 0.1,
 DEFINE_double(output_trajectory_time_resolution, 0.05,
               "Trajectory time resolution when publish");
 
+DEFINE_bool(enable_trajectory_check, true,
+            "Enable sanity check for planning trajectory.");
+
 DEFINE_double(speed_lower_bound, 0.0, "The lowest speed allowed.");
 DEFINE_double(speed_upper_bound, 40.0, "The highest speed allowed.");
 

modules/planning/common/planning_gflags.h

@@ -45,6 +45,7 @@ DECLARE_double(trajectory_time_resolution);
 DECLARE_double(output_trajectory_time_resolution);
 
 // parameters for trajectory sanity check
+DECLARE_bool(enable_trajectory_check);
 DECLARE_double(speed_lower_bound);
 DECLARE_double(speed_upper_bound);
 

modules/planning/constraint_checker/constraint_checker.cc

@@ -39,38 +39,38 @@ bool ConstraintChecker::ValidTrajectory(
     double t = p.relative_time();
     double lon_v = p.v();
     if (!WithinRange(lon_v, FLAGS_speed_lower_bound, FLAGS_speed_upper_bound)) {
-      AWARN << "Velocity at relative time " << t
-            << " exceeds bound, value: " << lon_v << ", bound ["
-            << FLAGS_speed_lower_bound << ", " << FLAGS_speed_upper_bound
-            << "].";
+      AERROR << "Velocity at relative time " << t
+             << " exceeds bound, value: " << lon_v << ", bound ["
+             << FLAGS_speed_lower_bound << ", " << FLAGS_speed_upper_bound
+             << "].";
       return false;
     }
 
     double lon_a = p.a();
     if (!WithinRange(lon_a, FLAGS_longitudinal_acceleration_lower_bound,
                      FLAGS_longitudinal_acceleration_upper_bound)) {
-      AWARN << "Longitudinal acceleration at relative time " << t
-            << " exceeds bound, value: " << lon_a << ", bound ["
-            << FLAGS_longitudinal_acceleration_lower_bound << ", "
-            << FLAGS_longitudinal_acceleration_upper_bound << "].";
+      AERROR << "Longitudinal acceleration at relative time " << t
+             << " exceeds bound, value: " << lon_a << ", bound ["
+             << FLAGS_longitudinal_acceleration_lower_bound << ", "
+             << FLAGS_longitudinal_acceleration_upper_bound << "].";
       return false;
     }
 
     double lat_a = lon_v * lon_v * p.path_point().kappa();
     if (!WithinRange(lat_a, -FLAGS_lateral_acceleration_bound,
                      FLAGS_lateral_acceleration_bound)) {
-      AWARN << "Lateral acceleration at relative time " << t
-            << " exceeds bound, value: " << lat_a << ", bound ["
-            << -FLAGS_lateral_acceleration_bound << ", "
-            << FLAGS_lateral_acceleration_bound << "].";
+      AERROR << "Lateral acceleration at relative time " << t
+             << " exceeds bound, value: " << lat_a << ", bound ["
+             << -FLAGS_lateral_acceleration_bound << ", "
+             << FLAGS_lateral_acceleration_bound << "].";
       return false;
     }
 
     double kappa = p.path_point().kappa();
     if (!WithinRange(kappa, -FLAGS_kappa_bound, FLAGS_kappa_bound)) {
-      AWARN << "Kappa at relative time " << t
-            << " exceeds bound, value: " << kappa << ", bound ["
-            << -FLAGS_kappa_bound << ", " << FLAGS_kappa_bound << "].";
+      AERROR << "Kappa at relative time " << t
+             << " exceeds bound, value: " << kappa << ", bound ["
+             << -FLAGS_kappa_bound << ", " << FLAGS_kappa_bound << "].";
       return false;
     }
   }

modules/planning/planner/em/em_planner.cc

@@ -153,7 +153,8 @@ Status EMPlanner::Plan(const TrajectoryPoint& planning_start_point,
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
 
-  if (!ConstraintChecker::ValidTrajectory(trajectory)) {
+  if (FLAGS_enable_trajectory_check &&
+      !ConstraintChecker::ValidTrajectory(trajectory)) {
     std::string msg = "Trajectory cannot pass constraint checker.";
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }

modules/planning/reference_line/reference_line_provider.cc

@@ -32,6 +32,12 @@ namespace planning {
 
 using apollo::common::VehicleState;
 
+ReferenceLineProvider::~ReferenceLineProvider() {
+  if (thread_ && thread_->joinable()) {
+    thread_->join();
+  }
+}
+
 void ReferenceLineProvider::Init(
     const hdmap::PncMap *pnc_map,
     const routing::RoutingResponse &routing_response,
@@ -49,20 +55,24 @@ bool ReferenceLineProvider::Start() {
   }
   const auto &func = [this] { Generate(); };
   thread_.reset(new std::thread(func));
-  return false;
+  return true;
 }
 
 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();
-  };
+
+  while (true) {
+    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::this_thread::sleep_for(std::chrono::duration<double, std::milli>(500));
+  }
 }
 
 std::vector<ReferenceLine> ReferenceLineProvider::GetReferenceLines() {

modules/planning/reference_line/reference_line_provider.h

@@ -50,7 +50,7 @@ class ReferenceLineProvider {
   /**
    * @brief Default destructor.
    */
-  virtual ~ReferenceLineProvider() = default;
+  ~ReferenceLineProvider();
 
   void Init(const hdmap::PncMap* pnc_map_,
             const routing::RoutingResponse& routing_response,