Planning: added a yeild test case in huanbaoyuan_bzone.

modules/planning/integration_tests/huanbaoyuan_bzone_test.cc

@@ -53,5 +53,19 @@ TEST_F(HuangbaoyuanBzoneTest, follow_01) {
   RUN_GOLDEN_TEST;
 }
 
+/*
+ * test yield a vehicle with short distance
+ * A follow test case
+ */
+TEST_F(HuangbaoyuanBzoneTest, yield_01) {
+  std::string seq_num = "2";
+  FLAGS_test_routing_response_file = seq_num + "_routing.pb.txt";
+  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";
+  PlanningTestBase::SetUp();
+  RUN_GOLDEN_TEST;
+}
+
 }  // namespace planning
 }  // namespace apollo

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

+engine_started: true
+engine_rpm: 1754.25
+speed_mps: 7.897222
+odometer_m: 0
+fuel_range_m: 0
+throttle_percentage: 15.666438
+brake_percentage: 16.562141
+steering_percentage: -0.063829787
+steering_torque_nm: -0.125
+parking_brake: false
+driving_mode: COMPLETE_AUTO_DRIVE
+error_code: NO_ERROR
+gear_location: GEAR_DRIVE
+header {
+  timestamp_sec: 1505101393.3468192
+  module_name: "chassis"
+  sequence_num: 282201
+}
+signal {
+  turn_signal: TURN_NONE
+  horn: false
+}

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

+header {
+  timestamp_sec: 1505101393.3447967
+  module_name: "localization"
+  sequence_num: 339279
+}
+pose {
+  position {
+    x: 431290.62080435769
+    y: 4434163.4986545779
+    z: 37.649068114347756
+  }
+  orientation {
+    qx: 0.013311572863710507
+    qy: -0.015911126320045706
+    qz: -0.98399622400351683
+    qw: 0.17697759528805437
+  }
+  linear_velocity {
+    x: 2.5814399587564454
+    y: -7.628428665599718
+    z: 0.037299487377163931
+  }
+  linear_acceleration {
+    x: -0.18383662202527579
+    y: -0.50694497810611439
+    z: -0.017856213299102652
+  }
+  angular_velocity {
+    x: -0.0073818565566837252
+    y: -0.020557571246561164
+    z: 0.0074780197974178928
+  }
+  heading: -1.2152603056341071
+  linear_acceleration_vrf {
+    x: 0.34940166934072847
+    y: 0.41033822782005736
+    z: -0.025474909802842449
+  }
+  angular_velocity_vrf {
+    x: 0.013931754337978606
+    y: 0.0169608813469603
+    z: 0.0071596820370750493
+  }
+  euler_angles {
+    x: 0.02058005938950478
+    y: -0.036032473302519442
+    z: -1.2152603056341069
+  }
+}

modules/planning/testdata/huanbaoyuan_bzone_test/2_prediction.pb.txt

