diff --git a/modules/drivers/lidar_velodyne/pointcloud/lib/util.h b/modules/drivers/lidar_velodyne/pointcloud/lib/util.h
index 59d9ece1b06..8349ca0425d 100644
--- a/modules/drivers/lidar_velodyne/pointcloud/lib/util.h
+++ b/modules/drivers/lidar_velodyne/pointcloud/lib/util.h
@@ -18,6 +18,7 @@
 #define MODULES_DRIVERS_LIDAR_VELODYNE_POINTCLOUD_LIB_UTIL_H_
 
 #include <fstream>
+#include <string>
 
 #include "angles/angles.h"
 #include "ros/ros.h"
@@ -34,7 +35,7 @@ void dump_msg(const T& msg, const std::string& file_path) {
   boost::shared_array<uint8_t> obuffer(new uint8_t[serial_size]);
   ros::serialization::OStream ostream(obuffer.get(), serial_size);
   ros::serialization::serialize(ostream, msg);
-  ofs.write((char*)obuffer.get(), serial_size);
+  ofs.write(reinterpret_cast<char*>(obuffer.get()), serial_size);
   ofs.close();
 }
 
@@ -49,7 +50,7 @@ void load_msg(const std::string& file_path, T* msg) {
 
   uint32_t file_size = end - begin;
   boost::shared_array<uint8_t> ibuffer(new uint8_t[file_size]);
-  ifs.read((char*)ibuffer.get(), file_size);
+  ifs.read(reinterpret_cast<char*>(ibuffer.get()), file_size);
   ros::serialization::IStream istream(ibuffer.get(), file_size);
   ros::serialization::deserialize(istream, *msg);
   ifs.close();
diff --git a/modules/planning/testdata/navigation_mode_test/result_cruise_0.pb.txt b/modules/planning/testdata/navigation_mode_test/result_cruise_0.pb.txt
index 0229d2cd431..66ff5fc2aa6 100644
--- a/modules/planning/testdata/navigation_mode_test/result_cruise_0.pb.txt
+++ b/modules/planning/testdata/navigation_mode_test/result_cruise_0.pb.txt
@@ -1,7 +1,7 @@
 header {
   module_name: "planning"
 }
-total_path_length: 58.30176070866267
+total_path_length: 58.526512922333943
 total_path_time: 5.0999999999999988
 is_replan: true
 gear: GEAR_DRIVE
@@ -17,1281 +17,1281 @@ trajectory_point {
     ddkappa: 0
   }
   v: 9.8694448471069336
-  a: 0.026777566547401647
+  a: 0.084516226919080284
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 0.19739509528791743
-    y: 0.00058071577795825217
-    theta: 3.5735119752189348e-07
-    kappa: 6.961279592891602e-07
-    s: 0.19739594598529006
-    dkappa: 6.9669113405596184e-07
+    x: 0.19740656651452035
+    y: 0.00058074945583410536
+    theta: 3.573719644656137e-07
+    kappa: 6.9616841339240927e-07
+    s: 0.19740741726132957
+    dkappa: 6.9673162088700165e-07
     ddkappa: 0
   }
-  v: 9.8702339777489545
-  a: 0.052045555940184007
+  v: 9.8713772981295875
+  a: 0.10864204635090255
   relative_time: 0.02
 }
 trajectory_point {
   path_point {
-    x: 0.39481099081155524
-    y: 0.0011603005295294491
-    theta: 7.1474005070015778e-07
-    kappa: 1.3923292721012315e-06
-    s: 0.39481269229594157
-    dkappa: 1.3934556809785986e-06
+    x: 0.39485657238293764
+    y: 0.0011604343504847262
+    theta: 7.1482256869259686e-07
+    kappa: 1.392490018781494e-06
+    s: 0.39485827406376311
+    dkappa: 1.393616557704599e-06
     ddkappa: 0
   }
-  v: 9.871523089941606
-  a: 0.076776634892794571
+  v: 9.8737870734736912
+  a: 0.13224954269108039
   relative_time: 0.04
 }
 trajectory_point {
   path_point {
-    x: 0.59225757914251753
-    y: 0.00173997539078032
-    theta: 1.0721844678762693e-06
-    kappa: 2.0886388252994002e-06
-    s: 0.59226013154619184
-    dkappa: 2.090328555854932e-06
+    x: 0.592359460739817
+    y: 0.0017402745005359139
+    theta: 1.0723689078950827e-06
+    kappa: 2.0889981180584918e-06
+    s: 0.592362013582562
+    dkappa: 2.0906881392856068e-06
     ddkappa: 0
   }
-  v: 9.873301500273028
-  a: 0.10097628309962037
+  v: 9.8766638596950234
+  a: 0.15534401770029954
   relative_time: 0.06
 }
 trajectory_point {
   path_point {
-    x: 0.78974454028102692
-    y: 0.0023197687808030015
-    theta: 1.429701973254538e-06
-    kappa: 2.7850907560986308e-06
-    s: 0.78974794377798052
-    dkappa: 2.7873439235172887e-06
+    x: 0.78992446951207562
+    y: 0.0023202970272159927
+    theta: 1.4300277055845356e-06
+    kappa: 2.7857252894351543e-06
+    s: 0.78992787378445539
+    dkappa: 2.787978970197888e-06
     ddkappa: 0
   }
-  v: 9.8755586349252482
-  a: 0.12464998025504848
+  v: 9.8799974493845681
+  a: 0.17793077313924571
   relative_time: 0.08
 }
 trajectory_point {
   path_point {
-    x: 0.98728134384779109
-    y: 0.0028997085010455531
-    theta: 1.7873097104548208e-06
-    kappa: 3.4817184597954425e-06
-    s: 0.98728559865282584
-    dkappa: 3.484535206279331e-06
+    x: 0.98756063353967771
+    y: 0.0029005284555191236
+    theta: 1.787815318454733e-06
+    kappa: 3.4827033949225285e-06
+    s: 0.9875648895483442
+    dkappa: 3.4855209382291896e-06
     ddkappa: 0
   }
-  v: 9.8782840296741838
-  a: 0.14780320605346586
+  v: 9.88377774116853
+  a: 0.20001511076860465
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 1.1848772439125443
-    y: 0.003482324067074806
-    theta: 2.7476030037831833e-06
-    kappa: 4.0759945488681314e-06
-    s: 1.184882357643704
-    dkappa: 3.4239964673013774e-06
+    x: 1.1852767793171579
+    y: 0.0034835024298297771
+    theta: 2.7496228209855644e-06
+    kappa: 4.07718286821693e-06
+    s: 1.1852818947860055
+    dkappa: 3.4237757963407842e-06
     ddkappa: 0
   }
