planning: add one sunnyvale loop integration test to trigger qp st failed to solve.

modules/planning/integration_tests/sunnyvale_loop_test.cc

@@ -53,5 +53,18 @@ TEST_F(SunnyvaleLoopTest, cruise) {
   RUN_GOLDEN_TEST;
 }
 
+/*
+ * stop case to trigger QP ST failed to solve
+ */
+TEST_F(SunnyvaleLoopTest, stop) {
+  std::string seq_num = "2";
+  FLAGS_test_prediction_file = seq_num + "_prediction.pb.txt";
+  FLAGS_test_localization_file = seq_num + "_localization.pb.txt";
+  FLAGS_test_chassis_file = seq_num + "_chassis.pb.txt";
+  FLAGS_test_routing_response_file = seq_num + "_routing.pb.txt";
+  PlanningTestBase::SetUp();
+  RUN_GOLDEN_TEST;
+}
+
 }  // namespace planning
 }  // namespace apollo

modules/planning/tasks/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc

@@ -103,8 +103,9 @@ Status QpSplineStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
                             path_data.discretized_path().Length());
   if (st_graph.Search(st_graph_data, path_data, speed_data) != Status::OK()) {
     RecordSTGraphDebug(boundaries, speed_limits, *speed_data);
-    return Status(ErrorCode::PLANNING_ERROR,
-                  "Failed to search graph with quadratic programming!");
+    return Status(
+        ErrorCode::PLANNING_ERROR,
+        Name() + ":Failed to search graph with quadratic programming!");
   }
 
   // record debug info

modules/planning/testdata/sunnyvale_loop_test/2_chassis.pb.txt

+engine_started: true
+engine_rpm: 0
+speed_mps: 2.7111111
+odometer_m: 0
+fuel_range_m: 0
+throttle_percentage: 15.031663
+brake_percentage: 23.480583
+steering_percentage: -11.893617
+steering_torque_nm: -1.625
+parking_brake: false
+driving_mode: COMPLETE_MANUAL
+error_code: NO_ERROR
+gear_location: GEAR_DRIVE
+header {
+  timestamp_sec: 1504137801.1602917
+  module_name: "chassis"
+  sequence_num: 8582
+}
+signal {
+  turn_signal: TURN_RIGHT
+  horn: false
+}

modules/planning/testdata/sunnyvale_loop_test/2_localization.pb.txt

+header {
+  timestamp_sec: 1504137801.1540642
+  module_name: "localization"
+  sequence_num: 9641
+}
+pose {
+  position {
+    x: 587045.66240733874
+    y: 4141549.0626950948
+    z: -31.299898234196007
+  }
+  orientation {
+    qx: -0.045296475715011039
+    qy: 0.017067935912339849
+    qz: -0.17942469644699402
+    qw: 0.98258012047689813
+  }
+  linear_velocity {
+    x: 0.95160587339251179
+    y: 2.5243101138202366
+    z: -0.03652591539262251
+  }
+  linear_acceleration {
+    x: -0.24884682217253135
+    y: -0.90949121068718553
+    z: -0.20129972315595951
+  }
+  angular_velocity {
+    x: 0.012665705286737822
+    y: 0.000502381408384895
+    z: -0.061508888409814014
+  }
+  heading: 1.2103929796890078
+  linear_acceleration_vrf {
+    x: 0.092891639215445687
+    y: -0.91540677177473739
+    z: -0.28813567519156147
+  }
+  angular_velocity_vrf {
+    x: 0.012728191094051541
+    y: 0.010766229267606368
+    z: -0.060548305424112284
+  }
+  euler_angles {
+    x: 0.017366259648608176
+    y: 0.09528376629154689
+    z: 1.2103929796890078
+  }
+}

modules/planning/testdata/sunnyvale_loop_test/2_routing.pb.txt

