diff --git a/modules/planning/integration_tests/sunnyvale_big_loop_test.cc b/modules/planning/integration_tests/sunnyvale_big_loop_test.cc index 10a3b57f9f22eb901c5f7f360cb75d96251b1b54..1b165df402507c19d5a84ecddc1d93249b4ea82e 100644 --- a/modules/planning/integration_tests/sunnyvale_big_loop_test.cc +++ b/modules/planning/integration_tests/sunnyvale_big_loop_test.cc @@ -315,6 +315,19 @@ TEST_F(SunnyvaleBigLoopTest, traffic_light_green) { FLAGS_enable_prediction = true; } +TEST_F(SunnyvaleBigLoopTest, abort_change_lane_for_fast_back_vehicle) { + std::string seq_num = "11"; + FLAGS_enable_traffic_light = true; + FLAGS_enable_keep_clear = false; + + 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); +} + } // namespace planning } // namespace apollo diff --git a/modules/planning/tasks/speed_decider/speed_decider.cc b/modules/planning/tasks/speed_decider/speed_decider.cc index 93e336f92ad8ab8210841ca4f2c9bbb23080b970..a5531ff983037156e3bb8948c8f5df3bb426e50e 100644 --- a/modules/planning/tasks/speed_decider/speed_decider.cc +++ b/modules/planning/tasks/speed_decider/speed_decider.cc @@ -203,6 +203,11 @@ Status SpeedDecider::MakeObjectDecision( path_obstacle->AddLongitudinalDecision("dp_st_graph/cross", stop_decision); } + const std::string msg = + "Failed to find a solution for crossing obstacle:" + + obstacle->Id(); + AERROR << msg; + return Status(ErrorCode::PLANNING_ERROR, msg); } break; default: diff --git a/modules/planning/testdata/sunnyvale_big_loop_test/11_chassis.pb.txt b/modules/planning/testdata/sunnyvale_big_loop_test/11_chassis.pb.txt new file mode 100755 index 0000000000000000000000000000000000000000..2ad0fcb11a809a87c6ac96bbaf4250ca2ef70978 --- /dev/null +++ b/modules/planning/testdata/sunnyvale_big_loop_test/11_chassis.pb.txt @@ -0,0 +1,47 @@ +engine_started: true +engine_rpm: 0 +speed_mps: 15.675 +odometer_m: 0 +fuel_range_m: 0 +throttle_percentage: 26.564432 +brake_percentage: 14.959945 +steering_percentage: -1.2978723 +steering_torque_nm: -2 +parking_brake: false +driving_mode: COMPLETE_AUTO_DRIVE +error_code: NO_ERROR +gear_location: GEAR_DRIVE +header { + timestamp_sec: 1518648377.319916 + module_name: "chassis" + sequence_num: 247176 +} +signal { + turn_signal: TURN_NONE + horn: false +} +chassis_gps { + latitude: 37.416343999999995 + longitude: -122.01127466666665 + gps_valid: true + year: 18 + month: 2 + day: 14 + hours: 22 + minutes: 46 + seconds: 16 + compass_direction: 270 + pdop: 1.2000000000000002 + is_gps_fault: false + is_inferred: false + altitude: -42.5 + heading: 280.79 + hdop: 0.60000000000000009 + vdop: 1 + quality: FIX_3D + num_satellites: 15 + gps_speed: 15.6464 +} +engage_advice { + advice: READY_TO_ENGAGE +} diff --git a/modules/planning/testdata/sunnyvale_big_loop_test/11_localization.pb.txt b/modules/planning/testdata/sunnyvale_big_loop_test/11_localization.pb.txt new file mode 100755 index 0000000000000000000000000000000000000000..a20758db6a4f1209367c47fe0f2b32a46e1a1972 --- /dev/null +++ b/modules/planning/testdata/sunnyvale_big_loop_test/11_localization.pb.txt @@ -0,0 +1,65 @@ +header { + timestamp_sec: 1518648377.318433 +} +pose { + position { + x: 587484.83465911169 + y: 4141516.1493382631 + z: -31.054244682315154 + } + orientation { + qx: -0.040942574445715293 + qy: -0.014721659937242887 + qz: 0.64633749625859693 + qw: 0.76181022522450914 + } + linear_velocity { + x: -15.653488587347258 + y: 2.7232072309594466 + z: 0.0389828765207829 + } + linear_acceleration { + x: -0.081311947742334745 + y: -0.33353668174305978 + z: -0.19380108962625364 + } + angular_velocity { + x: 0.026114438606160528 + y: -0.021704208479890529 + z: -0.0032407417076273865 + } + heading: 2.9792000682148903 + linear_acceleration_vrf { + x: -0.635143369436264 + y: -0.75582414865493774 + z: 9.56149771809578 + } + angular_velocity_vrf { + x: -0.017016663098035602 + y: -0.028918964590953212 + z: -0.0061372339261415885 + } + euler_angles { + x: -0.08150146416476374 + y: 0.030601559067132196 + z: 1.4084037414199937 + } +} +uncertainty { + position_std_dev { + x: 0.040671955381023875 + y: 0.018467040459102767 + z: 0.01216545095810509 + } + orientation_std_dev { + x: 0.076589186909105891 + y: 0.076741187330964841 + z: 0.05053614958913609 + } + linear_velocity_std_dev { + x: 0.027532722769003766 + y: 0.01751289443884721 + z: 0.0061467432555478117 + } +} +measurement_time: 1518648377.3248789 diff --git a/modules/planning/testdata/sunnyvale_big_loop_test/11_prediction.pb.txt b/modules/planning/testdata/sunnyvale_big_loop_test/11_prediction.pb.txt new file mode 100755 index 0000000000000000000000000000000000000000..1f7fce9f59edc499b984b20a1fed6f92f26d375c --- /dev/null +++ b/modules/planning/testdata/sunnyvale_big_loop_test/11_prediction.pb.txt @@ -0,0 +1,1381 @@ +header { + timestamp_sec: 1518648377.2489274 + module_name: "prediction" + sequence_num: 22421 +} +prediction_obstacle { + perception_obstacle { + id: 9099 + position { + x: 587499.01100828883 + y: 4141510.6121053016 + z: -31.788609421753417 + } + theta: 2.9482207125994213 + velocity { + x: -16.463579502969996 + y: 3.224101176331041 + z: 0 + } + length: 4.6487603187561035 + width: 1.9675694704055786 + height: 1.2520534992218018 + polygon_point { + x: 587500.12889798253 + y: 4141509.8400453958 + z: -31.780001749682135 + } + polygon_point { + x: 587499.58627894509 + y: 4141509.7755013248 + z: -31.774393057678562 + } + polygon_point { + x: 587498.59358272934 + y: 4141509.6659686323 + z: -31.764349385390826 + } + polygon_point { + x: 587497.08541240834 + y: 4141510.0011689449 + z: -31.761834610059619 + } + polygon_point { + x: 587496.614834307 + y: 4141510.4710345594 + z: -31.770330423153851 + } + polygon_point { + x: 587496.65982819092 + y: 4141510.589390398 + z: -31.773666548868555 + } + polygon_point { + x: 587497.00682514813 + y: 4141511.4141321317 + z: -31.797158522022904 + } + polygon_point { + x: 587497.75924868591 + y: 4141511.78365799 + z: -31.812050350214385 + } + polygon_point { + x: 587501.18837055238 + y: 4141511.1872928175 + z: -31.821980073759395 + } + polygon_point { + x: 587501.35360810161 + y: 4141510.4918677364 + z: -31.805520210036018 + } + tracking_time: 2.8014459609985352 + type: VEHICLE + timestamp: 1518648377.2048476 + confidence: 0.8642655611038208 + confidence_type: CONFIDENCE_CNN + } + timestamp: 1518648377.2048476 + predicted_period: 5 + trajectory { + probability: 0.35692847273361905 + trajectory_point { + path_point { + x: 587499.01089524094 + y: 4141510.6121293772 + z: 0 + theta: 2.9301381707821958 + lane_id: "3143_1_-1" + } + v: 16.77355842684053 + a: 1.5713588540747896 + relative_time: 0 + } + trajectory_point { + path_point { + x: 587497.35655346827 + y: 4141510.9341530167 + z: 0 + theta: 2.9333734535217388 + lane_id: "3143_1_-1" + } + v: 16.933774175601997 + a: 1.6323275776128914 + relative_time: 0.1 + } + trajectory_point { + path_point { + x: 587495.68540042336 + y: 4141511.2600212628 + z: 0 + theta: 2.9250440326487555 + lane_id: "3143_1_-1" + } + v: 17.099898233654784 + a: 1.6895250399012138 + relative_time: 0.2 + } + trajectory_point { + path_point { + x: 587494.00123123254 + y: 4141511.6043723114 + z: 0 + theta: 2.9224447266566553 + lane_id: "3143_1_-1" + } + v: 17.271553474873912 + a: 1.7429512409397567 + relative_time: 0.30000000000000004 + } + trajectory_point { + path_point { + x: 587492.30129216379 + y: 4141511.9537354973 + z: 0 + theta: 2.9270147581609631 + lane_id: "3143_1_-1" + } + v: 17.448362773134409 + a: 1.79260618072852 + relative_time: 0.4 + } + trajectory_point { + path_point { + x: 587490.583238886 + y: 4141512.307432943 + z: 0 + theta: 2.9262877058473507 + lane_id: "3143_1_-1" + } + v: 17.629949002311292 + a: 1.8384898592675039 + relative_time: 0.5 + } + trajectory_point { + path_point { + x: 587488.8528028141 + y: 4141512.6627523536 + z: 0 + theta: 2.9543256087276681 + lane_id: "3143_1_-1" + } + v: 17.815935036279583 + a: 1.8806022765567083 + relative_time: 0.60000000000000009 + } + trajectory_point { + path_point { + x: 587487.08773345978 + y: 4141512.9737546109 + z: 0 + theta: 2.949729336634249 + lane_id: "3143_1_-1" + } + v: 18.005943748914305 + a: 1.9189434325961332 + relative_time: 0.70000000000000007 + } + trajectory_point { + path_point { + x: 587485.307454554 + y: 4141513.2915134984 + z: 0 + theta: 2.9577084619116132 + lane_id: "3143_1_-1" + } + v: 18.199598014090483 + a: 1.9535133273857785 + relative_time: 0.8 + } + trajectory_point { + path_point { + x: 587483.50709827477 + y: 4141513.6055158069 + z: 0 + theta: 2.9661886476266321 + lane_id: "3143_1_-1" + } + v: 18.396520705683137 + a: 1.9843119609256443 + relative_time: 0.9 + } + trajectory_point { + path_point { + x: 587481.682182449 + y: 4141513.9066746272 + z: 0 + theta: 2.9666603482427467 + lane_id: "3143_1_-1" + } + v: 18.596334697567286 + a: 2.0113393332157306 + relative_time: 1 + } + trajectory_point { + path_point { + x: 587479.83885833563 + y: 4141514.2141646151 + z: 0 + theta: 2.9699851093904197 + lane_id: "3143_1_-1" + } + v: 18.798662863617956 + a: 2.0345954442560377 + relative_time: 1.1 + } + trajectory_point { + path_point { + x: 587477.97923928685 + y: 4141514.5256357472 + z: 0 + theta: 2.9844010300530748 + lane_id: "3143_1_-1" + } + v: 19.003128077710169 + a: 2.0540802940465652 + relative_time: 1.2000000000000002 + } + trajectory_point { + path_point { + x: 587476.0899822138 + y: 4141514.8211112008 + z: 0 + theta: 2.979383006106417 + lane_id: "3143_1_-1" + } + v: 19.209353213718945 + a: 2.0697938825873128 + relative_time: 1.3 + } + trajectory_point { + path_point { + x: 587474.18406548037 + y: 4141515.1224428462 + z: 0 + theta: 2.9845373133020683 + lane_id: "3143_1_-1" + } + v: 19.416961145519306 + a: 2.0817362098782812 + relative_time: 1.4000000000000001 + } + trajectory_point { + path_point { + x: 587472.25405740982 + y: 4141515.4291671417 + z: 0 + theta: 2.9782114573805121 + lane_id: "3143_1_-1" + } + v: 19.625574746986274 + a: 2.08990727591947 + relative_time: 1.5 + } + trajectory_point { + path_point { + x: 587470.31159914355 + y: 4141515.6986177489 + z: 0 + theta: 3.0131885542237029 + lane_id: "3143_1_-1" + } + v: 19.834816891994873 + a: 2.0943070807108795 + relative_time: 1.6 + } + trajectory_point { + path_point { + x: 587468.33315799537 + y: 4141515.9598969603 + z: 0 + theta: 3.0082454609365552 + lane_id: "3143_1_-1" + } + v: 20.044310454420124 + a: 2.0949356242525097 + relative_time: 1.7000000000000002 + } + trajectory_point { + path_point { + x: 587466.33934466622 + y: 4141516.2210678076 + z: 0 + theta: 3.0201356780670956 + lane_id: "3143_1_-1" + } + v: 20.253678308137047 + a: 2.09179290654436 + relative_time: 1.8 + } + trajectory_point { + path_point { + x: 587464.31835724635 + y: 4141516.4737464068 + z: 0 + theta: 3.017406599472686 + lane_id: "3143_1_-1" + } + v: 20.462543327020668 + a: 2.084878927586431 + relative_time: 1.9000000000000001 + } + trajectory_point { + path_point { + x: 587462.2775630299 + y: 4141516.7316681757 + z: 0 + theta: 3.0162516417136036 + lane_id: "3143_1_-1" + } + v: 20.67052838494601 + a: 2.0741936873787221 + relative_time: 2 + } + trajectory_point { + path_point { + x: 587460.22016242647 + y: 4141516.9841925045 + z: 0 + theta: 3.0303289866822665 + lane_id: "3143_1_-1" + } + v: 20.877256355788088 + a: 2.0597371859212341 + relative_time: 2.1 + } + trajectory_point { + path_point { + x: 587458.13733633189 + y: 4141517.2303301306 + z: 0 + theta: 3.0326127326235603 + lane_id: "3143_1_-1" + } + v: 21.082350113421931 + a: 2.0415094232139666 + relative_time: 2.2 + } + trajectory_point { + path_point { + x: 587456.03275376256 + y: 4141517.4783503814 + z: 0 + theta: 3.03029553420562 + lane_id: "3143_1_-1" + } + v: 21.285432531722556 + a: 2.0195103992569194 + relative_time: 2.3000000000000003 + } + trajectory_point { + path_point { + x: 587453.9118481609 + y: 4141517.7281919131 + z: 0 + theta: 3.0411568501152466 + lane_id: "3143_1_-1" + } + v: 21.486126484564988 + a: 1.993740114050093 + relative_time: 2.4000000000000004 + } + trajectory_point { + path_point { + x: 587451.76446797524 + y: 4141517.9672890864 + z: 0 + theta: 3.0335601281508264 + lane_id: "3143_1_-1" + } + v: 21.684054845824249 + a: 1.9641985675934872 + relative_time: 2.5 + } + trajectory_point { + path_point { + x: 587449.60626789951 + y: 4141518.19948165 + z: 0 + theta: 3.0627078650099797 + lane_id: "3143_1_-1" + } + v: 21.87884048937536 + a: 1.9308857598871016 + relative_time: 2.6 + } + trajectory_point { + path_point { + x: 587447.41783900862 + y: 4141518.3841256211 + z: 0 + theta: 3.0681881325258606 + lane_id: "3143_1_-1" + } + v: 22.070106289093342 + a: 1.8938016909309365 + relative_time: 2.7 + } + trajectory_point { + path_point { + x: 587445.20810228609 + y: 4141518.5695840772 + z: 0 + theta: 3.06359849836306 + lane_id: "3143_1_-1" + } + v: 22.257475118853222 + a: 1.8529463607249921 + relative_time: 2.8000000000000003 + } + trajectory_point { + path_point { + x: 587442.98160979443 + y: 4141518.75762341 + z: 0 + theta: 3.0663728320869623 + lane_id: "3143_1_-1" + } + v: 22.440569852530015 + a: 1.8083197692692679 + relative_time: 2.9000000000000004 + } + trajectory_point { + path_point { + x: 587440.73813325446 + y: 4141518.9076683437 + z: 0 + theta: 3.0892640960379136 + lane_id: "3143_1_-1" + } + v: 22.619013363998747 + a: 1.7599219165637643 + relative_time: 3 + } + trajectory_point { + path_point { + x: 587438.471867501 + y: 4141519.0426910343 + z: 0 + theta: 3.0915506449392396 + lane_id: "3143_1_-1" + } + v: 22.79242852713444 + a: 1.7077528026084814 + relative_time: 3.1 + } + trajectory_point { + path_point { + x: 587436.18920288177 + y: 4141519.1751849009 + z: 0 + theta: 3.0999023997947788 + lane_id: "3143_1_-1" + } + v: 22.960438215812118 + a: 1.6518124274034189 + relative_time: 3.2 + } + trajectory_point { + path_point { + x: 587433.88795856072 + y: 4141519.28988582 + z: 0 + theta: 3.1012020281929136 + lane_id: "3143_1_-1" + } + v: 23.1226653039068 + a: 1.5921007909485767 + relative_time: 3.3000000000000003 + } + trajectory_point { + path_point { + x: 587431.57004342938 + y: 4141519.4043553155 + z: 0 + theta: 3.0964570485205742 + lane_id: "3143_1_-1" + } + v: 23.278732665293507 + a: 1.5286178932439551 + relative_time: 3.4000000000000004 + } + trajectory_point { + path_point { + x: 587429.23777134658 + y: 4141519.5139805106 + z: 0 + theta: 3.1017224407147 + lane_id: "3143_1_-1" + } + v: 23.428263173847263 + a: 1.4613637342895545 + relative_time: 3.5 + } + trajectory_point { + path_point { + x: 587426.890129948 + y: 4141519.6187113221 + z: 0 + theta: 3.1023768322684209 + lane_id: "3143_1_-1" + } + v: 23.570879703443094 + a: 1.390338314085374 + relative_time: 3.6 + } + trajectory_point { + path_point { + x: 587424.53042949259 + y: 4141519.7013521888 + z: 0 + theta: 3.136116307897046 + lane_id: "3143_1_-1" + } + v: 23.706205127956014 + a: 1.3155416326314135 + relative_time: 3.7 + } + trajectory_point { + path_point { + x: 587422.15342379059 + y: 4141519.7226217161 + z: 0 + theta: 3.1355976023158387 + lane_id: "3143_1_-1" + } + v: 23.833862321261051 + a: 1.2369736899276742 + relative_time: 3.8000000000000003 + } + trajectory_point { + path_point { + x: 587419.76408716291 + y: 4141519.7426776141 + z: 0 + theta: 3.1357181589087366 + lane_id: "3143_1_-1" + } + v: 23.953474157233224 + a: 1.1546344859741553 + relative_time: 3.9000000000000004 + } + trajectory_point { + path_point { + x: 587417.36330255016 + y: 4141519.762115255 + z: 0 + theta: 3.1386384931180205 + lane_id: "3143_1_-1" + } + v: 24.064663509747554 + a: 1.0685240207708571 + relative_time: 4 + } + trajectory_point { + path_point { + x: 587414.95171006117 + y: 4141519.779842088 + z: 0 + theta: 3.1392340964499823 + lane_id: "3143_1_-1" + } + v: 24.167053252679068 + a: 0.97864229431777872 + relative_time: 4.1000000000000005 + } + trajectory_point { + path_point { + x: 587412.53112282185 + y: 4141519.756123079 + z: 0 + theta: -3.1200691749951277 + lane_id: "3143_1_-1" + } + v: 24.260266259902785 + a: 0.88498930661492137 + relative_time: 4.2 + } + trajectory_point { + path_point { + x: 587410.10120666563 + y: 4141519.7109102993 + z: 0 + theta: -3.1227857844253277 + lane_id: "3143_1_-1" + } + v: 24.343925405293724 + a: 0.78756505766228513 + relative_time: 4.3 + } + trajectory_point { + path_point { + x: 587407.663505855 + y: 4141519.6653698008 + z: 0 + theta: -3.1198522592265312 + lane_id: "3143_1_-1" + } + v: 24.417653562726915 + a: 0.68636954745986767 + relative_time: 4.4 + } + trajectory_point { + path_point { + x: 587405.21948891738 + y: 4141519.6027049371 + z: 0 + theta: -3.1038689791257119 + lane_id: "3143_1_-1" + } + v: 24.481073606077373 + a: 0.58140277600767243 + relative_time: 4.5 + } + trajectory_point { + path_point { + x: 587402.77046348923 + y: 4141519.5088655003 + z: 0 + theta: -3.1038692742186518 + lane_id: "3143_1_-1" + } + v: 24.533808409220125 + a: 0.47266474330569652 + relative_time: 4.6000000000000005 + } + trajectory_point { + path_point { + x: 587400.31672640413 + y: 4141519.4147363738 + z: 0 + theta: -3.0995454385100092 + lane_id: "3143_1_-1" + } + v: 24.575480846030189 + a: 0.36015544935394184 + relative_time: 4.7 + } + trajectory_point { + path_point { + x: 587397.85942337615 + y: 4141519.3193614 + z: 0 + theta: -3.1007821248458503 + lane_id: "3143_1_-1" + } + v: 24.605713790382588 + a: 0.24387489415240671 + relative_time: 4.8000000000000007 + } + trajectory_point { + path_point { + x: 587395.39973033324 + y: 4141519.2228295719 + z: 0 + theta: -3.0995892557474352 + lane_id: "3143_1_-1" + } + v: 24.624130116152344 + a: 0.12382307770109313 + relative_time: 4.9 + } + } +} +prediction_obstacle { + perception_obstacle { + id: 9101 + position { + x: 587448.96330931422 + y: 4141510.2445439612 + z: -31.76170531561328 + } + theta: -0.094712560455648881 + velocity { + x: 21.686636897460925 + y: -0.84155611026185762 + z: 0 + } + length: 4.1052846908569336 + width: 1.4486050605773926 + height: 1.3536442518234253 + polygon_point { + x: 587450.93726426968 + y: 4141509.9156836881 + z: -31.757036736661327 + } + polygon_point { + x: 587450.974719464 + y: 4141509.8089058269 + z: -31.7545978206033 + } + polygon_point { + x: 587450.97129886632 + y: 4141509.6949262256 + z: -31.751676957186632 + } + polygon_point { + x: 587450.69563798991 + y: 4141509.3594203531 + z: -31.741136616327655 + } + polygon_point { + x: 587450.45478152472 + y: 4141509.3719984279 + z: -31.739694503821109 + } + polygon_point { + x: 587449.90107552672 + y: 4141509.450468373 + z: -31.737638231960386 + } + polygon_point { + x: 587447.327754595 + y: 4141510.8286384051 + z: -31.753831107399247 + } + polygon_point { + x: 587446.97346095648 + y: 4141511.0337063214 + z: -31.75644981891584 + } + polygon_point { + x: 587447.30436196656 + y: 4141511.0388798285 + z: -31.759001546939086 + } + polygon_point { + x: 587448.2879187488 + y: 4141511.0361513072 + z: -31.766126185460507 + } + polygon_point { + x: 587449.70842077548 + y: 4141510.8120080759 + z: -31.770821327474035 + } + polygon_point { + x: 587450.37973211019 + y: 4141510.6790379216 + z: -31.772353122004542 + } + polygon_point { + x: 587450.4723045415 + y: 4141510.6359392777 + z: -31.771935224249997 + } + tracking_time: 0.20010209083557129 + type: VEHICLE + timestamp: 1518648377.2048476 + confidence: 0.93401879072189331 + confidence_type: CONFIDENCE_CNN + } + timestamp: 1518648377.2048476 + predicted_period: 5 + trajectory { + probability: 0.75592930580345 + trajectory_point { + path_point { + x: 587448.96203751781 + y: 4141510.2446660316 + z: 0 + theta: -0.094002479554877727 + lane_id: "2422_1_-1" + } + v: 21.702953695627837 + a: 4.263107653351522 + relative_time: 0 + } + trajectory_point { + path_point { + x: 587451.142898296 + y: 4141510.0392523445 + z: 0 + theta: -0.090389695145434157 + lane_id: "2422_1_-1" + } + v: 22.13762015196356 + a: 4.4285162303015611 + relative_time: 0.1 + } + trajectory_point { + path_point { + x: 587453.36980828689 + y: 4141509.8299938673 + z: 0 + theta: -0.091883413897834831 + lane_id: "2422_1_-1" + } + v: 22.58831589307588 + a: 4.5836933488835561 + relative_time: 0.2 + } + trajectory_point { + path_point { + x: 587455.64182144648 + y: 4141509.6144950856 + z: 0 + theta: -0.092460312554266455 + lane_id: "2422_1_-1" + } + v: 23.054017773128002 + a: 4.7286390090975079 + relative_time: 0.30000000000000004 + } + trajectory_point { + path_point { + x: 587457.96152015275 + y: 4141509.4008876807 + z: 0 + theta: -0.093155463228125512 + lane_id: "2422_1_-1" + } + v: 23.533702646283114 + a: 4.8633532109434165 + relative_time: 0.4 + } + trajectory_point { + path_point { + x: 587460.33603086718 + y: 4141509.1298092226 + z: 0 + theta: -0.12617658691173972 + lane_id: "2422_1_-1" + } + v: 24.026347366704417 + a: 4.98783595442128 + relative_time: 0.5 + } + trajectory_point { + path_point { + x: 587462.742460885 + y: 4141508.8273986294 + z: 0 + theta: -0.11962017868404029 + lane_id: "2422_1_-1" + } + v: 24.5309287885551 + a: 5.1020872395311017 + relative_time: 0.60000000000000009 + } + trajectory_point { + path_point { + x: 587465.20281783259 + y: 4141508.5205185162 + z: 0 + theta: -0.12140336448226696 + lane_id: "2422_1_-1" + } + v: 25.04642376599837 + a: 5.2061070662728781 + relative_time: 0.70000000000000007 + } + trajectory_point { + path_point { + x: 587467.72814874293 + y: 4141508.1982660424 + z: 0 + theta: -0.16385193332965953 + lane_id: "2422_1_-1" + } + v: 25.57180915319741 + a: 5.2998954346466123 + relative_time: 0.8 + } + trajectory_point { + path_point { + x: 587470.27467522654 + y: 4141507.7664919677 + z: 0 + theta: -0.16111801059061293 + lane_id: "2422_1_-1" + } + v: 26.106061804315424 + a: 5.3834523446523015 + relative_time: 0.9 + } + trajectory_point { + path_point { + x: 587472.87669116363 + y: 4141507.3256869735 + z: 0 + theta: -0.16529661495926273 + lane_id: "2422_1_-1" + } + v: 26.6481585735156 + a: 5.4567777962899484 + relative_time: 1 + } + trajectory_point { + path_point { + x: 587475.53114460234 + y: 4141506.8754129354 + z: 0 + theta: -0.16584057171225108 + lane_id: "2422_1_-1" + } + v: 27.197076314961144 + a: 5.51987178955955 + relative_time: 1.1 + } + trajectory_point { + path_point { + x: 587478.24016107607 + y: 4141506.3980043284 + z: 0 + theta: -0.17611561968377654 + lane_id: "2422_1_-1" + } + v: 27.751791882815247 + a: 5.5727343244611092 + relative_time: 1.2000000000000002 + } + trajectory_point { + path_point { + x: 587480.99789939111 + y: 4141505.8870286164 + z: 0 + theta: -0.18090251457382323 + lane_id: "2422_1_-1" + } + v: 28.3112821312411 + a: 5.6153654009946248 + relative_time: 1.3 + } + trajectory_point { + path_point { + x: 587483.80797714845 + y: 4141505.3646475961 + z: 0 + theta: -0.17756012941974086 + lane_id: "2422_1_-1" + } + v: 28.8745239144019 + a: 5.6477650191600963 + relative_time: 1.4000000000000001 + } + trajectory_point { + path_point { + x: 587486.67697119527 + y: 4141504.833624796 + z: 0 + theta: -0.18409274979485613 + lane_id: "2422_1_-1" + } + v: 29.440494086460852 + a: 5.6699331789575238 + relative_time: 1.5 + } + trajectory_point { + path_point { + x: 587489.60611415119 + y: 4141504.2655538693 + z: 0 + theta: -0.2244435364406292 + lane_id: "2422_1_-1" + } + v: 30.008169501581136 + a: 5.6818698803869081 + relative_time: 1.6 + } + trajectory_point { + path_point { + x: 587492.55598396761 + y: 4141503.5767543111 + z: 0 + theta: -0.22444317699987737 + lane_id: "2422_1_-1" + } + v: 30.576527013925965 + a: 5.6835751234482483 + relative_time: 1.7000000000000002 + } + trajectory_point { + path_point { + x: 587495.56055102556 + y: 4141502.8754737438 + z: 0 + theta: -0.2217392517763388 + lane_id: "2422_1_-1" + } + v: 31.144543477658519 + a: 5.6750489081415463 + relative_time: 1.8 + } + trajectory_point { + path_point { + x: 587498.62265436456 + y: 4141502.1647044085 + z: 0 + theta: -0.22444241259871411 + lane_id: "2422_1_-1" + } + v: 31.711195746942003 + a: 5.6562912344667993 + relative_time: 1.9000000000000001 + } + trajectory_point { + path_point { + x: 587501.73811009468 + y: 4141501.4460906275 + z: 0 + theta: -0.2159054859883982 + lane_id: "2422_1_-1" + } + v: 32.275460675939613 + a: 5.6273021024240091 + relative_time: 2 + } + trajectory_point { + path_point { + x: 587504.912956233 + y: 4141500.7162436796 + z: 0 + theta: -0.22444161560208942 + lane_id: "2422_1_-1" + } + v: 32.836315118814539 + a: 5.5880815120131748 + relative_time: 2.1 + } + trajectory_point { + path_point { + x: 587508.13948117534 + y: 4141499.9724935563 + z: 0 + theta: -0.22289959472038934 + lane_id: "2422_1_-1" + } + v: 33.39273592972998 + a: 5.5386294632342965 + relative_time: 2.2 + } + trajectory_point { + path_point { + x: 587508.44552557857 + y: 4141499.8895271835 + z: 0 + theta: -0.22204427227995993 + lane_id: "2422_1_-1" + } + v: 33.943699962849131 + a: 5.478945956087375 + relative_time: 2.3000000000000003 + } + trajectory_point { + path_point { + x: 587514.73135150317 + y: 4141498.337347283 + z: 0 + theta: -0.23748883722924674 + lane_id: "1445_1_-1" + } + v: 34.488184072335187 + a: 5.40903099057241 + relative_time: 2.4000000000000004 + } + trajectory_point { + path_point { + x: 587518.1045061606 + y: 4141497.4974653991 + z: 0 + theta: -0.23979398643137539 + lane_id: "1445_1_-1" + } + v: 35.025165112351345 + a: 5.3288845666894016 + relative_time: 2.5 + } + trajectory_point { + path_point { + x: 587521.53006167768 + y: 4141496.646727642 + z: 0 + theta: -0.24354127030093764 + lane_id: "1445_1_-1" + } + v: 35.553619937060795 + a: 5.23850668443835 + relative_time: 2.6 + } + trajectory_point { + path_point { + x: 587523.56403343962 + y: 4141496.1351589011 + z: 0 + theta: -0.23519111553216163 + lane_id: "1445_1_-1" + } + v: 36.072525400626745 + a: 5.1378973438192537 + relative_time: 2.7 + } + trajectory_point { + path_point { + x: 587528.52669949492 + y: 4141494.8891992276 + z: 0 + theta: -0.24284853167726528 + lane_id: "1446_1_-1" + } + v: 36.580858357212378 + a: 5.0270565448321136 + relative_time: 2.8000000000000003 + } + trajectory_point { + path_point { + x: 587532.09787742316 + y: 4141493.9895260045 + z: 0 + theta: -0.24070282426312106 + lane_id: "1446_1_-1" + } + v: 37.077595660980904 + a: 4.90598428747693 + relative_time: 2.9000000000000004 + } + trajectory_point { + path_point { + x: 587535.71713328618 + y: 4141493.0788500519 + z: 0 + theta: -0.24054396504043485 + lane_id: "1446_1_-1" + } + v: 37.5617141660955 + a: 4.774680571753704 + relative_time: 3 + } + trajectory_point { + path_point { + x: 587539.383434123 + y: 4141492.157118585 + z: 0 + theta: -0.24625988118635167 + lane_id: "1446_1_-1" + } + v: 38.032190726719378 + a: 4.6331453976624335 + relative_time: 3.1 + } + trajectory_point { + path_point { + x: 587543.093508448 + y: 4141491.2229120657 + z: 0 + theta: -0.24405249164825538 + lane_id: "1446_1_-1" + } + v: 38.488002197015717 + a: 4.481378765203119 + relative_time: 3.2 + } + trajectory_point { + path_point { + x: 587546.84811790043 + y: 4141490.2806311082 + z: 0 + theta: -0.24585248036141838 + lane_id: "1446_1_-1" + } + v: 38.928125431147734 + a: 4.31938067437576 + relative_time: 3.3000000000000003 + } + trajectory_point { + path_point { + x: 587550.64406840224 + y: 4141489.3277515536 + z: 0 + theta: -0.24054207007615069 + lane_id: "1446_1_-1" + } + v: 39.3515372832786 + a: 4.1471511251803586 + relative_time: 3.4000000000000004 + } + trajectory_point { + path_point { + x: 587554.4813997373 + y: 4141488.3665258326 + z: 0 + theta: -0.24569541824143526 + lane_id: "1446_1_-1" + } + v: 39.757214607571527 + a: 3.9646901176169149 + relative_time: 3.5 + } + trajectory_point { + path_point { + x: 587558.35602607648 + y: 4141487.3935280992 + z: 0 + theta: -0.24054109637213639 + lane_id: "1446_1_-1" + } + v: 40.144134258189716 + a: 3.7719976516854259 + relative_time: 3.6 + } + trajectory_point { + path_point { + x: 587562.26853770972 + y: 4141486.4148983615 + z: 0 + theta: -0.24351626598105636 + lane_id: "1446_1_-1" + } + v: 40.511273089296353 + a: 3.5690737273858932 + relative_time: 3.7 + } + trajectory_point { + path_point { + x: 587566.21590601711 + y: 4141485.4284007507 + z: 0 + theta: -0.24925257743022744 + lane_id: "1446_1_-1" + } + v: 40.857607955054625 + a: 3.355918344718317 + relative_time: 3.8000000000000003 + } + trajectory_point { + path_point { + x: 587567.38735144853 + y: 4141485.1307419762 + z: 0 + theta: -0.24172786632788479 + lane_id: "1446_1_-1" + } + v: 41.182115709627745 + a: 3.1325315036826966 + relative_time: 3.9000000000000004 + } + trajectory_point { + path_point { + x: 587574.2076409522 + y: 4141483.4407641748 + z: 0 + theta: -0.23708379155639969 + lane_id: "1450_1_-1" + } + v: 41.4837732071789 + a: 2.898913204279034 + relative_time: 4 + } + trajectory_point { + path_point { + x: 587578.24792296859 + y: 4141482.4387413105 + z: 0 + theta: -0.25474466603422741 + lane_id: "1450_1_-1" + } + v: 41.761557301871285 + a: 2.6550634465073255 + relative_time: 4.1000000000000005 + } + trajectory_point { + path_point { + x: 587578.35124553309 + y: 4141482.4075533464 + z: 0 + theta: -0.2547446660342273 + lane_id: "1450_1_-1" + } + v: 42.014444847868091 + a: 2.4009822303675756 + relative_time: 4.2 + } + trajectory_point { + path_point { + x: 587586.40327406721 + y: 4141480.4188542995 + z: 0 + theta: -0.24053754813310579 + lane_id: "1451_1_-1" + } + v: 42.241412699332528 + a: 2.1366695558597821 + relative_time: 4.3 + } + trajectory_point { + path_point { + x: 587590.51403322467 + y: 4141479.403277331 + z: 0 + theta: -0.24053703094514711 + lane_id: "1451_1_-1" + } + v: 42.441437710427785 + a: 1.8621254229839423 + relative_time: 4.4 + } + trajectory_point { + path_point { + x: 587594.642567402 + y: 4141478.3820629618 + z: 0 + theta: -0.24203370204474428 + lane_id: "1451_1_-1" + } + v: 42.613496735317057 + a: 1.5773498317400616 + relative_time: 4.5 + } + trajectory_point { + path_point { + x: 587598.78668356326 + y: 4141477.3582076188 + z: 0 + theta: -0.2405359842783148 + lane_id: "1451_1_-1" + } + v: 42.756566628163526 + a: 1.2823427821281346 + relative_time: 4.6000000000000005 + } + trajectory_point { + path_point { + x: 587602.94370152138 + y: 4141476.3331203032 + z: 0 + theta: -0.24053545933986076 + lane_id: "1451_1_-1" + } + v: 42.86962424313041 + a: 0.97710427414816614 + relative_time: 4.7 + } + trajectory_point { + path_point { + x: 587607.1100326064 + y: 4141475.3050243128 + z: 0 + theta: -0.25174600136026903 + lane_id: "1451_1_-1" + } + v: 42.951646434380891 + a: 0.66163430780015187 + relative_time: 4.8000000000000007 + } + trajectory_point { + path_point { + x: 587611.28272359725 + y: 4141474.275183945 + z: 0 + theta: -0.23801616052843233 + lane_id: "1451_1_-1" + } + v: 43.001610056078178 + a: 0.33593288308409663 + relative_time: 4.9 + } + } +} +perception_error_code: OK +start_timestamp: 1518648377.2372673 +end_timestamp: 1518648377.2489269 diff --git a/modules/planning/testdata/sunnyvale_big_loop_test/11_routing.pb.txt b/modules/planning/testdata/sunnyvale_big_loop_test/11_routing.pb.txt new file mode 100755 index 0000000000000000000000000000000000000000..c1b6dd03894e8b151f42f46c8e9cdac1f13610c4 --- /dev/null +++ b/modules/planning/testdata/sunnyvale_big_loop_test/11_routing.pb.txt @@ -0,0 +1,1122 @@ +header { + timestamp_sec: 1518718246.4781277 + module_name: "routing" + sequence_num: 1 +} +road { + id: "1771" + passage { + segment { + id: "1771_1_-3" + start_s: 32.241981920357361 + end_s: 45.6667 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "1415" + passage { + segment { + id: "1415_1_-3" + start_s: 0 + end_s: 12.6104 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "1416" + passage { + segment { + id: "1416_1_-3" + start_s: 0 + end_s: 37.2856 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "1422" + passage { + segment { + id: "1422_1_-3" + start_s: 0 + end_s: 11.7449 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "1423" + passage { + segment { + id: "1423_1_-3" + start_s: 0 + end_s: 7.71293 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "1425" + passage { + segment { + id: "1425_1_-3" + start_s: 0 + end_s: 11.7655 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "1426" + passage { + segment { + id: "1426_1_-3" + start_s: 0 + end_s: 54.4931 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "1430" + passage { + segment { + id: "1430_1_-3" + start_s: 0 + end_s: 14.2715 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "1431" + passage { + segment { + id: "1431_1_-3" + start_s: 0 + end_s: 35.883 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "1436" + passage { + segment { + id: "1436_1_-3" + start_s: 0 + end_s: 10.9945 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "1781" + passage { + segment { + id: "1781_1_-3" + start_s: 0 + end_s: 6.07775 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "822" + passage { + segment { + id: "822_1_-3" + start_s: 0 + end_s: 7.23053 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "1441" + passage { + segment { + id: "1441_1_-3" + start_s: 0 + end_s: 10.7866 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "1440" + passage { + segment { + id: "1440_1_-3" + start_s: 0 + end_s: 39.7135 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "50a" + passage { + segment { + id: "50a_1_-1" + start_s: 0 + end_s: 41.1822 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "2421" + passage { + segment { + id: "2421_1_-1" + start_s: 0 + end_s: 1.62432 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "2622" + passage { + segment { + id: "2622_1_-1" + start_s: 0 + end_s: 8.51337 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "2623" + passage { + segment { + id: "2623_1_-1" + start_s: 0 + end_s: 60.4448 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "2628" + passage { + segment { + id: "2628_1_-1" + start_s: 0 + end_s: 11.0946 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "2629" + passage { + segment { + id: "2629_1_-1" + start_s: 0 + end_s: 34.4977 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "2631" + passage { + segment { + id: "2631_1_-1" + start_s: 0 + end_s: 11.5307 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "2632" + passage { + segment { + id: "2632_1_-1" + start_s: 0 + end_s: 79.7525 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "2634" + passage { + segment { + id: "2634_1_-1" + start_s: 0 + end_s: 11.1465 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "2635" + passage { + segment { + id: "2635_1_-1" + start_s: 0 + end_s: 87.7161 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "144" + passage { + segment { + id: "144_1_-1" + start_s: 0 + end_s: 46.7371 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "140" + passage { + segment { + id: "140_1_-1" + start_s: 0 + end_s: 56.3946 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "2644" + passage { + segment { + id: "2644_1_-1" + start_s: 0 + end_s: 12.9006 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "2645" + passage { + segment { + id: "2645_1_-1" + start_s: 0 + end_s: 11.2649 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "2647" + passage { + segment { + id: "2647_1_-1" + start_s: 0 + end_s: 11.7247 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "2648" + passage { + segment { + id: "2648_1_-1" + start_s: 0 + end_s: 10.7638 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "2650" + passage { + segment { + id: "2650_1_-1" + start_s: 0 + end_s: 10.2608 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "2651" + passage { + segment { + id: "2651_1_-1" + start_s: 0 + end_s: 10.0043 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "2659" + passage { + segment { + id: "2659_1_-1" + start_s: 0 + end_s: 10.8523 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "2658" + passage { + segment { + id: "2658_1_-1" + start_s: 0 + end_s: 10.5482 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "142" + passage { + segment { + id: "142_1_-1" + start_s: 0 + end_s: 17.1695 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "141" + passage { + segment { + id: "141_1_-1" + start_s: 0 + end_s: 41.9457 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "1954" + passage { + segment { + id: "1954_1_-1" + start_s: 0 + end_s: 9.41719 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "224a" + passage { + segment { + id: "224a_1_-1" + start_s: 0 + end_s: 42.7685 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "8" + passage { + segment { + id: "8_1_-1" + start_s: 0 + end_s: 10.9817 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "2448" + passage { + segment { + id: "2448_1_-1" + start_s: 0 + end_s: 60.7203 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "3" + passage { + segment { + id: "3_1_-1" + start_s: 0 + end_s: 16.2251 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "1948" + passage { + segment { + id: "1948_1_-1" + start_s: 0 + end_s: 12.3247 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "1949" + passage { + segment { + id: "1949_1_-1" + start_s: 0 + end_s: 113.652 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "1940" + passage { + segment { + id: "1940_1_-1" + start_s: 0 + end_s: 14.5523 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "1941" + passage { + segment { + id: "1941_1_-1" + start_s: 0 + end_s: 16.6837 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "154" + passage { + segment { + id: "154_1_-1" + start_s: 0 + end_s: 29.6923 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "4" + passage { + segment { + id: "4_1_-1" + start_s: 0 + end_s: 52.1101 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "311a" + passage { + segment { + id: "311a_1_-1" + start_s: 0 + end_s: 36.8369 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "52" + passage { + segment { + id: "52_1_-1" + start_s: 0 + end_s: 5.90927 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "2579" + passage { + segment { + id: "2579_1_-1" + start_s: 0 + end_s: 23.4018 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "57" + passage { + segment { + id: "57_1_-2" + start_s: 0 + end_s: 3.78832 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "2147" + passage { + segment { + id: "2147_1_-2" + start_s: 0 + end_s: 11.9045 + } + segment { + id: "2146_1_-2" + start_s: 0 + end_s: 25.9879 + } + segment { + id: "1851_1_-2" + start_s: 0 + end_s: 91.5222 + } + segment { + id: "1862_1_-2" + start_s: 0 + end_s: 11.4315 + } + segment { + id: "1861_1_-2" + start_s: 0 + end_s: 29.0621 + } + segment { + id: "1860_1_-2" + start_s: 0 + end_s: 10.9588 + } + segment { + id: "1859_1_-2" + start_s: 0 + end_s: 75.3919 + } + segment { + id: "1856_1_-2" + start_s: 0 + end_s: 31.7363 + } + segment { + id: "1855_1_-2" + start_s: 0 + end_s: 68.4178 + } + segment { + id: "1854_1_-2" + start_s: 0 + end_s: 39.9753 + } + segment { + id: "1853_1_-2" + start_s: 0 + end_s: 10.0026 + } + segment { + id: "1852_1_-2" + start_s: 0 + end_s: 29.6248 + } + segment { + id: "1850_1_-2" + start_s: 0 + end_s: 40.4842 + } + segment { + id: "1270_1_-2" + start_s: 0 + end_s: 16.9255 + } + segment { + id: "1848_1_-2" + start_s: 0 + end_s: 9.68808 + } + segment { + id: "1847_1_-2" + start_s: 0 + end_s: 18.7645 + } + segment { + id: "695_1_-2" + start_s: 0 + end_s: 11.8026 + } + segment { + id: "1844_1_-2" + start_s: 0 + end_s: 17.8526 + } + segment { + id: "1842_1_-2" + start_s: 0 + end_s: 14.8271 + } + segment { + id: "687_1_-2" + start_s: 0 + end_s: 75.707 + } + segment { + id: "1838_1_-2" + start_s: 0 + end_s: 9.72376 + } + segment { + id: "1837_1_-2" + start_s: 0 + end_s: 11.0742 + } + segment { + id: "2193_1_-2" + start_s: 0 + end_s: 11.5568 + } + can_exit: false + change_lane_type: LEFT + } + passage { + segment { + id: "2147_1_-1" + start_s: 0 + end_s: 11.9317 + } + segment { + id: "2146_1_-1" + start_s: 0 + end_s: 25.9902 + } + segment { + id: "1851_1_-1" + start_s: 0 + end_s: 91.4233 + } + segment { + id: "1862_1_-1" + start_s: 0 + end_s: 11.5187 + } + segment { + id: "1861_1_-1" + start_s: 0 + end_s: 28.9878 + } + segment { + id: "1860_1_-1" + start_s: 0 + end_s: 11.0053 + } + segment { + id: "1859_1_-1" + start_s: 0 + end_s: 75.3014 + } + segment { + id: "1856_1_-1" + start_s: 0 + end_s: 31.8025 + } + segment { + id: "1855_1_-1" + start_s: 0 + end_s: 68.3164 + } + segment { + id: "1854_1_-1" + start_s: 0 + end_s: 40.0975 + } + segment { + id: "1853_1_-1" + start_s: 0 + end_s: 9.95527 + } + segment { + id: "1852_1_-1" + start_s: 0 + end_s: 29.6546 + } + segment { + id: "1850_1_-1" + start_s: 0 + end_s: 40.4792 + } + segment { + id: "1270_1_-1" + start_s: 0 + end_s: 16.9229 + } + segment { + id: "1848_1_-1" + start_s: 0 + end_s: 9.61716 + } + segment { + id: "1847_1_-1" + start_s: 0 + end_s: 18.8422 + } + segment { + id: "695_1_-1" + start_s: 0 + end_s: 11.7727 + } + segment { + id: "1844_1_-1" + start_s: 0 + end_s: 17.847 + } + segment { + id: "1842_1_-1" + start_s: 0 + end_s: 14.6566 + } + segment { + id: "687_1_-1" + start_s: 0 + end_s: 75.8955 + } + segment { + id: "1838_1_-1" + start_s: 0 + end_s: 9.7439 + } + segment { + id: "1837_1_-1" + start_s: 0 + end_s: 11.0469 + } + segment { + id: "2193_1_-1" + start_s: 0 + end_s: 11.6358 + } + can_exit: true + change_lane_type: LEFT + } +} +road { + id: "69" + passage { + segment { + id: "69_1_-1" + start_s: 0 + end_s: 39.0273 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "68" + passage { + segment { + id: "68_1_-1" + start_s: 0 + end_s: 16.5147 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "17a" + passage { + segment { + id: "17a_1_-1" + start_s: 0 + end_s: 50.1648 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "3143" + passage { + segment { + id: "3143_1_-2" + start_s: 0 + end_s: 592.733 + } + can_exit: false + change_lane_type: LEFT + } + passage { + segment { + id: "3143_1_-1" + start_s: 0 + end_s: 592.354 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "1770" + passage { + segment { + id: "1770_1_-1" + start_s: 0 + end_s: 30.9261 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "42" + passage { + segment { + id: "42_1_-1" + start_s: 0 + end_s: 16.1103 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "2414" + passage { + segment { + id: "2414_1_-1" + start_s: 0 + end_s: 11.3162 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "67a" + passage { + segment { + id: "67a_1_-1" + start_s: 0 + end_s: 25.1158 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "160" + passage { + segment { + id: "160_1_-3" + start_s: 0 + end_s: 6.08098 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "9760" + passage { + segment { + id: "9760_1_-3" + start_s: 0 + end_s: 4.31703 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "1771" + passage { + segment { + id: "1771_1_-3" + start_s: 0 + end_s: 45.6667 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "1415" + passage { + segment { + id: "1415_1_-3" + start_s: 0 + end_s: 12.6104 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "1416" + passage { + segment { + id: "1416_1_-3" + start_s: 0 + end_s: 37.2856 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "1422" + passage { + segment { + id: "1422_1_-3" + start_s: 0 + end_s: 11.7449 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "1423" + passage { + segment { + id: "1423_1_-3" + start_s: 0 + end_s: 7.71293 + } + can_exit: true + change_lane_type: FORWARD + } +} +road { + id: "1425" + passage { + segment { + id: "1425_1_-3" + start_s: 0 + end_s: 11.738259580610574 + } + can_exit: true + change_lane_type: FORWARD + } +} +measurement { + distance: 2914.615037660255 +} +routing_request { + header { + timestamp_sec: 1518718242.7385607 + module_name: "dreamview" + sequence_num: 2 + } + waypoint { + id: "1771_1_-3" + s: 32.241981920357361 + pose { + x: 587102.288778 + y: 4141556.49511 + } + } + waypoint { + id: "2632_1_-1" + s: 21.273899233984654 + pose { + x: 587357.133333 + y: 4141331.45953 + } + } + waypoint { + id: "1949_1_-1" + s: 70.329705015993255 + pose { + x: 587413.080673 + y: 4140799.88239 + } + } + waypoint { + id: "1851_1_-2" + s: 7.4581926197514905 + pose { + x: 587558.489074 + y: 4140761.33465 + } + } + waypoint { + id: "3143_1_-2" + s: 83.448750506239932 + pose { + x: 587620.792609 + y: 4141484.54766 + } + } + waypoint { + id: "1425_1_-3" + s: 11.738259580610574 + pose { + x: 587193.997163 + y: 4141533.60712 + } + } +} +map_version: "1.000000" +status { + error_code: OK + msg: "Success!" +} diff --git a/modules/planning/testdata/sunnyvale_big_loop_test/result_abort_change_lane_for_fast_back_vehicle_0.pb.txt b/modules/planning/testdata/sunnyvale_big_loop_test/result_abort_change_lane_for_fast_back_vehicle_0.pb.txt new file mode 100644 index 0000000000000000000000000000000000000000..f4267666b519df50a969628f5917b6f1aaaa721c --- /dev/null +++ b/modules/planning/testdata/sunnyvale_big_loop_test/result_abort_change_lane_for_fast_back_vehicle_0.pb.txt @@ -0,0 +1,1727 @@ +header { + module_name: "planning" +} +total_path_length: 103.00720450971026 +total_path_time: 7.9999999999999885 +is_replan: true +gear: GEAR_DRIVE +trajectory_point { + path_point { + x: 587484.8344602522 + y: 4141516.1493750131 + z: 0 + theta: 2.9792000682148903 + kappa: -0.057097310200057858 + s: 0 + dkappa: 0 + ddkappa: 0 + } + v: 15.675000190734863 + a: -0.75582414865493774 + relative_time: 0 +} +trajectory_point { + path_point { + x: 587484.52664588357 + y: 4141516.2081588698 + theta: 2.9630166497330466 + kappa: -0.053356630073179687 + s: 0.31337713274756751 + dkappa: 0.0037434036831689613 + ddkappa: 0 + } + v: 15.674385835399214 + a: -0.68331351046768773 + relative_time: 0.02 +} +trajectory_point { + path_point { + x: 587484.21883151494 + y: 4141516.2669427264 + theta: 2.9468281900497382 + kappa: -0.049615949946301516 + s: 0.626754265495135 + dkappa: 0.0074868073663379225 + ddkappa: 0 + } + v: 15.673771480063566 + a: -0.61080287228043773 + relative_time: 0.04 +} +trajectory_point { + path_point { + x: 587483.91101714643 + y: 4141516.3257265831 + theta: 2.9306431712243093 + kappa: -0.045875269819423352 + s: 0.94013139824270253 + dkappa: 0.011230211049506885 + ddkappa: 0 + } + v: 15.673157124727917 + a: -0.53829223409318772 + relative_time: 0.06 +} +trajectory_point { + path_point { + x: 587483.605612104 + y: 4141516.3957429198 + theta: 2.9175818748967215 + kappa: -0.042380683068845196 + s: 1.25350853099027 + dkappa: 0.011690672485491386 + ddkappa: 0 + } + v: 15.672542769392269 + a: -0.46578159590593771 + relative_time: 0.08 +} +trajectory_point { + path_point { + x: 587483.30076752487 + y: 4141516.4683721834 + theta: 2.9052434402263714 + kappa: -0.038943343179407043 + s: 1.5668856637378377 + dkappa: 0.011387447625725643 + ddkappa: 0 + } + v: 15.67192841405662 + a: -0.3932709577186877 + relative_time: 0.1 +} +trajectory_point { + path_point { + x: 587482.995922946 + y: 4141516.5410014479 + theta: 2.8929058137417676 + kappa: -0.03550600328996889 + s: 1.8802627964854051 + dkappa: 0.011084222765959901 + ddkappa: 0 + } + v: 15.67131405872097 + a: -0.32076031953143769 + relative_time: 0.12000000000000001 +} +trajectory_point { + path_point { + x: 587482.69273555186 + y: 4141516.6200915882 + theta: 2.8825869998756404 + kappa: -0.032303970526024489 + s: 2.1936399292329729 + dkappa: 0.010734152833727053 + ddkappa: 0 + } + v: 15.670699703385322 + a: -0.24824968134418768 + relative_time: 0.14 +} +trajectory_point { + path_point { + x: 587482.39056807943 + y: 4141516.7031581029 + theta: 2.8735080276241916 + kappa: -0.029246758565813956 + s: 2.50701706198054 + dkappa: 0.010355251895924684 + ddkappa: 0 + } + v: 15.670085348049673 + a: -0.17573904315693767 + relative_time: 0.16 +} +trajectory_point { + path_point { + x: 587482.08840060688 + y: 4141516.7862246181 + theta: 2.8644290828111578 + kappa: -0.026189546605603423 + s: 2.8203941947281077 + dkappa: 0.009976350958122316 + ddkappa: 0 + } + v: 15.669470992714025 + a: -0.10322840496968777 + relative_time: 0.18 +} +trajectory_point { + path_point { + x: 587481.78712656139 + y: 4141516.872391108 + theta: 2.8565531263626278 + kappa: -0.023293933682797516 + s: 3.1337713274756749 + dkappa: 0.0095951144509479475 + ddkappa: 0 + } + v: 15.668856637378376 + a: -0.030717766782437761 + relative_time: 0.19999999999999998 +} +trajectory_point { + path_point { + x: 587481.48723282106 + y: 4141516.9628395792 + theta: 2.8503555897981743 + kappa: -0.020624849068023171 + s: 3.4470079496138988 + dkappa: 0.0092107936618550032 + ddkappa: 0 + } + v: 15.668154084331659 + a: -0.0311587553377759 + relative_time: 0.21999999999999997 +} +trajectory_point { + path_point { + x: 587481.18733908061 + y: 4141517.0532880505 + theta: 2.8441579634782328 + kappa: -0.017955764453248819 + s: 3.7602445717521231 + dkappa: 0.0088264728727620572 + ddkappa: 0 + } + v: 15.667451531284943 + a: -0.031599743893114152 + relative_time: 0.23999999999999996 +} +trajectory_point { + path_point { + x: 587480.8877502674 + y: 4141517.1447190265 + theta: 2.8384195109994548 + kappa: -0.015360765286027108 + s: 4.0734811938903475 + dkappa: 0.0084475690105815186 + ddkappa: 0 + } + v: 15.666748978238227 + a: -0.0320407324484524 + relative_time: 0.25999999999999995 +} +trajectory_point { + path_point { + x: 587480.58934308356 + y: 4141517.239957328 + theta: 2.834459096467421 + kappa: -0.013052856108136023 + s: 4.3867178160285718 + dkappa: 0.0080896563894727463 + ddkappa: 0 + } + v: 15.66604642519151 + a: -0.032481721003790653 + relative_time: 0.27999999999999997 +} +trajectory_point { + path_point { + x: 587480.29093589971 + y: 4141517.3351956294 + theta: 2.83049863153547 + kappa: -0.010744946930244935 + s: 4.6999544381667961 + dkappa: 0.0077317437683639733 + ddkappa: 0 + } + v: 15.665343872144794 + a: -0.032922709559128904 + relative_time: 0.3 +} +trajectory_point { + path_point { + x: 587479.99252871587 + y: 4141517.4304339313 + theta: 2.8265382404431363 + kappa: -0.00843703775235385 + s: 5.0131910603050205 + dkappa: 0.0073738311472552019 + ddkappa: 0 + } + v: 15.664641319098077 + a: -0.033363698114467155 + relative_time: 0.32 +} +trajectory_point { + path_point { + x: 587479.69493479957 + y: 4141517.5281832321 + theta: 2.8249690481612859 + kappa: -0.0064479736359350712 + s: 5.3264276824432457 + dkappa: 0.0070525424443862918 + ddkappa: 0 + } + v: 15.663938766051361 + a: -0.033804686669805406 + relative_time: 0.34 +} +trajectory_point { + path_point { + x: 587479.39735448931 + y: 4141517.6259745429 + theta: 2.8234398553742106 + kappa: -0.004464243865013616 + s: 5.6396643045814692 + dkappa: 0.0067318664674365326 + ddkappa: 0 + } + v: 15.663236213004645 + a: -0.034245675225143657 + relative_time: 0.36000000000000004 +} +trajectory_point { + path_point { + x: 587479.099774179 + y: 4141517.7237658533 + theta: 2.8219106652323638 + kappa: -0.0024805140940921491 + s: 5.9529009267196944 + dkappa: 0.0064111904904867716 + ddkappa: 0 + } + v: 15.662533659957928 + a: -0.034686663780481908 + relative_time: 0.38000000000000006 +} +trajectory_point { + path_point { + x: 587478.80235043727 + y: 4141517.8220319143 + theta: 2.8217875018319285 + kappa: -0.00071512842240149192 + s: 6.2661375488579187 + dkappa: 0.0061172645557079968 + ddkappa: 0 + } + v: 15.661831106911212 + a: -0.035127652335820263 + relative_time: 0.40000000000000008 +} +trajectory_point { + path_point { + x: 587478.50655669661 + y: 4141517.91992828 + theta: 2.8221159076882905 + kappa: 0.00097085671771142857 + s: 6.5777103902787077 + dkappa: 0.0058334786429010757 + ddkappa: 0 + } + v: 15.653512203324034 + a: -0.073209405038123482 + relative_time: 0.4200000000000001 +} +trajectory_point { + path_point { + x: 587478.21076295606 + y: 4141518.017824647 + theta: 2.82244431353501 + kappa: 0.0026568418578243485 + s: 6.8892832316994967 + dkappa: 0.0055496927300941554 + ddkappa: 0 + } + v: 15.645193299736858 + a: -0.11129115774042668 + relative_time: 0.44000000000000011 +} +trajectory_point { + path_point { + x: 587477.91479783331 + y: 4141518.1152001829 + theta: 2.8235794752844465 + kappa: 0.0042112317381615859 + s: 7.2008560731202857 + dkappa: 0.0052812180135676772 + ddkappa: 0 + } + v: 15.636874396149681 + a: -0.14937291044272988 + relative_time: 0.46000000000000013 +} +trajectory_point { + path_point { + x: 587477.61866936577 + y: 4141518.2120793145 + theta: 2.825483566761489 + kappa: 0.0056401980601038706 + s: 7.5124289145410748 + dkappa: 0.00502733641174799 + ddkappa: 0 + } + v: 15.628555492562503 + a: -0.18745466314503309 + relative_time: 0.48000000000000015 +} +trajectory_point { + path_point { + x: 587477.32254089811 + y: 4141518.3089584466 + theta: 2.8273876597929717 + kappa: 0.0070691643820461518 + s: 7.8240017559618629 + dkappa: 0.0047734548099283049 + ddkappa: 0 + } + v: 15.620236588975326 + a: -0.22553641584733616 + relative_time: 0.50000000000000011 +} +trajectory_point { + path_point { + x: 587477.026207324 + y: 4141518.4052018714 + theta: 2.829566851467578 + kappa: 0.0084369125473945736 + s: 8.1355745973826519 + dkappa: 0.0045257747954492384 + ddkappa: 0 + } + v: 15.61191768538815 + a: -0.26361816854963938 + relative_time: 0.52000000000000013 +} +trajectory_point { + path_point { + x: 587476.7292984603 + y: 4141518.4996622452 + theta: 2.8325177109436885 + kappa: 0.0096329542339826 + s: 8.44714743880344 + dkappa: 0.0042954891747943365 + ddkappa: 0 + } + v: 15.603598781800972 + a: -0.30169992125194262 + relative_time: 0.54000000000000015 +} +trajectory_point { + path_point { + x: 587476.43238959671 + y: 4141518.5941226189 + theta: 2.8354685889370925 + kappa: 0.010828995920570629 + s: 8.75872028022423 + dkappa: 0.0040652035541394356 + ddkappa: 0 + } + v: 15.595279878213795 + a: -0.3397816739542458 + relative_time: 0.56000000000000016 +} +trajectory_point { + path_point { + x: 587476.13546217442 + y: 4141518.6885234118 + theta: 2.8384421902493027 + kappa: 0.012021461903474903 + s: 9.0702931216450189 + dkappa: 0.0038351854374416329 + ddkappa: 0 + } + v: 15.586960974626619 + a: -0.377863426656549 + relative_time: 0.58000000000000018 +} +trajectory_point { + path_point { + x: 587475.83742999192 + y: 4141518.7793774353 + theta: 2.8427705000247374 + kappa: 0.013001072544505218 + s: 9.3818659630658079 + dkappa: 0.0036210913601153073 + ddkappa: 0 + } + v: 15.578642071039441 + a: -0.41594517935885233 + relative_time: 0.6000000000000002 +} +trajectory_point { + path_point { + x: 587475.54314959107 + y: 4141518.8690877426 + theta: 2.8470444198851137 + kappa: 0.013968351345487771 + s: 9.6895165661168843 + dkappa: 0.0034096924085932341 + ddkappa: 0 + } + v: 15.559030879190875 + a: -0.47240662066580036 + relative_time: 0.62000000000000022 +} +trajectory_point { + path_point { + x: 587475.24886919011 + y: 4141518.95879805 + theta: 2.8513182793960286 + kappa: 0.014935630146470319 + s: 9.9971671691679589 + dkappa: 0.0031982934570711622 + ddkappa: 0 + } + v: 15.539419687342308 + a: -0.52886806197274838 + relative_time: 0.64000000000000024 +} +trajectory_point { + path_point { + x: 587474.95359625982 + y: 4141519.0451611578 + theta: 2.8562436653397203 + kappa: 0.01575397559003467 + s: 10.304817772219035 + dkappa: 0.0029965719649322569 + ddkappa: 0 + } + v: 15.519808495493741 + a: -0.58532950327969635 + relative_time: 0.66000000000000025 +} +trajectory_point { + path_point { + x: 587474.65798102412 + y: 4141519.1303698784 + theta: 2.8613939828355148 + kappa: 0.016520956618178963 + s: 10.612468375270112 + dkappa: 0.0027981880531217926 + ddkappa: 0 + } + v: 15.500197303645175 + a: -0.64179094458664443 + relative_time: 0.68000000000000027 +} +trajectory_point { + path_point { + x: 587474.36236578831 + y: 4141519.215578598 + theta: 2.8665442722813621 + kappa: 0.017287937646323252 + s: 10.920118978321186 + dkappa: 0.00259980414131133 + ddkappa: 0 + } + v: 15.480586111796608 + a: -0.69825238589359251 + relative_time: 0.70000000000000029 +} +trajectory_point { + path_point { + x: 587474.06607476785 + y: 4141519.2983622779 + theta: 2.8718758040045542 + kappa: 0.017967408607992955 + s: 11.227769581372263 + dkappa: 0.00240616201367657 + ddkappa: 0 + } + v: 15.460974919948042 + a: -0.75471382720054048 + relative_time: 0.72000000000000031 +} +trajectory_point { + path_point { + x: 587473.76899603847 + y: 4141519.3783192793 + theta: 2.8774190759299834 + kappa: 0.018544876000602145 + s: 11.535420184423339 + dkappa: 0.0022180470091388632 + ddkappa: 0 + } + v: 15.441363728099475 + a: -0.81117526850748845 + relative_time: 0.74000000000000032 +} +trajectory_point { + path_point { + x: 587473.47191730922 + y: 4141519.4582762811 + theta: 2.882962407657744 + kappa: 0.019122343393211335 + s: 11.843070787474414 + dkappa: 0.002029932004601157 + ddkappa: 0 + } + v: 15.421752536250908 + a: -0.86763670981443664 + relative_time: 0.76000000000000034 +} +trajectory_point { + path_point { + x: 587473.174553075 + y: 4141519.5371290282 + theta: 2.888650083994428 + kappa: 0.019665923913939069 + s: 12.15072139052549 + dkappa: 0.0018433660809897968 + ddkappa: 0 + } + v: 15.402141344402342 + a: -0.9240981511213846 + relative_time: 0.78000000000000036 +} +trajectory_point { + path_point { + x: 587472.87594883633 + y: 4141519.6111857789 + theta: 2.8949660521259344 + kappa: 0.020062327005514094 + s: 12.458371993576566 + dkappa: 0.0016635281241582185 + ddkappa: 0 + } + v: 15.382530152553775 + a: -0.98055959242833279 + relative_time: 0.80000000000000038 +} +trajectory_point { + path_point { + x: 587472.58363307919 + y: 4141519.6836829241 + theta: 2.9011492312561908 + kappa: 0.020450382012274754 + s: 12.759543602437015 + dkappa: 0.0014874774801614298 + ddkappa: 0 + } + v: 15.350135181600642 + a: -1.0444784879511648 + relative_time: 0.8200000000000004 +} +trajectory_point { + path_point { + x: 587472.29131732218 + y: 4141519.75618007 + theta: 2.9073321545326052 + kappa: 0.020838437019035417 + s: 13.060715211297465 + dkappa: 0.00131142683616464 + ddkappa: 0 + } + v: 15.317740210647509 + a: -1.1083973834739971 + relative_time: 0.84000000000000041 +} +trajectory_point { + path_point { + x: 587471.99773702084 + y: 4141519.823339853 + theta: 2.91378162328618 + kappa: 0.021081008419335471 + s: 13.361886820157913 + dkappa: 0.0011438089423191759 + ddkappa: 0 + } + v: 15.285345239694376 + a: -1.1723162789968291 + relative_time: 0.86000000000000043 +} +trajectory_point { + path_point { + x: 587471.70396723715 + y: 4141519.8896998717 + theta: 2.9202714684565891 + kappa: 0.021301780204406758 + s: 13.663058429018363 + dkappa: 0.00097745463213850582 + ddkappa: 0 + } + v: 15.252950268741243 + a: -1.2362351745196611 + relative_time: 0.88000000000000045 +} +trajectory_point { + path_point { + x: 587471.41019745334 + y: 4141519.9560598908 + theta: 2.9267612041673523 + kappa: 0.021522551989478049 + s: 13.964230037878812 + dkappa: 0.00081110032195783706 + ddkappa: 0 + } + v: 15.22055529778811 + a: -1.3001540700424932 + relative_time: 0.90000000000000047 +} +trajectory_point { + path_point { + x: 587471.11570119252 + y: 4141520.0190335875 + theta: 2.9333263178450553 + kappa: 0.021660256563055449 + s: 14.265401646739262 + dkappa: 0.00065036414947868383 + ddkappa: 0 + } + v: 15.188160326834977 + a: -1.3640729655653254 + relative_time: 0.92000000000000048 +} +trajectory_point { + path_point { + x: 587470.82056270551 + y: 4141520.07901368 + theta: 2.9399588690823948 + kappa: 0.021724527369390127 + s: 14.56657325559971 + dkappa: 0.00049459456982107054 + ddkappa: 0 + } + v: 15.155765355881844 + a: -1.4279918610881575 + relative_time: 0.9400000000000005 +} +trajectory_point { + path_point { + x: 587470.5254242185 + y: 4141520.1389937717 + theta: 2.9465915003835539 + kappa: 0.0217887981757248 + s: 14.86774486446016 + dkappa: 0.00033882499016345637 + ddkappa: 0 + } + v: 15.123370384928711 + a: -1.4919107566109897 + relative_time: 0.96000000000000052 +} +trajectory_point { + path_point { + x: 587470.23004400334 + y: 4141520.1977139986 + theta: 2.9531658000915368 + kappa: 0.021824995555921104 + s: 15.168916473320609 + dkappa: 0.00018540667897578355 + ddkappa: 0 + } + v: 15.090975413975578 + a: -1.5558296521338217 + relative_time: 0.98000000000000054 +} +trajectory_point { + path_point { + x: 587469.93366474682 + y: 4141520.2512273081 + theta: 2.9595011220621195 + kappa: 0.021745167850158061 + s: 15.470088082181057 + dkappa: 4.1705960681606471e-05 + ddkappa: 0 + } + v: 15.058580443022443 + a: -1.6197485476566533 + relative_time: 1.0000000000000004 +} +trajectory_point { + path_point { + x: 587468.49336737907 + y: 4141520.49327957 + theta: 2.9906090139939918 + kappa: 0.021005872250384437 + s: 16.930667639195406 + dkappa: -0.00061595547310262268 + ddkappa: 0 + } + v: 14.832188006582955 + a: -1.9418364560257713 + relative_time: 1.1000000000000005 +} +trajectory_point { + path_point { + x: 587467.046181143 + y: 4141520.6899342355 + theta: 3.0205454922365784 + kappa: 0.019418572347451445 + s: 18.391247196209754 + dkappa: -0.0011624242091571434 + ddkappa: 0 + } + v: 14.605795570143465 + a: -2.2639243643948888 + relative_time: 1.2000000000000006 +} +trajectory_point { + path_point { + x: 587465.65085579129 + y: 4141520.8404019293 + theta: 3.0460274051310514 + kappa: 0.017273520266100476 + s: 19.794722054062042 + dkappa: -0.0015799188638536497 + ddkappa: 0 + } + v: 14.320272074333172 + a: -2.5595796612489141 + relative_time: 1.3000000000000007 +} +trajectory_point { + path_point { + x: 587464.2524090244 + y: 4141520.9585457835 + theta: 3.068667377672897 + kappa: 0.014693619763678122 + s: 21.19819691191433 + dkappa: -0.00189124395410754 + ddkappa: 0 + } + v: 14.034748578522876 + a: -2.855234958102939 + relative_time: 1.4000000000000008 +} +trajectory_point { + path_point { + x: 587462.91804206022 + y: 4141521.041849175 + theta: 3.0866347474482296 + kappa: 0.011922384924599405 + s: 22.535184562579293 + dkappa: -0.002083408471687804 + ddkappa: 0 + } + v: 13.70231254258624 + a: -3.0897976587346521 + relative_time: 1.5000000000000009 +} +trajectory_point { + path_point { + x: 587461.5825799302 + y: 4141521.1052948334 + theta: 3.1002646920358892 + kappa: 0.0090040235736203057 + s: 23.872172213244255 + dkappa: -0.002193947800728748 + ddkappa: 0 + } + v: 13.369876506649604 + a: -3.3243603593663642 + relative_time: 1.600000000000001 +} +trajectory_point { + path_point { + x: 587460.3176753798 + y: 4141521.1508777766 + theta: 3.1101223395012196 + kappa: 0.0061933655936122836 + s: 25.137905384656108 + dkappa: -0.0022230689117052889 + ddkappa: 0 + } + v: 13.013604110384064 + a: -3.4435421610108832 + relative_time: 1.7000000000000011 +} +trajectory_point { + path_point { + x: 587459.0524169557 + y: 4141521.1854714472 + theta: 3.1163418484836956 + kappa: 0.0034346122018738532 + s: 26.403638556067961 + dkappa: -0.0021755305473271495 + ddkappa: 0 + } + v: 12.657331714118522 + a: -3.5627239626553995 + relative_time: 1.8000000000000012 +} +trajectory_point { + path_point { + x: 587457.85459125519 + y: 4141521.2134576552 + theta: 3.1187247806743614 + kappa: 0.00094601407726309784 + s: 27.601791657553349 + dkappa: -0.0020688501968417492 + ddkappa: 0 + } + v: 12.319431364486183 + a: -3.4708637294893916 + relative_time: 1.9000000000000012 +} +trajectory_point { + path_point { + x: 587456.65673479612 + y: 4141521.2401173986 + theta: 3.1181642697554328 + kappa: -0.0013494942706119145 + s: 28.799944759038734 + dkappa: -0.0018992516496796996 + ddkappa: 0 + } + v: 11.981531014853847 + a: -3.3790034963233775 + relative_time: 2.0000000000000013 +} +trajectory_point { + path_point { + x: 587455.50735682715 + y: 4141521.2673964892 + theta: 3.1158039873515588 + kappa: -0.00329849716056913 + s: 29.949646930680551 + dkappa: -0.0016746643726269687 + ddkappa: 0 + } + v: 11.739276365636005 + a: -2.9007749942508907 + relative_time: 2.1000000000000014 +} +trajectory_point { + path_point { + x: 587454.35808572371 + y: 4141521.2988309553 + theta: 3.1114051176844177 + kappa: -0.0049277828922269541 + s: 31.099349102322371 + dkappa: -0.0013867647521653668 + ddkappa: 0 + } + v: 11.497021716418168 + a: -2.422546492178391 + relative_time: 2.2000000000000015 +} +trajectory_point { + path_point { + x: 587453.20987395535 + y: 4141521.3367842049 + theta: 3.1042238960680137 + kappa: -0.0061403410234592056 + s: 32.248190141148335 + dkappa: -0.0010293702084448455 + ddkappa: 0 + } + v: 11.49271605233892 + a: -1.2328015664854208 + relative_time: 2.3000000000000016 +} +trajectory_point { + path_point { + x: 587452.06197317422 + y: 4141521.3831582442 + theta: 3.0969751463846986 + kappa: -0.0068402903533344214 + s: 33.397031179974306 + dkappa: -0.00059811501168602311 + ddkappa: 0 + } + v: 11.488410388259675 + a: -0.043056640792462589 + relative_time: 2.4000000000000017 +} +trajectory_point { + path_point { + x: 587450.8952575475 + y: 4141521.4394226791 + theta: 3.0891323444131009 + kappa: -0.0069732435524839775 + s: 34.565108983804791 + dkappa: -8.8541784794862212e-05 + ddkappa: 0 + } + v: 11.584594213282264 + a: 0.45939080471671723 + relative_time: 2.5000000000000018 +} +trajectory_point { + path_point { + x: 587449.72902625671 + y: 4141521.50495546 + theta: 3.0809209142906879 + kappa: -0.0064467972884544918 + s: 35.733186787635276 + dkappa: 0.000491408041932059 + ddkappa: 0 + } + v: 11.680778038304853 + a: 0.96183825022589065 + relative_time: 2.6000000000000019 +} +trajectory_point { + path_point { + x: 587448.53849538253 + y: 4141521.5806348789 + theta: 3.0736787859641472 + kappa: -0.0051518018313563025 + s: 36.926125703853771 + dkappa: 0.0011536013673031705 + ddkappa: 0 + } + v: 11.805083600244918 + a: 1.102446934813275 + relative_time: 2.700000000000002 +} +trajectory_point { + path_point { + x: 587447.34843853209 + y: 4141521.66347403 + theta: 3.0689010626431807 + kappa: -0.003015862648037835 + s: 38.119064620072272 + dkappa: 0.0018837593450168435 + ddkappa: 0 + } + v: 11.929389162184984 + a: 1.2430556194006561 + relative_time: 2.800000000000002 +} +trajectory_point { + path_point { + x: 587446.135706558 + y: 4141521.7524740854 + theta: 3.0674708268283903 + kappa: 0.00019878390354747291 + s: 39.335058268155962 + dkappa: 0.0027024961304222862 + ddkappa: 0 + } + v: 12.04466282151092 + a: 1.1978961063300089 + relative_time: 2.9000000000000021 +} +trajectory_point { + path_point { + x: 587444.92294345715 + y: 4141521.8410421661 + theta: 3.0696032939004811 + kappa: 0.0030459928165780777 + s: 40.551051916239643 + dkappa: 0.0021303571560985783 + ddkappa: 0 + } + v: 12.159936480836857 + a: 1.15273659325936 + relative_time: 3.0000000000000022 +} +trajectory_point { + path_point { + x: 587443.69224134728 + y: 4141521.9269270934 + theta: 3.0729577356639597 + kappa: 0.0030448419565999927 + s: 41.784748584956645 + dkappa: -1.1511037261808905e-06 + ddkappa: 0 + } + v: 12.248451584003421 + a: 1.0189438124625003 + relative_time: 3.1000000000000023 +} +trajectory_point { + path_point { + x: 587442.46123635524 + y: 4141522.0083586429 + theta: 3.0770106865530233 + kappa: 0.0030408496934260904 + s: 43.01844525367364 + dkappa: -3.5375099032908462e-06 + ddkappa: 0 + } + v: 12.336966687169985 + a: 0.88515103166564013 + relative_time: 3.2000000000000024 +} +trajectory_point { + path_point { + x: 587441.21886543534 + y: 4141522.0858404646 + theta: 3.0810135997631884 + kappa: 0.0030338288879898155 + s: 44.263231024406252 + dkappa: -5.9370529257161804e-06 + ddkappa: 0 + } + v: 12.392412197248055 + a: 0.71980306622316514 + relative_time: 3.3000000000000025 +} +trajectory_point { + path_point { + x: 587439.97619075538 + y: 4141522.1582866856 + theta: 3.0840696918090305 + kappa: 0.0030235872171118193 + s: 45.508016795138865 + dkappa: -8.34972273865022e-06 + ddkappa: 0 + } + v: 12.447857707326122 + a: 0.55445510078069027 + relative_time: 3.4000000000000026 +} +trajectory_point { + path_point { + x: 587438.7284583099 + y: 4141522.2264414779 + theta: 3.0881357148321911 + kappa: 0.0030103800413083933 + s: 46.757610652424674 + dkappa: -1.078625501184908e-05 + ddkappa: 0 + } + v: 12.47189814009209 + a: 0.39742971422018492 + relative_time: 3.5000000000000027 +} +trajectory_point { + path_point { + x: 587437.4804891 + y: 4141522.2901164009 + theta: 3.0921799477454663 + kappa: 0.0029942886126471803 + s: 48.007204509710476 + dkappa: -1.319004901206296e-05 + ddkappa: 0 + } + v: 12.495938572858059 + a: 0.24040432765968039 + relative_time: 3.6000000000000028 +} +trajectory_point { + path_point { + x: 587436.23188537511 + y: 4141522.3491595872 + theta: 3.0961304736576634 + kappa: 0.0029751156871611556 + s: 49.257204509710476 + dkappa: -1.565880895410863e-05 + ddkappa: 0 + } + v: 12.497969286429029 + a: 0.13035573168469289 + relative_time: 3.7000000000000028 +} +trajectory_point { + path_point { + x: 587434.983053928 + y: 4141522.4031723747 + theta: 3.09912660727589 + kappa: 0.0029525802167454152 + s: 50.507204509710476 + dkappa: -1.8151265555904658e-05 + ddkappa: 0 + } + v: 12.5 + a: 0.020307135709707144 + relative_time: 3.8000000000000029 +} +trajectory_point { + path_point { + x: 587433.734038367 + y: 4141522.4527387461 + theta: 3.1030853963011809 + kappa: 0.0029271230486420437 + s: 51.757204509710476 + dkappa: -2.058401308125127e-05 + ddkappa: 0 + } + v: 12.5 + a: 0.010153567854853459 + relative_time: 3.900000000000003 +} +trajectory_point { + path_point { + x: 587432.484857172 + y: 4141522.4979422316 + theta: 3.1070060451247716 + kappa: 0.0028987571465851104 + s: 53.007204509710476 + dkappa: -2.3009099426990956e-05 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 4.0000000000000027 +} +trajectory_point { + path_point { + x: 587431.235520697 + y: 4141522.538634934 + theta: 3.1108172956874971 + kappa: 0.0028673405186739041 + s: 54.257204509710469 + dkappa: -2.5448896381222552e-05 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 4.1000000000000023 +} +trajectory_point { + path_point { + x: 587429.98603517 + y: 4141522.5744611509 + theta: 3.1136931414606632 + kappa: 0.0028325791244854744 + s: 55.507204509710469 + dkappa: -2.7934042237835314e-05 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 4.200000000000002 +} +trajectory_point { + path_point { + x: 587428.73643526575 + y: 4141522.6060373 + theta: 3.1174794173971039 + kappa: 0.002794842563243569 + s: 56.757204509710462 + dkappa: -3.0412101059356067e-05 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 4.3000000000000016 +} +trajectory_point { + path_point { + x: 587427.48673685128 + y: 4141522.6334445579 + theta: 3.1212111053603628 + kappa: 0.0027541375496869767 + s: 58.007204509710455 + dkappa: -3.2886748760061423e-05 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 4.4000000000000012 +} +trajectory_point { + path_point { + x: 587426.23695152707 + y: 4141522.656561892 + theta: 3.1248198677172843 + kappa: 0.0027103218634484 + s: 59.257204509710448 + dkappa: -3.5374355351572492e-05 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 4.5000000000000009 +} +trajectory_point { + path_point { + x: 587424.98708938691 + y: 4141522.6750691859 + theta: 3.1275261313143972 + kappa: 0.0026630916429426707 + s: 60.507204509710441 + dkappa: -3.7910875365402283e-05 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 4.6000000000000005 +} +trajectory_point { + path_point { + x: 587423.73717479594 + y: 4141522.68959006 + theta: 3.1310751886267485 + kappa: 0.0026128409264880425 + s: 61.757204509710441 + dkappa: -4.0427177563852196e-05 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 4.7 +} +trajectory_point { + path_point { + x: 587422.48722086882 + y: 4141522.7002115352 + theta: 3.1345528163906309 + kappa: 0.0025595728179659506 + s: 63.007204509710434 + dkappa: -4.2942437059474e-05 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 4.8 +} +trajectory_point { + path_point { + x: 587421.23723923 + y: 4141522.7068433855 + theta: 3.1378947007035802 + kappa: 0.0025031450321805395 + s: 64.257204509710419 + dkappa: -4.5469094703083633e-05 + ddkappa: 0 + } + v: 12.499999999999982 + a: -8.8817841970010554e-14 + relative_time: 4.8999999999999995 +} +trajectory_point { + path_point { + x: 587419.98724218854 + y: 4141522.7092139442 + theta: 3.1403819848934402 + kappa: 0.0024432470187410291 + s: 65.507204509710419 + dkappa: -4.8046736389408483e-05 + ddkappa: 0 + } + v: 12.499999999999964 + a: -1.7763568394002267e-13 + relative_time: 4.9999999999999991 +} +trajectory_point { + path_point { + x: 587418.73724373477 + y: 4141522.7079317952 + theta: -3.1395574428771256 + kappa: 0.0023802884913449061 + s: 66.7572045097104 + dkappa: -5.0596565722067216e-05 + ddkappa: 0 + } + v: 12.499999999999982 + a: -5.5283372237888507e-27 + relative_time: 5.0999999999999988 +} +trajectory_point { + path_point { + x: 587417.48725386919 + y: 4141522.7030962203 + theta: -3.136400197171171 + kappa: 0.0023142713954920364 + s: 68.0072045097104 + dkappa: -5.3146039470264136e-05 + ddkappa: 0 + } + v: 12.5 + a: 1.7763568394001874e-13 + relative_time: 5.1999999999999984 +} +trajectory_point { + path_point { + x: 587416.23728301865 + y: 4141522.6946515883 + theta: -3.1333906884085541 + kappa: 0.002245052803602268 + s: 69.257204509710391 + dkappa: -5.57060530175477e-05 + ddkappa: 0 + } + v: 12.5 + a: 8.881784197001607e-14 + relative_time: 5.299999999999998 +} +trajectory_point { + path_point { + x: 587414.98734376265 + y: 4141522.6823859815 + theta: -3.1311717958153076 + kappa: 0.0021723176626972807 + s: 70.507204509710391 + dkappa: -5.8318115089937952e-05 + ddkappa: 0 + } + v: 12.5 + a: 3.9379936388632909e-27 + relative_time: 5.3999999999999977 +} +trajectory_point { + path_point { + x: 587413.73744070984 + y: 4141522.6668740618 + theta: -3.1282961991675227 + kappa: 0.0020964825077528304 + s: 71.757204509710391 + dkappa: -6.090076619892034e-05 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 5.4999999999999973 +} +trajectory_point { + path_point { + x: 587412.48758033186 + y: 4141522.6482318849 + theta: -3.1255267839457335 + kappa: 0.0020175501794998706 + s: 73.007204509710377 + dkappa: -6.3482421426389065e-05 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 5.599999999999997 +} +trajectory_point { + path_point { + x: 587411.237770735 + y: 4141522.6264420673 + theta: -3.122916166231426 + kappa: 0.0019354388687655181 + s: 74.257204509710377 + dkappa: -6.60129074076999e-05 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 5.6999999999999966 +} +trajectory_point { + path_point { + x: 587409.98802278214 + y: 4141522.6013617888 + theta: -3.1210145886585536 + kappa: 0.0018504132530806272 + s: 75.507204509710377 + dkappa: -6.8189886060427565e-05 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 5.7999999999999963 +} +trajectory_point { + path_point { + x: 587408.7383334795 + y: 4141522.5735157235 + theta: -3.1185764351108816 + kappa: 0.0017614132431271546 + s: 76.757204509710363 + dkappa: -7.1490154755134958e-05 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 5.8999999999999959 +} +trajectory_point { + path_point { + x: 587407.4887054764 + y: 4141522.5430394653 + theta: -3.1162628116804 + kappa: 0.0016686838746704487 + s: 78.007204509710363 + dkappa: -7.4576570176601949e-05 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 5.9999999999999956 +} +trajectory_point { + path_point { + x: 587406.23914361082 + y: 4141522.509958481 + theta: -3.1141192589044153 + kappa: 0.0015723641987647672 + s: 79.257204509710348 + dkappa: -7.7418692969168481e-05 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 6.0999999999999952 +} +trajectory_point { + path_point { + x: 587404.989655165 + y: 4141522.474209934 + theta: -3.1125873125183512 + kappa: 0.0014714834644002125 + s: 80.507204509710348 + dkappa: -8.1026738254211215e-05 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 6.1999999999999948 +} +trajectory_point { + path_point { + x: 587403.74023163388 + y: 4141522.4362616064 + theta: -3.1106649220555704 + kappa: 0.0013645394609505695 + s: 81.757204509710348 + dkappa: -8.5807964663348659e-05 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 6.2999999999999945 +} +trajectory_point { + path_point { + x: 587402.49087160756 + y: 4141522.3962752819 + theta: -3.1088909205156372 + kappa: 0.0012543448405436682 + s: 83.007204509710334 + dkappa: -8.8498481030719115e-05 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 6.3999999999999941 +} +trajectory_point { + path_point { + x: 587401.24157578463 + y: 4141522.35432876 + theta: -3.1073008861326032 + kappa: 0.0011410170187693086 + s: 84.257204509710334 + dkappa: -9.0979056603314711e-05 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 6.4999999999999938 +} +trajectory_point { + path_point { + x: 587399.99234624417 + y: 4141522.3104513958 + theta: -3.1062083832478464 + kappa: 0.0010245846751274009 + s: 85.50720450971032 + dkappa: -9.3253915899653059e-05 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 6.5999999999999934 +} +trajectory_point { + path_point { + x: 587398.74317147071 + y: 4141522.2650417797 + theta: -3.1048875249319217 + kappa: 0.00090566283038258583 + s: 86.75720450971032 + dkappa: -9.5313453689536555e-05 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 6.6999999999999931 +} +trajectory_point { + path_point { + x: 587397.49404658633 + y: 4141522.2182789622 + theta: -3.1037306956887751 + kappa: 0.00078451036170003471 + s: 88.00720450971032 + dkappa: -9.7154124509333384e-05 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 6.7999999999999927 +} +trajectory_point { + path_point { + x: 587396.24496828287 + y: 4141522.1702869846 + theta: -3.1027614894287674 + kappa: 0.00066128701149488077 + s: 89.2572045097103 + dkappa: -9.8785739281785119e-05 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 6.8999999999999924 +} +trajectory_point { + path_point { + x: 587394.99593331013 + y: 4141522.1211795527 + theta: -3.1021526894594738 + kappa: 0.000536117065982084 + s: 90.5072045097103 + dkappa: -0.00010020129450248334 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 6.999999999999992 +} +trajectory_point { + path_point { + x: 587393.7469300183 + y: 4141522.0712726992 + theta: -3.1014856926185144 + kappa: 0.000409474902888954 + s: 91.757204509710292 + dkappa: -0.00010141349063525908 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 7.0999999999999917 +} +trajectory_point { + path_point { + x: 587392.49795134622 + y: 4141522.0207531024 + theta: -3.1009925227748791 + kappa: 0.00028161501696382746 + s: 93.007204509710292 + dkappa: -0.00010241015283795853 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 7.1999999999999913 +} +trajectory_point { + path_point { + x: 587391.24899081385 + y: 4141521.9697867213 + theta: -3.1006822767771389 + kappa: 0.00015273998895439067 + s: 94.257204509710292 + dkappa: -0.00010319802808580158 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 7.2999999999999909 +} +trajectory_point { + path_point { + x: 587390.0000407605 + y: 4141521.9185640384 + theta: -3.1005665847923294 + kappa: 2.3065259695486419e-05 + s: 95.507204509710277 + dkappa: -0.00010376204988611439 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 7.3999999999999906 +} +trajectory_point { + path_point { + x: 587388.75109173742 + y: 4141521.8673162377 + theta: -3.1006013277224826 + kappa: -0.00010705938362093556 + s: 96.757204509710277 + dkappa: -0.00010412335159324344 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 7.49999999999999 +} +trajectory_point { + path_point { + x: 587387.50213623885 + y: 4141521.8162265676 + theta: -3.1007956709248332 + kappa: -0.0002373884837108298 + s: 98.007204509710277 + dkappa: -0.00010427543077590656 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 7.59999999999999 +} +trajectory_point { + path_point { + x: 587386.25316603738 + y: 4141521.7654977706 + theta: -3.101175510219361 + kappa: -0.00036765941013360015 + s: 99.257204509710263 + dkappa: -0.00010420245285532225 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 7.6999999999999895 +} +trajectory_point { + path_point { + x: 587385.00417086878 + y: 4141521.7153878463 + theta: -3.101692871070032 + kappa: -0.0004972532346352069 + s: 100.50720450971026 + dkappa: -0.00010364085603791281 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 7.7999999999999892 +} +trajectory_point { + path_point { + x: 587383.75514544221 + y: 4141521.6660385095 + theta: -3.102291168672743 + kappa: -0.00062602536763775064 + s: 101.75720450971026 + dkappa: -0.00010295528205220845 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 7.8999999999999888 +} +trajectory_point { + path_point { + x: 587382.506083396 + y: 4141521.6176252244 + theta: -3.1031727922884182 + kappa: -0.00075384684928513491 + s: 103.00720450971026 + dkappa: -0.00010214295809386046 + ddkappa: 0 + } + v: 12.5 + a: 0 + relative_time: 7.9999999999999885 +} +decision { + main_decision { + cruise { + change_lane_type: FORWARD + } + } + object_decision { + } + vehicle_signal { + turn_signal: TURN_NONE + } +} +routing_header { + timestamp_sec: 1518718246.4781277 + module_name: "routing" + sequence_num: 1 +} +right_of_way_status: UNPROTECTED +lane_id { + id: "3143_1_-2" +} +engage_advice { + advice: PREPARE_DISENGAGE + reason: "Not on reference line" +}