-  v: 9.8814673298896363
-  a: 0.17044144018925964
+  v: 9.88799473970832
+  a: 0.22160233234906207
   relative_time: 0.12000000000000001
 }
 trajectory_point {
   path_point {
-    x: 1.3825413028010782
-    y: 0.0040653011006518986
-    theta: 3.7468767679627035e-06
-    kappa: 4.6638974556617209e-06
-    s: 1.3825472762269253
-    dkappa: 3.3148228690520327e-06
+    x: 1.3830815487872732
+    y: 0.00406689446569866
+    theta: 3.7496079356991174e-06
+    kappa: 4.6655042838704179e-06
+    s: 1.3830875245627967
+    dkappa: 3.3145244809758981e-06
     ddkappa: 0
   }
-  v: 9.8850982905352982
-  a: 0.19257016235681676
+  v: 9.89263855570057
+  a: 0.2426977396413037
   relative_time: 0.14
 }
 trajectory_point {
   path_point {
-    x: 1.5802823726185375
-    y: 0.0046485052650647668
-    theta: 4.7465398544943582e-06
-    kappa: 5.2520294124364992e-06
-    s: 1.5802892060740135
-    dkappa: 3.2056067362100944e-06
+    x: 1.5809833807140095
+    y: 0.0046505727710256873
+    theta: 4.7500837410119345e-06
+    kappa: 5.254114387820143e-06
+    s: 1.58099021721836
+    dkappa: 3.2052195561804269e-06
     ddkappa: 0
   }
-  v: 9.88916677616875
-  a: 0.21419485225052429
+  v: 9.8976994058771215
+  a: 0.26330663440601526
   relative_time: 0.16
 }
 trajectory_point {
   path_point {
-    x: 1.7781091033859155
-    y: 0.0052319620721014764
-    theta: 5.7466359923985522e-06
-    kappa: 5.84041614654277e-06
-    s: 1.7781167972435827
-    dkappa: 3.0963432912053648e-06
+    x: 1.7789905187961448
+    y: 0.00523456165921961
+    theta: 5.7510919129200033e-06
+    kappa: 5.8430376989113718e-06
+    s: 1.7789982164873273
+    dkappa: 3.0958564688036178e-06
     ddkappa: 0
   }
-  v: 9.8936627609414565
-  a: 0.23532098956476924
+  v: 9.9031676130050315
+  a: 0.28343431840388256
   relative_time: 0.18
 }
 trajectory_point {
   path_point {
-    x: 1.9760299457040393
-    y: 0.00581569644539372
-    theta: 6.7472079035013621e-06
-    kappa: 6.4290827922048281e-06
-    s: 1.9760385003732148
-    dkappa: 2.9870278666111762e-06
+    x: 1.9771110142489858
+    y: 0.0058188848759914234
+    theta: 6.752673153087585e-06
+    kappa: 6.4322981634948388e-06
+    s: 1.9771195736200222
+    dkappa: 2.9864307720069797e-06
     ddkappa: 0
   }
-  v: 9.8985763285987733
-  a: 0.25595405399393867
+  v: 9.90903360588657
+  a: 0.30308609339559117
   relative_time: 0.19999999999999998
 }
 trajectory_point {
   path_point {
-    x: 2.1740531453217224
-    y: 0.0064023111777622484
-    theta: 8.2224988386947473e-06
-    kappa: 6.9270957623788929e-06
-    s: 2.1740625688713378
-    dkappa: 2.8832818757156543e-06
+    x: 2.17535272024469
+    y: 0.0064061632186522052
+    theta: 8.2325928318915942e-06
+    kappa: 6.9302850574226227e-06
+    s: 2.175362149503167
+    dkappa: 2.8826059061669504e-06
     ddkappa: 0
   }
-  v: 9.9038976724799426
-  a: 0.27609952523241965
+  v: 9.91528791935922
+  a: 0.32226726114182691
   relative_time: 0.21999999999999997
 }
 trajectory_point {
   path_point {
-    x: 2.3721867671845835
-    y: 0.0069895946037520033
-    theta: 9.7614323979478e-06
-    kappa: 7.4133367378205958e-06
-    s: 2.3721970611091043
-    dkappa: 2.7802233351215608e-06
+    x: 2.3737233161062079
+    y: 0.0069941490539579213
+    theta: 9.77336700369591e-06
+    kappa: 7.4171075922542862e-06
+    s: 2.3737336167805863
+    dkappa: 2.7794241043402055e-06
     ddkappa: 0
   }
-  v: 9.9096170955180956
-  a: 0.29576288297459918
+  v: 9.9219211942956775
+  a: 0.34098312340327558
   relative_time: 0.23999999999999996
 }
 trajectory_point {
   path_point {
-    x: 2.5704386777932138
-    y: 0.0075772286467608108
-    theta: 1.1301284723597149e-05
-    kappa: 7.8998680064219644e-06
-    s: 2.5704498426122675
-    dkappa: 2.6771032670320664e-06
+    x: 2.5722302892845423
+    y: 0.0075825391222105448
+    theta: 1.1315200438843931e-05
+    kappa: 7.904264811522307e-06
+    s: 2.5722414619739089
+    dkappa: 2.6761713663083433e-06
     ddkappa: 0
   }
-  v: 9.9157250102402479
-  a: 0.31494960691486423
+  v: 9.9289241776038555
+  a: 0.35923898194062281
   relative_time: 0.25999999999999995
 }
 trajectory_point {
   path_point {
-    x: 2.7688165519861321
-    y: 0.0081652360556054362
-    theta: 1.2842115427513789e-05
-    kappa: 8.3867084030527835e-06
-    s: 2.7688285882530597
-    dkappa: 2.5739176794055919e-06
+    x: 2.7708809422677541
+    y: 0.0081713550685512028
+    theta: 1.285814985640954e-05
+    kappa: 8.3917746363088585e-06
+    s: 2.770892987603276
+    dkappa: 2.5728438937066263e-06
     ddkappa: 0
   }
-  v: 9.9222119387673064
-  a: 0.333665176747602
+  v: 9.93628772222688
+  a: 0.37704013851455437
   relative_time: 0.27999999999999997
 }
 trajectory_point {
   path_point {
-    x: 2.9673278761410411
-    y: 0.0087536390204901657
-    theta: 1.4383982656962502e-05
-    kappa: 8.8738763000799555e-06
-    s: 2.9673407844420705
-    dkappa: 2.4706626782278191e-06
+    x: 2.9696823956639182
+    y: 0.00876061799901463
+    theta: 1.4402270563262931e-05
+    kappa: 8.879654541343283e-06
+    s: 2.969695314308042
+    dkappa: 2.4694379827745857e-06
     ddkappa: 0
   }
-  v: 9.9290685128140641
-  a: 0.35191507216719936
+  v: 9.9440027871430878
+  a: 0.394391894885756
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 3.1659799426993445
-    y: 0.0093450392056819744
-    theta: 1.6294948741446547e-05
-    kappa: 9.2795196753144527e-06
-    s: 3.1659937313201243
-    dkappa: 2.3722256159847327e-06
+    x: 3.1686415825321204
+    y: 0.0093529696823678558
+    theta: 1.6321494349380572e-05
+    kappa: 9.28474522745888e-06
+    s: 3.1686553829674815
+    dkappa: 2.370919220744732e-06
     ddkappa: 0
   }
-  v: 9.9362854736892032
-  a: 0.36970477286804332
+  v: 9.9520604373660291
+  a: 0.41129955281491337
   relative_time: 0.32
 }
 trajectory_point {
   path_point {
-    x: 3.3647798738891845
-    y: 0.0099373726236153884
-    theta: 1.8277660656540462e-05
-    kappa: 9.66982019379498e-06
-    s: 3.3647945449501577
-    dkappa: 2.274649948300192e-06
+    x: 3.3677652725088238
+    y: 0.0099462677543012031
+    theta: 1.83074352415602e-05
+    kappa: 9.6756813760506388e-06
+    s: 3.36777995682149
+    dkappa: 2.27318464465611e-06
     ddkappa: 0
   }
-  v: 9.94385367229529
-  a: 0.38703975854452111
+  v: 9.9604518439444742
+  a: 0.42776841406271227
   relative_time: 0.34
 }
 trajectory_point {
   path_point {
-    x: 3.5637346053208
-    y: 0.010530167275895104
-    theta: 2.0261916457098295e-05
-    kappa: 1.0060424628953263e-05
-    s: 3.5637501595090981
-    dkappa: 2.1769983010272361e-06
+    x: 3.5670600546418729
+    y: 0.010540075603079759
+    theta: 2.029508250478429e-05
+    kappa: 1.0066953426958512e-05
+    s: 3.5670756235912942
+    dkappa: 2.1753660925253881e-06
     ddkappa: 0
   }
-  v: 9.9517640691287834
-  a: 0.40392550889101958
+  v: 9.9691682839624
+  a: 0.44380378038983842
   relative_time: 0.36000000000000004
 }
 trajectory_point {
   path_point {
-    x: 3.7628508914470009
-    y: 0.011123443287719841
-    theta: 2.2247783507456376e-05
-    kappa: 1.0451346241691453e-05
-    s: 3.7628673294797403
-    dkappa: 2.0792673589220455e-06
+    x: 3.76653234322605
+    y: 0.011134412340385955
+    theta: 2.228450011143579e-05
+    kappa: 1.0458573973258178e-05
+    s: 3.7665487976001479
+    dkappa: 2.077460416066285e-06
     ddkappa: 0
   }
-  v: 9.9600077342800262
-  a: 0.42036750360192582
+  v: 9.9782011405390012
+  a: 0.45941095355697747
   relative_time: 0.38000000000000006
 }
 trajectory_point {
   path_point {
-    x: 3.9621353092191374
-    y: 0.01171722025541465
-    theta: 2.4235327402699625e-05
-    kappa: 1.0842597944426093e-05
-    s: 3.9621526318426254
-    dkappa: 1.9814538938626831e-06
+    x: 3.9661883812796366
+    y: 0.011729296567576113
+    theta: 2.4275750325486456e-05
+    kappa: 1.085055527176109e-05
+    s: 3.966205721894045
+    dkappa: 1.9794645510590421e-06
     ddkappa: 0
   }
-  v: 9.968575847433252
-  a: 0.43637122237162695
+  v: 9.9875419028286849
+  a: 0.47459523532481518
   relative_time: 0.40000000000000008
 }
 trajectory_point {
   path_point {
-    x: 4.161594252654524
-    y: 0.012314069447612742
-    theta: 2.6505612242377907e-05
-    kappa: 1.1159235282603631e-05
-    s: 4.1616124682679176
-    dkappa: 1.8883172350034956e-06
+    x: 4.1660342348322468
+    y: 0.012327368344581657
+    theta: 2.6557575525831822e-05
+    kappa: 1.1165903165779167e-05
+    s: 4.1660524703624171
+    dkappa: 1.8862681765777089e-06
     ddkappa: 0
   }
-  v: 9.97745969786658
-  a: 0.45194214489450985
+  v: 9.99718216602107
+  a: 0.48936192745403739
   relative_time: 0.4200000000000001
 }
 trajectory_point {
   path_point {
-    x: 4.36123395615441
-    y: 0.012912042083870263
-    theta: 2.8842093371839184e-05
-    kappa: 1.1459050499613846e-05
-    s: 4.361253067307282
-    dkappa: 1.7961832127926724e-06
+    x: 4.366075816986454
+    y: 0.012926544711530857
+    theta: 2.88987600378654e-05
+    kappa: 1.1466321916724988e-05
+    s: 4.3660949498588417
+    dkappa: 1.7939486867659313e-06
     ddkappa: 0
   }
-  v: 9.986650684452016
-  a: 0.46708575086496174
+  v: 10.007113631340992
+  a: 0.5037163317053297
   relative_time: 0.44000000000000011
 }
 trajectory_point {
   path_point {
-    x: 4.5610604790553833
-    y: 0.013510574292636728
-    theta: 3.1180760940330288e-05
-    kappa: 1.1759146278547852e-05
-    s: 4.5610804865857641
-    dkappa: 1.7039629731484052e-06
+    x: 4.56631887120338
+    y: 0.013526324539506914
+    theta: 3.1242302476197636e-05
+    kappa: 1.1767043234691034e-05
+    s: 4.5663389023217436
+    dkappa: 1.701536217295419e-06
     ddkappa: 0
   }
-  v: 9.9961403156554578
-  a: 0.48180751997736937
+  v: 10.017328106048502
+  a: 0.51766374983937791
   relative_time: 0.46000000000000013
 }
 trajectory_point {
   path_point {
-    x: 4.7610797102213294
-    y: 0.014109683712585207
-    theta: 3.3521683867832053e-05
-    kappa: 1.2059531463192591e-05
-    s: 4.7611006149936648
-    dkappa: 1.6116537983511912e-06
+    x: 4.7667689766019805
+    y: 0.014126724539416585
+    theta: 3.3588268135709143e-05
+    kappa: 1.2068075498295031e-05
+    s: 4.7667899068951041
+    dkappa: 1.6090281933944174e-06
     ddkappa: 0
   }
-  v: 10.005920209536686
-  a: 0.49611293192612005
+  v: 10.027817503438857
+  a: 0.53120948361686771
   relative_time: 0.48000000000000015
 }
 trajectory_point {
   path_point {
-    x: 4.9612973719740134
-    y: 0.014709387483551977
-    theta: 3.586492912521777e-05
-    kappa: 1.2360214647225135e-05
-    s: 4.96131917487842
-    dkappa: 1.5192530475409625e-06
+    x: 4.96743155162818
+    y: 0.014727760940909275
+    theta: 3.5936720431006819e-05
+    kappa: 1.2369426844858932e-05
+    s: 4.9674533820491567
+    dkappa: 1.5164221144420139e-06
     ddkappa: 0
   }
-  v: 10.015982093749374
-  a: 0.51000746640560057
+  v: 10.038573842842537
+  a: 0.5443588347984849
   relative_time: 0.50000000000000011
 }
 trajectory_point {
   path_point {
-    x: 5.1617190147273506
-    y: 0.015312218937296141
-    theta: 3.8419153314261933e-05
-    kappa: 1.2590953850102578e-05
-    s: 5.1617417242364789
-    dkappa: 1.4315244631136061e-06
+    x: 5.1683118483114523
+    y: 0.015332068257231438
+    theta: 3.8504772749359262e-05
+    kappa: 1.259800555998953e-05
+    s: 5.1683345877011018
+    dkappa: 1.4286751772749634e-06
     ddkappa: 0
   }
-  v: 10.026317805541078
-  a: 0.52349660311019819
+  v: 10.04958924962523
+  a: 0.55711710514491508
   relative_time: 0.52000000000000013
 }
 trajectory_point {
   path_point {
-    x: 5.3623500400814814
-    y: 0.015916267160324904
-    theta: 4.1024696865843424e-05
-    kappa: 1.2805549253895363e-05
-    s: 5.3623736589051791
-    dkappa: 1.3448158981177618e-06
+    x: 5.3694149764918855
+    y: 0.015937537859993529
+    theta: 4.1116447378808374e-05
+    kappa: 1.2813105926045571e-05
+    s: 5.3694386273358026
+    dkappa: 1.3417625792362397e-06
     ddkappa: 0
   }
-  v: 10.036919291753247
-  a: 0.53658582173429992
+  v: 10.060855955187836
+  a: 0.569489596416844
   relative_time: 0.54000000000000015
 }
 trajectory_point {
   path_point {
-    x: 5.5631956856436435
-    y: 0.016520961549394329
-    theta: 4.3633027634992771e-05
-    kappa: 1.3020374215953742e-05
-    s: 5.5632202147546277
-    dkappa: 1.2580145787225191e-06
+    x: 5.5707458867959367
+    y: 0.016543693255926271
+    theta: 4.3731080156650393e-05
+    kappa: 1.3028449928382232e-05
+    s: 5.570770450126493
+    dkappa: 1.2547515384913797e-06
     ddkappa: 0
   }
-  v: 10.04777860882121
-  a: 0.54928060197229267
+  v: 10.072366296966477
+  a: 0.5814816103749576
   relative_time: 0.56000000000000016
 }
 trajectory_point {
   path_point {
-    x: 5.76426102948557
-    y: 0.017126317393267469
-    theta: 4.6244211569401728e-05
-    kappa: 1.3235434167794859e-05
-    s: 5.7642864698795737
-    dkappa: 1.171118310290671e-06
+    x: 5.7723093761841735
+    y: 0.017150548887439571
+    theta: 4.63487333788315e-05
+    kappa: 1.3244042697839514e-05
+    s: 5.7723348530554812
+    dkappa: 1.1676399818935892e-06
     ddkappa: 0
   }
-  v: 10.058887922774193
-  a: 0.56158642351856358
+  v: 10.084112718432481
+  a: 0.59309844877994133
   relative_time: 0.58000000000000018
 }
 trajectory_point {
   path_point {
-    x: 5.96555099409623
-    y: 0.017732349512287832
-    theta: 4.885831259571205e-05
-    kappa: 1.3450734374524179e-05
-    s: 5.9655773467912949
-    dkappa: 1.084124965424654e-06
+    x: 5.9741100915476224
+    y: 0.017758118745122675
+    theta: 4.8969467393966681e-05
+    kappa: 1.3459889204742693e-05
+    s: 5.9741364830348545
+    dkappa: 1.0804259011530141e-06
     ddkappa: 0
   }
-  v: 10.070239509235304
-  a: 0.57350876606749956
+  v: 10.096087769092392
+  a: 0.604345413392481
   relative_time: 0.6000000000000002
 }
 trajectory_point {
   path_point {
-    x: 6.1670703410409926
-    y: 0.018341568041794879
-    theta: 5.1620752136827264e-05
-    kappa: 1.3598638318905217e-05
-    s: 6.1670976146094665
-    dkappa: 1.0019558335586943e-06
+    x: 6.1761525238883248
+    y: 0.018369047150696653
+    theta: 5.1746562796495255e-05
+    kappa: 1.3604693821490872e-05
+    s: 6.1761798390271849
+    dkappa: 9.9829701276974331e-07
     ddkappa: 0
   }
-  v: 10.081825753421541
-  a: 0.58505310931348775
+  v: 10.108284104487968
+  a: 0.61522780597326254
   relative_time: 0.62000000000000022
 }
 trajectory_point {
   path_point {
-    x: 6.3688236942330292
-    y: 0.018951994241845526
-    theta: 5.4415534274099286e-05
-    kappa: 1.3733156415760941e-05
-    s: 6.3688518912540433
-    dkappa: 9.20678086203449e-07
+    x: 6.3784410331253465
+    y: 0.018981092522204951
+    theta: 5.4548758166017564e-05
+    kappa: 1.3739568731116015e-05
+    s: 6.3784692741662283
+    dkappa: 9.1680367406430869e-07
     ddkappa: 0
   }
-  v: 10.093639150143787
-  a: 0.59622493295091517
+  v: 10.120694486196182
+  a: 0.62575092828297141
   relative_time: 0.64000000000000024
 }
 trajectory_point {
   path_point {
-    x: 6.5708155240720405
-    y: 0.019563141978322266
-    theta: 5.7213619901741453e-05
-    kappa: 1.3867833515796478e-05
-    s: 6.5708446456371359
-    dkappa: 8.3930426686547642e-07
+    x: 6.5809798297891309
+    y: 0.019593895164908115
+    theta: 5.7354420635036263e-05
+    kappa: 1.3874610518702699e-05
+    s: 6.5810089978776345
+    dkappa: 8.3520950532102435e-07
     ddkappa: 0
   }
-  v: 10.105672303806815
-  a: 0.60702971667416883
+  v: 10.133311781829219
+  a: 0.63592008208229345
   relative_time: 0.66000000000000025
 }
 trajectory_point {
   path_point {
-    x: 6.7730501526343936
-    y: 0.02017502432812622
-    theta: 6.0015068890972856e-05
-    kappa: 1.4002672500735864e-05
-    s: 6.7730801998548911
-    dkappa: 7.5783263436612859e-07
+    x: 6.7837729816993049
+    y: 0.0202074673864265
+    theta: 6.0163606551810744e-05
+    kappa: 1.4009821896450483e-05
+    s: 6.7838030779996492
+    dkappa: 7.5351286779034458e-07
     ddkappa: 0
   }
-  v: 10.117917928409287
-  a: 0.61747294017763577
+  v: 10.146128965034476
+  a: 0.64574056913191424
   relative_time: 0.68000000000000027
 }
 trajectory_point {
   path_point {
-    x: 6.9755317573730231
-    y: 0.02078765393058498
-    theta: 6.2819939110614342e-05
-    kappa: 1.4137676155876147e-05
-    s: 6.9755627313793669
-    dkappa: 6.7626150578931485e-07
+    x: 6.9868244172094318
+    y: 0.020821821072411081
+    theta: 6.29763703345887e-05
+    kappa: 1.4145205483570576e-05
+    s: 6.9868554429038161
+    dkappa: 6.7171217890760129e-07
     ddkappa: 0
   }
-  v: 10.130368847543751
-  a: 0.627560083155703
+  v: 10.159139115494568
+  a: 0.65521769119251982
   relative_time: 0.70000000000000029
 }
 trajectory_point {
   path_point {
-    x: 7.17826436583095
-    y: 0.021403509071709952
-    theta: 6.5713814231038725e-05
-    kappa: 1.4205946057869228e-05
-    s: 7.1782962752504105
-    dkappa: 5.9984113308969516e-07
+    x: 7.190137919353579
+    y: 0.021439597257921564
+    theta: 6.5883962293789011e-05
+    kappa: 1.4209427519416184e-05
+    s: 7.1901698836156855
+    dkappa: 5.9540595942197971e-07
     ddkappa: 0
   }
-  v: 10.143017994396642
-  a: 0.63729662530275755
+  v: 10.172335418927322
+  a: 0.66435675002479577
   relative_time: 0.72000000000000031
 }
 trajectory_point {
   path_point {
-    x: 7.3812518792709438
-    y: 0.022020464321854095
-    theta: 6.8622625990055752e-05
-    kappa: 1.4265464315921177e-05
-    s: 7.3812847262675367
-    dkappa: 5.2401843411686782e-07
+    x: 7.3937171513631954
+    y: 0.0220583509629162
+    theta: 6.88012533816007e-05
+    kappa: 1.426911927611307e-05
+    s: 7.3937500559355174
+    dkappa: 5.1936223355828234e-07
     ddkappa: 0
   }
-  v: 10.155858411748284
-  a: 0.64668804631318644
+  v: 10.185711167085776
+  a: 0.67316304738942767
   relative_time: 0.74000000000000032
 }
 trajectory_point {
   path_point {
-    x: 7.5844980554133477
-    y: 0.022638205745046838
-    theta: 7.153514438629216e-05
-    kappa: 1.4325058416832665e-05
-    s: 7.5845318411818079
-    dkappa: 4.4809911587969281e-07
+    x: 7.5975656364327921
+    y: 0.022677923029031467
+    theta: 7.1722402867013813e-05
+    kappa: 1.4328889980883327e-05
+    s: 7.59759948255898
+    dkappa: 4.4321793257364055e-07
     ddkappa: 0
   }
-  v: 10.168883251972888
-  a: 0.6557398258813768
+  v: 10.199259757758185
+  a: 0.68164188504710133
   relative_time: 0.76000000000000034
 }
 trajectory_point {
   path_point {
-    x: 7.7880065151359172
-    y: 0.02325674434649487
-    theta: 7.4451421307131227e-05
-    kappa: 1.4384729422286422e-05
-    s: 7.7880412408877051
-    dkappa: 3.7208182585791153e-07
+    x: 7.8016867662584932
+    y: 0.023298323764905048
+    theta: 7.4647459352483736e-05
+    kappa: 1.4388740628211016e-05
+    s: 7.8017215551978651
+    dkappa: 3.6697178955488354e-07
     ddkappa: 0
   }
-  v: 10.182085777038553
-  a: 0.66445744370171567
+  v: 10.212974694768018
+  a: 0.68979856475850276
   relative_time: 0.78000000000000036
 }
 trajectory_point {
   path_point {
-    x: 7.9917807456523366
-    y: 0.023876090725149592
-    theta: 7.737150672459947e-05
-    kappa: 1.444447835477334e-05
-    s: 7.9918164126150169
-    dkappa: 2.9596526145931652e-07
+    x: 8.00608380341275
+    y: 0.023919648815500846
+    theta: 7.7577291629271627e-05
+    kappa: 1.4446287805862262e-05
+    s: 8.00611953670078
+    dkappa: 2.9082570112814231e-07
     ddkappa: 0
   }
-  v: 10.195459358507268
-  a: 0.67284637946858994
+  v: 10.226849587973955
+  a: 0.69763838828431735
   relative_time: 0.80000000000000038
 }
 trajectory_point {
   path_point {
-    x: 8.1958240952913961
-    y: 0.024498689004168
-    theta: 8.03187873064104e-05
-    kappa: 1.4436611009878593e-05
-    s: 8.1958607121207034
-    dkappa: 2.2551486720995666e-07
+    x: 8.2107598767948566
+    y: 0.024544269295280793
+    theta: 8.0534589548708624e-05
+    kappa: 1.4435849281786819e-05
+    s: 8.21079656317386
+    dkappa: 2.2037379603549707e-07
     ddkappa: 0
   }
-  v: 10.208997477534904
-  a: 0.68091211287638675
+  v: 10.240878153269891
+  a: 0.70516665738523088
   relative_time: 0.8200000000000004
 }
 trajectory_point {
   path_point {
-    x: 8.4001397976386016
-    y: 0.02512220972116181
-    theta: 8.32708783420344e-05
-    kappa: 1.4426190864803987e-05
-    s: 8.4001773658807846
-    dkappa: 1.5518700605429674e-07
+    x: 8.4157180053180856
+    y: 0.025169750537213646
+    theta: 8.3495962790891554e-05
+    kappa: 1.4425396372838084e-05
+    s: 8.4157556461014735
+    dkappa: 1.498248042550686e-07
     ddkappa: 0
   }
-  v: 10.222693724871224
-  a: 0.688660123619493
+  v: 10.255054212584936
+  a: 0.71238867382192916
   relative_time: 0.84000000000000041
 }
 trajectory_point {
   path_point {
-    x: 8.60473095234449
-    y: 0.025746571050258404
-    theta: 8.6226949298939815e-05
-    kappa: 1.4415756671599425e-05
-    s: 8.6047694732822126
-    dkappa: 8.47643309660133e-08
+    x: 8.6209610779522787
+    y: 0.025796101357716962
+    theta: 8.6461453097541607e-05
+    kappa: 1.4414928931677973e-05
+    s: 8.6209996744669173
+    dkappa: 7.91777313695523e-08
     ddkappa: 0
   }
-  v: 10.236541800859881
-  a: 0.696095891392296
+  v: 10.269371693883413
+  a: 0.71930973935509779
   relative_time: 0.86000000000000043
 }
 trajectory_point {
   path_point {
-    x: 8.809600533884975
-    y: 0.026371782068818635
-    theta: 8.9187043154748125e-05
-    kappa: 1.440530827856599e-05
-    s: 8.8096400088147568
-    dkappa: 1.4245818095607847e-08
+    x: 8.8264918632874849
+    y: 0.026423330205839125
+    theta: 8.943110047132663e-05
+    kappa: 1.4404446817107791e-05
+    s: 8.8265314168731326
+    dkappa: 8.431624397780428e-09
     ddkappa: 0
   }
-  v: 10.250535515438411
-  a: 0.70322489588918258
+  v: 10.283824631164855
+  a: 0.72593515574542256
   relative_time: 0.88000000000000045
 }
 trajectory_point {
   path_point {
-    x: 9.0147513935245449
-    y: 0.026998017223086285
-    theta: 9.214791125655708e-05
-    kappa: 1.4389933406104881e-05
-    s: 9.01479182426287
-    dkappa: -5.5912111806325388e-08
+    x: 9.0323130105775373
+    y: 0.027051797503611329
+    theta: 9.2397949695399717e-05
+    kappa: 1.4383507777784173e-05
+    s: 9.0323535236634
+    dkappa: -6.1442092173418486e-08
     ddkappa: 0
   }
-  v: 10.264668788138238
-  a: 0.71005261680453979
+  v: 10.298407164464015
+  a: 0.73227022475358949
   relative_time: 0.90000000000000047
 }
 trajectory_point {
   path_point {
-    x: 9.2201862568621475
-    y: 0.027627136087629435
-    theta: 9.5072847629218415e-05
-    kappa: 1.4314766747366124e-05
-    s: 9.2202276508975771
-    dkappa: -1.206015283643401e-07
+    x: 9.2384270494743852
+    y: 0.027682996259316649
+    theta: 9.5332556020011339e-05
+    kappa: 1.4308092615180705e-05
+    s: 9.2384685290420485
+    dkappa: -1.2634537443053712e-07
     ddkappa: 0
   }
-  v: 10.278935648084678
-  a: 0.71658453383275456
+  v: 10.313113539850855
+  a: 0.738320248140284
   relative_time: 0.92000000000000048
 }
 trajectory_point {
   path_point {
-    x: 9.4259077429918179
-    y: 0.028257132699029019
-    theta: 9.800186487396445e-05
-    kappa: 1.4239495216079211e-05
-    s: 9.4259501016683434
-    dkappa: -1.8538119961861129e-07
+    x: 9.4448364057609311
+    y: 0.028315099388008455
+    theta: 9.8271367008795352e-05
+    kappa: 1.4232569398758666e-05
+    s: 9.4448788531951564
+    dkappa: -1.9134164922713296e-07
     ddkappa: 0
   }
-  v: 10.293330233996929
-  a: 0.72282612666821411
+  v: 10.327938109430553
+  a: 0.74409052766619177
   relative_time: 0.9400000000000005
 }
 trajectory_point {
   path_point {
-    x: 9.6319183487216371
-    y: 0.028888014703451037
-    theta: 0.00010093499853969234
-    kappa: 1.4164117898685859e-05
-    s: 9.63196167339496
-    dkappa: -2.5025191178943058e-07
+    x: 9.6515433877148862
+    y: 0.02894811395850181
+    theta: 0.00010121441552612964
+    kappa: 1.4156937283941244e-05
+    s: 9.6515868044112612
+    dkappa: -2.5643164341714e-07
     ddkappa: 0
   }
-  v: 10.307846794188084
-  a: 0.72878287500530536
+  v: 10.342875331343501
+  a: 0.7495863650919985
   relative_time: 0.96000000000000052
 }
 trajectory_point {
   path_point {
-    x: 9.8382204569224267
-    y: 0.029519789398142704
-    theta: 0.00010387228255392955
-    kappa: 1.4088633923316344e-05
-    s: 9.83826474895942
-    dkappa: -3.1521441521936677e-07
+    x: 9.85855019383764
+    y: 0.029582046703435338
+    theta: 0.00010416173287364217
+    kappa: 1.4081195466317722e-05
+    s: 9.858594581202059
+    dkappa: -3.2161604928700738e-07
     ddkappa: 0
   }
-  v: 10.322479686565115
-  a: 0.73446025853841546
+  v: 10.357919769765305
+  a: 0.75481306217839039
   relative_time: 0.98000000000000054
 }
 trajectory_point {
   path_point {
-    x: 10.044816337531056
-    y: 0.030152851314152747
-    theta: 0.00010679040679262286
-    kappa: 1.4000000938792297e-05
-    s: 10.044861599497789
-    dkappa: -3.7893027025861392e-07
+    x: 10.06585891323876
+    y: 0.030217470182976658
+    theta: 0.00010707925098962789
+    kappa: 1.3986292593242489e-05
+    s: 10.065904274423108
+    dkappa: -3.849393356404148e-07
     ddkappa: 0
   }
-  v: 10.33722337862889
-  a: 0.73986375696193141
+  v: 10.373066094906779
+  a: 0.7597759206860526
   relative_time: 1.0000000000000004
 }
 trajectory_point {
   path_point {
-    x: 11.082274265273158
-    y: 0.033339255447987236
-    theta: 0.00012096622551327485
-    kappa: 1.3302839173564902e-05
-    s: 11.082324420522841
-    dkappa: -6.7275643327546345e-07
+    x: 11.106997125916928
+    y: 0.033415328498630638
+    theta: 0.00012128625421325978
+    kappa: 1.3280398172572186e-05
+    s: 11.107047398205967
+    dkappa: -6.7909153646257485e-07
     ddkappa: 0
   }
-  v: 10.412417868351286
-  a: 0.76296476173914418
+  v: 10.450146901252856
+  a: 0.78081819616742254
   relative_time: 1.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 12.127356764081114
-    y: 0.03655545657666659
-    theta: 0.00013436320250770351
-    kappa: 1.2325119674962168e-05
-    s: 12.127411868187302
-    dkappa: -9.3679369380808782e-07
+    x: 12.155938630311711
+    y: 0.036643503803641828
+    theta: 0.00013470392761982808
+    kappa: 1.2292689973800088e-05
+    s: 12.155993870033532
+    dkappa: -9.4327956218917349e-07
     ddkappa: 0
   }