+header {
+  timestamp_sec: 1504201419.0536358
+  module_name: "routing"
+  sequence_num: 36
+}
+route {
+  road_info {
+    id: "r0"
+    in_lane {
+      id: "27_1_-1"
+      s: 0
+    }
+    out_lane {
+      id: "27_1_-1"
+      s: 14.3635
+    }
+    passage_region {
+      segment {
+        id: "27_1_-1"
+        start_s: 0
+        end_s: 14.3635
+      }
+    }
+  }
+}
+route {
+  road_info {
+    id: "r1"
+    in_lane {
+      id: "81_1_-1"
+      s: 0
+    }
+    out_lane {
+      id: "81_1_-1"
+      s: 17.986
+    }
+    passage_region {
+      segment {
+        id: "81_1_-1"
+        start_s: 0
+        end_s: 17.986
+      }
+    }
+  }
+}
+route {
+  road_info {
+    id: "r2"
+    in_lane {
+      id: "160_1_-3"
+      s: 0
+    }
+    out_lane {
+      id: "160_1_-3"
+      s: 251.678
+    }
+    passage_region {
+      segment {
+        id: "160_1_-3"
+        start_s: 0
+        end_s: 251.678
+      }
+    }
+  }
+}
+route {
+  road_info {
+    id: "r3"
+    in_lane {
+      id: "822_1_-3"
+      s: 0
+    }
+    out_lane {
+      id: "822_1_-3"
+      s: 58.4734
+    }
+    passage_region {
+      segment {
+        id: "822_1_-3"
+        start_s: 0
+        end_s: 58.4734
+      }
+    }
+  }
+}
+route {
+  road_info {
+    id: "r4"
+    in_lane {
+      id: "165_1_-1"
+      s: 0
+    }
+    out_lane {
+      id: "165_1_-1"
+      s: 37.7612
+    }
+    passage_region {
+      segment {
+        id: "165_1_-1"
+        start_s: 0
+        end_s: 37.7612
+      }
+    }
+  }
+}
+route {
+  road_info {
+    id: "r5"
+    in_lane {
+      id: "138_1_-1"
+      s: 0
+    }
+    out_lane {
+      id: "138_1_-1"
+      s: 308.143
+    }
+    passage_region {
+      segment {
+        id: "138_1_-1"
+        start_s: 0
+        end_s: 308.143
+      }
+    }
+  }
+}
+route {
+  road_info {
+    id: "r6"
+    in_lane {
+      id: "144_1_-1"
+      s: 0
+    }
+    out_lane {
+      id: "144_1_-1"
+      s: 46.5655
+    }
+    passage_region {
+      segment {
+        id: "144_1_-1"
+        start_s: 0
+        end_s: 46.5655
+      }
+    }
+  }
+}
+route {
+  road_info {
+    id: "r7"
+    in_lane {
+      id: "140_1_-1"
+      s: 0
+    }
+    out_lane {
+      id: "140_1_-1"
+      s: 146.337
+    }
+    passage_region {
+      segment {
+        id: "140_1_-1"
+        start_s: 0
+        end_s: 146.337
+      }
+    }
+  }
+}
+route {
+  road_info {
+    id: "r8"
+    in_lane {
+      id: "747_1_-1"
+      s: 0
+    }
+    out_lane {
+      id: "747_1_-1"
+      s: 15.6326
+    }
+    passage_region {
+      segment {
+        id: "747_1_-1"
+        start_s: 0
+        end_s: 15.6326
+      }
+    }
+  }
+}
+route {
+  road_info {
+    id: "r9"
+    in_lane {
+      id: "141_1_-2"
+      s: 0
+    }
+    out_lane {
+      id: "141_1_-2"
+      s: 41.6019
+    }
+    passage_region {
+      segment {
+        id: "141_1_-2"
+        start_s: 0
+        end_s: 41.6019
+      }
+    }
+  }
+}
+route {
+  road_info {
+    id: "r10"
+    in_lane {
+      id: "906_1_-1"
+      s: 0
+    }
+    out_lane {
+      id: "906_1_-1"
+      s: 27.921
+    }
+    passage_region {
+      segment {
+        id: "906_1_-1"
+        start_s: 0
+        end_s: 27.921
+      }
+    }
+  }
+}
+route {
+  road_info {
+    id: "r11"
+    in_lane {
+      id: "9_1_-2"
+      s: 0
+    }
+    out_lane {
+      id: "9_1_-2"
+      s: 161.962
+    }
+    passage_region {
+      segment {
+        id: "9_1_-2"
+        start_s: 0
+        end_s: 161.962
+      }
+    }
+  }
+}
+route {
+  road_info {
+    id: "r12"
+    in_lane {
+      id: "13_1_-2"
+      s: 0
+    }
+    out_lane {
+      id: "13_1_-2"
+      s: 35.3398
+    }
+    passage_region {
+      segment {
+        id: "13_1_-2"
+        start_s: 0
+        end_s: 35.3398
+      }
+    }
+  }
+}
+route {
+  road_info {
+    id: "r13"
+    in_lane {
+      id: "12_1_-3"
+      s: 0
+    }
+    out_lane {
+      id: "12_1_-3"
+      s: 122.494
+    }
+    passage_region {
+      segment {
+        id: "12_1_-3"
+        start_s: 0
+        end_s: 122.494
+      }
+    }
+  }
+}
+route {
+  road_info {
+    id: "r14"
+    in_lane {
+      id: "133_1_-3"
+      s: 0
+    }
+    out_lane {
+      id: "133_1_-3"
+      s: 3.63182
+    }
+    passage_region {
+      segment {
+        id: "133_1_-3"
+        start_s: 0
+        end_s: 3.63182
+      }
+    }
+  }
+}
+route {
+  road_info {
+    id: "r15"
+    in_lane {
+      id: "88_1_-1"
+      s: 0
+    }
+    out_lane {
+      id: "88_1_-1"
+      s: 19.7333
+    }
+    passage_region {
+      segment {
+        id: "88_1_-1"
+        start_s: 0
+        end_s: 19.7333
+      }
+    }
+  }
+}
+route {
+  road_info {
+    id: "r16"
+    in_lane {
+      id: "21_1_-1"
+      s: 0
+    }
+    out_lane {
+      id: "21_1_-1"
+      s: 128.73
+    }
+    passage_region {
+      segment {
+        id: "21_1_-1"
+        start_s: 0
+        end_s: 128.73
+      }
+    }
+  }
+}
+route {
+  road_info {
+    id: "r17"
+    in_lane {
+      id: "22_1_-1"
+      s: 0
+    }
+    out_lane {
+      id: "22_1_-1"
+      s: 31.9409
+    }
+    passage_region {
+      segment {
+        id: "22_1_-1"
+        start_s: 0
+        end_s: 31.9409
+      }
+    }
+  }
+}
+route {
+  road_info {
+    id: "r18"
+    in_lane {
+      id: "23_1_-1"
+      s: 0
+    }
+    out_lane {
+      id: "23_1_-1"
+      s: 49.0576
+    }
+    passage_region {
+      segment {
+        id: "23_1_-1"
+        start_s: 0
+        end_s: 49.0576
+      }
+    }
+  }
+}
+measurement {
+  distance: 1519.3525200000001
+}
+routing_request {
+  header {
+    timestamp_sec: 1504201419.052084
+    module_name: "hmi_ros_bridge"
+    sequence_num: 34
+  }
+  start {
+    id: "27_1_-1"
+    s: 1.8700738747954115
+    pose {
+      x: 587045.54288088426
+      y: 4141548.7253392646
+      z: -31.29764156229794
+    }
+  }
+  end {
+    id: "23_1_-1"
+    s: 0
+    pose {
+      x: 586948.74012
+      y: 4141171.118641
+    }
+  }
+}
+map_version: "1.400000"

modules/planning/testdata/sunnyvale_loop_test/result_stop_0.pb.txt

+header {
+  module_name: "planning"
+  sequence_num: 2
+  status {
+    error_code: PLANNING_ERROR
+    msg: "QpSplineStSpeedOptimizer:Failed to search graph with quadratic programming!"
+  }
+}
+is_replan: true
+gear: GEAR_DRIVE