From ed8ec2985c06316434bbd2e6d776f0a37bace980 Mon Sep 17 00:00:00 2001 From: hrjp Date: Sat, 12 Oct 2024 23:40:33 +0900 Subject: [PATCH] add csv to traj --- .../launch/components/planning.launch.xml | 20 +-- .../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 ++++++++++++++++++ .../path_to_trajectory/CMakeLists.txt | 3 + .../path_to_trajectory/csv_to_trajectory.hpp | 52 ++++++ .../src/csv_to_trajectory.cpp | 125 +++++++++++++ 9 files changed, 996 insertions(+), 11 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/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/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..93c43ede 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,16 @@ - - - + + + + + + + - - - - - - - - + + 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/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..64a870ad --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/include/path_to_trajectory/csv_to_trajectory.hpp @@ -0,0 +1,52 @@ +// 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 + +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_rate_ = 1.0f; + float trajectory_length_ = 200.0f; + float trajectory_margin_ = 2.0f; + float next_point_threshold_ = 30.0f; + int now_index_ = 0; + float z_position_ = 0.0f; + + void odomCallback(const nav_msgs::msg::Odometry::SharedPtr odometry); + void readCsv(const std::string& csv_file_path); +}; + +#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/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..6a097c79 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/src/csv_to_trajectory.cpp @@ -0,0 +1,125 @@ +// 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") { + using std::placeholders::_1; + this->declare_parameter("csv_file_path", ""); + this->declare_parameter("velocity_rate", 1.0f); + this->declare_parameter("trajectory_length", 100.0f); + this->declare_parameter("trajectory_margin", 2.0f); + this->declare_parameter("next_point_threshold", 30.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("velocity_rate", this->velocity_rate_); + this->get_parameter("trajectory_length", this->trajectory_length_); + this->get_parameter("trajectory_margin", this->trajectory_margin_); + this->get_parameter("next_point_threshold", this->next_point_threshold_); + this->get_parameter("z_position", this->z_position_); + + 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::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)); + } + // s_m; x_m; y_m; psi_rad; kappa_radpm; vx_mps; ax_mps2 + TrajectoryPoint point; + point.pose.position.x = values[0]; + point.pose.position.y = values[1]; + point.pose.position.z = z_position_; + double yaw = std::atan2(point.pose.position.y-old_y, point.pose.position.x-old_x); + 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 = 30.0;//values[5] * this->velocity_rate_; + point.acceleration_mps2 = 0.0; //values[6]; + + trajectory_points_.push_back(point); + old_x = point.pose.position.x; + old_y = point.pose.position.y; + } + 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()); +} + +void CsvToTrajectory::odomCallback(const nav_msgs::msg::Odometry::SharedPtr odometry) +{ + if (current_point_index_ >= trajectory_points_.size()) return; + + Trajectory trajectory; + // Set trajectory header + trajectory.header = odometry->header; + while(true){ + const float dis = std::hypot( + trajectory_points_[current_point_index_].pose.position.x - odometry->pose.pose.position.x, + trajectory_points_[current_point_index_].pose.position.y - odometry->pose.pose.position.y); + if(dis > next_point_threshold_){ + break; + } + current_point_index_++; + } + // const int start_index = std::max(0, int(current_point_index_ - (2.0*next_point_threshold_)/trajectory_margin_)); + // const int end_index = std::min(int(trajectory_points_.size()), int(current_point_index_ + trajectory_length_/trajectory_margin_)); + const int start_index = 0; + const int end_index = trajectory_points_.size(); + for(int i = start_index; i < end_index; i++){ + trajectory.points.push_back(trajectory_points_[i]); + } + //RCLCPP_INFO(this->get_logger(), "Now waypoint: %d", int(start_index+next_point_threshold_/trajectory_margin_)); + std_msgs::msg::Int32 now_point; + now_point.data = int(current_point_index_ - (next_point_threshold_)/trajectory_margin_); + pub_now_point_->publish(now_point); + pub_->publish(trajectory); +} + +int main(int argc, char const* argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +}