Planning: used constant ratio between evaluated points and spline and third...

 Planning: used constant ratio between evaluated points and spline and third order smoothness. (#690)

* Planning: use third order smoothness.

* Planning: used constant ratio between evaluated points and spline
number.

modules/planning/conf/planning_config.pb.txt

@@ -90,7 +90,6 @@ em_planner_config {
 
         qp_spline_config {
             number_of_discrete_graph_t: 4
-            number_of_evaluated_graph_t: 40
             spline_order: 6
             speed_kernel_weight: 0.0
             accel_kernel_weight: 1000.0

modules/planning/proto/qp_st_speed_config.proto

@@ -6,16 +6,15 @@ import "modules/planning/proto/st_boundary_config.proto";
 
 message QpSplineConfig {
   optional uint32 number_of_discrete_graph_t = 1;
-  optional uint32 number_of_evaluated_graph_t = 2;
-  optional uint32 spline_order = 3;
-  optional double speed_kernel_weight = 4;
-  optional double accel_kernel_weight = 5;
-  optional double jerk_kernel_weight = 6;
-  optional double follow_weight = 7;
-  optional double stop_weight = 8;
-  optional double cruise_weight = 9;
-  optional double regularization_weight = 10 [ default = 0.1 ];
-  optional double follow_drag_distance = 11;
+  optional uint32 spline_order = 2;
+  optional double speed_kernel_weight = 3;
+  optional double accel_kernel_weight = 4;
+  optional double jerk_kernel_weight = 5;
+  optional double follow_weight = 6;
+  optional double stop_weight = 7;
+  optional double cruise_weight = 8;
+  optional double regularization_weight = 9 [ default = 0.1 ];
+  optional double follow_drag_distance = 10;
 }
 
 message QpPiecewiseConfig {

modules/planning/tasks/qp_spline_st_speed/qp_spline_st_graph.cc

@@ -43,29 +43,27 @@ QpSplineStGraph::QpSplineStGraph(Spline1dGenerator* spline_generator,
       qp_st_speed_config_(qp_st_speed_config),
       t_knots_resolution_(
           qp_st_speed_config_.total_time() /
-          qp_st_speed_config_.qp_spline_config().number_of_discrete_graph_t()),
-      t_evaluated_resolution_(qp_st_speed_config_.total_time() /
-                              qp_st_speed_config_.qp_spline_config()
-                                  .number_of_evaluated_graph_t()) {
+          qp_st_speed_config_.qp_spline_config().number_of_discrete_graph_t()) {
   Init();
 }
 
 void QpSplineStGraph::Init() {
   // init knots
   double curr_t = 0.0;
-  for (uint32_t i = 0;
-       i <= qp_st_speed_config_.qp_spline_config().number_of_discrete_graph_t();
-       ++i) {
+  uint32_t num_spline =
+      qp_st_speed_config_.qp_spline_config().number_of_discrete_graph_t() - 1;
+  for (uint32_t i = 0; i <= num_spline; ++i) {
     t_knots_.push_back(curr_t);
     curr_t += t_knots_resolution_;
   }
 
+  uint32_t num_evaluated_t = 10 * num_spline + 1;
+
   // init evaluated t positions
   curr_t = 0;
-  for (uint32_t i = 0;
-       i <=
-       qp_st_speed_config_.qp_spline_config().number_of_evaluated_graph_t();
-       ++i) {
+  t_evaluated_resolution_ =
+      qp_st_speed_config_.total_time() / (num_evaluated_t - 1);
+  for (uint32_t i = 0; i < num_evaluated_t; ++i) {
     t_evaluated_.push_back(curr_t);
     curr_t += t_evaluated_resolution_;
   }
@@ -143,6 +141,7 @@ Status QpSplineStGraph::ApplyConstraint(
     AERROR << msg;
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
+
   if (!constraint->AddPointDerivativeConstraint(0.0, init_point_.v())) {
     const std::string msg = "add st start point velocity constraint failed!";
     AERROR << msg;
@@ -157,7 +156,7 @@ Status QpSplineStGraph::ApplyConstraint(
   }
 
   // smoothness constraint
-  if (!constraint->AddSecondDerivativeSmoothConstraint()) {
+  if (!constraint->AddThirdDerivativeSmoothConstraint()) {
     const std::string msg = "add smoothness joint constraint failed!";
     AERROR << msg;
     return Status(ErrorCode::PLANNING_ERROR, msg);