-  v: 10.489615491857325
-  a: 0.78004059293455719
+  v: 10.529036377461418
+  a: 0.79605977128341565
   relative_time: 1.2000000000000006
 }
 trajectory_point {
   path_point {
-    x: 13.180235171411992
-    y: 0.039798999078869017
-    theta: 0.00014669077412765574
-    kappa: 1.1094712066207876e-05
-    s: 13.180295271604622
-    dkappa: -1.1704050434927064e-06
+    x: 13.212836402976883
+    y: 0.039899449950628246
+    theta: 0.00014703912328162261
+    kappa: 1.1051279907673015e-05
+    s: 13.212896657923695
+    dkappa: -1.1768470253204947e-06
     ddkappa: 0
   }
-  v: 10.56824797987875
-  a: 0.7917762123465496
+  v: 10.609187589500216
+  a: 0.80616336611974759
   relative_time: 1.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 14.241027423882031
-    y: 0.043066939585575038
-    theta: 0.00015768230065305033
-    kappa: 9.6407926558306338e-06
-    s: 14.24109255777042
-    dkappa: -1.3729241427838609e-06
+    x: 14.277792041953273
+    y: 0.043180131867216184
+    theta: 0.00015802297499201146
+    kappa: 9.5856272524176469e-06
+    s: 14.277857350091564
+    dkappa: -1.3791108196522571e-06
     ddkappa: 0
   }
-  v: 10.647815559327134
-  a: 0.7988565817735005
+  v: 10.69011987534557
+  a: 0.81179170076213514
   relative_time: 1.4000000000000008
 }
 trajectory_point {
   path_point {
-    x: 15.309804907087328
-    y: 0.046355781923636381
-    theta: 0.00016709745686149802
-    kappa: 7.9939186480343088e-06
-    s: 15.309875101180461
-    dkappa: -1.5436660929513698e-06
+    x: 15.350862394172031
+    y: 0.046481960012526982
+    theta: 0.00016741334991721146
+    kappa: 7.9266066064911479e-06
+    s: 15.350932782150297
+    dkappa: -1.549371268342101e-06
     ddkappa: 0
   }
-  v: 10.727886953293893
-  a: 0.80196666301378927
+  v: 10.771418844982369
+  a: 0.81360749529629373
   relative_time: 1.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 16.38659930508301
-    y: 0.049661519469379689
-    theta: 0.0001747247878149949
-    kappa: 6.1860788327835032e-06
-    s: 16.386674573448655
-    dkappa: -1.6819308487196344e-06
+    x: 16.432066182489283
+    y: 0.049800841024626633
+    theta: 0.00017499746194449273
+    kappa: 6.1065531240930033e-06
+    s: 16.43214166431197
+    dkappa: -1.6869155033692207e-06
     ddkappa: 0
   }
