From 2fbf6e3c1e90b77aa27a36910ac20aabdd1b2c2e Mon Sep 17 00:00:00 2001 From: Shunya Hara Date: Tue, 22 Oct 2024 22:11:53 +0900 Subject: [PATCH] add csv to traj (#60) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * add csv to traj * kdtree * path ok * コメント追記 * オフライン最適化したcsvの読み取りに対応 * add spell * add spell --- .cspell.json | 4 +- .../launch/components/planning.launch.xml | 24 +- .../map/csv/traj_mincurv_1.0m.csv | 157 ++++++++ .../map/csv/traj_mincurv_1.5m.csv | 159 ++++++++ .../map/csv/traj_mincurv_2.0m.csv | 161 ++++++++ .../map/csv/traj_mincurv_2.5m.csv | 164 ++++++++ .../map/csv/traj_mincurv_3.0m.csv | 166 +++++++++ .../booars_launch/map/csv/traj_race_cl.csv | 350 ++++++++++++++++++ .../path_to_trajectory/CMakeLists.txt | 3 + .../path_to_trajectory/csv_to_trajectory.hpp | 61 +++ .../path_to_trajectory/package.xml | 1 + .../src/csv_to_trajectory.cpp | 149 ++++++++ 12 files changed, 1387 insertions(+), 12 deletions(-) create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_mincurv_1.0m.csv create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_mincurv_1.5m.csv create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_mincurv_2.0m.csv create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_mincurv_2.5m.csv create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_mincurv_3.0m.csv create mode 100644 aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_race_cl.csv create mode 100644 aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/include/path_to_trajectory/csv_to_trajectory.hpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/src/csv_to_trajectory.cpp diff --git a/.cspell.json b/.cspell.json index 2e2fad7a..ba876664 100644 --- a/.cspell.json +++ b/.cspell.json @@ -154,6 +154,8 @@ "xyzrpy", "zcvf", "zenoh", - "zenohd" + "zenohd", + "coef", + "radpm" ] } diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml index a1f3cb0c..a34d6d35 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml @@ -94,18 +94,20 @@ - - - + + + + + + + + + + + + - - - - - - - - + diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_mincurv_1.0m.csv b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_mincurv_1.0m.csv new file mode 100644 index 00000000..d1d5f58f --- /dev/null +++ b/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 diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_mincurv_1.5m.csv b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_mincurv_1.5m.csv new file mode 100644 index 00000000..f12c0a84 --- /dev/null +++ b/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 diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_mincurv_2.0m.csv b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_mincurv_2.0m.csv new file mode 100644 index 00000000..300c4371 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_mincurv_2.0m.csv @@ -0,0 +1,161 @@ +x,y,z,yaw +89614.1888304,43153.0951277,0,0 +89613.8604961,43155.0251471,0,0 +89613.7357355,43157.0517504,0,0 +89613.8312773,43159.0455738,0,0 +89614.2516341,43161.1343465,0,0 +89615.1139846,43162.8895688,0,0 +89616.5495887,43164.0155522,0,0 +89618.3815366,43164.5597184,0,0 +89620.418949,43164.4674815,0,0 +89622.3244026,43163.7991169,0,0 +89624.0957178,43162.7990514,0,0 +89625.6479643,43161.572704,0,0 +89627.0467624,43160.1447138,0,0 +89628.270265,43158.5652338,0,0 +89629.2981005,43156.8726696,0,0 +89630.1413756,43155.0695613,0,0 +89630.8214183,43153.2025867,0,0 +89631.4082196,43151.2893354,0,0 +89631.9750423,43149.3751204,0,0 +89632.5923813,43147.4737935,0,0 +89633.3268758,43145.625833,0,0 +89634.2538758,43143.8646386,0,0 +89635.3906704,43142.2608096,0,0 +89636.7832019,43140.8302542,0,0 +89638.477711,43139.5878449,0,0 +89640.305543,43138.7687445,0,0 +89642.2154722,43138.5549285,0,0 +89644.0914636,43138.9132112,0,0 +89645.3662624,43140.2365637,0,0 +89646.0531871,43142.1786426,0,0 +89646.2450862,43144.1766077,0,0 +89646.1385891,43146.1626973,0,0 +89645.789889,43148.1590595,0,0 +89645.2282295,43150.0840479,0,0 +89644.4774572,43151.9479681,0,0 +89643.5728556,43153.6944368,0,0 +89642.5028311,43155.383359,0,0 +89641.3514747,43156.9920421,0,0 +89640.1374589,43158.5758447,0,0 +89638.8828738,43160.1085556,0,0 +89637.5714694,43161.5994308,0,0 +89636.1876984,43163.0657185,0,0 +89634.777626,43164.4681257,0,0 +89633.3101396,43165.8471566,0,0 +89631.823372,43167.172597,0,0 +89630.2802634,43168.4848834,0,0 +89628.7604064,43169.7549558,0,0 +89627.2831442,43171.0403964,0,0 +89625.8910867,43172.4050465,0,0 +89624.5946027,43173.8761969,0,0 +89623.3877963,43175.5893407,0,0 +89622.454276,43177.4665048,0,0 +89621.9300995,43179.3478018,0,0 +89622.0551944,43181.1881974,0,0 +89622.8301386,43183.0632467,0,0 +89624.1508904,43184.5692173,0,0 +89625.7451277,43185.6621788,0,0 +89627.6696358,43186.3290869,0,0 +89629.6967374,43186.3837556,0,0 +89631.4860656,43185.983524,0,0 +89633.4711013,43185.5056281,0,0 +89635.3650253,43184.774602,0,0 +89637.3819815,43184.0236076,0,0 +89639.1472978,43183.4548248,0,0 +89640.9996314,43182.7376338,0,0 +89642.9569433,43182.5320958,0,0 +89644.9334044,43182.7328654,0,0 +89646.8617813,43183.201608,0,0 +89648.7889926,43183.8317625,0,0 +89650.6501943,43184.4989333,0,0 +89652.5645188,43185.1281735,0,0 +89654.5047138,43185.597445,0,0 +89656.5611552,43185.7952186,0,0 +89658.5553979,43185.5541575,0,0 +89660.2458562,43184.7232997,0,0 +89661.5684813,43183.2461104,0,0 +89662.11321,43181.3526966,0,0 +89661.5458183,43179.7059734,0,0 +89659.9549214,43178.2585652,0,0 +89658.1867858,43177.309776,0,0 +89656.3043811,43176.6147966,0,0 +89654.4770191,43175.9826569,0,0 +89652.6425774,43175.1412364,0,0 +89651.0141768,43174.1396118,0,0 +89649.3815467,43172.7759579,0,0 +89648.0054868,43171.2090394,0,0 +89647.2944886,43169.3930916,0,0 +89647.2362186,43167.5798982,0,0 +89647.9177838,43165.6843397,0,0 +89648.906547,43163.984636,0,0 +89650.3471401,43162.5920429,0,0 +89652.0973991,43161.5827482,0,0 +89653.996368,43161.0247266,0,0 +89655.9880784,43160.8789068,0,0 +89658.0430069,43161.229751,0,0 +89659.8149402,43161.9978674,0,0 +89661.3244332,43162.8149524,0,0 +89663.0193136,43164.1834116,0,0 +89664.3663519,43165.7521368,0,0 +89665.9119864,43167.2909503,0,0 +89667.3162511,43168.3314353,0,0 +89669.1731668,43168.893563,0,0 +89671.1537876,43168.841531,0,0 +89673.0548335,43168.3159271,0,0 +89674.9199506,43167.4357848,0,0 +89676.4895479,43166.207282,0,0 +89677.7416703,43164.6736748,0,0 +89678.6300184,43162.8893102,0,0 +89679.1345115,43160.9659585,0,0 +89679.2513009,43158.9458088,0,0 +89678.9764085,43156.9948512,0,0 +89678.2765424,43155.0663818,0,0 +89677.1926671,43153.4303075,0,0 +89675.7919729,43152.1176983,0,0 +89674.0911059,43151.0627304,0,0 +89672.2594116,43150.2368306,0,0 +89670.5330182,43149.4599744,0,0 +89668.675276,43148.5964832,0,0 +89666.8241779,43147.8023277,0,0 +89664.9060101,43146.9545997,0,0 +89663.1730219,43146.1144271,0,0 +89661.3991713,43145.1264084,0,0 +89659.8345132,43143.9921671,0,0 +89658.4984161,43142.5920942,0,0 +89657.4430276,43140.8675205,0,0 +89656.7566057,43138.9875437,0,0 +89656.3242844,43137.0177825,0,0 +89656.037316,43135.0375922,0,0 +89655.7631365,43133.0279777,0,0 +89655.4173891,43131.0583212,0,0 +89654.9391269,43129.1565366,0,0 +89654.235047,43127.302228,0,0 +89653.1064801,43125.6566362,0,0 +89651.6855457,43124.298988,0,0 +89649.9857055,43123.3234544,0,0 +89648.0620278,43122.7806389,0,0 +89646.044473,43122.6823887,0,0 +89644.0747315,43122.9802598,0,0 +89642.1631321,43123.6032356,0,0 +89640.3515902,43124.4471719,0,0 +89638.6235533,43125.4029107,0,0 +89636.9149691,43126.3785631,0,0 +89635.1615379,43127.3494624,0,0 +89633.4787779,43128.3171686,0,0 +89631.7594637,43129.4058462,0,0 +89630.1186539,43130.5615053,0,0 +89628.5101955,43131.8066425,0,0 +89627.0211824,43133.0924366,0,0 +89625.6010279,43134.5161353,0,0 +89624.3456694,43136.0297732,0,0 +89623.1982468,43137.6573852,0,0 +89622.1277907,43139.3323056,0,0 +89621.0851761,43141.0312464,0,0 +89620.0279248,43142.7326816,0,0 +89618.9384325,43144.4024619,0,0 +89617.8114348,43146.0567803,0,0 +89616.6980095,43147.702665,0,0 +89615.650553,43149.4170157,0,0 +89614.7955339,43151.1831775,0,0 +89614.1888304,43153.0951277,0,0 diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_mincurv_2.5m.csv b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_mincurv_2.5m.csv new file mode 100644 index 00000000..cbf123b4 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_mincurv_2.5m.csv @@ -0,0 +1,164 @@ +x,y,z,yaw +89614.2109582,43153.1095289,0,0 +89613.8163734,43155.0240282,0,0 +89613.6298823,43157.0377329,0,0 +89613.6706549,43159.0289712,0,0 +89614.0401039,43161.1186796,0,0 +89614.8486098,43162.89755,0,0 +89616.2579846,43164.1201307,0,0 +89618.0716828,43164.7292801,0,0 +89620.110197,43164.7628651,0,0 +89621.9772891,43164.2080228,0,0 +89623.8017827,43163.2690191,0,0 +89625.3890772,43162.0893297,0,0 +89626.810851,43160.693022,0,0 +89628.0504928,43159.1400961,0,0 +89629.1012883,43157.4500112,0,0 +89629.9477505,43155.6621471,0,0 +89630.6202431,43153.7918892,0,0 +89631.1719944,43151.8826442,0,0 +89631.686295,43149.9513883,0,0 +89632.2291783,43148.036485,0,0 +89632.8736856,43146.1466634,0,0 +89633.690176,43144.3401698,0,0 +89634.7346675,43142.6539496,0,0 +89635.9994524,43141.172946,0,0 +89637.5962234,43139.8476986,0,0 +89639.3183931,43138.8572704,0,0 +89641.2893263,43138.421849,0,0 +89643.1715705,43138.2810286,0,0 +89644.8833794,43139.1756198,0,0 +89645.8549311,43140.8311763,0,0 +89646.3037355,43142.8094516,0,0 +89646.3242609,43144.8469476,0,0 +89646.0722445,43146.8081918,0,0 +89645.5804394,43148.7663479,0,0 +89644.8933406,43150.6315751,0,0 +89644.0218788,43152.4478941,0,0 +89643.0239128,43154.1440156,0,0 +89641.9136686,43155.7880539,0,0 +89640.7636777,43157.4037231,0,0 +89639.591271,43159.0033498,0,0 +89638.3784332,43160.5881195,0,0 +89637.1334919,43162.1180288,0,0 +89635.8182586,43163.6301066,0,0 +89634.4600695,43165.0907897,0,0 +89633.0429676,43166.5165293,0,0 +89631.5962474,43167.8765776,0,0 +89630.0923389,43169.2016212,0,0 +89628.5657747,43170.4932796,0,0 +89627.0940494,43171.7337085,0,0 +89625.5936749,43173.0642898,0,0 +89624.2181509,43174.4391844,0,0 +89622.9507238,43176.1342529,0,0 +89622.0619824,43177.8839389,0,0 +89621.6755867,43179.7980073,0,0 +89621.940658,43181.7189118,0,0 +89622.849873,43183.5058518,0,0 +89624.2362618,43184.9332776,0,0 +89625.8595637,43185.9879252,0,0 +89627.8012257,43186.6028831,0,0 +89629.8229403,43186.617224,0,0 +89631.610808,43186.1883472,0,0 +89633.5734232,43185.6319469,0,0 +89635.4379274,43184.8447504,0,0 +89637.4039049,43184.0200066,0,0 +89639.1296384,43183.3341801,0,0 +89640.9300523,43182.5063603,0,0 +89642.8745652,43182.2372506,0,0 +89644.8563256,43182.4265219,0,0 +89646.7767765,43182.9242659,0,0 +89648.6627158,43183.6073007,0,0 +89650.4978824,43184.3573823,0,0 +89652.3713744,43185.0829188,0,0 +89654.281092,43185.6559651,0,0 +89656.3107154,43185.9588042,0,0 +89658.2873334,43185.8375216,0,0 +89660.0817241,43185.1315349,0,0 +89661.4816086,43183.78837,0,0 +89662.3326214,43181.9136871,0,0 +89662.1119862,43180.074781,0,0 +89660.8246497,43178.6437507,0,0 +89659.0814004,43177.5185166,0,0 +89657.2256228,43176.7748306,0,0 +89655.334593,43176.2121034,0,0 +89653.5033759,43175.563902,0,0 +89651.7500783,43174.6944985,0,0 +89650.0785254,43173.546125,0,0 +89648.5133682,43172.0682948,0,0 +89647.3817099,43170.5107457,0,0 +89647.0084529,43168.669576,0,0 +89647.035444,43166.7934402,0,0 +89647.7977884,43164.8756289,0,0 +89648.8843047,43163.1772881,0,0 +89650.3532612,43161.8401466,0,0 +89652.1010109,43160.8675066,0,0 +89654.0021026,43160.3356956,0,0 +89655.9830462,43160.2292362,0,0 +89658.0257027,43160.6453129,0,0 +89659.7580838,43161.4867957,0,0 +89661.1830472,43162.5567365,0,0 +89662.7872969,43163.9482816,0,0 +89664.0450091,43165.5497878,0,0 +89665.4773843,43167.1502373,0,0 +89666.915949,43168.2813175,0,0 +89668.5890062,43169.0575121,0,0 +89670.6254834,43169.1537738,0,0 +89672.5471602,43168.7141297,0,0 +89674.4179008,43167.9686377,0,0 +89676.1078265,43166.8721176,0,0 +89677.4904592,43165.4585194,0,0 +89678.5297408,43163.7740586,0,0 +89679.1949819,43161.9022565,0,0 +89679.4867379,43159.9240861,0,0 +89679.3844445,43157.908141,0,0 +89678.8896479,43155.9457151,0,0 +89678.040546,43154.1973653,0,0 +89676.8153463,43152.6801698,0,0 +89675.2891529,43151.4188934,0,0 +89673.5333161,43150.4577494,0,0 +89671.6853787,43149.718718,0,0 +89669.9274262,43149.0019164,0,0 +89668.0467313,43148.2590512,0,0 +89666.1356372,43147.5281828,0,0 +89664.2197987,43146.7551606,0,0 +89662.4312596,43145.9461355,0,0 +89660.7357357,43144.9957412,0,0 +89659.2025382,43143.7472635,0,0 +89657.9730942,43142.2479198,0,0 +89657.0708923,43140.4643718,0,0 +89656.5052895,43138.5169388,0,0 +89656.1951564,43136.5454929,0,0 +89655.9959463,43134.5440752,0,0 +89655.7897942,43132.5633197,0,0 +89655.4809211,43130.5814997,0,0 +89655.0165727,43128.6627751,0,0 +89654.2679668,43126.8370439,0,0 +89653.0916495,43125.2534409,0,0 +89651.6087355,43123.9367072,0,0 +89649.8789487,43123.0166429,0,0 +89647.9539724,43122.5245255,0,0 +89645.9408798,43122.4656911,0,0 +89643.9815202,43122.7953596,0,0 +89642.0828176,43123.4465093,0,0 +89640.2866421,43124.3171381,0,0 +89638.5781433,43125.3014591,0,0 +89636.8922141,43126.3105115,0,0 +89635.1658465,43127.312322,0,0 +89633.5001702,43128.3076937,0,0 +89631.8011646,43129.4193736,0,0 +89630.1806684,43130.5965879,0,0 +89628.5955371,43131.8627723,0,0 +89627.1290422,43133.1718895,0,0 +89625.7380812,43134.6158445,0,0 +89624.508426,43136.1477507,0,0 +89623.3836029,43137.787274,0,0 +89622.3302984,43139.4693053,0,0 +89621.2992619,43141.1714126,0,0 +89620.2481509,43142.8709119,0,0 +89619.1571881,43144.5347267,0,0 +89618.0205356,43146.1769211,0,0 +89616.8857258,43147.8051339,0,0 +89615.8013228,43149.4929891,0,0 +89614.8886374,43151.2296293,0,0 +89614.2109582,43153.1095289,0,0 diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_mincurv_3.0m.csv b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_mincurv_3.0m.csv new file mode 100644 index 00000000..e9a8fae7 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_mincurv_3.0m.csv @@ -0,0 +1,166 @@ +x,y,z,yaw +89614.3421119,43153.1948861,0,0 +89613.8679098,43155.1052204,0,0 +89613.6033303,43157.1125259,0,0 +89613.5765991,43159.1129193,0,0 +89613.8921413,43161.2160418,0,0 +89614.6896491,43163.0216902,0,0 +89616.0696914,43164.2881399,0,0 +89617.9181683,43164.9300709,0,0 +89619.8943933,43165.0218647,0,0 +89621.816333,43164.5275868,0,0 +89623.6722883,43163.6266237,0,0 +89625.2949807,43162.4635661,0,0 +89626.7302617,43161.0767554,0,0 +89627.9763778,43159.5250299,0,0 +89629.0338521,43157.8176647,0,0 +89629.870432,43156.0186978,0,0 +89630.5230701,43154.1263118,0,0 +89631.0377488,43152.2048534,0,0 +89631.505085,43150.24923,0,0 +89631.9915603,43148.3097739,0,0 +89632.5716304,43146.3858536,0,0 +89633.3166754,43144.5408915,0,0 +89634.2979544,43142.8041653,0,0 +89635.5074609,43141.2739121,0,0 +89637.052591,43139.906052,0,0 +89638.788467,43138.8314752,0,0 +89640.7531847,43138.2488036,0,0 +89642.6275411,43137.9982877,0,0 +89644.4883107,43138.5758771,0,0 +89645.7360762,43139.993797,0,0 +89646.3833001,43141.9488883,0,0 +89646.5104771,43143.9174658,0,0 +89646.3052764,43145.9182158,0,0 +89645.8422018,43147.8879121,0,0 +89645.1572117,43149.7778035,0,0 +89644.2903666,43151.589826,0,0 +89643.28832,43153.3045741,0,0 +89642.1772869,43154.9697418,0,0 +89641.0429812,43156.6009358,0,0 +89639.9119516,43158.250455,0,0 +89638.7864391,43159.8899276,0,0 +89637.6315691,43161.5137187,0,0 +89636.4161154,43163.1223153,0,0 +89635.1598599,43164.6688119,0,0 +89633.8292397,43166.182325,0,0 +89632.4467666,43167.6244364,0,0 +89630.9782082,43169.0181463,0,0 +89629.4739659,43170.3247913,0,0 +89627.934216,43171.5649294,0,0 +89626.37813,43172.7594933,0,0 +89624.8311611,43173.9879183,0,0 +89623.3604143,43175.4402919,0,0 +89622.1808435,43177.1492622,0,0 +89621.4877182,43178.9983575,0,0 +89621.512234,43180.8764603,0,0 +89622.1461221,43182.7916736,0,0 +89623.336987,43184.4166875,0,0 +89624.8693133,43185.6556809,0,0 +89626.6533328,43186.5651043,0,0 +89628.6395561,43186.9378701,0,0 +89630.6397659,43186.6910979,0,0 +89632.3752725,43186.2005146,0,0 +89634.2978656,43185.4054373,0,0 +89636.1593665,43184.4811638,0,0 +89638.0642541,43183.5792728,0,0 +89639.7338589,43182.752092,0,0 +89641.5939132,43182.0939498,0,0 +89643.616022,43182.0328762,0,0 +89645.5648441,43182.3858444,0,0 +89647.4445609,43183.016449,0,0 +89649.3047315,43183.8120903,0,0 +89651.1188637,43184.6416041,0,0 +89652.983321,43185.4027523,0,0 +89654.90261,43185.9592578,0,0 +89656.9468148,43186.1845596,0,0 +89658.9701379,43185.8885637,0,0 +89660.6240537,43185.0466975,0,0 +89661.9662521,43183.5622101,0,0 +89662.5996995,43181.6271797,0,0 +89662.2352506,43179.818686,0,0 +89660.8673593,43178.3858048,0,0 +89659.1001932,43177.3119633,0,0 +89657.2149275,43176.6193767,0,0 +89655.2990388,43176.1203212,0,0 +89653.4309567,43175.5296539,0,0 +89651.6552671,43174.7085176,0,0 +89649.9430405,43173.5759562,0,0 +89648.3622925,43172.1258507,0,0 +89647.1990883,43170.5394451,0,0 +89646.7478404,43168.7350413,0,0 +89646.6521077,43166.8363848,0,0 +89647.3588958,43164.8807755,0,0 +89648.3652625,43163.1591084,0,0 +89649.7602697,43161.7061368,0,0 +89651.4384169,43160.6144508,0,0 +89653.3078991,43159.9431208,0,0 +89655.2785523,43159.7187256,0,0 +89657.2943174,43159.962808,0,0 +89659.2311086,43160.7182742,0,0 +89660.7224531,43161.8912072,0,0 +89662.0704977,43163.1381952,0,0 +89663.3504335,43164.8178571,0,0 +89664.5282248,43166.4813303,0,0 +89666.0881676,43167.9193434,0,0 +89667.5672722,43169.0317219,0,0 +89669.504074,43169.4210066,0,0 +89671.4660285,43169.2503623,0,0 +89673.3638282,43168.6720857,0,0 +89675.224044,43167.7917297,0,0 +89676.8177496,43166.5842759,0,0 +89678.097116,43165.0756086,0,0 +89679.0286539,43163.2992456,0,0 +89679.5753604,43161.3875385,0,0 +89679.7472137,43159.3702119,0,0 +89679.533481,43157.3845961,0,0 +89678.9260102,43155.4226277,0,0 +89677.9786594,43153.6974423,0,0 +89676.725492,43152.234721,0,0 +89675.1648591,43150.9953922,0,0 +89673.3689505,43150.0619596,0,0 +89671.4936125,43149.3817616,0,0 +89669.6691291,43148.7433882,0,0 +89667.7630693,43148.0971358,0,0 +89665.8071915,43147.4388276,0,0 +89663.885334,43146.7525311,0,0 +89662.0434189,43145.9879078,0,0 +89660.3408521,43145.0348332,0,0 +89658.8384805,43143.7270261,0,0 +89657.6628428,43142.1677424,0,0 +89656.8388995,43140.344906,0,0 +89656.356088,43138.3660058,0,0 +89656.1330907,43136.3743093,0,0 +89656.0138976,43134.3545887,0,0 +89655.8763082,43132.3736989,0,0 +89655.6194696,43130.3740099,0,0 +89655.1836282,43128.4313745,0,0 +89654.4195772,43126.5979065,0,0 +89653.2218221,43125.0225659,0,0 +89651.7218676,43123.7123568,0,0 +89649.9868998,43122.7809333,0,0 +89648.0573217,43122.2617658,0,0 +89646.0370282,43122.1698336,0,0 +89644.0621462,43122.469414,0,0 +89642.1460963,43123.0973764,0,0 +89640.3368604,43123.9569983,0,0 +89638.6219627,43124.9502378,0,0 +89636.9354701,43126.0005171,0,0 +89635.2504363,43127.0383943,0,0 +89633.5884902,43128.0854248,0,0 +89631.9012034,43129.2388787,0,0 +89630.3035877,43130.4513467,0,0 +89628.7409566,43131.7576383,0,0 +89627.3008139,43133.1043339,0,0 +89625.9331708,43134.5920124,0,0 +89624.7300849,43136.1565162,0,0 +89623.6254448,43137.8238142,0,0 +89622.5844618,43139.5227039,0,0 +89621.5534917,43141.2369758,0,0 +89620.4973273,43142.9432346,0,0 +89619.4019143,43144.6146083,0,0 +89618.2670558,43146.2685056,0,0 +89617.1347293,43147.9112705,0,0 +89616.0421712,43149.6038814,0,0 +89615.0897684,43151.3355203,0,0 +89614.3421119,43153.1948861,0,0 diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_race_cl.csv b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_race_cl.csv new file mode 100644 index 00000000..1d59fcc2 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/map/csv/traj_race_cl.csv @@ -0,0 +1,350 @@ +#s_m, x_m, y_m, psi_rad, kappa_radpm, vx_mps, ax_mps2 +0.0000000, 89656.3124028, 43129.1596191, 2.9710383, -0.0704782, 8.0400106, -0.8017657 +0.9998594, 89656.0988324, 43128.1506178, 2.8932293, -0.0806240, 7.9396766, -0.7916973 +1.9997188, 89655.8152814, 43127.2008284, 2.8078850, -0.0917243, 7.8393427, 0.1294625 +2.9995781, 89655.4547491, 43126.2990223, 2.7117458, -0.1103903, 7.8558374, 1.6561174 +3.9994375, 89654.9949272, 43125.4321397, 2.5933242, -0.1268743, 8.0638670, 1.6993998 +4.9992969, 89654.4118673, 43124.6018868, 2.4668299, -0.1184667, 8.2718965, 0.2328912 +5.9991563, 89653.7545795, 43123.8649030, 2.3648651, -0.0895602, 8.2999994, 0.0000028 +6.9990156, 89653.0150408, 43123.1741314, 2.2812087, -0.0784903, 8.2999997, 0.0000027 +7.9988750, 89652.2238597, 43122.5464303, 2.2010040, -0.0838306, 8.3000000, 0.0000006 +8.9987344, 89651.3612649, 43121.9795461, 2.0975822, -0.1176290, 8.3000001, 0.0000006 +9.9985938, 89650.4796899, 43121.5396170, 1.9637661, -0.1538389, 8.3000002, 0.0000004 +10.9984531, 89649.5624757, 43121.2416464, 1.8043798, -0.1621119, 8.3000002, 0.0000002 +11.9983125, 89648.5951491, 43121.0858332, 1.6628731, -0.1242461, 8.3000002, 0.0000002 +12.9981719, 89647.5733780, 43121.0489017, 1.5584392, -0.0806883, 8.3000003, 0.0000001 +13.9980313, 89646.6120496, 43121.0934732, 1.4944829, -0.0548923, 8.3000003, 0.0000001 +14.9978906, 89645.6226258, 43121.1931582, 1.4492285, -0.0370975, 8.3000003, 0.0000001 +15.9977500, 89644.5894279, 43121.3370462, 1.4179727, -0.0235035, 8.3000003, 0.0000000 +16.9976094, 89643.5992668, 43121.5026701, 1.3897752, -0.0355872, 8.3000003, 0.0000000 +17.9974688, 89642.6113649, 43121.7047225, 1.3454062, -0.0530324, 8.3000003, 0.0000000 +18.9973281, 89641.6434445, 43121.9574805, 1.2815122, -0.0759721, 8.3000003, 0.0000000 +19.9971875, 89640.6727901, 43122.2920052, 1.1949126, -0.0861396, 8.3000003, -0.0000000 +20.9970469, 89639.7452907, 43122.7059629, 1.1068948, -0.0874171, 8.3000003, -0.0000000 +21.9969063, 89638.8962696, 43123.1773188, 1.0205216, -0.0909017, 8.3000003, -0.0000000 +22.9967656, 89638.0710625, 43123.7369566, 0.9299548, -0.0868046, 8.3000003, -0.0000000 +23.9966250, 89637.2979445, 43124.3636825, 0.8526314, -0.0664526, 8.3000003, -0.0000000 +24.9964844, 89636.5740693, 43125.0331661, 0.8009705, -0.0379736, 8.3000003, -0.0000000 +25.9963438, 89635.8828896, 43125.7286323, 0.7636271, -0.0380729, 8.3000003, -0.0000000 +26.9962031, 89635.2036002, 43126.4657157, 0.7257057, -0.0374790, 8.3000003, -0.0000000 +27.9960625, 89634.5486441, 43127.2323225, 0.6887877, -0.0341683, 8.3000003, 0.0000000 +28.9959219, 89633.9330782, 43128.0042350, 0.6587905, -0.0268485, 8.3000003, 0.0000000 +29.9957813, 89633.3258889, 43128.8090550, 0.6349171, -0.0206945, 8.3000003, 0.0000000 +30.9956406, 89632.7354951, 43129.6267080, 0.6166343, -0.0160177, 8.3000003, 0.0000000 +31.9955000, 89632.1651332, 43130.4441432, 0.6023311, -0.0127490, 8.3000003, 0.0000000 +32.9953594, 89631.6014885, 43131.2743469, 0.5910427, -0.0097948, 8.3000003, 0.0000000 +33.9952188, 89631.0460076, 43132.1101335, 0.5825368, -0.0073784, 8.3000003, 0.0000000 +34.9950781, 89630.4987186, 43132.9470078, 0.5760939, -0.0055085, 8.3000003, 0.0000000 +35.9949375, 89629.9560984, 43133.7868053, 0.5715202, -0.0036404, 8.3000003, 0.0000000 +36.9947969, 89629.4165053, 43134.6285998, 0.5689190, -0.0012120, 8.3000003, 0.0000000 +37.9946563, 89628.8778050, 43135.4711331, 0.5693912, 0.0021574, 8.3000003, 0.0000000 +38.9945156, 89628.3374405, 43136.3122657, 0.5732342, 0.0055312, 8.3000003, 0.0000000 +39.9943750, 89627.7924339, 43137.1504105, 0.5803801, 0.0084955, 8.3000003, 0.0000000 +40.9942344, 89627.2402353, 43137.9841322, 0.5899745, 0.0106940, 8.3000003, 0.0000000 +41.9940938, 89626.6792862, 43138.8117055, 0.6017668, 0.0128962, 8.3000003, 0.0000000 +42.9939531, 89626.1078968, 43139.6317276, 0.6157118, 0.0148212, 8.3000003, 0.0000000 +43.9938125, 89625.5245618, 43140.4430835, 0.6311894, 0.0161501, 8.3000003, 0.0000000 +44.9936719, 89624.9278445, 43141.2454067, 0.6479869, 0.0174405, 8.3000003, 0.0000000 +45.9935313, 89624.3173131, 43142.0371122, 0.6659693, 0.0181556, 8.3000003, 0.0000000 +46.9933906, 89623.6934799, 43142.8165786, 0.6838314, 0.0176259, 8.3000003, 0.0000000 +47.9932500, 89623.0547413, 43143.5864707, 0.7011970, 0.0170919, 8.3000003, 0.0000000 +48.9931094, 89622.4024334, 43144.3459149, 0.7179380, 0.0157780, 8.3000003, 0.0000000 +49.9929688, 89621.7396557, 43145.0936421, 0.7319788, 0.0123406, 8.3000003, 0.0000000 +50.9928281, 89621.0668882, 43145.8340249, 0.7426345, 0.0089721, 8.3000003, 0.0000000 +51.9926875, 89620.3872746, 43146.5685824, 0.7499110, 0.0053080, 8.3000003, 0.0000000 +52.9925469, 89619.7039463, 43147.2995260, 0.7527510, 0.0003650, 8.3000003, 0.0000000 +53.9924063, 89619.0209375, 43148.0295944, 0.7506370, -0.0045989, 8.3000003, 0.0000000 +54.9922656, 89618.3418722, 43148.7621733, 0.7434995, -0.0102695, 8.3000003, 0.0000000 +55.9921250, 89617.6705887, 43149.5020719, 0.7285746, -0.0196001, 8.3000003, 0.0000000 +56.9919844, 89617.0136989, 43150.2552176, 0.7043705, -0.0288001, 8.3000003, 0.0000000 +57.9918438, 89616.3779845, 43151.0278812, 0.6710778, -0.0376815, 8.3000003, -0.0000000 +58.9917031, 89615.7743388, 43151.8205471, 0.6291523, -0.0464712, 8.3000003, -0.0000000 +59.9915625, 89615.2061138, 43152.6432349, 0.5782932, -0.0552246, 8.3000003, -0.0000001 +60.9914219, 89614.6823992, 43153.4990699, 0.5185880, -0.0636500, 8.3000003, -0.0000027 +61.9912813, 89614.2189238, 43154.3767824, 0.4519216, -0.0706496, 8.3000000, -0.0000027 +62.9911406, 89613.8158285, 43155.2904822, 0.3779150, -0.0774496, 8.2999997, -0.0000027 +63.9910000, 89613.4821325, 43156.2403144, 0.2967618, -0.0835492, 8.2999993, -1.4399608 +64.9908594, 89613.2283017, 43157.2122028, 0.2152328, -0.0782096, 8.1246829, -1.5594748 +65.9907188, 89613.0518050, 43158.2016790, 0.1382305, -0.0756518, 7.9304452, -1.5217412 +66.9905781, 89612.9519733, 43159.1917049, 0.0625030, -0.0772455, 7.7362075, -1.3284503 +67.9904375, 89612.9308967, 43160.2412430, -0.0255569, -0.0948625, 7.5625644, -1.2426946 +68.9902969, 89613.0123665, 43161.2598890, -0.1410059, -0.1360321, 7.3964411, -1.2150938 +69.9901563, 89613.2118973, 43162.1739980, -0.3011613, -0.2159617, 7.2303177, -1.2107535 +70.9900156, 89613.6208873, 43163.0877616, -0.5457459, -0.2504769, 7.0609014, -1.1850830 +71.9898750, 89614.2211207, 43163.8598037, -0.7649228, -0.1846335, 6.8910446, -1.0427014 +72.9897344, 89614.9642472, 43164.5231902, -0.9076129, -0.1158088, 6.7380550, -0.9302546 +73.9895938, 89615.7954461, 43165.1025725, -1.0135551, -0.0981842, 6.5985709, -0.7064136 +74.9894531, 89616.6665074, 43165.5871292, -1.1150610, -0.1174187, 6.4906479, 0.8885042 +75.9893125, 89617.5997987, 43165.9609191, -1.2802295, -0.2087243, 6.6261050, 0.9068554 +76.9891719, 89618.5677800, 43166.1357850, -1.5089611, -0.2388138, 6.7615622, 2.4183023 +77.9890313, 89619.4843155, 43166.0972430, -1.7099181, -0.1964340, 7.1101792, 2.7156408 +78.9888906, 89620.4615976, 43165.8683372, -1.8844472, -0.1524398, 7.4823236, 2.3701374 +79.9887500, 89621.4390333, 43165.4695609, -2.0247917, -0.1101501, 7.7926103, 1.3467921 +80.9886094, 89622.2975312, 43164.9985314, -2.1154828, -0.0794027, 7.9635407, 1.3760134 +81.9884688, 89623.1500278, 43164.4369554, -2.1889987, -0.0669226, 8.1344711, 1.3603809 +82.9883281, 89623.9617461, 43163.8175243, -2.2562265, -0.0667474, 8.2999999, 0.0000008 +83.9881875, 89624.7319192, 43163.1408273, -2.3282430, -0.0732256, 8.3000000, 0.0000008 +84.9880469, 89625.4287284, 43162.4312325, -2.4027778, -0.0759232, 8.3000001, 0.0000008 +85.9879063, 89626.0614235, 43161.6826678, -2.4766975, -0.0741176, 8.3000002, 0.0000003 +86.9877656, 89626.6478141, 43160.8772376, -2.5463402, -0.0652551, 8.3000002, 0.0000002 +87.9876250, 89627.1828074, 43160.0315794, -2.6072703, -0.0566674, 8.3000003, 0.0000002 +88.9874844, 89627.6686699, 43159.1574236, -2.6600635, -0.0490512, 8.3000003, 0.0000000 +89.9873438, 89628.1117133, 43158.2592642, -2.7052198, -0.0408320, 8.3000003, -0.0000000 +90.9872031, 89628.5169487, 43157.3455618, -2.7415840, -0.0318614, 8.3000003, -0.0000000 +91.9870625, 89628.8924963, 43156.4204684, -2.7688055, -0.0226421, 8.3000003, -0.0000000 +92.9869219, 89629.2471457, 43155.4860424, -2.7879288, -0.0164592, 8.3000003, -0.0000000 +93.9867813, 89629.5865591, 43154.5453405, -2.8018779, -0.0114427, 8.3000003, -0.0000000 +94.9866406, 89629.9151072, 43153.6010386, -2.8108217, -0.0064493, 8.3000003, -0.0000000 +95.9865000, 89630.2375512, 43152.6543939, -2.8151606, -0.0026067, 8.3000003, -0.0000000 +96.9863594, 89630.5574397, 43151.7071417, -2.8161229, 0.0006837, 8.3000003, -0.0000000 +97.9862188, 89630.8779039, 43150.7603200, -2.8137934, 0.0039762, 8.3000003, 0.0000000 +98.9860781, 89631.2022478, 43149.8146010, -2.8079055, 0.0081448, 8.3000003, 0.0000000 +99.9859375, 89631.5343622, 43148.8713745, -2.7973931, 0.0128831, 8.3000003, 0.0000000 +100.9857969, 89631.8785533, 43147.9326356, -2.7821343, 0.0176430, 8.3000003, 0.0000000 +101.9856563, 89632.2390827, 43147.0007419, -2.7617106, 0.0238939, 8.3000003, 0.0000001 +102.9855156, 89632.6216965, 43146.0779462, -2.7341466, 0.0312689, 8.3000003, 0.0000001 +103.9853750, 89633.0334069, 43145.1665381, -2.6992576, 0.0384511, 8.3000003, 0.0000001 +104.9852344, 89633.4801930, 43144.2709043, -2.6573602, 0.0451655, 8.3000003, 0.0000001 +105.9850938, 89633.9670403, 43143.3963955, -2.6086932, 0.0521924, 8.3000003, 0.0000001 +106.9849531, 89634.4983699, 43142.5493961, -2.5526842, 0.0599503, 8.3000003, -0.0000007 +107.9848125, 89635.0748637, 43141.7418026, -2.4895337, 0.0660454, 8.3000003, -0.0000017 +108.9846719, 89635.6997502, 43140.9768506, -2.4235867, 0.0669377, 8.3000001, -0.0000017 +109.9845313, 89636.3835481, 43140.2447518, -2.3573679, 0.0647602, 8.2999998, -1.1331489 +110.9843906, 89637.1273130, 43139.5459842, -2.2932634, 0.0617702, 8.1623537, -3.4635076 +111.9842500, 89637.9162800, 43138.8945898, -2.2275794, 0.0682447, 7.7264466, -3.2734659 +112.9841094, 89638.7355039, 43138.3097471, -2.1509776, 0.0864134, 7.2905395, -2.8659207 +113.9839688, 89639.5829971, 43137.8104182, -2.0492791, 0.1224312, 6.8862857, -1.3605057 +114.9838281, 89640.5689589, 43137.3871312, -1.8950286, 0.1691862, 6.6858284, -1.3203170 +115.9836875, 89641.4986619, 43137.1662046, -1.7018411, 0.2403805, 6.4853712, -1.2818200 +116.9835469, 89642.4415024, 43137.1618484, -1.4415569, 0.2845193, 6.2846448, -1.2427545 +117.9834063, 89643.3877107, 43137.4104466, -1.2024510, 0.1881998, 6.0837160, -0.9242547 +118.9832656, 89644.3021803, 43137.8450735, -1.0658363, 0.1105286, 5.9298694, -0.5008503 +119.9831250, 89645.1586707, 43138.3854273, -0.9465888, 0.1314619, 5.8448089, -0.4936140 +120.9829844, 89645.9211053, 43139.0261707, -0.7852909, 0.2044410, 5.7597484, 0.2082658 +121.9828438, 89646.5283750, 43139.7940662, -0.5397369, 0.2829817, 5.7957894, 0.2275484 +122.9827031, 89646.9143361, 43140.7070396, -0.2663789, 0.2474890, 5.8349128, 1.9079864 +123.9825625, 89647.0692671, 43141.6536354, -0.0738346, 0.1544654, 6.1531816, 2.8988652 +124.9824219, 89647.0763560, 43142.6418072, 0.0516792, 0.1064207, 6.6074624, 3.1052652 +125.9822813, 89646.9676221, 43143.6905690, 0.1514136, 0.0861390, 7.0617431, 2.6845565 +126.9821406, 89646.7719587, 43144.6884764, 0.2342048, 0.0767851, 7.4321312, 2.6160727 +127.9820000, 89646.5029465, 43145.6548235, 0.3077224, 0.0702899, 7.7761162, 2.7344150 +128.9818594, 89646.1673311, 43146.5970447, 0.3760715, 0.0668480, 8.1201013, 1.1928387 +129.9817188, 89645.7671289, 43147.5197646, 0.4415064, 0.0617140, 8.2656752, 0.1152616 +130.9815781, 89645.3118576, 43148.4137540, 0.4990373, 0.0526974, 8.2796061, 0.1154557 +131.9814375, 89644.8153826, 43149.2737540, 0.5463897, 0.0424963, 8.2935370, 0.0536318 +132.9812969, 89644.2816015, 43150.1142525, 0.5836531, 0.0329045, 8.3000003, 0.0000000 +133.9811563, 89643.7182064, 43150.9402051, 0.6120884, 0.0238945, 8.3000003, 0.0000000 +134.9810156, 89643.1359873, 43151.7514888, 0.6313083, 0.0145695, 8.3000003, 0.0000000 +135.9808750, 89642.5418421, 43152.5544271, 0.6413215, 0.0057653, 8.3000003, -0.0000000 +136.9807344, 89641.9425242, 43153.3546288, 0.6429809, -0.0024343, 8.3000003, -0.0000000 +137.9805938, 89641.3447404, 43154.1568452, 0.6364546, -0.0106157, 8.3000003, 0.0000000 +138.9804531, 89640.7554311, 43154.9652258, 0.6226017, -0.0154134, 8.3000003, 0.0000000 +139.9803125, 89640.1787820, 43155.7821873, 0.6064196, -0.0169499, 8.3000003, 0.0000000 +140.9801719, 89639.6162781, 43156.6084801, 0.5887137, -0.0184732, 8.3000003, 0.0000000 +141.9800313, 89639.0692068, 43157.4447281, 0.5700299, -0.0178520, 8.3000003, 0.0000000 +142.9798906, 89638.5369958, 43158.2906632, 0.5535727, -0.0150796, 8.3000003, 0.0000000 +143.9797500, 89638.0172253, 43159.1451378, 0.5398793, -0.0123037, 8.3000003, 0.0000000 +144.9796094, 89637.5081610, 43160.0055826, 0.5292811, -0.0082188, 8.3000003, 0.0000000 +145.9794688, 89637.0063287, 43160.8698108, 0.5238005, -0.0027552, 8.3000003, 0.0000000 +146.9793282, 89636.5063949, 43161.7362590, 0.5237661, 0.0026813, 8.3000003, 0.0000000 +147.9791875, 89636.0042770, 43162.6010966, 0.5292690, 0.0085912, 8.3000003, 0.0000000 +148.9790469, 89635.4950422, 43163.4610461, 0.5410907, 0.0150617, 8.3000003, 0.0000000 +149.9789063, 89634.9725351, 43164.3138537, 0.5593806, 0.0215049, 8.3000003, 0.0000000 +150.9787657, 89634.4322437, 43165.1546303, 0.5838589, 0.0269179, 8.3000003, 0.0000000 +151.9786250, 89633.8701008, 43165.9798875, 0.6127749, 0.0309854, 8.3000003, 0.0000000 +152.9784844, 89633.2815538, 43166.7888901, 0.6457736, 0.0349576, 8.3000003, 0.0000000 +153.9783438, 89632.6659475, 43167.5758827, 0.6820706, 0.0359204, 8.3000003, 0.0000000 +154.9782032, 89632.0243967, 43168.3385326, 0.7160703, 0.0323097, 8.3000003, 0.0000000 +155.9780625, 89631.3554211, 43169.0835887, 0.7466234, 0.0287252, 8.3000003, 0.0000000 +156.9779219, 89630.6595412, 43169.8149335, 0.7741120, 0.0269722, 8.3000003, -0.0000000 +157.9777813, 89629.9393864, 43170.5309232, 0.8030599, 0.0303823, 8.3000003, -0.0000000 +158.9776407, 89629.2095637, 43171.2131611, 0.8360754, 0.0362124, 8.3000003, -0.0000001 +159.9775000, 89628.4667524, 43171.8587762, 0.8761632, 0.0465315, 8.3000003, -0.0000001 +160.9773594, 89627.6847737, 43172.4774322, 0.9286938, 0.0578544, 8.3000003, -0.0000001 +161.9772188, 89626.8739629, 43173.0471164, 0.9885355, 0.0614044, 8.3000003, -0.0000018 +162.9770782, 89626.0278183, 43173.5689882, 1.0447809, 0.0425652, 8.3000001, -0.0000040 +163.9769375, 89625.1595619, 43174.0554509, 1.0699050, 0.0091977, 8.2999996, -0.0000040 +164.9767969, 89624.2726879, 43174.5414717, 1.0640148, -0.0207924, 8.2999991, -1.0808892 +165.9766563, 89623.3866375, 43175.0520038, 1.0270720, -0.0500100, 8.1687521, -2.6580054 +166.9765157, 89622.5275927, 43175.6076493, 0.9610482, -0.0805986, 7.8366605, -2.5477050 +167.9763750, 89621.7480044, 43176.2071139, 0.8628487, -0.1212590, 7.5045689, -1.8718633 +168.9762344, 89621.0445186, 43176.8963110, 0.7234617, -0.1511687, 7.2508864, -1.2510546 +169.9760938, 89620.4432000, 43177.6911831, 0.5720819, -0.1496603, 7.0762699, -1.2205594 +170.9759532, 89619.9682392, 43178.5606181, 0.4305096, -0.1339580, 6.9016535, -1.2280904 +171.9758125, 89619.6190289, 43179.4783467, 0.2933385, -0.1506959, 6.7213827, -1.2013608 +172.9756719, 89619.4070995, 43180.4526180, 0.1312065, -0.1735889, 6.5402295, -0.9452154 +173.9755313, 89619.3663432, 43181.4622918, -0.0526455, -0.1822964, 6.3940939, 0.3156427 +174.9753907, 89619.4993208, 43182.4277035, -0.2169420, -0.1566067, 6.4432626, 0.3180606 +175.9752500, 89619.7899954, 43183.3914534, -0.3664766, -0.1420140, 6.4924314, 0.4398005 +176.9751094, 89620.2226307, 43184.3157714, -0.5081215, -0.1358765, 6.5598127, 0.9457623 +177.9749688, 89620.7639894, 43185.1512607, -0.6411230, -0.1320941, 6.7024175, 0.9661013 +178.9748282, 89621.4150211, 43185.9126599, -0.7740185, -0.1339307, 6.8450224, 1.0796639 +179.9746875, 89622.1614162, 43186.5789649, -0.9111481, -0.1395266, 7.0009539, 2.0148540 +180.9745469, 89622.9782994, 43187.1283668, -1.0449529, -0.1313433, 7.2830280, 2.0944309 +181.9744063, 89623.8694293, 43187.5723917, -1.1701616, -0.1196428, 7.5651021, 2.1740079 +182.9742657, 89624.8277479, 43187.9135026, -1.2852772, -0.1065436, 7.8471762, 0.5303850 +183.9741250, 89625.8271264, 43188.1520326, -1.3845416, -0.0885494, 7.9144675, 0.4241213 +184.9739844, 89626.8369731, 43188.2967922, -1.4715607, -0.0842602, 7.9678679, 0.4269733 +185.9738438, 89627.8082175, 43188.3523874, -1.5574649, -0.0955533, 8.0212683, -0.1216381 +186.9737032, 89628.8695456, 43188.3074942, -1.6733835, -0.1223647, 8.0060916, -0.3134519 +187.9735625, 89629.8480896, 43188.1420542, -1.8071793, -0.1463519, 7.9668493, 0.0830821 +188.9734219, 89630.7192822, 43187.8669712, -1.9486928, -0.1645822, 7.9772695, 1.2837299 +189.9732813, 89631.5741456, 43187.4497553, -2.0944296, -0.1249685, 8.1365796, 1.2577677 +190.9731407, 89632.4564364, 43186.8793235, -2.1806997, -0.0452768, 8.2896990, 0.0284581 +191.9730000, 89633.1960857, 43186.3432010, -2.2126734, -0.0276875, 8.2931307, 0.0284699 +192.9728594, 89634.0009629, 43185.7257799, -2.2363772, -0.0201640, 8.2965625, 0.0284817 +193.9727188, 89634.8416357, 43185.0518698, -2.2561340, -0.0170557, 8.2999942, 0.0000441 +194.9725782, 89635.6351605, 43184.3958440, -2.2638902, 0.0017676, 8.2999995, -0.0000056 +195.9724375, 89636.4204499, 43183.7490901, -2.2522042, 0.0220388, 8.2999989, -0.0000056 +196.9722969, 89637.1839305, 43183.1483968, -2.2180168, 0.0509159, 8.2999982, -0.9224153 +197.9721563, 89638.0416649, 43182.5433713, -2.1473318, 0.0809622, 8.1881255, -1.2222212 +198.9720157, 89638.9018442, 43182.0377005, -2.0523489, 0.1097369, 8.0374934, -1.2049495 +199.9718750, 89639.7494267, 43181.6526293, -1.9383916, 0.1339677, 7.8861740, -1.2198247 +200.9717344, 89640.6800414, 43181.3645712, -1.8032261, 0.1359829, 7.7299698, -1.1954215 +201.9715938, 89641.6743767, 43181.1952185, -1.6817645, 0.1010210, 7.5737657, 1.2382010 +202.9714532, 89642.6351383, 43181.1307431, -1.5962667, 0.0836040, 7.7355013, 2.0994069 +203.9713125, 89643.6281896, 43181.1461574, -1.5145489, 0.0819396, 8.0022624, 2.1705784 +204.9711719, 89644.6736375, 43181.2507561, -1.4270055, 0.0853282, 8.2690235, 0.2566181 +205.9710313, 89645.6482825, 43181.4325528, -1.3483816, 0.0715856, 8.2999948, 0.0000180 +206.9708907, 89646.6169930, 43181.6862765, -1.2827772, 0.0602015, 8.2999970, 0.0000180 +207.9707500, 89647.5781123, 43182.0016586, -1.2259271, 0.0527675, 8.2999992, 0.0000062 +208.9706094, 89648.5283020, 43182.3710266, -1.1752873, 0.0462764, 8.2999999, 0.0000008 +209.9704688, 89649.4520627, 43182.7811392, -1.1317083, 0.0400859, 8.3000000, 0.0000008 +210.9703282, 89650.3384295, 43183.2180651, -1.0948070, 0.0346745, 8.3000001, -0.0000001 +211.9701875, 89651.2141256, 43183.6876221, -1.0638574, 0.0258320, 8.3000001, -0.0000009 +212.9700469, 89652.0812021, 43184.1811448, -1.0453613, 0.0106996, 8.3000000, -0.0000009 +213.9699063, 89652.9383152, 43184.6810408, -1.0429778, -0.0058328, 8.2999999, -3.1596772 +214.9697657, 89653.8086307, 43185.1819808, -1.0566188, -0.0211692, 7.9102169, -4.8932619 +215.9696250, 89654.6955962, 43185.6674442, -1.0862001, -0.0379105, 7.2654239, -4.4774454 +216.9694844, 89655.5858515, 43186.1111767, -1.1342796, -0.0601786, 6.6206309, -2.3371616 +217.9693438, 89656.5583196, 43186.5228696, -1.2091902, -0.0782896, 6.2577222, -1.3086921 +218.9692032, 89657.5280976, 43186.8419152, -1.3017087, -0.1081263, 6.0450038, -1.3144603 +219.9690625, 89658.4311737, 43187.0342483, -1.4331364, -0.1973613, 5.8235316, -1.5967584 +220.9689219, 89659.4193274, 43187.0443809, -1.7114918, -0.3292005, 5.5426034, -1.2792353 +221.9687813, 89660.3402376, 43186.7758478, -1.9761381, -0.2168737, 5.3068203, -0.4812483 +222.9686407, 89661.1655119, 43186.3218332, -2.1682537, -0.1962455, 5.2153601, -0.4728821 +223.9685000, 89661.9801866, 43185.6337279, -2.3754806, -0.1924939, 5.1238998, 0.2056979 +224.9683594, 89662.6095365, 43184.8477283, -2.5490402, -0.1502335, 5.1638830, 0.3402492 +225.9682188, 89663.1139315, 43183.9653248, -2.6940441, -0.1411011, 5.2293489, 0.3445356 +226.9680782, 89663.4783174, 43183.0367842, -2.8461913, -0.1715229, 5.2948148, 0.8419499 +227.9679375, 89663.6864066, 43182.0002679, -3.0467628, -0.2069518, 5.4514885, 0.8956232 +228.9677969, 89663.6741712, 43181.0219092, 3.0156443, -0.2442462, 5.6133521, 1.1607871 +229.9676563, 89663.4444163, 43180.1110533, 2.7670498, -0.2821409, 5.8164396, 1.5259940 +230.9675157, 89662.9744792, 43179.2692336, 2.5037308, -0.2468862, 6.0730987, 1.6209100 +231.9673750, 89662.2656461, 43178.5101648, 2.2925740, -0.1567606, 6.3343423, 2.0418117 +232.9672344, 89661.5266187, 43177.9342153, 2.1843583, -0.0833895, 6.6488301, 2.1407282 +233.9670938, 89660.6758757, 43177.3780358, 2.1220091, -0.0431483, 6.9633179, 2.2168327 +234.9669532, 89659.7664562, 43176.8409333, 2.0911428, -0.0180613, 7.2746710, 1.9922749 +235.9668125, 89658.8776406, 43176.3424077, 2.0727477, -0.0182370, 7.5435288, 2.0645696 +236.9666719, 89657.9911298, 43175.8665948, 2.0537948, -0.0196710, 7.8123867, 2.1368643 +237.9665313, 89657.1216814, 43175.4216833, 2.0332583, -0.0227227, 8.0812445, 0.6964881 +238.9663907, 89656.1958777, 43174.9769264, 2.0004016, -0.0429626, 8.1669635, 0.5685413 +239.9662500, 89655.2822377, 43174.5858054, 1.9463719, -0.0657591, 8.2362744, 0.2972547 +240.9661094, 89654.3653516, 43174.2612601, 1.8726437, -0.0798390, 8.2722816, -2.7866607 +241.9659688, 89653.4210516, 43173.9985747, 1.8202498, -0.0270210, 7.9283103, -2.6683278 +242.9658282, 89652.4508416, 43173.7564802, 1.8186911, 0.0225670, 7.5843391, -2.2185637 +243.9656875, 89651.4741216, 43173.4898068, 1.8625290, 0.0609603, 7.2859931, -1.2327134 +244.9655469, 89650.5401952, 43173.1735962, 1.9366704, 0.0894321, 7.1148166, -1.2034079 +245.9654063, 89649.6197587, 43172.7671087, 2.0416820, 0.1193522, 6.9436402, -1.1730853 +246.9652657, 89648.7406935, 43172.2436731, 2.1778511, 0.1354947, 6.7726138, -1.1399869 +247.9651250, 89647.9273063, 43171.5958501, 2.3048492, 0.1121405, 6.6021697, -1.1109317 +248.9649844, 89647.2188636, 43170.8820318, 2.4146814, 0.1098222, 6.4317256, -1.0927676 +249.9648438, 89646.6137984, 43170.1189657, 2.5327864, 0.1436583, 6.2595420, -1.1091078 +250.9647032, 89646.0950622, 43169.2111570, 2.7272287, 0.2287952, 6.0797995, -1.0767959 +251.9645625, 89645.8215631, 43168.3248139, 2.9629134, 0.2632378, 5.9000571, 0.8606790 +252.9644219, 89645.7662589, 43167.4008410, -3.0887896, 0.2221929, 6.0441533, 1.0443016 +253.9642813, 89645.9199779, 43166.3904756, -2.9079516, 0.1326110, 6.2145071, 2.2228672 +254.9641407, 89646.2035014, 43165.4397834, -2.8008433, 0.0979760, 6.5624087, 2.9031060 +255.9640000, 89646.5827704, 43164.5162522, -2.7022514, 0.1023214, 6.9907513, 3.0866092 +256.9638594, 89647.0765958, 43163.6024670, -2.5863579, 0.1235147, 7.4190939, 2.2474988 +257.9637188, 89647.6641428, 43162.7800802, -2.4560294, 0.1273259, 7.7160430, 2.0063350 +258.9635782, 89648.3429119, 43162.0487510, -2.3318844, 0.1212709, 7.9717893, 2.0717504 +259.9634375, 89649.1062238, 43161.4038749, -2.2137012, 0.1152131, 8.2275357, 0.5989158 +260.9632969, 89649.9235557, 43160.8595478, -2.1046149, 0.1053248, 8.3000004, 0.0000002 +261.9631563, 89650.7977194, 43160.3999095, -2.0075144, 0.0908999, 8.3000004, 0.0000002 +262.9630157, 89651.7338645, 43160.0115042, -1.9232831, 0.0753243, 8.3000004, 0.0000001 +263.9628750, 89652.6818703, 43159.7008798, -1.8515595, 0.0747501, 8.3000004, -0.0000000 +264.9627344, 89653.6412961, 43159.4640711, -1.7721321, 0.0861215, 8.3000004, -0.0000000 +265.9625938, 89654.6372024, 43159.3075883, -1.6790853, 0.0984713, 8.3000004, -0.0000001 +266.9624532, 89655.6641717, 43159.2505445, -1.5717627, 0.1053786, 8.3000004, -0.0000004 +267.9623125, 89656.7167405, 43159.3048858, -1.4689093, 0.0925712, 8.3000004, -0.0000004 +268.9621719, 89657.7267746, 43159.4566835, -1.3735637, 0.0972910, 8.3000003, -0.0000004 +269.9620313, 89658.6388310, 43159.6860381, -1.2705384, 0.1285065, 8.3000003, -4.0976406 +270.9618907, 89659.6518184, 43160.0913414, -1.1018669, 0.1741469, 7.7907558, -4.7550427 +271.9617500, 89660.4646780, 43160.5946200, -0.9315115, 0.1693455, 7.1545180, -3.4150065 +272.9616094, 89661.1312878, 43161.1651862, -0.7911339, 0.1851752, 6.6601858, -2.3221857 +273.9614688, 89661.7540129, 43161.8957875, -0.6321464, 0.1203042, 6.3019328, 0.6745839 +274.9613282, 89662.3202971, 43162.7480877, -0.5514743, 0.0581784, 6.4080679, 3.7398680 +275.9611875, 89662.8131289, 43163.6020678, -0.4951216, 0.0593049, 6.9672103, 4.0525521 +276.9610469, 89663.2808843, 43164.5430379, -0.4240119, 0.0795908, 7.5263526, 0.0390734 +277.9609063, 89663.6812320, 43165.5619425, -0.3244693, 0.0758289, 7.5315416, -5.5368962 +278.9607657, 89663.9983800, 43166.5940078, -0.2892184, -0.0231320, 6.7566178, -4.9363048 +279.9606250, 89664.2869826, 43167.4488533, -0.3984714, -0.2772338, 5.9816940, -2.2998458 +280.9604844, 89664.7837572, 43168.1904656, -0.8162421, -0.4859913, 5.5840503, -1.5324764 +281.9603438, 89665.6233343, 43168.7575715, -1.0583019, -0.0437354, 5.3025556, -0.1608180 +282.9602032, 89666.4656037, 43169.2200586, -1.0794523, -0.0272203, 5.2721443, 0.0357915 +283.9600625, 89667.3847058, 43169.6920717, -1.1164692, -0.0460987, 5.2789278, 0.0358375 +284.9599219, 89668.3284009, 43170.1189980, -1.1826503, -0.0890172, 5.2857112, 1.7027767 +285.9597813, 89669.3358189, 43170.4623751, -1.3153542, -0.1643776, 5.5985550, 1.8300032 +286.9596407, 89670.2892308, 43170.6197354, -1.5105321, -0.2338776, 5.9163595, 2.4491592 +287.9595000, 89671.2143974, 43170.5713661, -1.7360717, -0.2381767, 6.3167190, 3.2893507 +288.9593594, 89672.1345636, 43170.3135552, -1.9447726, -0.1943544, 6.8175300, 3.5401976 +289.9592188, 89673.0771841, 43169.8385158, -2.1208459, -0.1402562, 7.3183410, 2.3498479 +290.9590782, 89673.8900199, 43169.2700437, -2.2316416, -0.0851020, 7.6326372, 2.1316309 +291.9589376, 89674.6663349, 43168.6186578, -2.3012759, -0.0557224, 7.9069471, 2.2068874 +292.9587969, 89675.4088787, 43167.9187333, -2.3493851, -0.0400085, 8.1812570, 0.9786558 +293.9586563, 89676.1279052, 43167.1809636, -2.3899434, -0.0458753, 8.3000002, 0.0000005 +294.9585157, 89676.7958209, 43166.4279063, -2.4450648, -0.0631895, 8.3000002, 0.0000005 +295.9583751, 89677.3942271, 43165.6628087, -2.5123422, -0.0737221, 8.3000003, 0.0000003 +296.9582344, 89677.9330842, 43164.8640192, -2.5835466, -0.0719959, 8.3000003, 0.0000001 +297.9580938, 89678.4246159, 43164.0128544, -2.6511100, -0.0650967, 8.3000003, 0.0000001 +298.9579532, 89678.8766928, 43163.0973697, -2.7132273, -0.0564312, 8.3000003, 0.0000000 +299.9578126, 89679.2727227, 43162.1627113, -2.7682026, -0.0549298, 8.3000004, 0.0000000 +300.9576719, 89679.6141074, 43161.2112613, -2.8274056, -0.0633691, 8.3000004, 0.0000000 +301.9575313, 89679.8917618, 43160.2454456, -2.8987448, -0.0803671, 8.3000004, -0.0000001 +302.9573907, 89680.0916145, 43159.2471214, -2.9930481, -0.1011281, 8.3000003, -0.0000002 +303.9572501, 89680.1887743, 43158.2190338, -3.1039277, -0.1141717, 8.3000003, -0.0000002 +304.9571094, 89680.1691170, 43157.2489391, 3.0606927, -0.1309769, 8.3000003, -0.0000005 +305.9569688, 89680.0260426, 43156.2873696, 2.9248970, -0.1435266, 8.3000003, -0.0000007 +306.9568282, 89679.7471581, 43155.3386892, 2.7895280, -0.1250876, 8.3000002, -0.0000007 +307.9566876, 89679.3493600, 43154.4198131, 2.6832153, -0.0855197, 8.3000001, 0.0000001 +308.9565469, 89678.8868460, 43153.5732051, 2.5991996, -0.0895623, 8.3000001, 0.0000001 +309.9564063, 89678.3289051, 43152.7370989, 2.5060215, -0.0959301, 8.3000001, 0.0000002 +310.9562657, 89677.6805654, 43151.9448683, 2.4047179, -0.0983610, 8.3000001, 0.0000004 +311.9561251, 89676.9811798, 43151.2431866, 2.3125369, -0.0887174, 8.3000002, 0.0000004 +312.9559844, 89676.2103470, 43150.5965616, 2.2257203, -0.0846758, 8.3000002, 0.0000003 +313.9558438, 89675.3794475, 43150.0133403, 2.1401435, -0.0815722, 8.3000003, -0.0000000 +314.9557032, 89674.4956549, 43149.4958283, 2.0629047, -0.0699087, 8.3000003, -0.0000000 +315.9555626, 89673.6028865, 43149.0549455, 1.9973583, -0.0623245, 8.3000003, -0.0000001 +316.9554219, 89672.6884026, 43148.6722231, 1.9369643, -0.0630982, 8.3000002, -0.0000003 +317.9552813, 89671.7274590, 43148.3404117, 1.8681720, -0.0713722, 8.3000002, -0.0000003 +318.9551407, 89670.7891955, 43148.0886394, 1.7975236, -0.0727092, 8.3000002, 0.0000001 +319.9550001, 89669.8492647, 43147.9059624, 1.7279369, -0.0718879, 8.3000002, 0.0000002 +320.9548594, 89668.8677931, 43147.7844303, 1.6621362, -0.0594515, 8.3000002, -0.0000018 +321.9547188, 89667.8477743, 43147.7189888, 1.6115983, -0.0323566, 8.3000000, -0.0000075 +322.9545782, 89666.8870461, 43147.6878232, 1.6012438, 0.0078553, 8.2999991, -0.0000075 +323.9544376, 89665.8702204, 43147.6471983, 1.6253914, 0.0385325, 8.2999982, -0.4965420 +324.9542969, 89664.8285192, 43147.5640542, 1.6800015, 0.0618168, 8.2399651, -2.7764627 +325.9541563, 89663.8180387, 43147.4205799, 1.7442555, 0.0652220, 7.8958775, -2.6580497 +326.9540157, 89662.8316136, 43147.2125712, 1.8148382, 0.0762882, 7.5517898, -2.5396368 +327.9538751, 89661.9004967, 43146.9407488, 1.8983880, 0.0984561, 7.2077021, -1.2478391 +328.9537344, 89660.9125367, 43146.5417532, 2.0146056, 0.1201909, 7.0324706, -1.1603200 +329.9535938, 89660.0473698, 43146.0605634, 2.1467704, 0.1482842, 6.8655174, -0.8675554 +330.9534532, 89659.2985548, 43145.4898060, 2.3031280, 0.1839607, 6.7379865, 0.0151902 +331.9533126, 89658.6331506, 43144.7692404, 2.4886387, 0.1827575, 6.7402402, 0.0151953 +332.9531719, 89658.0915381, 43143.9219930, 2.6476890, 0.1285749, 6.7424939, 2.6104415 +333.9530313, 89657.6836940, 43143.0553096, 2.7500975, 0.0912958, 7.1190851, 3.2575383 +334.9528907, 89657.3405269, 43142.1126528, 2.8319816, 0.0741205, 7.5627729, 3.3328650 +335.9527501, 89657.0591878, 43141.1058887, 2.9049336, 0.0664698, 7.9912656, 0.8277498 +336.9526094, 89656.8575800, 43140.1384001, 2.9658322, 0.0571485, 8.0941703, 0.8383407 +337.9524688, 89656.7093917, 43139.1473195, 3.0192909, 0.0498528, 8.1970750, 0.8489315 +338.9523282, 89656.6104514, 43138.1424047, 3.0666705, 0.0442394, 8.2999797, 0.0001660 +339.9521876, 89656.5559492, 43137.1435252, 3.1057862, 0.0339869, 8.2999997, 0.0000020 +340.9520469, 89656.5354507, 43136.1433162, 3.1347380, 0.0239193, 8.2999999, 0.0000020 +341.9519063, 89656.5388818, 43135.1444283, -3.1295233, 0.0139746, 8.3000002, -0.0000002 +342.9517657, 89656.5556990, 43134.1362332, -3.1229445, -0.0014044, 8.3000001, -0.0000004 +343.9516251, 89656.5709672, 43133.1343800, -3.1325104, -0.0178663, 8.3000001, -0.0000004 +344.9514844, 89656.5683763, 43132.1434946, 3.1243702, -0.0354989, 8.3000001, -0.0000000 +345.9513438, 89656.5303615, 43131.1236043, 3.0828151, -0.0451562, 8.3000001, 0.0000000 +346.9512032, 89656.4470347, 43130.1235739, 3.0322801, -0.0560909, 8.3000001, -2.1244137 +347.9510626, 89656.3124028, 43129.1596191, 2.9710383, -0.0704782, 8.0400106, -0.8017657 diff --git a/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/CMakeLists.txt index 01e1d6f3..3a47d493 100644 --- a/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/CMakeLists.txt +++ b/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/CMakeLists.txt @@ -13,5 +13,8 @@ ament_auto_find_build_dependencies() ament_auto_add_executable(path_to_trajectory_node src/path_to_trajectory.cpp ) +ament_auto_add_executable(csv_to_trajectory_node + src/csv_to_trajectory.cpp +) ament_auto_package() diff --git a/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/include/path_to_trajectory/csv_to_trajectory.hpp b/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/include/path_to_trajectory/csv_to_trajectory.hpp new file mode 100644 index 00000000..d512016e --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/include/path_to_trajectory/csv_to_trajectory.hpp @@ -0,0 +1,61 @@ +// Copyright 2023 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PATH_TO_TRAJECTORY__CSV_TO_TRAJECTORY_HPP_ +#define PATH_TO_TRAJECTORY__CSV_TO_TRAJECTORY_HPP_ + +#include "nav_msgs/msg/odometry.hpp" +#include "std_msgs/msg/int32.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "rclcpp/rclcpp.hpp" +#include +#include +#include +#include + +#include +#include +#include + +class CsvToTrajectory : public rclcpp::Node { +public: + using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; + using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; + + CsvToTrajectory(); + +private: + rclcpp::Subscription::SharedPtr sub_odom_; + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Publisher::SharedPtr pub_now_point_; + std::vector trajectory_points_; + size_t current_point_index_ = 0; + float velocity_coef_ = 1.0f; + float trajectory_length_ = 200.0f; + float trajectory_margin_ = 2.0f; + float trajectory_rear_length_ = 30.0f; + int now_index_ = 0; + float z_position_ = 0.0f; + + pcl::PointCloud::Ptr cloud_; + pcl::KdTreeFLANN kdtree_; + + + void odomCallback(const nav_msgs::msg::Odometry::SharedPtr odometry); + void readCsv(const std::string& csv_file_path); + void dynamicLoadParam(); +}; + +#endif // PATH_TO_TRAJECTORY__CSV_TO_TRAJECTORY_HPP_ \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/package.xml b/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/package.xml index 89857611..9bd557c6 100644 --- a/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/package.xml +++ b/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/package.xml @@ -11,6 +11,7 @@ rclcpp autoware_auto_planning_msgs + pcl_ros ament_lint_auto ament_lint_common diff --git a/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/src/csv_to_trajectory.cpp b/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/src/csv_to_trajectory.cpp new file mode 100644 index 00000000..a0ea9624 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/src/csv_to_trajectory.cpp @@ -0,0 +1,149 @@ +// Copyright 2023 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "path_to_trajectory/csv_to_trajectory.hpp" +#include +#include + + +CsvToTrajectory::CsvToTrajectory() : Node("csv_to_trajectory_node") +, cloud_(new pcl::PointCloud()) + { + using std::placeholders::_1; + this->declare_parameter("csv_file_path", ""); + this->declare_parameter("velocity_coef", 30.0f); + this->declare_parameter("trajectory_length", 100.0f); + this->declare_parameter("trajectory_margin", 2.0f); + this->declare_parameter("trajectory_rear_length", 10.0f); + this->declare_parameter("z_position", 0.0f); + + std::string csv_file_path; + this->get_parameter("csv_file_path", csv_file_path); + this->get_parameter("trajectory_length", this->trajectory_length_); + this->get_parameter("z_position", this->z_position_); + dynamicLoadParam(); + + if (csv_file_path.empty()) { + RCLCPP_ERROR(this->get_logger(), "No CSV file path provided"); + rclcpp::shutdown(); + return; + } + rclcpp::QoS qos(rclcpp::KeepLast(1)); + this->sub_odom_ = this->create_subscription( + "in_odom", qos, std::bind(&CsvToTrajectory::odomCallback, this, _1)); + this->pub_ = this->create_publisher("output", 1); + this->pub_now_point_ = this->create_publisher("now_waypoint", 1); + this->readCsv(csv_file_path); + +} + +void CsvToTrajectory::dynamicLoadParam(){ + this->get_parameter("velocity_coef", this->velocity_coef_); + this->get_parameter("trajectory_margin", this->trajectory_margin_); + this->get_parameter("trajectory_rear_length", this->trajectory_rear_length_); +} + +void CsvToTrajectory::readCsv(const std::string& file_path) { + std::ifstream file(file_path); + std::string line; + double old_x=0.0; + double old_y=0.0; + while (std::getline(file, line)) { + if (line.empty() || line[0] == '#' || line[0] == 'x') continue; // Skip empty lines and comments + std::istringstream s(line); + std::string field; + std::vector values; + + while (getline(s, field, ',')) { + values.push_back(std::stod(field)); + } + // x,y,z,yaw + TrajectoryPoint point; + point.pose.position.x = values[1]; + point.pose.position.y = values[2]; + point.pose.position.z = z_position_; + //const double yaw = std::atan2(point.pose.position.y-old_y, point.pose.position.x-old_x); + const double yaw = values[3]; + point.pose.orientation.x = 0.0; + point.pose.orientation.y = 0.0; + point.pose.orientation.z = sin(yaw / 2); + point.pose.orientation.w = cos(yaw / 2); + point.longitudinal_velocity_mps = values[5]; + point.acceleration_mps2 = 0.0; //values[6]; + + trajectory_points_.push_back(point); + old_x = point.pose.position.x; + old_y = point.pose.position.y; + cloud_->points.push_back(pcl::PointXYZ(point.pose.position.x, point.pose.position.y, z_position_)); + } + // double yaw = std::atan2(trajectory_points_.front().pose.position.y-old_y, trajectory_points_.front().pose.position.x-old_x); + // trajectory_points_.front().pose.orientation.x = 0.0; + // trajectory_points_.front().pose.orientation.y = 0.0; + // trajectory_points_.front().pose.orientation.z = sin(yaw / 2); + // trajectory_points_.front().pose.orientation.w = cos(yaw / 2); + RCLCPP_INFO(this->get_logger(), "Loaded %zu trajectory points", trajectory_points_.size()); + + kdtree_.setInputCloud(cloud_); + +} + +void CsvToTrajectory::odomCallback(const nav_msgs::msg::Odometry::SharedPtr odometry) +{ + if (current_point_index_ >= trajectory_points_.size()) return; + + dynamicLoadParam(); + + Trajectory trajectory; + // Set trajectory header + trajectory.header = odometry->header; + + const int K = 1; // 1点のみ取得 + std::vector pointIdxNKNSearch(K); + std::vector pointNKNSquaredDistance(K); + pcl::PointXYZ searchPoint(odometry->pose.pose.position.x, odometry->pose.pose.position.y, z_position_); + + if (kdtree_.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) + { + // std::cout << "The closest point index: " << pointIdxNKNSearch[0] << std::endl; + // std::cout << "Distance: " << pointNKNSquaredDistance[0] << std::endl; + } + else + { + // std::cout << "No neighbors found!" << std::endl; + return; + } + int start_index = pointIdxNKNSearch[0]- std::round(trajectory_rear_length_/trajectory_margin_); + if(start_index<0){ + start_index+=trajectory_points_.size(); + } + const int points_num = std::round((trajectory_length_+trajectory_rear_length_)/trajectory_margin_); + for(int i = 0; i < points_num; i++){ + int p = start_index + i; + if(p>=trajectory_points_.size()){ + p-=(trajectory_points_.size()); + } + trajectory.points.push_back(trajectory_points_[p]); + } + for(auto && p : trajectory.points){ + p.longitudinal_velocity_mps*=this->velocity_coef_; + } + pub_->publish(trajectory); +} + +int main(int argc, char const* argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +}