add csv to traj (#60)

* add csv to traj

* kdtree

* path ok

* コメント追記

* オフライン最適化したcsvの読み取りに対応

* add spell

* add spell

.cspell.json

@@ -154,6 +154,8 @@
     "xyzrpy",
     "zcvf",
     "zenoh",
-    "zenohd"
+    "zenohd",
+    "coef",
+    "radpm"
   ]
 }

aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml

@@ -94,18 +94,20 @@
         </group> <!-- behavior_planning -->
       </group> <!-- lane_driving -->
 
-      <!-- Customizable -->
-      <node pkg="path_to_trajectory" exec="path_to_trajectory_node" name="path_to_trajectory" output="screen">
-        <remap from="input" to="/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id"/>
+      <node pkg="path_to_trajectory" exec="csv_to_trajectory_node" name="csv_to_trajectory" output="screen">
+        <param name="csv_file_path" value="$(find-pkg-share booars_launch)/map/csv/traj_race_cl.csv"/>
+        <!-- csvから読み込んだ速度に掛ける係数 -->
+        <param name="velocity_coef" value="1.0"/>
+        <!-- 車両前方のtrajectoryの長さ -->
+        <param name="trajectory_length" value="150.0"/>
+        <!-- 車両後方のtrajectoryの長さ -->
+        <param name="trajectory_rear_length" value="20.0"/>
+        <!-- trajectoryの間隔 -->
+        <param name="trajectory_margin" value="2.0"/>
+        <!-- pose.position.zの値 -->
+        <param name="z_position" value="0.0"/>
         <remap from="output" to="/planning/scenario_planning/trajectory"/>
-        <!-- 減速度 -->
-        <param name="deceleration" value="-14.0"/>
-        <!-- ピットの何メートル前で速度を0にするか -->
-        <param name="stop_offset" value="1.5"/>
-        <!-- 最高速度 -->
-        <param name="max_speed" value="30.0"/>
-        <!-- trajectoryの点の間隔 -->
-        <param name="traj_width" value="1.0"/>
+        <remap from="in_odom" to="/localization/kinematic_state"/>
       </node>
 
     </group>

aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_mincurv_1.0m.csv