-  v: 10.808099381050283
-  a: 0.801791417865795
+  v: 10.852736380404078
+  a: 0.81227346980793946
   relative_time: 1.600000000000001
 }
 trajectory_point {
   path_point {
-    x: 17.471409449539628
-    y: 0.052979779100158574
-    theta: 0.00018038441781564885
-    kappa: 4.2507171417025893e-06
-    s: 17.471489792925009
-    dkappa: -1.7870128253141644e-06
+    x: 17.521390632449457
+    y: 0.053132313213229608
+    theta: 0.00018059461471464644
+    kappa: 4.159281491541742e-06
+    s: 17.521471208588448
+    dkappa: -1.7910268365673476e-06
     ddkappa: 0
   }
-  v: 10.88815855804739
-  a: 0.79901580812789663
+  v: 10.933790635612731
+  a: 0.80845234438278912
   relative_time: 1.7000000000000011
 }
 trajectory_point {
   path_point {
-    x: 18.564208169444115
-    y: 0.056305785156102926
-    theta: 0.00018393063969446288
-    kappa: 2.2227363836095303e-06
-    s: 18.564293574313655
-    dkappa: -1.8582061577573085e-06
+    x: 18.618798099562348
+    y: 0.056471511989175979
+    theta: 0.00018405882743133262
+    kappa: 2.1200860429512483e-06
+    s: 18.618883755992208
+    dkappa: -1.8609896026253325e-06
     ddkappa: 0
   }
-  v: 10.967838695916145
-  a: 0.79432479559847358
+  v: 11.014366036618933
+  a: 0.80280683910655792
   relative_time: 1.8000000000000012
 }
 trajectory_point {
   path_point {
-    x: 19.664949140456972
-    y: 0.059634432672747525
-    theta: 0.00018525449723982845
-    kappa: 1.3848841672342092e-07
-    s: 19.665039578290791
-    dkappa: -1.894807086915506e-06
+    x: 19.72423269622648
+    y: 0.059813247904324116
+    theta: 0.00018528144172069716
+    kappa: 2.5727799572777194e-08
+    s: 19.724323403737227
+    dkappa: -1.8960915241290617e-06
     ddkappa: 0
   }
-  v: 11.046982502467314
-  a: 0.788403342075905
+  v: 11.094313281441863
+  a: 0.79599967406496264
   relative_time: 1.9000000000000012
 }
 trajectory_point {
   path_point {
-    x: 20.773573734234922
-    y: 0.062960370330341131
-    theta: 0.00018428637746259469
-    kappa: -1.9642475286638212e-06
-    s: 20.773669161122683
-    dkappa: -1.8961173859364252e-06
+    x: 20.837626918644965
+    y: 0.06315208550741247
+    theta: 0.00018419374721379
+    kappa: -2.0855898283473093e-06
+    s: 20.837722632439824
+    dkappa: -1.8956269785301754e-06
     ddkappa: 0
   }
-  v: 11.12550118169149
-  a: 0.78193640935856767
+  v: 11.173549340109265
+  a: 0.78869356934372159
   relative_time: 2.0000000000000013
 }
 trajectory_point {
   path_point {
-    x: 21.89001684544878
-    y: 0.0662780718104858
-    theta: 0.00018099837040352895
-    kappa: -4.0462912637410216e-06
-    s: 21.890117201948751
-    dkappa: -1.8614456680248951e-06
+    x: 21.958907274873084
+    y: 0.066482418446346364
+    theta: 0.00018076934319122273
+    kappa: -4.1742497364664413e-06
+    s: 21.959007934444585
+    dkappa: -1.8588981798642503e-06
     ddkappa: 0
   }
-  v: 11.203333137116438
-  a: 0.774349897666475
+  v: 11.252017111607751
+  a: 0.780321550955707
   relative_time: 2.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 23.014201851853475
-    y: 0.069581345505388359
-    theta: 0.00017538032632380407
-    kappa: -6.0656280837587996e-06
-    s: 23.014307061521887
-    dkappa: -1.7896668034342958e-06
+    x: 23.087989312153255
+    y: 0.06979547420291965
+    theta: 0.00017487141579852761
+    kappa: -6.19073438212475e-06
+    s: 23.088094832518106
+    dkappa: -1.7826109799055509e-06
     ddkappa: 0
   }
-  v: 11.280303952041
-  a: 0.76474711046088584
+  v: 11.329548872668591
+  a: 0.77000620361325778
   relative_time: 2.2000000000000015
 }
 trajectory_point {
   path_point {
-    x: 24.146032919867693
-    y: 0.072860399103546067
-    theta: 0.00016733379371469681
-    kappa: -7.9662824932727414e-06
-    s: 24.146142879525588
-    dkappa: -1.6771283381890346e-06
+    x: 24.224770054675144
+    y: 0.073085954343068374
+    theta: 0.00016666197462722465
+    kappa: -8.08994304861924e-06
+    s: 24.22488033740208
+    dkappa: -1.6672904637479914e-06
     ddkappa: 0
   }
-  v: 11.35622205224684
-  a: 0.75332911440205652
+  v: 11.405960143303085
+  a: 0.757944585451279
   relative_time: 2.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 25.285396042004244
-    y: 0.0761126631661421
-    theta: 0.00015718397590269717
-    kappa: -9.7115402575742e-06
-    s: 25.285510643554815
-    dkappa: -1.5263747309885729e-06
+    x: 25.369129061682322
+    y: 0.07634912994172241
+    theta: 0.00015634427617206015
+    kappa: -9.8301298272152141e-06
+    s: 25.369243997129992
+    dkappa: -1.5134574822740619e-06
     ddkappa: 0
   }