+header {
+  timestamp_sec: 1505101393.3019331
+  module_name: "prediction"
+  sequence_num: 28068
+}
+prediction_obstacle {
+  perception_obstacle {
+    id: 4158
+    position {
+      x: 431296.87429503544
+      y: 4434154.3371385094
+      z: 36.9573671016528
+    }
+    theta: -1.3921904617986804
+    velocity {
+      x: 2.3102715015411377
+      y: -12.797189712524414
+      z: 0
+    }
+    length: 3.3672661781311035
+    width: 2.0058133602142334
+    height: 1.5423381328582764
+    polygon_point {
+      x: 431296.33161370753
+      y: 4434155.949719443
+      z: 36.924911757003478
+    }
+    polygon_point {
+      x: 431296.89339112473
+      y: 4434156.0083878208
+      z: 36.938396735268952
+    }
+    polygon_point {
+      x: 431297.25667218777
+      y: 4434155.97731573
+      z: 36.94726413645752
+    }
+    polygon_point {
+      x: 431297.61403230415
+      y: 4434155.88530886
+      z: 36.956118026279711
+    }
+    polygon_point {
+      x: 431297.77263301687
+      y: 4434154.6420603916
+      z: 36.962611459158339
+    }
+    polygon_point {
+      x: 431296.8649937968
+      y: 4434152.8384185694
+      z: 36.944468245431928
+    }
+    polygon_point {
+      x: 431296.75801689969
+      y: 4434152.7424579421
+      z: 36.942081157241013
+    }
+    polygon_point {
+      x: 431296.72408223752
+      y: 4434152.7208246123
+      z: 36.941305158172746
+    }
+    polygon_point {
+      x: 431296.4043362916
+      y: 4434152.5387077313
+      z: 36.93394706810647
+    }
+    polygon_point {
+      x: 431296.30206146132
+      y: 4434152.5320757786
+      z: 36.931483415949209
+    }
+    polygon_point {
+      x: 431296.05896869168
+      y: 4434153.2152535506
+      z: 36.924137276600639
+    }
+    polygon_point {
+      x: 431295.74234338262
+      y: 4434155.6332200859
+      z: 36.911310499179784
+    }
+    polygon_point {
+      x: 431296.1361018655
+      y: 4434155.8770893607
+      z: 36.920329999808914
+    }
+    tracking_time: 57.699061155319214
+    type: UNKNOWN
+    timestamp: 0
+  }
+  timestamp: 1505101393.3015127
+  predicted_period: 3
+  trajectory {
+    probability: 1
+    trajectory_point {
+      path_point {
+        x: 431296.87429503544
+        y: 4434154.3371385094
+        z: 0
+        theta: -1.395290767519392
+      }
+      v: 13.004053942873885
+      a: 0.85043566191641462
+      relative_time: 0
+    }
+    trajectory_point {
+      path_point {
+        x: 431297.10111647064
+        y: 4434153.058046421
+        z: 0
+        theta: -1.4015107181298847
+      }
+      v: 12.977022157450948
+      a: 0.85043566191641462
+      relative_time: 0.1
+    }
+    trajectory_point {
+      path_point {
+        x: 431297.31952647591
+        y: 4434151.7802080987
+        z: 0
+        theta: -1.4077561782177936
+      }
+      v: 12.950492423729418
+      a: 0.85043566191641462
+      relative_time: 0.2
+    }
+    trajectory_point {
+      path_point {
+        x: 431297.5295250513
+        y: 4434150.5036235414
+        z: 0
+        theta: -1.414026814844852
+      }
+      v: 12.924467833356795
+      a: 0.85043566191641462
+      relative_time: 0.30000000000000004
+    }
+    trajectory_point {
+      path_point {
+        x: 431297.73111219675
+        y: 4434149.22829275
+        z: 0
+        theta: -1.4203222844643495
+      }
+      v: 12.898951443825853
+      a: 0.85043566191641462
+      relative_time: 0.4
+    }
+    trajectory_point {
+      path_point {
+        x: 431297.92428791232
+        y: 4434147.954215724
+        z: 0
+        theta: -1.4266422329304784
+      }
+      v: 12.873946276926157
+      a: 0.85043566191641462
+      relative_time: 0.5
+    }
+    trajectory_point {
+      path_point {
+        x: 431298.109052198
+        y: 4434146.6813924639
+        z: 0
+        theta: -1.4329862955171528
+      }
+      v: 12.849455317190912
+      a: 0.85043566191641462
+      relative_time: 0.60000000000000009
+    }
+    trajectory_point {
+      path_point {
+        x: 431298.28540505376
+        y: 4434145.40982297
+        z: 0
+        theta: -1.4393540969464997
+      }
+      v: 12.82548151034073
+      a: 0.85043566191641462
+      relative_time: 0.70000000000000007
+    }
+    trajectory_point {
+      path_point {
+        x: 431298.45334647957
+        y: 4434144.1395072415
+        z: 0
+        theta: -1.4457452514272087
+      }
+      v: 12.802027761725844
+      a: 0.85043566191641462
+      relative_time: 0.8
+    }
+    trajectory_point {
+      path_point {
+        x: 431298.6128764755
+        y: 4434142.8704452785
+        z: 0
+        theta: -1.452159362702919
+      }
+      v: 12.779096934768425
+      a: 0.85043566191641462
+      relative_time: 0.9
+    }
+    trajectory_point {
+      path_point {
+        x: 431298.76399504155
+        y: 4434141.6026370814
+        z: 0
+        theta: -1.4585960241107991
+      }
+      v: 12.756691849406627
+      a: 0.85043566191641462
+      relative_time: 1
+    }
+    trajectory_point {
+      path_point {
+        x: 431298.90670217766
+        y: 4434140.3360826494
+        z: 0
+        theta: -1.4650548186504697
+      }
+      v: 12.73481528054209
+      a: 0.85043566191641462
+      relative_time: 1.1
+    }
+    trajectory_point {
+      path_point {
+        x: 431299.04099788383
+        y: 4434139.0707819834
+        z: 0
+        theta: -1.4715353190633953
+      }
+      v: 12.713469956492617
+      a: 0.85043566191641462
+      relative_time: 1.2000000000000002
+    }
+    trajectory_point {
+      path_point {
+        x: 431299.16688216012
+        y: 4434137.8067350835
+        z: 0
+        theta: -1.4780370879228606
+      }
+      v: 12.69265855745177
+      a: 0.85043566191641462
+      relative_time: 1.3
+    }
+    trajectory_point {
+      path_point {
+        x: 431299.28435500647
+        y: 4434136.5439419486
+        z: 0
+        theta: -1.4845596777346275
+      }
+      v: 12.672383713957215
+      a: 0.85043566191641462
+      relative_time: 1.4000000000000001
+    }
+    trajectory_point {
+      path_point {
+        x: 431299.39341642294
+        y: 4434135.28240258
+        z: 0
+        theta: -1.4911026310483504
+      }
+      v: 12.652648005369613
+      a: 0.85043566191641462
+      relative_time: 1.5
+    }
+    trajectory_point {
+      path_point {
+        x: 431299.49406640953
+        y: 4434134.0221169768
+        z: 0
+        theta: -1.4976654805798071
+      }
+      v: 12.6334539583639
+      a: 0.85043566191641462
+      relative_time: 1.6
+    }
+    trajectory_point {
+      path_point {
+        x: 431299.58630496619
+        y: 4434132.763085139
+        z: 0
+        theta: -1.5042477493439925
+      }
+      v: 12.614804045434809
+      a: 0.85043566191641462
+      relative_time: 1.7000000000000002
+    }
+    trajectory_point {
+      path_point {
+        x: 431299.67013209296
+        y: 4434131.5053070672
+        z: 0
+        theta: -1.5108489507990832
+      }
+      v: 12.596700683418559
+      a: 0.85043566191641462
+      relative_time: 1.8
+    }
+    trajectory_point {
+      path_point {
+        x: 431299.74554778979
+        y: 4434130.2487827614
+        z: 0
+        theta: -1.517468589001286
+      }
+      v: 12.579146232032521
+      a: 0.85043566191641462
+      relative_time: 1.9000000000000001
+    }
+    trajectory_point {
+      path_point {
+        x: 431299.81255205668
+        y: 4434128.9935122207
+        z: 0
+        theta: -1.5241061587705427
+      }
+      v: 12.562142992434831
+      a: 0.85043566191641462
+      relative_time: 2
+    }
+    trajectory_point {
+      path_point {
+        x: 431299.8711448937
+        y: 4434127.739495446
+        z: 0
+        theta: -1.5307611458670434
+      }
+      v: 12.545693205805817
+      a: 0.85043566191641462
+      relative_time: 2.1
+    }
+    trajectory_point {
+      path_point {
+        x: 431299.92132630083
+        y: 4434126.4867324373
+        z: 0
+        theta: -1.5374330271784975
+      }
+      v: 12.529799051953153
+      a: 0.85043566191641462
+      relative_time: 2.2
+    }
+    trajectory_point {
+      path_point {
+        x: 431299.963096278
+        y: 4434125.2352231937
+        z: 0
+        theta: -1.5441212709180629
+      }
+      v: 12.514462647942628
+      a: 0.85043566191641462
+      relative_time: 2.3000000000000003
+    }
+    trajectory_point {
+      path_point {
+        x: 431299.99645482533
+        y: 4434123.984967716
+        z: 0
+        theta: -1.550825336832832
+      }
+      v: 12.499686046756434
+      a: 0.85043566191641462
+      relative_time: 2.4000000000000004
+    }
+    trajectory_point {
+      path_point {
+        x: 431300.02140194271
+        y: 4434122.7359660044
+        z: 0
+        theta: -1.5575446764227407
+      }
+      v: 12.485471235980841
+      a: 0.85043566191641462
+      relative_time: 2.5
+    }
+    trajectory_point {
+      path_point {
+        x: 431300.03793763014
+        y: 4434121.4882180579
+        z: 0
+        theta: -1.5642787331697479
+      }
+      v: 12.471820136525123
+      a: 0.85043566191641462
+      relative_time: 2.6
+    }
+    trajectory_point {
+      path_point {
+        x: 431300.0460618877
+        y: 4434120.2417238774
+        z: 0
+        theta: -1.5710269427771091
+      }
+      v: 12.458734601373584
+      a: 0.85043566191641462
+      relative_time: 2.7
+    }
+    trajectory_point {
+      path_point {
+        x: 431300.04577471537
+        y: 4434118.9964834619
+        z: 0
+        theta: -1.577788733418541
+      }
+      v: 12.446216414372458
+      a: 0.85043566191641462
+      relative_time: 2.8000000000000003
+    }
+    trajectory_point {
+      path_point {
+        x: 431300.03707611311
+        y: 4434117.7524968134
+        z: 0
+        theta: -1.577788733418541
+      }
+      v: 12.434267289053528
+      a: 0.85043566191641462
+      relative_time: 2.9000000000000004
+    }
+  }
+}
+perception_error_code: OK

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