@@ -0,0 +1,157 @@
+x,y,z,yaw
+89614.2712031,43153.1487373,0,0
+89614.0619002,43155.0924205,0,0
+89614.0392936,43157.1218632,0,0
+89614.2210932,43159.1078409,0,0
+89614.7173211,43161.1786337,0,0
+89615.6728789,43162.8713898,0,0
+89617.1482951,43163.7683119,0,0
+89619.09648,43164.1869667,0,0
+89621.0429118,43163.8048284,0,0
+89622.9393715,43162.9648365,0,0
+89624.6009732,43161.8822774,0,0
+89626.108526,43160.5884374,0,0
+89627.461646,43159.1351064,0,0
+89628.658612,43157.5378791,0,0
+89629.67288,43155.8389378,0,0
+89630.5229818,43154.0464154,0,0
+89631.2440803,43152.1997998,0,0
+89631.9125908,43150.3215501,0,0
+89632.5933863,43148.4529121,0,0
+89633.3550535,43146.6105018,0,0
+89634.2594662,43144.8478959,0,0
+89635.3603067,43143.1941073,0,0
+89636.6433604,43141.7265211,0,0
+89638.2357234,43140.3882668,0,0
+89639.9531918,43139.3782332,0,0
+89641.8662542,43139.0736935,0,0
+89643.7647485,43139.2935718,0,0
+89645.0595383,43140.5308895,0,0
+89645.7624628,43142.4579623,0,0
+89646.0081315,43144.4993487,0,0
+89645.9771746,43146.4556331,0,0
+89645.7198198,43148.4673203,0,0
+89645.2624841,43150.4106217,0,0
+89644.6154754,43152.3177971,0,0
+89643.8085286,43154.0890375,0,0
+89642.7958461,43155.8110566,0,0
+89641.6602423,43157.4203093,0,0
+89640.4166734,43158.9702951,0,0
+89639.0953128,43160.4426856,0,0
+89637.7040156,43161.8450013,0,0
+89636.2268079,43163.2146307,0,0
+89634.7271196,43164.5146481,0,0
+89633.1715659,43165.7909099,0,0
+89631.6079678,43167.0152327,0,0
+89629.9916742,43168.2359144,0,0
+89628.422,43169.4292028,0,0
+89626.9360487,43170.681392,0,0
+89625.6326773,43172.1100207,0,0
+89624.5239839,43173.7090058,0,0
+89623.524659,43175.5548381,0,0
+89622.7633999,43177.5643722,0,0
+89622.3711667,43179.4617198,0,0
+89622.5814588,43181.2514461,0,0
+89623.4299206,43183.0977527,0,0
+89624.8156263,43184.5193965,0,0
+89626.4797642,43185.5093081,0,0
+89628.484469,43185.940562,0,0
+89630.4632873,43185.6828705,0,0
+89632.2196039,43185.5010472,0,0
+89634.2063309,43184.9716756,0,0
+89636.1984292,43184.3283004,0,0
+89638.3077263,43183.8316224,0,0
+89640.0901843,43183.4529033,0,0
+89641.9742669,43183.1289157,0,0
+89643.9821159,43183.1891656,0,0
+89645.9626256,43183.4864691,0,0
+89647.8713634,43183.922052,0,0
+89649.8112317,43184.4281576,0,0
+89651.7625555,43184.9147432,0,0
+89653.7272335,43185.2910258,0,0
+89655.7477277,43185.4683624,0,0
+89657.7245184,43185.3317496,0,0
+89659.6004571,43184.6202134,0,0
+89660.901783,43183.2703759,0,0
+89661.6170827,43181.4327615,0,0
+89661.101341,43179.8365554,0,0
+89659.5350704,43178.3332268,0,0
+89657.8222414,43177.3093738,0,0
+89656.0175449,43176.4547337,0,0
+89654.2905771,43175.6155904,0,0
+89652.5425231,43174.5945501,0,0
+89650.9640144,43173.4770669,0,0
+89649.3862254,43172.0291476,0,0
+89648.1141963,43170.438947,0,0
+89647.7584965,43168.6940736,0,0
+89648.0154508,43166.8524383,0,0
+89648.8633571,43165.0200887,0,0
+89650.16556,43163.4816081,0,0
+89651.8365398,43162.3515623,0,0
+89653.7091456,43161.6593857,0,0
+89655.6806727,43161.4125898,0,0
+89657.7000924,43161.6497986,0,0
+89659.5198962,43162.3911177,0,0
+89661.0152539,43163.1402004,0,0
+89662.7054761,43164.509828,0,0
+89664.162321,43165.9853292,0,0
+89665.908329,43167.3717298,0,0
+89667.5143238,43168.0278643,0,0
+89669.4231683,43168.4217884,0,0
+89671.3865022,43168.3912346,0,0
+89673.2617433,43167.8559389,0,0
+89675.071175,43166.8898285,0,0
+89676.5590524,43165.5715231,0,0
+89677.7102492,43163.9626915,0,0
+89678.4725422,43162.1341784,0,0
+89678.8416685,43160.1773652,0,0
+89678.7961682,43158.1527543,0,0
+89678.3324913,43156.2031372,0,0
+89677.4645742,43154.4739372,0,0
+89676.1235041,43153.0436769,0,0
+89674.4824288,43151.9531868,0,0
+89672.6853623,43151.0574827,0,0
+89670.9878927,43150.1588722,0,0
+89669.2487641,43149.0960882,0,0
+89667.5256427,43148.1800496,0,0
+89665.6861127,43147.2185175,0,0
+89663.9059965,43146.1971357,0,0
+89662.2370499,43145.1316272,0,0
+89660.6901829,43144.0278443,0,0
+89659.2499356,43142.7512552,0,0
+89658.0221413,43141.1336978,0,0
+89657.1882676,43139.3698501,0,0
+89656.5742286,43137.4356928,0,0
+89656.1250119,43135.4925293,0,0
+89655.7085679,43133.5002162,0,0
+89655.257571,43131.5741382,0,0
+89654.6900228,43129.6887965,0,0
+89653.9517859,43127.8275812,0,0
+89652.8700679,43126.171316,0,0
+89651.5069349,43124.7916923,0,0
+89649.8298991,43123.7864081,0,0
+89647.9146208,43123.2334074,0,0
+89645.900589,43123.1297578,0,0
+89643.9318194,43123.4182197,0,0
+89642.0265807,43124.019508,0,0
+89640.2035701,43124.8280252,0,0
+89638.4528357,43125.7268284,0,0
+89636.7216135,43126.6209795,0,0
+89634.9319301,43127.5296529,0,0
+89633.2159501,43128.4600235,0,0
+89631.4874813,43129.5062187,0,0
+89629.8229766,43130.626731,0,0
+89628.1900881,43131.8323441,0,0
+89626.671489,43133.0803448,0,0
+89625.2259497,43134.4565148,0,0
+89623.93602,43135.9348222,0,0
+89622.7554649,43137.5276128,0,0
+89621.6509513,43139.1771369,0,0
+89620.5839073,43140.8507265,0,0
+89619.5142317,43142.5404459,0,0
+89618.4393963,43144.2149781,0,0
+89617.3634361,43145.8979473,0,0
+89616.3422622,43147.592973,0,0
+89615.425912,43149.3693474,0,0
+89614.7257215,43151.1923925,0,0
+89614.2712031,43153.1487373,0,0

aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_mincurv_1.5m.csv

@@ -0,0 +1,159 @@
+x,y,z,yaw
+89614.230018,43153.1219333,0,0
+89613.9620072,43155.0590934,0,0
+89613.8888178,43157.0874386,0,0
+89614.0274533,43159.0766627,0,0
+89614.4853701,43161.1567451,0,0
+89615.3926235,43162.882723,0,0
+89616.8259125,43163.8841552,0,0
+89618.7164025,43164.3847416,0,0
+89620.7271951,43164.1479877,0,0
+89622.646509,43163.3827658,0,0
+89624.3529257,43162.3413867,0,0
+89625.8801337,43161.0780172,0,0
+89627.2522823,43159.6373167,0,0
+89628.4621114,43158.0455619,0,0
+89629.479867,43156.3504671,0,0
+89630.3240271,43154.5517684,0,0
+89631.0210884,43152.6957393,0,0
+89631.6466558,43150.7980876,0,0
+89632.2687561,43148.9058171,0,0
+89632.9571615,43147.0316523,0,0
+89633.7758758,43145.2261007,0,0
+89634.7920408,43143.5183512,0,0
+89636.0022877,43141.9853767,0,0
+89637.4953485,43140.6016047,0,0
+89639.2284777,43139.4542522,0,0
+89641.1578315,43138.8628265,0,0
+89642.9878724,43138.7559844,0,0
+89644.7019936,43139.6407985,0,0
+89645.6367779,43141.3738255,0,0
+89646.0382691,43143.2978998,0,0
+89646.0947836,43145.3264454,0,0
+89645.9021903,43147.322266,0,0
+89645.4863099,43149.296434,0,0
+89644.887412,43151.1854541,0,0
+89644.0979399,43153.0422536,0,0
+89643.1562259,43154.7744132,0,0
+89642.0767306,43156.4255799,0,0
+89640.9060521,43158.0307139,0,0
+89639.6733302,43159.5765192,0,0
+89638.3600015,43161.0824464,0,0
+89636.996851,43162.5154783,0,0
+89635.5625639,43163.912139,0,0
+89634.0844064,43165.2577618,0,0
+89632.5534359,43166.5675322,0,0
+89630.9998544,43167.8202216,0,0
+89629.4285984,43169.0301436,0,0
+89627.8361032,43170.260222,0,0
+89626.4107275,43171.4867511,0,0
+89625.0890585,43172.9719649,0,0
+89623.9588589,43174.6177875,0,0
+89622.9854349,43176.5591211,0,0
+89622.3645835,43178.3496398,0,0
+89622.1784689,43180.2663169,0,0
+89622.6177106,43182.2066783,0,0
+89623.7204359,43183.8791678,0,0
+89625.2404337,43185.1125697,0,0
+89627.0512198,43185.94922,0,0
+89629.0384012,43186.1978709,0,0
+89630.9700317,43185.8289,0,0
+89632.9161285,43185.51846,0,0
+89634.7642764,43184.89876,0,0
+89636.7373767,43184.2094395,0,0
+89638.7360525,43183.6811424,0,0
+89640.5205314,43183.1195091,0,0
+89642.4420746,43182.8260512,0,0
+89644.4382753,43182.945727,0,0
+89646.3934313,43183.3238792,0,0
+89648.3179341,43183.8537604,0,0
+89650.2195652,43184.4403969,0,0
+89652.1543343,43185.0004604,0,0
+89654.1058935,43185.4280822,0,0
+89656.1495222,43185.6236624,0,0
+89658.0725896,43185.4621821,0,0
+89659.8890579,43184.6960194,0,0
+89661.234018,43183.2703552,0,0
+89661.8646684,43181.401584,0,0
+89661.3145309,43179.7872266,0,0
+89659.7234678,43178.3223519,0,0
+89657.9736836,43177.3507571,0,0
+89656.1212338,43176.5933145,0,0
+89654.3386822,43175.8687334,0,0
+89652.5408196,43174.9388795,0,0
+89650.9432072,43173.8810669,0,0
+89649.3441194,43172.4644155,0,0
+89648.0329728,43170.8720742,0,0
+89647.5048395,43169.0542922,0,0
+89647.6005434,43167.282978,0,0
+89648.3792356,43165.394914,0,0
+89649.5218683,43163.7277282,0,0
+89651.0679381,43162.4761758,0,0
+89652.8845139,43161.6094658,0,0
+89654.8125288,43161.2010246,0,0
+89656.8050596,43161.2132184,0,0
+89658.8421791,43161.7916661,0,0
+89660.4342685,43162.6170974,0,0
+89662.1180421,43163.6446027,0,0
+89663.5427469,43165.0922012,0,0
+89664.9362233,43166.5594328,0,0
+89666.6501839,43167.7821324,0,0
+89668.2764273,43168.4539059,0,0
+89670.2965984,43168.7166566,0,0
+89672.2350222,43168.3871529,0,0
+89674.1194722,43167.6715985,0,0
+89675.803243,43166.568045,0,0
+89677.1770823,43165.1364035,0,0
+89678.1942817,43163.4372025,0,0
+89678.8242872,43161.554815,0,0
+89679.0666859,43159.5647249,0,0
+89678.8996113,43157.5622925,0,0
+89678.3092236,43155.61139,0,0
+89677.3203961,43153.9253042,0,0
+89675.9608799,43152.5884661,0,0
+89674.2902219,43151.5099615,0,0
+89672.4772614,43150.6404083,0,0
+89670.773337,43149.7995331,0,0
+89668.9591128,43148.8285682,0,0
+89667.1780394,43147.9768295,0,0
+89665.2907394,43147.0645407,0,0
+89663.5404585,43146.1364628,0,0
+89661.8139627,43145.1123313,0,0
+89660.2548644,43144.0109577,0,0
+89658.8660753,43142.6760626,0,0
+89657.724156,43141.0016557,0,0
+89656.961688,43139.1750567,0,0
+89656.4386897,43137.222336,0,0
+89656.0710599,43135.2619766,0,0
+89655.7253506,43133.2564686,0,0
+89655.3262449,43131.3060418,0,0
+89654.8097205,43129.4297187,0,0
+89654.0885719,43127.5577907,0,0
+89652.9833785,43125.9073271,0,0
+89651.5900162,43124.5397499,0,0
+89649.9006057,43123.5519586,0,0
+89647.9815452,43123.0062233,0,0
+89645.9662869,43122.9069273,0,0
+89643.9994797,43123.2009825,0,0
+89642.0903564,43123.8145813,0,0
+89640.2739548,43124.6418047,0,0
+89638.5354964,43125.5698848,0,0
+89636.8164102,43126.50449,0,0
+89635.036289,43127.4480197,0,0
+89633.3457703,43128.3917248,0,0
+89631.6219584,43129.4588332,0,0
+89629.9694711,43130.5967794,0,0
+89628.3491905,43131.8218518,0,0
+89626.8458613,43133.0884502,0,0
+89625.4133309,43134.4881024,0,0
+89624.1411176,43135.9836206,0,0
+89622.9773545,43137.5933936,0,0
+89621.8901113,43139.2549875,0,0
+89620.8353039,43140.9406686,0,0
+89619.7717976,43142.635496,0,0
+89618.6895488,43144.3069084,0,0
+89617.5878726,43145.9749948,0,0
+89616.5205248,43147.6447178,0,0
+89615.537728,43149.3910593,0,0
+89614.7600395,43151.1866812,0,0
+89614.230018,43153.1219333,0,0