From 189858a5942186c1786530bb53170824f0e6622e Mon Sep 17 00:00:00 2001 From: tamago117 Date: Wed, 23 Oct 2024 00:15:11 +0900 Subject: [PATCH 1/3] adjust param --- .../planning/mppi_controller.param.yaml | 6 +- .../launch/components/control.launch.xml | 6 +- .../booars_launch/map/csv/traj_race_cl.csv | 252 +++++++++--------- .../src/csv_to_trajectory.cpp | 2 +- .../src/simple_pure_pursuit.cpp | 8 +- .../config/debug_sensing.rviz | 108 ++++---- memo | 6 + 7 files changed, 193 insertions(+), 195 deletions(-) create mode 100644 memo diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml index 913df7b7..fb78057c 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml @@ -3,7 +3,7 @@ # mppi horizon : 25 num_samples : 2500 - u_min : [-4.0, -0.25] # accel(m/s2), steer angle(rad) + u_min : [-2.0, -0.25] # accel(m/s2), steer angle(rad) u_max : [2.0, 0.25] sigmas : [2.0, 0.25] # sample range lambda : 1.0 @@ -14,8 +14,8 @@ reference_path_interval : 0.83 # cost weights Qc : 20.0 - Ql : 5.0 - Qv : 0.5 + Ql : 1.0 + Qv : 10.0 Qo : 1000.0 Qin : 0.01 Qdin : 0.0 diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml index 926aa618..cb670343 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml @@ -14,11 +14,11 @@ output="screen" unless="$(var use_stanley)"> - - + + - + 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 index 1d59fcc2..a8dc2642 100644 --- 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 @@ -65,22 +65,22 @@ 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 +65.9907188, 89613.0518050, 43158.2016790, 0.1382305, -0.0756518, 5.9304452, -1.5217412 +66.9905781, 89612.9519733, 43159.1917049, 0.0625030, -0.0772455, 5.7362075, -1.3284503 +67.9904375, 89612.9308967, 43160.2412430, -0.0255569, -0.0948625, 5.5625644, -1.2426946 +68.9902969, 89613.0123665, 43161.2598890, -0.1410059, -0.1360321, 5.3964411, -1.2150938 +69.9901563, 89613.2118973, 43162.1739980, -0.3011613, -0.2159617, 5.2303177, -1.2107535 +70.9900156, 89613.6208873, 43163.0877616, -0.5457459, -0.2504769, 5.0609014, -1.1850830 +71.9898750, 89614.2211207, 43163.8598037, -0.7649228, -0.1846335, 4.8910446, -1.0427014 +72.9897344, 89614.9642472, 43164.5231902, -0.9076129, -0.1158088, 4.7380550, -0.9302546 +73.9895938, 89615.7954461, 43165.1025725, -1.0135551, -0.0981842, 4.5985709, -0.7064136 +74.9894531, 89616.6665074, 43165.5871292, -1.1150610, -0.1174187, 4.4906479, 0.8885042 +75.9893125, 89617.5997987, 43165.9609191, -1.2802295, -0.2087243, 4.6261050, 0.9068554 +76.9891719, 89618.5677800, 43166.1357850, -1.5089611, -0.2388138, 4.7615622, 2.4183023 +77.9890313, 89619.4843155, 43166.0972430, -1.7099181, -0.1964340, 5.1101792, 2.7156408 +78.9888906, 89620.4615976, 43165.8683372, -1.8844472, -0.1524398, 5.4823236, 2.3701374 +79.9887500, 89621.4390333, 43165.4695609, -2.0247917, -0.1101501, 5.7926103, 1.3467921 +80.9886094, 89622.2975312, 43164.9985314, -2.1154828, -0.0794027, 5.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 @@ -111,23 +111,23 @@ 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 +111.9842500, 89637.9162800, 43138.8945898, -2.2275794, 0.0682447, 5.7264466, -3.2734659 +112.9841094, 89638.7355039, 43138.3097471, -2.1509776, 0.0864134, 5.2905395, -2.8659207 +113.9839688, 89639.5829971, 43137.8104182, -2.0492791, 0.1224312, 4.8862857, -1.3605057 +114.9838281, 89640.5689589, 43137.3871312, -1.8950286, 0.1691862, 4.6858284, -1.3203170 +115.9836875, 89641.4986619, 43137.1662046, -1.7018411, 0.2403805, 4.4853712, -1.2818200 +116.9835469, 89642.4415024, 43137.1618484, -1.4415569, 0.2845193, 4.2846448, -1.2427545 +117.9834063, 89643.3877107, 43137.4104466, -1.2024510, 0.1881998, 4.0837160, -0.9242547 +118.9832656, 89644.3021803, 43137.8450735, -1.0658363, 0.1105286, 3.9298694, -0.5008503 +119.9831250, 89645.1586707, 43138.3854273, -0.9465888, 0.1314619, 3.8448089, -0.4936140 +120.9829844, 89645.9211053, 43139.0261707, -0.7852909, 0.2044410, 3.7597484, 0.2082658 +121.9828438, 89646.5283750, 43139.7940662, -0.5397369, 0.2829817, 3.7957894, 0.2275484 +122.9827031, 89646.9143361, 43140.7070396, -0.2663789, 0.2474890, 3.8349128, 1.9079864 +123.9825625, 89647.0692671, 43141.6536354, -0.0738346, 0.1544654, 4.1531816, 2.8988652 +124.9824219, 89647.0763560, 43142.6418072, 0.0516792, 0.1064207, 4.6074624, 3.1052652 +125.9822813, 89646.9676221, 43143.6905690, 0.1514136, 0.0861390, 5.0617431, 2.6845565 +126.9821406, 89646.7719587, 43144.6884764, 0.2342048, 0.0767851, 5.4321312, 2.6160727 +127.9820000, 89646.5029465, 43145.6548235, 0.3077224, 0.0702899, 5.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 @@ -166,25 +166,25 @@ 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 +166.9765157, 89622.5275927, 43175.6076493, 0.9610482, -0.0805986, 5.8366605, -2.5477050 +167.9763750, 89621.7480044, 43176.2071139, 0.8628487, -0.1212590, 5.5045689, -1.8718633 +168.9762344, 89621.0445186, 43176.8963110, 0.7234617, -0.1511687, 5.2508864, -1.2510546 +169.9760938, 89620.4432000, 43177.6911831, 0.5720819, -0.1496603, 5.0762699, -1.2205594 +170.9759532, 89619.9682392, 43178.5606181, 0.4305096, -0.1339580, 4.9016535, -1.2280904 +171.9758125, 89619.6190289, 43179.4783467, 0.2933385, -0.1506959, 4.7213827, -1.2013608 +172.9756719, 89619.4070995, 43180.4526180, 0.1312065, -0.1735889, 4.5402295, -0.9452154 +173.9755313, 89619.3663432, 43181.4622918, -0.0526455, -0.1822964, 4.3940939, 0.3156427 +174.9753907, 89619.4993208, 43182.4277035, -0.2169420, -0.1566067, 4.4432626, 0.3180606 +175.9752500, 89619.7899954, 43183.3914534, -0.3664766, -0.1420140, 4.4924314, 0.4398005 +176.9751094, 89620.2226307, 43184.3157714, -0.5081215, -0.1358765, 4.5598127, 0.9457623 +177.9749688, 89620.7639894, 43185.1512607, -0.6411230, -0.1320941, 4.7024175, 0.9661013 +178.9748282, 89621.4150211, 43185.9126599, -0.7740185, -0.1339307, 4.8450224, 1.0796639 +179.9746875, 89622.1614162, 43186.5789649, -0.9111481, -0.1395266, 5.0009539, 2.0148540 +180.9745469, 89622.9782994, 43187.1283668, -1.0449529, -0.1313433, 5.2830280, 2.0944309 +181.9744063, 89623.8694293, 43187.5723917, -1.1701616, -0.1196428, 5.5651021, 2.1740079 +182.9742657, 89624.8277479, 43187.9135026, -1.2852772, -0.1065436, 5.8471762, 0.5303850 +183.9741250, 89625.8271264, 43188.1520326, -1.3845416, -0.0885494, 5.9144675, 0.4241213 +184.9739844, 89626.8369731, 43188.2967922, -1.4715607, -0.0842602, 5.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 @@ -214,51 +214,51 @@ 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 +214.9697657, 89653.8086307, 43185.1819808, -1.0566188, -0.0211692, 5.9102169, -4.8932619 +215.9696250, 89654.6955962, 43185.6674442, -1.0862001, -0.0379105, 5.2654239, -4.4774454 +216.9694844, 89655.5858515, 43186.1111767, -1.1342796, -0.0601786, 4.6206309, -2.3371616 +217.9693438, 89656.5583196, 43186.5228696, -1.2091902, -0.0782896, 4.2577222, -1.3086921 +218.9692032, 89657.5280976, 43186.8419152, -1.3017087, -0.1081263, 4.0450038, -1.3144603 +219.9690625, 89658.4311737, 43187.0342483, -1.4331364, -0.1973613, 3.8235316, -1.5967584 +220.9689219, 89659.4193274, 43187.0443809, -1.7114918, -0.3292005, 3.5426034, -1.2792353 +221.9687813, 89660.3402376, 43186.7758478, -1.9761381, -0.2168737, 3.3068203, -0.4812483 +222.9686407, 89661.1655119, 43186.3218332, -2.1682537, -0.1962455, 3.2153601, -0.4728821 +223.9685000, 89661.9801866, 43185.6337279, -2.3754806, -0.1924939, 3.1238998, 0.2056979 +224.9683594, 89662.6095365, 43184.8477283, -2.5490402, -0.1502335, 3.1638830, 0.3402492 +225.9682188, 89663.1139315, 43183.9653248, -2.6940441, -0.1411011, 3.2293489, 0.3445356 +226.9680782, 89663.4783174, 43183.0367842, -2.8461913, -0.1715229, 3.2948148, 0.8419499 +227.9679375, 89663.6864066, 43182.0002679, -3.0467628, -0.2069518, 3.4514885, 0.8956232 +228.9677969, 89663.6741712, 43181.0219092, 3.0156443, -0.2442462, 3.6133521, 1.1607871 +229.9676563, 89663.4444163, 43180.1110533, 2.7670498, -0.2821409, 3.8164396, 1.5259940 +230.9675157, 89662.9744792, 43179.2692336, 2.5037308, -0.2468862, 4.0730987, 1.6209100 +231.9673750, 89662.2656461, 43178.5101648, 2.2925740, -0.1567606, 4.3343423, 2.0418117 +232.9672344, 89661.5266187, 43177.9342153, 2.1843583, -0.0833895, 4.6488301, 2.1407282 +233.9670938, 89660.6758757, 43177.3780358, 2.1220091, -0.0431483, 4.9633179, 2.2168327 +234.9669532, 89659.7664562, 43176.8409333, 2.0911428, -0.0180613, 5.2746710, 1.9922749 +235.9668125, 89658.8776406, 43176.3424077, 2.0727477, -0.0182370, 5.5435288, 2.0645696 +236.9666719, 89657.9911298, 43175.8665948, 2.0537948, -0.0196710, 5.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 +241.9659688, 89653.4210516, 43173.9985747, 1.8202498, -0.0270210, 5.9283103, -2.6683278 +242.9658282, 89652.4508416, 43173.7564802, 1.8186911, 0.0225670, 5.5843391, -2.2185637 +243.9656875, 89651.4741216, 43173.4898068, 1.8625290, 0.0609603, 5.2859931, -1.2327134 +244.9655469, 89650.5401952, 43173.1735962, 1.9366704, 0.0894321, 5.1148166, -1.2034079 +245.9654063, 89649.6197587, 43172.7671087, 2.0416820, 0.1193522, 4.9436402, -1.1730853 +246.9652657, 89648.7406935, 43172.2436731, 2.1778511, 0.1354947, 4.7726138, -1.1399869 +247.9651250, 89647.9273063, 43171.5958501, 2.3048492, 0.1121405, 4.6021697, -1.1109317 +248.9649844, 89647.2188636, 43170.8820318, 2.4146814, 0.1098222, 4.4317256, -1.0927676 +249.9648438, 89646.6137984, 43170.1189657, 2.5327864, 0.1436583, 4.2595420, -1.1091078 +250.9647032, 89646.0950622, 43169.2111570, 2.7272287, 0.2287952, 4.0797995, -1.0767959 +251.9645625, 89645.8215631, 43168.3248139, 2.9629134, 0.2632378, 3.9000571, 0.8606790 +252.9644219, 89645.7662589, 43167.4008410, -3.0887896, 0.2221929, 4.0441533, 1.0443016 +253.9642813, 89645.9199779, 43166.3904756, -2.9079516, 0.1326110, 4.2145071, 2.2228672 +254.9641407, 89646.2035014, 43165.4397834, -2.8008433, 0.0979760, 4.5624087, 2.9031060 +255.9640000, 89646.5827704, 43164.5162522, -2.7022514, 0.1023214, 4.9907513, 3.0866092 +256.9638594, 89647.0765958, 43163.6024670, -2.5863579, 0.1235147, 5.4190939, 2.2474988 +257.9637188, 89647.6641428, 43162.7800802, -2.4560294, 0.1273259, 5.7160430, 2.0063350 +258.9635782, 89648.3429119, 43162.0487510, -2.3318844, 0.1212709, 5.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 @@ -270,28 +270,28 @@ 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 +270.9618907, 89659.6518184, 43160.0913414, -1.1018669, 0.1741469, 5.7907558, -4.7550427 +271.9617500, 89660.4646780, 43160.5946200, -0.9315115, 0.1693455, 5.1545180, -3.4150065 +272.9616094, 89661.1312878, 43161.1651862, -0.7911339, 0.1851752, 4.6601858, -2.3221857 +273.9614688, 89661.7540129, 43161.8957875, -0.6321464, 0.1203042, 4.3019328, 0.6745839 +274.9613282, 89662.3202971, 43162.7480877, -0.5514743, 0.0581784, 4.4080679, 3.7398680 +275.9611875, 89662.8131289, 43163.6020678, -0.4951216, 0.0593049, 4.9672103, 4.0525521 +276.9610469, 89663.2808843, 43164.5430379, -0.4240119, 0.0795908, 5.5263526, 0.0390734 +277.9609063, 89663.6812320, 43165.5619425, -0.3244693, 0.0758289, 5.5315416, -5.5368962 +278.9607657, 89663.9983800, 43166.5940078, -0.2892184, -0.0231320, 4.7566178, -4.9363048 +279.9606250, 89664.2869826, 43167.4488533, -0.3984714, -0.2772338, 3.9816940, -2.2998458 +280.9604844, 89664.7837572, 43168.1904656, -0.8162421, -0.4859913, 3.5840503, -1.5324764 +281.9603438, 89665.6233343, 43168.7575715, -1.0583019, -0.0437354, 3.3025556, -0.1608180 +282.9602032, 89666.4656037, 43169.2200586, -1.0794523, -0.0272203, 3.2721443, 0.0357915 +283.9600625, 89667.3847058, 43169.6920717, -1.1164692, -0.0460987, 3.2789278, 0.0358375 +284.9599219, 89668.3284009, 43170.1189980, -1.1826503, -0.0890172, 3.2857112, 1.7027767 +285.9597813, 89669.3358189, 43170.4623751, -1.3153542, -0.1643776, 3.5985550, 1.8300032 +286.9596407, 89670.2892308, 43170.6197354, -1.5105321, -0.2338776, 3.9163595, 2.4491592 +287.9595000, 89671.2143974, 43170.5713661, -1.7360717, -0.2381767, 4.3167190, 3.2893507 +288.9593594, 89672.1345636, 43170.3135552, -1.9447726, -0.1943544, 4.8175300, 3.5401976 +289.9592188, 89673.0771841, 43169.8385158, -2.1208459, -0.1402562, 5.3183410, 2.3498479 +290.9590782, 89673.8900199, 43169.2700437, -2.2316416, -0.0851020, 5.6326372, 2.1316309 +291.9589376, 89674.6663349, 43168.6186578, -2.3012759, -0.0557224, 5.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 @@ -325,17 +325,17 @@ 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 +325.9541563, 89663.8180387, 43147.4205799, 1.7442555, 0.0652220, 5.8958775, -2.6580497 +326.9540157, 89662.8316136, 43147.2125712, 1.8148382, 0.0762882, 5.5517898, -2.5396368 +327.9538751, 89661.9004967, 43146.9407488, 1.8983880, 0.0984561, 5.2077021, -1.2478391 +328.9537344, 89660.9125367, 43146.5417532, 2.0146056, 0.1201909, 5.0324706, -1.1603200 +329.9535938, 89660.0473698, 43146.0605634, 2.1467704, 0.1482842, 4.8655174, -0.8675554 +330.9534532, 89659.2985548, 43145.4898060, 2.3031280, 0.1839607, 4.7379865, 0.0151902 +331.9533126, 89658.6331506, 43144.7692404, 2.4886387, 0.1827575, 4.7402402, 0.0151953 +332.9531719, 89658.0915381, 43143.9219930, 2.6476890, 0.1285749, 4.7424939, 2.6104415 +333.9530313, 89657.6836940, 43143.0553096, 2.7500975, 0.0912958, 5.1190851, 3.2575383 +334.9528907, 89657.3405269, 43142.1126528, 2.8319816, 0.0741205, 5.5627729, 3.3328650 +335.9527501, 89657.0591878, 43141.1058887, 2.9049336, 0.0664698, 5.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 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 index a0ea9624..2017f6ed 100644 --- 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 @@ -74,7 +74,7 @@ void CsvToTrajectory::readCsv(const std::string& file_path) { 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]; + const double yaw = values[3] + M_PI/2; point.pose.orientation.x = 0.0; point.pose.orientation.y = 0.0; point.pose.orientation.z = sin(yaw / 2); diff --git a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp index b8f4db96..2f4cd8de 100644 --- a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp @@ -78,11 +78,12 @@ void SimplePurePursuit::onTimer() RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "reached to the goal"); } else { // get closest trajectory point from current position - TrajectoryPoint closet_traj_point = trajectory_->points.at(closet_traj_point_idx); + //TrajectoryPoint closet_traj_point = trajectory_->points.at(closet_traj_point_idx); // calc lateral control //// calc lookahead distance - double lookahead_distance = lookahead_gain_ * closet_traj_point.longitudinal_velocity_mps + lookahead_min_distance_; + // double lookahead_distance = lookahead_gain_ * closet_traj_point.longitudinal_velocity_mps + lookahead_min_distance_; + double lookahead_distance = lookahead_gain_ * odometry_->twist.twist.linear.x + lookahead_min_distance_; //// calc center coordinate of rear wheel double rear_x = odometry_->pose.pose.position.x - wheel_base_ / 2.0 * std::cos(odometry_->pose.pose.orientation.z); @@ -140,7 +141,6 @@ void SimplePurePursuit::onTimer() // calc longitudinal speed and acceleration double target_longitudinal_vel = use_external_target_vel_ ? external_target_vel_ : lookahead_point_itr->longitudinal_velocity_mps; - double current_longitudinal_vel = odometry_->twist.twist.linear.x; // ステアリング角度が大きい場合は減速する if (std::abs(cmd.lateral.steering_tire_angle) > curve_param_max_steer_angle_) { @@ -149,7 +149,7 @@ void SimplePurePursuit::onTimer() cmd.longitudinal.speed = target_longitudinal_vel; cmd.longitudinal.acceleration = - speed_proportional_gain_ * (target_longitudinal_vel - current_longitudinal_vel); + speed_proportional_gain_ * (target_longitudinal_vel - odometry_->twist.twist.linear.x); } diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/debug_sensing.rviz b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/debug_sensing.rviz index 1fe22598..3bda19a0 100644 --- a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/debug_sensing.rviz +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/debug_sensing.rviz @@ -6,22 +6,14 @@ Panels: Expanded: - /System1/TF1 - /System1/Grid1 - - /Map1 - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 - /Sensing1/GNSS1/PoseWithCovariance1 - /Sensing1/Odometry1/Shape1 - /Sensing1/BaseLink1 - /Sensing1/GNSSLink1 - /Sensing1/Sensorkit1 - - /Localization1 - /Localization1/EKF1/PoseHistory1 - - /Planning1 - - /Planning1/ScenarioPlanning1/ScenarioTrajectory1/View Path1 - - /Planning1/ScenarioPlanning1/ScenarioTrajectory1/View Velocity1 - - /Planning1/ScenarioPlanning1/LaneDriving1 - - /Planning1/MPPITrajectory1 - - /Trajectory1 - - /Trajectory1/Topic1 + - /Planning1/MPPITrajectory1/View Path1 Splitter Ratio: 0.557669460773468 Tree Height: 385 - Class: rviz_common/Selection @@ -705,7 +697,7 @@ Visualization Manager: - Class: rviz_common/Group Displays: - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 + Color Border Vel Max: 8 Enabled: true Name: ScenarioTrajectory Topic: @@ -722,14 +714,14 @@ Visualization Manager: Offset from BaseLink: 0 Rear Overhang: 1.0299999713897705 Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 + Vehicle Length: 3 + Vehicle Width: 1 View Path: - Alpha: 0.9990000128746033 + Alpha: 1 Color: 0; 0; 0 Constant Color: false Value: true - Width: 0.30000001192092896 + Width: 1 View Point: Alpha: 1 Color: 0; 60; 255 @@ -1839,6 +1831,47 @@ Visualization Manager: Constant Color: false Scale: 0.30000001192092896 Value: true + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 8.300000190734863 + Enabled: false + Name: Trajectory + Topic: + Depth: 15 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/path + Value: false + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Value: true + Width: 1 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true Enabled: true Name: Planning - Class: rviz_common/Group @@ -1905,47 +1938,6 @@ Visualization Manager: Reliability Policy: Reliable Value: /simple_pure_pursuit_node/debug/markers Value: true - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 8.300000190734863 - Enabled: true - Name: Trajectory - Topic: - Depth: 15 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/path - Value: true - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Value: true - Width: 1 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true Enabled: true Global Options: Background Color: 10; 10; 10 @@ -2046,11 +2038,11 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 14.891090393066406 + Scale: 31.983718872070312 Target Frame: base_link Value: TopDownOrtho (rviz_default_plugins) - X: 0 - Y: 0 + X: -2.4108195304870605 + Y: -1.57545006275177 Saved: - Class: rviz_default_plugins/ThirdPersonFollower Distance: 18 diff --git a/memo b/memo new file mode 100644 index 00000000..d872cef0 --- /dev/null +++ b/memo @@ -0,0 +1,6 @@ +ros2 param set /planning/mppi_controller_node V_MAX 4.0 +ros2 param set /control/simple_pure_pursuit_node curve_param_deceleration_vel 4.0 +ros2 param set /control/simple_pure_pursuit_node curve_param_max_steer_angle 0.2 +ros2 param set /planning/scenario_planning/csv_to_trajectory velocity_coef 1.0 + +rviz2 -d workspace/src/aichallenge_system/aichallenge_system_launch/config/debug_sensing.rviz \ No newline at end of file From e4a3be2afb3966fb210709b61f647ad5bfefbdb3 Mon Sep 17 00:00:00 2001 From: tamago117 Date: Thu, 24 Oct 2024 20:24:12 +0900 Subject: [PATCH 2/3] change rviz setting --- .../planning/costmap_generator.param.yaml | 2 +- .../planning/mppi_controller.param.yaml | 6 +++--- .../launch/components/control.launch.xml | 2 +- .../src/simple_pure_pursuit.cpp | 8 ++++---- .../config/debug_sensing.rviz | 19 ++++++++++++++++--- 5 files changed, 25 insertions(+), 12 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml index 62292907..569506bb 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml @@ -13,7 +13,7 @@ type: "cached_lanelet" map_topic: "/map/vector_map" costmap_topic: "~/debug/cached_costmap" - inflation_radius: 1.8 # [m] + inflation_radius: 1.6 # [m] cached_costmap: min_x: 89607.0 # [m] max_x: 89687.0 # [m] diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml index fb78057c..dca66acb 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/mppi_controller.param.yaml @@ -2,7 +2,7 @@ ros__parameters: # mppi horizon : 25 - num_samples : 2500 + num_samples : 3000 u_min : [-2.0, -0.25] # accel(m/s2), steer angle(rad) u_max : [2.0, 0.25] sigmas : [2.0, 0.25] # sample range @@ -13,9 +13,9 @@ lookahead_distance : 0.1 reference_path_interval : 0.83 # cost weights - Qc : 20.0 + Qc : 10.0 Ql : 1.0 - Qv : 10.0 + Qv : 20.0 Qo : 1000.0 Qin : 0.01 Qdin : 0.0 diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml index cb670343..ae82ed7d 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml @@ -15,7 +15,7 @@ - + diff --git a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp index 2f4cd8de..424051fe 100644 --- a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp @@ -113,14 +113,14 @@ void SimplePurePursuit::onTimer() marker_msg.action = visualization_msgs::msg::Marker::ADD; marker_msg.pose.position.x = lookahead_point_x; marker_msg.pose.position.y = lookahead_point_y; - marker_msg.pose.position.z = 80; + marker_msg.pose.position.z = 0.0; marker_msg.pose.orientation.x = 0.0; marker_msg.pose.orientation.y = 0.0; marker_msg.pose.orientation.z = 0.0; marker_msg.pose.orientation.w = 1.0; - marker_msg.scale.x = 3.0; - marker_msg.scale.y = 3.0; - marker_msg.scale.z = 3.0; + marker_msg.scale.x = 1.5; + marker_msg.scale.y = 1.5; + marker_msg.scale.z = 1.5; marker_msg.color.r = 1.0f; marker_msg.color.g = 0.0f; marker_msg.color.b = 0.0f; diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/debug_sensing.rviz b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/debug_sensing.rviz index 3bda19a0..a2e0c88f 100644 --- a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/debug_sensing.rviz +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/debug_sensing.rviz @@ -1936,7 +1936,20 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /simple_pure_pursuit_node/debug/markers + Value: /control/debug/lookahead_point + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + basic_shapes: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/debug/pursuit_lookahead Value: true Enabled: true Global Options: @@ -2038,7 +2051,7 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 31.983718872070312 + Scale: 19.18048667907715 Target Frame: base_link Value: TopDownOrtho (rviz_default_plugins) X: -2.4108195304870605 @@ -2089,7 +2102,7 @@ Window Geometry: Hide Right Dock: false InitialPoseButtonPanel: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Tool Properties: From 71f9c50e19c37a7b0064d6e21fc0f23ee6c99b08 Mon Sep 17 00:00:00 2001 From: tamago117 Date: Fri, 25 Oct 2024 03:22:26 +0900 Subject: [PATCH 3/3] fix bit --- .../launch/components/control.launch.xml | 10 ++-- .../booars_launch/map/csv/traj_race_cl.csv | 56 +++++++++---------- .../src/simple_pure_pursuit.cpp | 2 +- 3 files changed, 35 insertions(+), 33 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml index ae82ed7d..19af9f8b 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml @@ -7,8 +7,10 @@ - - + + + + @@ -18,8 +20,8 @@ - - + + 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 index a8dc2642..fe0ca27d 100644 --- 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 @@ -319,32 +319,32 @@ 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, 5.8958775, -2.6580497 -326.9540157, 89662.8316136, 43147.2125712, 1.8148382, 0.0762882, 5.5517898, -2.5396368 -327.9538751, 89661.9004967, 43146.9407488, 1.8983880, 0.0984561, 5.2077021, -1.2478391 -328.9537344, 89660.9125367, 43146.5417532, 2.0146056, 0.1201909, 5.0324706, -1.1603200 -329.9535938, 89660.0473698, 43146.0605634, 2.1467704, 0.1482842, 4.8655174, -0.8675554 -330.9534532, 89659.2985548, 43145.4898060, 2.3031280, 0.1839607, 4.7379865, 0.0151902 -331.9533126, 89658.6331506, 43144.7692404, 2.4886387, 0.1827575, 4.7402402, 0.0151953 -332.9531719, 89658.0915381, 43143.9219930, 2.6476890, 0.1285749, 4.7424939, 2.6104415 -333.9530313, 89657.6836940, 43143.0553096, 2.7500975, 0.0912958, 5.1190851, 3.2575383 -334.9528907, 89657.3405269, 43142.1126528, 2.8319816, 0.0741205, 5.5627729, 3.3328650 -335.9527501, 89657.0591878, 43141.1058887, 2.9049336, 0.0664698, 5.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 +319.9550001, 89669.8492647, 43147.9059624, 1.7279369, -0.0718879, 4.3000002, 0.0000002 +320.9548594, 89668.8677931, 43147.7844303, 1.6621362, -0.0594515, 4.3000002, -0.0000018 +321.9547188, 89667.8477743, 43147.7189888, 1.6115983, -0.0323566, 4.3000000, -0.0000075 +322.9545782, 89666.8870461, 43147.6878232, 1.6012438, 0.0078553, 4.2999991, -0.0000075 +323.9544376, 89665.8702204, 43147.6471983, 1.6253914, 0.0385325, 4.2999982, -0.4965420 +324.9542969, 89664.8285192, 43147.5640542, 1.6800015, 0.0618168, 3.2399651, -2.7764627 +325.9541563, 89663.8180387, 43147.4205799, 1.7442555, 0.0652220, 3.8958775, -2.6580497 +326.9540157, 89662.8316136, 43147.2125712, 1.8148382, 0.0762882, 3.5517898, -2.5396368 +327.9538751, 89661.9004967, 43146.9407488, 1.8983880, 0.0984561, 3.2077021, -1.2478391 +328.9537344, 89660.9125367, 43146.5417532, 2.0146056, 0.1201909, 3.0324706, -1.1603200 +329.9535938, 89660.0473698, 43146.0605634, 2.1467704, 0.1482842, 2.8655174, -0.8675554 +330.9534532, 89659.2985548, 43145.4898060, 2.3031280, 0.1839607, 2.7379865, 0.0151902 +331.9533126, 89658.6331506, 43144.7692404, 2.4886387, 0.1827575, 2.7402402, 0.0151953 +332.9531719, 89658.0915381, 43143.9219930, 2.6476890, 0.1285749, 2.7424939, 2.6104415 +333.9530313, 89657.6836940, 43143.0553096, 2.7500975, 0.0912958, 3.1190851, 3.2575383 +334.9528907, 89657.3405269, 43142.1126528, 2.8319816, 0.0741205, 3.5627729, 3.3328650 +335.9527501, 89657.0591878, 43141.1058887, 2.9049336, 0.0664698, 3.9912656, 0.8277498 +336.9526094, 89656.8575800, 43140.1384001, 2.9658322, 0.0571485, 3.0941703, 0.8383407 +337.9524688, 89656.7093917, 43139.1473195, 3.0192909, 0.0498528, 3.1970750, 0.8489315 +338.9523282, 89656.6104514, 43138.1424047, 3.0666705, 0.0442394, 3.2999797, 0.0001660 +339.9521876, 89656.5559492, 43137.1435252, 3.1057862, 0.0339869, 3.2999997, 0.0000020 +340.9520469, 89656.5354507, 43136.1433162, 3.1347380, 0.0239193, 3.2999999, 0.0000020 +341.9519063, 89656.5388818, 43135.1444283, -3.1295233, 0.0139746, 4.3000002, -0.0000002 +342.9517657, 89656.5556990, 43134.1362332, -3.1229445, -0.0014044, 4.3000001, -0.0000004 +343.9516251, 89656.5709672, 43133.1343800, -3.1325104, -0.0178663, 4.3000001, -0.0000004 +344.9514844, 89656.5683763, 43132.1434946, 3.1243702, -0.0354989, 4.3000001, -0.0000000 +345.9513438, 89656.5303615, 43131.1236043, 3.0828151, -0.0451562, 4.3000001, 0.0000000 +346.9512032, 89656.4470347, 43130.1235739, 3.0322801, -0.0560909, 4.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/simple_pure_pursuit/src/simple_pure_pursuit.cpp b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp index 424051fe..553a9886 100644 --- a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp @@ -156,7 +156,7 @@ void SimplePurePursuit::onTimer() pub_cmd_->publish(cmd); // publish real-machine command - cmd.lateral.steering_tire_angle /= steering_tire_angle_gain_; + //cmd.lateral.steering_tire_angle /= steering_tire_angle_gain_; pub_raw_cmd_->publish(cmd); }