Planning: use piecewise linear method to solve qp problem when spline (#493)

fails. fixed init point printout bug.

modules/common/vehicle_state/vehicle_state.cc

@@ -55,6 +55,12 @@ Status VehicleState::Update(
 
   InitAdcBoundingBox();
 
+  ADEBUG << x_ << ", " << y_ << ", " << z_ << ", " << roll_ << ", " << pitch_
+         << ", " << yaw_ << ", " << heading_ << ", " << kappa_ << ", "
+         << linear_v_ << ", " << angular_v_ << ", " << timestamp_ << ", "
+         << linear_a_y_ << ", " << pose_.ShortDebugString() << ", "
+         << adc_bounding_box_->DebugString();
+
   return Status::OK();
 }
 

modules/planning/common/planning_gflags.cc

@@ -134,18 +134,16 @@ DEFINE_bool(enable_rule_layer, true,
 /// common
 DEFINE_double(stop_max_distance_buffer, 4.0,
               "distance buffer of passing stop line");
-DEFINE_double(stop_min_speed, 0.1,
-              "min speed for computing stop");
-DEFINE_double(stop_max_deceleration, 6.0,
-              "max deceleration");
+DEFINE_double(stop_min_speed, 0.1, "min speed for computing stop");
+DEFINE_double(stop_max_deceleration, 6.0, "max deceleration");
 /// traffic light
 DEFINE_bool(enable_signal_lights, false, "enable signal_lights");
 DEFINE_string(signal_light_virtual_object_id_prefix, "SL_",
               "prefix for converting signal id to virtual object id");
 DEFINE_double(max_deacceleration_for_yellow_light_stop, 2.0,
               "treat yellow light as red when deceleration (abstract value"
-                  " in m/s^2) is less than this threshold; otherwise treated"
-                  " as green light");
+              " in m/s^2) is less than this threshold; otherwise treated"
+              " as green light");
 /// crosswalk
 DEFINE_bool(enable_crosswalk, false, "enable crosswalk");
 DEFINE_string(crosswalk_virtual_object_id_prefix, "CW_",

modules/planning/conf/planning_config.pb.txt

@@ -107,7 +107,7 @@ em_planner_config {
         qp_piecewise_config {
             number_of_evaluated_graph_t: 40
             accel_kernel_weight: 1000.0
-            jerk_kernel_weight: 500.0
+            jerk_kernel_weight: 0.0
             follow_weight: 8.0
             stop_weight: 0.2
             cruise_weight: 0.1

modules/planning/tasks/qp_spline_path/qp_spline_path_generator.cc

@@ -62,6 +62,11 @@ bool QpSplinePathGenerator::Generate(
     const std::vector<const PathObstacle*>& path_obstacles,
     const SpeedData& speed_data, const common::TrajectoryPoint& init_point,
     PathData* const path_data) {
+  knots_.clear();
+  evaluated_s_.clear();
+
+  ADEBUG << "Init point: " << init_point.DebugString();
+
   if (!CalculateInitFrenetPoint(init_point, &init_frenet_point_)) {
     AERROR << "Fail to map init point: " << init_point.ShortDebugString();
     return false;

modules/planning/tasks/qp_spline_st_speed/qp_piecewise_st_graph.cc

@@ -68,17 +68,16 @@ void QpPiecewiseStGraph::SetDebugLogger(
 Status QpPiecewiseStGraph::Search(
     const StGraphData& st_graph_data, SpeedData* const speed_data,
     const std::pair<double, double>& accel_bound) {
-  ADEBUG << init_point_.DebugString();
   cruise_.clear();
 
+  init_point_ = st_graph_data.init_point();
+  ADEBUG << "Init point:" << init_point_.DebugString();
+
   // reset piecewise linear generator
   generator_.reset(new PiecewiseLinearGenerator(
       qp_st_speed_config_.qp_piecewise_config().number_of_evaluated_graph_t(),
       t_evaluated_resolution_));
 
-  // start to search for best st points
-  init_point_ = st_graph_data.init_point();
-
   if (!ApplyConstraint(st_graph_data.init_point(), st_graph_data.speed_limit(),
                        st_graph_data.st_boundaries(), accel_bound)
            .ok()) {

modules/planning/tasks/qp_spline_st_speed/qp_spline_st_graph.cc

@@ -82,13 +82,13 @@ Status QpSplineStGraph::Search(const StGraphData& st_graph_data,
                                const std::pair<double, double>& accel_bound) {
   cruise_.clear();
 
+  init_point_ = st_graph_data.init_point();
+  ADEBUG << "init point:" << init_point_.DebugString();
+
   // reset spline generator
   spline_generator_.reset(new Spline1dGenerator(
       t_knots_, qp_st_speed_config_.qp_spline_config().spline_order()));
 
-  // start to search for best st points
-  init_point_ = st_graph_data.init_point();
-
   if (!ApplyConstraint(st_graph_data.init_point(), st_graph_data.speed_limit(),
                        st_graph_data.st_boundaries(), accel_bound)
            .ok()) {
@@ -142,7 +142,6 @@ Status QpSplineStGraph::ApplyConstraint(
     AERROR << msg;
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
-  ADEBUG << "init point constraint:" << init_point.DebugString();
   if (!constraint->AddPointDerivativeConstraint(0.0, init_point_.v())) {
     const std::string msg = "add st start point velocity constraint failed!";
     AERROR << msg;

modules/planning/tasks/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc

@@ -121,7 +121,8 @@ Status QpSplineStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
 
     accel_bound.first = qp_st_speed_config_.min_deceleration();
     accel_bound.second = qp_st_speed_config_.max_acceleration();
-    ret = st_graph.Search(st_graph_data, speed_data, accel_bound);
+    QpPiecewiseStGraph piecewise_st_graph(qp_st_speed_config_, veh_param);
+    ret = piecewise_st_graph.Search(st_graph_data, speed_data, accel_bound);
 
     if (ret != Status::OK()) {
       std::string msg = common::util::StrCat(