+header {
+  timestamp_sec: 1505101372.3203726
+  module_name: "routing"
+  sequence_num: 29
+}
+route {
+  road_info {
+    id: "131-72-64"
+    in_lane {
+      id: "64_1_-1"
+      s: 63.683761497934711
+    }
+    out_lane {
+      id: "72_1_-1"
+      s: 42.389566612060662
+    }
+    passage_region {
+      segment {
+        id: "64_1_-1"
+        start_s: 63.683761497934711
+        end_s: 144.45
+      }
+      segment {
+        id: "131_1_-1"
+        start_s: 0
+        end_s: 36.9394
+      }
+      segment {
+        id: "72_1_-1"
+        start_s: 0
+        end_s: 42.389566612060662
+      }
+    }
+  }
+}
+measurement {
+  distance: 160.09520511412595
+}
+routing_request {
+  header {
+    timestamp_sec: 1505101372.320029
+    module_name: "dreamview"
+    sequence_num: 35
+  }
+  start {
+    id: "64_1_-1"
+    s: 63.683761497934711
+    pose {
+      x: 431259.224200729
+      y: 4434253.73378437
+    }
+  }
+  end {
+    id: "72_1_-1"
+    s: 42.389566612060662
+    pose {
+      x: 431311.80160710122
+      y: 4434102.6230789386
+    }
+  }
+}
+map_version: "1.400000"
+error_code {
+  error_id: SUCCESS
+  error_string: "Success!"
+}