-  v: 11.430915970181646
-  a: 0.74029697615024348
+  v: 11.481086149336026
+  a: 0.74433375460467566
   relative_time: 2.4000000000000017
 }
 trajectory_point {
   path_point {
-    x: 26.432161078411788
-    y: 0.079332255237245047
-    theta: 0.00014510920694243978
-    kappa: -1.1256173519407752e-05
-    s: 26.432280199782603
-    dkappa: -1.33676703933168e-06
+    x: 26.520930403134571
+    y: 0.079579063221182592
+    theta: 0.00014410121054542202
+    kappa: -1.1365598637949132e-05
+    s: 26.521049867608497
+    dkappa: -1.3204697817667222e-06
     ddkappa: 0
   }
-  v: 11.504234344959132
-  a: 0.72585176236570292
+  v: 11.554781822405696
+  a: 0.729370769208353
   relative_time: 2.5000000000000018
 }
 trajectory_point {
   path_point {
-    x: 27.586183757373949
-    y: 0.082513418636178712
-    theta: 0.000131342366483711
-    kappa: -1.2553725093406782e-05
-    s: 27.586307263626651
-    dkappa: -1.1076846272159564e-06
+    x: 27.680024626315159
+    y: 0.0827699476498231
+    theta: 0.00013017086671718303
+    kappa: -1.2649422037760581e-05
+    s: 27.680148483198735
+    dkappa: -1.0877048297248163e-06
     ddkappa: 0
   }
-  v: 11.576045922359038
-  a: 0.71019453970869129
+  v: 11.626921799963871
+  a: 0.713252687397216
   relative_time: 2.6000000000000019
 }
 trajectory_point {
   path_point {
-    x: 28.747307685890313
-    y: 0.085650548411547067
-    theta: 0.00011617277749387256
-    kappa: -1.3556565719993061e-05
-    s: 28.747435430415944
-    dkappa: -8.3852618199402048e-07
+    x: 28.846250726327032
+    y: 0.085916133879812256
+    theta: 0.00011484842444309251
+    kappa: -1.3633498124962707e-05
+    s: 28.846378827297713
+    dkappa: -8.1455857646747122e-07
     ddkappa: 0
   }
-  v: 11.64623955482713
-  a: 0.69352637483946467
+  v: 11.697400425275809
+  a: 0.69617656730617006
   relative_time: 2.700000000000002
 }
 trajectory_point {
   path_point {
-    x: 29.915366360055614
-    y: 0.088738298671816457
-    theta: 9.9948024101070843e-05
-    kappa: -1.4215955705944659e-05
-    s: 29.915498186057341
-    dkappa: -5.2871461332035186e-07
+    x: 30.019438119236717
+    y: 0.089011143222560138
+    theta: 9.8480687567459313e-05
+    kappa: -1.4262486053194084e-05
+    s: 30.019570302919636
+    dkappa: -4.9985602451302552e-07
     ddkappa: 0
   }
-  v: 11.7147242014752
-  a: 0.67604833441827961
+  v: 11.766131747420271
+  a: 0.67833946707012
   relative_time: 2.800000000000002
 }
 trajectory_point {
   path_point {
-    x: 31.090185188531432
-    y: 0.091766569406757661
-    theta: 8.3071764644948587e-05
-    kappa: -1.4452015369306788e-05
-    s: 31.090320917702172
-    dkappa: -1.7501797360884317e-07
+    x: 31.199408626727827
+    y: 0.092042039747971191
+    theta: 8.149570872317824e-05
+    kappa: -1.4438351968946111e-05
+    s: 31.199544703277247
+    dkappa: -1.3895216858092152e-07
     ddkappa: 0
   }
-  v: 11.781428928081063
-  a: 0.65796148510539221
+  v: 11.833049521289498
+  a: 0.65993844482397113
   relative_time: 2.9000000000000021
 }
 trajectory_point {
   path_point {
-    x: 32.271583477051962
-    y: 0.094730438059010325
-    theta: 6.6107017128391732e-05
-    kappa: -1.4206249716794459e-05
-    s: 32.27172292441287
-    dkappa: 2.2307306895702622e-07
+    x: 32.385978387621329
+    y: 0.09501235533297786
+    theta: 6.4490934011729451e-05
+    kappa: -1.415081163785642e-05
+    s: 32.386118182363205
+    dkappa: 2.6420081349816854e-07
     ddkappa: 0
   }
-  v: 11.846302907088564
-  a: 0.639466893561059
+  v: 11.898107207589225
+  a: 0.6411705587026284
   relative_time: 3.0000000000000022
 }
 trajectory_point {
   path_point {
-    x: 33.459376439508361
-    y: 0.0976305599873331
-    theta: 4.9638242774374675e-05
-    kappa: -1.3451629091353087e-05
-    s: 33.459519427829534
-    dkappa: 6.636109108885532e-07
+    x: 33.578959891216371
+    y: 0.097918223680888275
+    theta: 4.8029769909252451e-05
+    kappa: -1.3347176945747479e-05
+    s: 33.579103225531405
+    dkappa: 7.1011050883806074e-07
     ddkappa: 0
   }
-  v: 11.909315417607569
-  a: 0.62076562644553612
+  v: 11.96127797283868
+  a: 0.6222328668409971
   relative_time: 3.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 34.653377232822109
-    y: 0.10046366390371642
-    theta: 3.4284916719418135e-05
-    kappa: -1.2135093497085374e-05
-    s: 34.653523582836556
-    dkappa: 1.1470799206498331e-06
+    x: 34.778163926740106
+    y: 0.10075638322086264
+    theta: 3.2741240462819121e-05
+    kappa: -1.1973914628237099e-05
+    s: 34.778310620078344
+    dkappa: 1.1992617969460608e-06
     ddkappa: 0
   }
-  v: 11.970455845413973
-  a: 0.60205875041908
+  v: 12.022554689370578
+  a: 0.60332242737398212
   relative_time: 3.2000000000000024
 }
 trajectory_point {
   path_point {
-    x: 35.853398955593761
-    y: 0.10322724322933852
-    theta: 2.0732591317518256e-05
-    kappa: -1.0202792484045815e-05
-    s: 35.853548488229229
-    dkappa: 1.673942521999869e-06
+    x: 35.983401553619707
+    y: 0.10352437938635266
+    theta: 1.932107711200004e-05
+    kappa: -9.9767016309273324e-06
+    s: 35.983551425824459
+    dkappa: 1.7321174818091891e-06
     ddkappa: 0
   }
-  v: 12.029733682949695
-  a: 0.58354733214194687
+  v: 12.081949935331128
+  a: 0.58463629843648868
   relative_time: 3.3000000000000025
 }
 trajectory_point {
   path_point {
-    x: 37.059256666708443
-    y: 0.1059160259799832
-    theta: 9.882392783655547e-06
-    kappa: -7.5696619227610907e-06
-    s: 37.059409197380333
-    dkappa: 2.2464064732016013e-06
+    x: 37.194486097862665
+    y: 0.10620890665551853
+    theta: 9.0147703670773183e-06
+    kappa: -7.2015573476717447e-06
+    s: 37.1946389456955
+    dkappa: 2.3148508849419775e-06
     ddkappa: 0
   }
-  v: 12.087178529322681
-  a: 0.56543243827439316
+  v: 12.139495994680024
+  a: 0.566371538163422
   relative_time: 3.4000000000000026
 }
 trajectory_point {
   path_point {
-    x: 38.270769401366486
-    y: 0.10851898076485519
-    theta: 3.1443455523749719e-06
-    kappa: -4.0900096176083952e-06
-    s: 38.270924728906735
-    dkappa: 2.8451071053177648e-06
+    x: 38.411235062356155
+    y: 0.10881237391055423
+    theta: 2.7781327482934159e-06
+    kappa: -3.6136580635695804e-06
+    s: 38.41139069630384
+    dkappa: 2.9087117508282566e-06
     ddkappa: 0
   }
-  v: 12.142840090306901
-  a: 0.54791513547667514
+  v: 12.195244857190456
+  a: 0.54872520468968711
   relative_time: 3.5000000000000027
 }
 trajectory_point {
   path_point {
-    x: 39.487762096665094
-    y: 0.11106016613795401
-    theta: -1.3414505284359279e-07
-    kappa: -2.5454798736161278e-07
-    s: 39.487920077336014
-    dkappa: 3.0995818031015352e-06
+    x: 39.633472101050913
+    y: 0.11135426796522499
+    theta: -1.9211089554715954e-06
+    kappa: -3.6454152287720693e-06
+    s: 39.633630378529858
+    dkappa: -7.8542173733395344e-07
     ddkappa: 0
   }
-  v: 12.196788178342352
-  a: 0.53119649040904893
+  v: 12.249268218449103
+  a: 0.531894356150189
   relative_time: 3.6000000000000028
 }
 trajectory_point {
   path_point {
-    x: 40.710067790478021
-    y: 0.11350847582696991
-    theta: -1.9703164324091915e-05
-    kappa: -2.7031086964139441e-05
-    s: 40.710228223773044
-    dkappa: -2.1603140679498398e-05
+    x: 40.861029131370408
+    y: 0.11380101025588693
+    theta: -2.4518056973565905e-05
+    kappa: -2.946449819460274e-05
+    s: 40.861189848103265
+    dkappa: -2.052347952751584e-05
     ddkappa: 0
   }
-  v: 12.249112712535057
-  a: 0.51547756973177128
+  v: 12.301657479856132
+  a: 0.516076050679833
   relative_time: 3.7000000000000028
 }
 trajectory_point {
   path_point {
-    x: 41.9375294943998
-    y: 0.11584165688555154
-    theta: -6.4780035566869287e-05
-    kappa: -4.38778883302789e-05
-    s: 41.937692146566604
-    dkappa: -1.318021840824374e-05
+    x: 42.0937481697859
+    y: 0.11612898237872771
+    theta: -7.17724000489639e-05
+    kappa: -4.5399417830602538e-05
+    s: 42.093911086184463
+    dkappa: -1.218359177219583e-05
     ddkappa: 0
   }
-  v: 12.299923718657061
-  a: 0.50095944010509807
+  v: 12.352523748625204
+  a: 0.501467346413524
   relative_time: 3.8000000000000029
 }
 trajectory_point {
   path_point {
-    x: 43.170002231256419
-    y: 0.11803221724911137
-    theta: -0.00012470896233018891
-    kappa: -5.1994833305407456e-05
-    s: 43.170166831975962
-    dkappa: -5.8527046905385484e-06
+    x: 43.331483327794054
+    y: 0.11831145473740985
+    theta: -0.00013304651426748748
+    kappa: -5.2662114404449684e-05
+    s: 43.331648169945929
+    dkappa: -4.9472017913874685e-06
     ddkappa: 0
   }
-  v: 12.349351329146439
-  a: 0.48784316818928586
+  v: 12.401997837783469
+  a: 0.48826530148616737
   relative_time: 3.900000000000003
 }
 trajectory_point {
   path_point {
-    x: 44.407355014499743
-    y: 0.12006395939036216
-    theta: -0.00019013128131817325
-    kappa: -5.2608486925511591e-05
-    s: 44.407521284837536
-    dkappa: 3.671553621169577e-07
+    x: 44.574102773532715
+    y: 0.12032157819437288
+    theta: -0.00019874363756722246
+    kappa: -5.209597366310306e-05
+    s: 44.574269243153488
+    dkappa: 1.0982994898868233e-06
     ddkappa: 0
   }
-  v: 12.397545783107287
-  a: 0.47632982064458951
+  v: 12.450230266171571
+  a: 0.47666697403267133
   relative_time: 4.0000000000000027
 }
 trajectory_point {
   path_point {
-    x: 45.649471282199457
-    y: 0.12191085335896101
-    theta: -0.00025191347754249804
-    kappa: -4.6402493564917886e-05
-    s: 45.649638927148835
-    dkappa: 5.3342630074246372e-06
+    x: 45.821489069857677
+    y: 0.12214860271510443
+    theta: -0.00025948350909654749
+    kappa: -4.5016559452659442e-05
+    s: 45.821656879106143
+    dkappa: 5.9003474493265784e-06
     ddkappa: 0
   }
-  v: 12.444613098207066
-  a: 0.4646983810423635
+  v: 12.497327111146207
+  a: 0.46495292150037876
   relative_time: 4.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 46.896233348427458
-    y: 0.12358881984669964
-    theta: -0.00030284562824522965
-    kappa: -3.5301056734449413e-05
-    s: 46.896402124146704
-    dkappa: 9.1132870539210374e-06
+    x: 47.073523700872052
+    y: 0.12381474647435167
+    theta: -0.00030898459789430177
+    kappa: -3.342611182732297e-05
+    s: 47.073692620543916
+    dkappa: 9.5598175991546042e-06
     ddkappa: 0
   }
