Planning: add failsafe for speed limit violation and some refactors

modules/planning/conf/planning_config.pb.txt

@@ -37,7 +37,7 @@ default_task_config: {
     jerk_weight: 300.0
     lat_acc_weight: 1000.0
     ref_v_weight: 0.0
-    ref_s_weight: 10.0
+    ref_s_weight: 100.0
     use_warm_start: true
   }
 }

modules/planning/conf/scenario/lane_follow_config.pb.txt

@@ -16,8 +16,8 @@ stage_config: {
   task_type: DP_ST_SPEED_OPTIMIZER
   task_type: SPEED_DECIDER
   task_type: SPEED_BOUNDS_FINAL_DECIDER
-  # task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
-  task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
+  task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
+  # task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
   task_type: DECIDER_RSS
 
   task_config: {

modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_nonlinear_optimizer.h

@@ -40,14 +40,55 @@ class PiecewiseJerkSpeedNonlinearOptimizer : public SpeedOptimizer {
                          const common::TrajectoryPoint& init_point,
                          SpeedData* const speed_data) override;
 
-  PiecewiseJerkTrajectory1d SmoothSpeedLimit(const SpeedLimit& speed_limit);
+  common::Status SetUpStatesAndBounds(const PathData& path_data);
 
-  PiecewiseJerkTrajectory1d SmoothPathCurvature(const PathData& path_data);
+  bool CheckSpeedLimitFeasibility();
 
-  bool CheckStBoundFeasibility(
-      const int num_of_knots, const double delta_t,
-      const std::array<double, 3>& init_s,
-      const std::vector<std::pair<double, double>>& s_bounds);
+  common::Status SmoothSpeedLimit();
+
+  common::Status SmoothPathCurvature(const PathData& path_data);
+
+  common::Status OptimizeByQP(SpeedData* const speed_data,
+                              std::vector<double>* distance,
+                              std::vector<double>* velocity,
+                              std::vector<double>* acceleration);
+
+  common::Status OptimizeByNLP(std::vector<double>* distance,
+                               std::vector<double>* velocity,
+                               std::vector<double>* acceleration);
+
+  // st problem dimensions
+  double delta_t_ = 0.0;
+  double total_length_ = 0.0;
+  double total_time_ = 0.0;
+  int num_of_knots_ = 0;
+
+  // st initial values
+  double s_init_ = 0.0;
+  double s_dot_init_ = 0.0;
+  double s_ddot_init_ = 0.0;
+
+  // st states dynamically feasibility bounds
+  double s_dot_max_ = 0.0;
+  double s_ddot_max_ = 0.0;
+  double s_ddot_min_ = 0.0;
+  double s_dddot_abs_max_ = 0.0;
+
+  // st safety bounds
+  std::vector<std::pair<double, double>> s_bounds_;
+
+  // speed limits
+  SpeedLimit speed_limit_;
+  PiecewiseJerkTrajectory1d smoothed_speed_limit_;
+
+  // smoothed path curvature profile as a function of traversal distance
+  PiecewiseJerkTrajectory1d smoothed_path_curvature_;
+
+  // reference speed profile
+  SpeedData reference_speed_data_;
+
+  // reference cruise speed
+  double cruise_speed_;
 };
 
 }  // namespace planning