planning: added bypass parked bus test case

Author: startcode

modules/planning/common/path/frenet_frame_path.cc

@@ -59,7 +59,11 @@ const common::FrenetFramePoint& FrenetFramePath::PointAt(
 
 common::FrenetFramePoint FrenetFramePath::EvaluateByS(const double s) const {
   CHECK_GT(points_.size(), 1);
-  CHECK(s < points_.back().s() + 1.0e-6 && s > points_.front().s() - 1.0e-6);
+  if (s < points_.front().s()) {
+    return points_.front();
+  } else if (s > points_.back().s()) {
+    return points_.back();
+  }
   auto func = [](const common::FrenetFramePoint& p, const double s) {
     return p.s() < s;
   };

modules/planning/integration_tests/sunnyvale_big_loop_test.cc

@@ -383,6 +383,22 @@ TEST_F(SunnyvaleBigLoopTest, abort_change_lane_for_fast_back_vehicle) {
   RUN_GOLDEN_TEST(0);
 }
 
+TEST_F(SunnyvaleBigLoopTest, bypass_parked_bus) {
+  std::string seq_num = "14";
+  ENABLE_RULE(TrafficRuleConfig::SIGNAL_LIGHT, false);
+  ENABLE_RULE(TrafficRuleConfig::KEEP_CLEAR, false);
+  double acc_lower_bound = FLAGS_longitudinal_acceleration_lower_bound;
+  FLAGS_longitudinal_acceleration_lower_bound = -5.0;
+
+  FLAGS_test_routing_response_file = seq_num + "_routing.pb.txt";
+  FLAGS_test_localization_file = seq_num + "_localization.pb.txt";
+  FLAGS_test_chassis_file = seq_num + "_chassis.pb.txt";
+  FLAGS_test_prediction_file = seq_num + "_prediction.pb.txt";
+  PlanningTestBase::SetUp();
+  RUN_GOLDEN_TEST(0);
+  FLAGS_longitudinal_acceleration_lower_bound = acc_lower_bound;
+}
+
 }  // namespace planning
 }  // namespace apollo
 

modules/planning/tasks/path_decider/path_decider.cc

@@ -119,7 +119,7 @@ bool PathDecider::MakeStaticObstacleDecision(
 
     const auto &sl_boundary = path_obstacle->PerceptionSLBoundary();
 
-    if (sl_boundary.start_s() < frenet_points.front().s() ||
+    if (sl_boundary.end_s() < frenet_points.front().s() ||
         sl_boundary.start_s() > frenet_points.back().s()) {
       path_decision->AddLongitudinalDecision("PathDecider/not-in-s",
                                              obstacle.Id(), object_decision);

modules/planning/testdata/sunnyvale_big_loop_test/14_chassis.pb.txt

@@ -0,0 +1,47 @@
+engine_started: true
+engine_rpm: 1579.75
+speed_mps: 5.7388887
+odometer_m: 0
+fuel_range_m: 0
+throttle_percentage: 20.282291
+brake_percentage: 13.795682
+steering_percentage: 6.93617
+steering_torque_nm: -0.6875
+parking_brake: false
+driving_mode: EMERGENCY_MODE
+error_code: NO_ERROR
+gear_location: GEAR_DRIVE
+header {
+  timestamp_sec: 1522106647.1464088
+  module_name: "ReadChassis"
+  sequence_num: 467
+}
+signal {
+  turn_signal: TURN_NONE
+  horn: false
+}
+chassis_gps {
+  latitude: 37.415354666666666
+  longitude: -122.02169599999999
+  gps_valid: true
+  year: 18
+  month: 3
+  day: 26
+  hours: 23
+  minutes: 24
+  seconds: 5
+  compass_direction: 315
+  pdop: 0.8
+  is_gps_fault: false
+  is_inferred: false
+  altitude: -42.5
+  heading: 326.62
+  hdop: 0.4
+  vdop: 0.8
+  quality: FIX_3D
+  num_satellites: 20
+  gps_speed: 2.2352
+}
+engage_advice {
+  advice: READY_TO_ENGAGE
+}

modules/planning/testdata/sunnyvale_big_loop_test/14_localization.pb.txt

@@ -0,0 +1,67 @@
+header {
+  timestamp_sec: 1522106647.144696
+  module_name: "ReadLocalization"
+  sequence_num: 466
+}
+pose {
+  position {
+    x: 586570.3472055248
+    y: 4141402.880225373
+    z: -30.680353011945414
+  }
+  orientation {
+    qx: -0.010145286116276428
+    qy: -0.027475237214918437
+    qz: 0.32649884305782262
+    qw: 0.94474371656630041
+  }
+  linear_velocity {
+    x: -3.5773662857560424
+    y: 4.6205659153984753
+    z: -0.039802275369643883
+  }
+  linear_acceleration {
+    x: -0.42941585748983885
+    y: 0.013148328436435376
+    z: -0.9648528347682479
+  }
+  angular_velocity {
+    x: -0.01418661182677949
+    y: 0.016210112362066036
+    z: 0.076738693818134945
+  }
+  heading: 2.235445261914399
+  linear_acceleration_vrf {
+    x: 0.0710412859916687
+    y: -0.052861869335174561
+    z: 8.8451430201530457
+  }
+  angular_velocity_vrf {
+    x: 0.0023441799302176968
+    y: 0.018646939005570923
+    z: 0.077457416126574155
+  }
+  euler_angles {
+    x: -0.037119180370530878
+    y: -0.045336014204200852
+    z: 0.66464893511950229
+  }
+}
+uncertainty {
+  position_std_dev {
+    x: 0.010972627294067385
+    y: 0.0094467178561181558
+    z: 0.012520283942028898
+  }
+  orientation_std_dev {
+    x: 0.059240168003579841
+    y: 0.0693377333103652
+    z: 0.049555902244396544
+  }
+  linear_velocity_std_dev {
+    x: 0.012027445662990917
+    y: 0.011323486403483626
+    z: 0.0065185331184555474
+  }
+}
+measurement_time: 1522106647.105104