-  v: 12.490428294361474
-  a: 0.45135199281160709
+  v: 12.54316390127604
+  a: 0.45153039309990478
   relative_time: 4.200000000000002
 }
 trajectory_point {
   path_point {
-    x: 48.147508024343878
-    y: 0.1251169456115537
-    theta: -0.00033845125459164294
-    kappa: -2.0896802007026232e-05
-    s: 48.14767773470421
-    dkappa: 1.1746876908776732e-05
+    x: 48.3300727283089
+    y: 0.12533151591383385
+    theta: -0.00034267096921780649
+    kappa: -1.864724213170384e-05
+    s: 48.330242564762607
+    dkappa: 1.2065713029670886e-05
     ddkappa: 0
   }
-  v: 12.534839242029994
-  a: 0.43667796239905088
+  v: 12.587589143763241
+  a: 0.43678648461105113
   relative_time: 4.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 49.403148856357078
-    y: 0.1265107675738803
-    theta: -0.00035554835944662244
-    kappa: -4.5223568294479713e-06
-    s: 49.403319341272592
-    dkappa: 1.3224435249602321e-05
+    x: 49.59098900861099
+    y: 0.12670613494704014
+    theta: -0.00035593480993023618
+    kappa: -2.001730594306759e-06
+    s: 49.591159595257167
+    dkappa: 1.3319261280522397e-05
     ddkappa: 0
   }
-  v: 12.577732542316783
-  a: 0.42106359625142542
+  v: 12.630490055387964
+  a: 0.42110829181361953
   relative_time: 4.4000000000000012
 }
 trajectory_point {
   path_point {
-    x: 50.663000001507314
-    y: 0.1277766040844148
-    theta: -0.00035000918766669997
-    kappa: 1.2362332841878058e-05
-    s: 50.663171122945748
-    dkappa: 1.3392037088129766e-05
+    x: 50.856116045078849
+    y: 0.12795902827537259
+    theta: -0.00034682297684529573
+    kappa: 1.4858938861358466e-05
+    s: 50.856287252679408
+    dkappa: 1.3281922107638988e-05
     ddkappa: 0
   }
-  v: 12.619033526970668
-  a: 0.40489620081546124
+  v: 12.67179256250834
+  a: 0.4048829104874116
   relative_time: 4.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 51.926900065521437
-    y: 0.12894617474729234
-    theta: -0.00032361050710916217
-    kappa: 2.8097835348878664e-05
-    s: 51.927071728524687
-    dkappa: 1.2323794323434383e-05
+    x: 52.12529186412651
+    y: 0.12912284800219331
+    theta: -0.00031789283833960624
+    kappa: 3.0396323437158402e-05
+    s: 52.125463605795908
+    dkappa: 1.2057471418354904e-05
     ddkappa: 0
   }
-  v: 12.658706258385152
-  a: 0.388563082537889
+  v: 12.71146130106048
+  a: 0.38849743641222889
   relative_time: 4.6000000000000005
 }
 trajectory_point {
   path_point {
-    x: 53.194686011490973
-    y: 0.13004337339747052
-    theta: -0.00027946854547122868
-    kappa: 4.1267851802184719e-05
-    s: 53.194858149582011
-    dkappa: 1.0067550798541817e-05
+    x: 53.39835291143099
+    y: 0.13021572271100762
+    theta: -0.00027144147296409571
+    kappa: 4.3196776613291428e-05
+    s: 53.398525122445712
+    dkappa: 9.6368722727656723e-06
     ddkappa: 0
   }
-  v: 12.696753529598416
-  a: 0.37245154786543927
+  v: 12.749499616558481
+  a: 0.3723389653678732
   relative_time: 4.7
 }
 trajectory_point {
   path_point {
-    x: 54.466197030146326
-    y: 0.13108315489990205
-    theta: -0.00022134875654966635
-    kappa: 5.0454100927841282e-05
-    s: 54.466369593526359
-    dkappa: 6.61477036912586e-06
+    x: 54.675137913651653
+    y: 0.13124600260102631
+    theta: -0.00021046733244878268
+    kappa: 5.1104090693933582e-05
+    s: 54.675310540498216
+    dkappa: 5.85843918512449e-06
     ddkappa: 0
   }
-  v: 12.733216864293302
-  a: 0.35694890324484257
+  v: 12.785949564094409
+  a: 0.35679459313414624
   relative_time: 4.8
 }
 trajectory_point {
   path_point {
-    x: 55.741278413532058
-    y: 0.13206710919431067
-    theta: -0.00015447834321991039
-    kappa: 5.30507217341463e-05
-    s: 55.741451356666921
-    dkappa: 1.7531616408515027e-06
+    x: 55.955491735894277
+    y: 0.13222714698360083
+    theta: -0.00014310415927054621
+    kappa: 5.2728087873690225e-05
+    s: 55.955664738810881
+    dkappa: 8.0392964079113882e-07
     ddkappa: 0
   }
-  v: 12.768176516797336
-  a: 0.34244245512282956
+  v: 12.820891908338323
+  a: 0.34225141549084948
   relative_time: 4.8999999999999995
 }
 trajectory_point {
   path_point {
-    x: 57.019785410803536
-    y: 0.13300383933950807
-    theta: -8.8788423608043843e-05
-    kappa: 4.8299738295446893e-05
-    s: 57.019958697277815
-    dkappa: -4.3315145168870515e-06
+    x: 57.239269265901349
+    y: 0.13316036229726008
+    theta: -7.802187583161313e-05
+    kappa: 4.6827091346073132e-05
+    s: 57.23944260818709
+    dkappa: -5.4735895673895661e-06
     ddkappa: 0
   }
-  v: 12.801751472082712
-  a: 0.32931950994613074
+  v: 12.854446123538253
+  a: 0.32909652821778473
   relative_time: 4.9999999999999991
 }
 trajectory_point {
   path_point {
-    x: 58.301587120482
-    y: 0.13388291598690225
-    theta: -3.3856202931392687e-05
-    kappa: 3.4770742331594504e-05
-    s: 58.30176070866267
-    dkappa: -1.1638220300975541e-05
+    x: 58.526339285418182
+    y: 0.13403082977812664
+    theta: -2.5788598374099081e-05
+    kappa: 3.1585441062047558e-05
+    s: 58.526512922333943
+    dkappa: -1.3019516278096136e-05
     ddkappa: 0
   }
-  v: 12.834099445766297
-  a: 0.3179673741614768
+  v: 12.886770393520212
+  a: 0.31771702709475369
   relative_time: 5.0999999999999988
 }
 decision {