Merge pull request #150 from una-auxme/106-feature-extended-kalman-fi…

…lter-for-more-precise-location-measurements

106 feature extended kalman filter for more precise location measurements

.flake8

@@ -1,3 +1,5 @@
 [flake8]
 exclude= code/planning/behavior_agent/src/behavior_agent/behavior_tree.py,
-         code/planning/behavior_agent/src/behavior_agent/behaviours/__init__.py
+         code/planning/behavior_agent/src/behavior_agent/behaviours/__init__.py,
+         code/planning/src/behavior_agent/behavior_tree.py,
+         code/planning/src/behavior_agent/behaviours/__init__.py

build/docker-compose.yml

@@ -63,7 +63,6 @@ services:
     #command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/opt/leaderboard/leaderboard/autoagents/npc_agent.py --host=carla-simulator --track=SENSORS"
     #command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch"
     command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=carla-simulator --track=MAP"
-
     logging:
       driver: "local"
     environment:

code/acting/launch/acting.launch

@@ -29,10 +29,10 @@
         <param name="role_name" value="$(arg role_name)" />
     </node>
 
-    <!--node pkg="acting" type="Acting_DebuggerNode.py" name="Acting_Debugger" output="screen">
+    <!-- <node pkg="acting" type="Acting_DebuggerNode.py" name="Acting_Debugger" output="screen">
         <param name="control_loop_rate" value="0.05" />
         <param name="role_name" value="$(arg role_name)" />
-    </node-->
+    </node> -->
 
     <node pkg="acting" type="MainFramePublisher.py" name="MainFramePublisher" output="screen">
         <param name="control_loop_rate" value="0.05" />

code/acting/src/acting/Test_Data/EmergencyBreaks/data_163.csv

@@ -0,0 +1,9 @@
+26.981568377,984.2717165869615,-5448.485194551819,8.417115265892138
+27.031568378,984.2792978342279,-5448.873475258132,8.619125878345864
+27.081568378,984.2735361090026,-5449.161074787122,7.959971041195068
+27.131568379,984.2581308173458,-5449.549872551304,6.811253635596549
+27.18156838,984.2628199611521,-5449.863462370065,5.54839077601463
+27.231568381,984.2574266575106,-5450.27355683064,4.218688283576547
+27.281568381,984.1285128579091,-5450.717472808519,2.8618739700625686
+27.331568382,984.1522863982133,-5451.065071808009,1.4981709512937293
+27.381568383,984.0214572275615,-5451.424143213324,0.23036178039557187

code/acting/src/acting/Test_Data/EmergencyBreaks/data_164.csv

@@ -0,0 +1,4 @@
+27.919097566,984.5907760045518,-5442.637786124205,2.4023866272343857
+27.969097567,984.6297488374014,-5442.828314882379,2.441572425190621
+28.019097568,984.5860139850143,-5442.967454002059,1.523014260639284
+28.069097569,984.5045482266069,-5443.048108780194,0.23518438266291816

code/acting/src/acting/Test_Data/EmergencyBreaks/data_165.csv

@@ -0,0 +1,6 @@
+28.343297006,984.3303220273465,-5446.18445725235,5.587330838903736
+28.393297007,984.3496799520339,-5446.46472197028,5.923707170032014
+28.443297008,984.4161284320851,-5446.617017853547,5.327457608754818
+28.493297008,984.4033707912206,-5446.956345052893,3.9773150246975857
+28.543297009,984.3434573096177,-5447.2756730668225,2.621979748121838
+28.59329701,984.3473215158685,-5447.499988929676,1.2768072059537472

code/acting/src/acting/Test_Data/EmergencyBreaks/data_166.csv

@@ -0,0 +1,11 @@
+28.749856719,984.1712770425149,-5453.133524958701,11.461792210469415
+28.79985672,984.1075362768577,-5453.582626897223,11.567725538499538
+28.849856721,984.1166067238639,-5454.202713453926,10.899249276844666
+28.899856721,984.3036117596566,-5454.765534869517,9.741898137251859
+28.949856722,984.2757075096051,-5455.237651433255,8.482967333363248
+28.999856723,984.4833719737189,-5455.777828552064,7.158032586191067
+29.049856724,984.5931570057013,-5456.235156611839,5.813047685154365
+29.099856724,984.6567161809662,-5456.622275950971,4.4514906917563435
+29.149856725,984.8709000658648,-5456.9896322044215,3.0719345530968405
+29.199856726,984.8975495532666,-5457.427498858877,1.6925814367901548
+29.249856727,984.9841321194835,-5457.752909041925,0.3552234813675058

code/acting/src/acting/Test_Data/EmergencyBreaks/data_167.csv

@@ -0,0 +1,13 @@
+29.260238919,984.8724327337006,-5457.95220759107,14.51719421409275
+29.31023892,985.1517472338335,-5458.55789392279,14.692727301003442
+29.36023892,985.2587861255322,-5459.285360186912,14.004082677600758
+29.410238921,985.439576455421,-5459.9526649012205,12.876344831240313
+29.460238922,985.6645498570354,-5460.594564755004,11.622049914170198
+29.510238923,985.863132156185,-5461.284601154003,10.309557122233636
+29.560238923,985.9664829631196,-5461.876969140037,8.975671943873676
+29.610238924,986.2014177045672,-5462.6357670909765,7.627177486359203
+29.660238925,986.1818367750896,-5463.158926345137,6.262908300241682
+29.710238926,986.2313284468919,-5463.65483956209,4.891301435404886
+29.760238926,986.3351798495357,-5464.211215121675,3.5193893356667107
+29.810238927,986.3401577710523,-5464.754961150278,2.140060077660462
+29.860238928,986.3848863520783,-5464.986284160806,0.7605522045620094

code/acting/src/acting/Test_Data/EmergencyBreaks/data_168.csv

@@ -0,0 +1,14 @@
+28.50194643,986.1698991498795,-5462.280640138209,15.443508506090048
+28.551946431,986.1878731520426,-5462.947695877082,15.475470785727046
+28.601946432,986.28008943322,-5463.636626672141,14.764133479341682
+28.651946432,986.4299491366452,-5464.43738699696,13.630258258853646
+28.701946433,986.4822537183179,-5465.277812713691,12.362720416996947
+28.751946434,986.5734849848908,-5465.856645489968,11.042089993679664
+28.801946435,986.5189095426471,-5466.553299929926,9.695100197039432
+28.851946435,986.5362776719703,-5467.373300744202,8.328904391234909
+28.901946436,986.547531761873,-5468.0635452855695,6.949620931817815
+28.951946437,986.5675153585873,-5468.722548896473,5.56729335302051
+29.001946438,986.3915792848481,-5469.185514964522,4.190993722888829
+29.051946438,986.6127212598519,-5469.679277090343,2.8007994927738995
+29.101946439,986.6651426853908,-5470.119225648421,1.4175989126461008
+29.15194644,986.6416609006621,-5470.448968485687,0.11462092878534418

code/acting/src/acting/Test_Data/EmergencyBreaks/data_169.csv

@@ -0,0 +1,15 @@
+39.848709245,988.5884704621461,-5466.13653011856,17.588450918232162
+39.898709245,988.5241977539602,-5465.305470057078,17.794014559530716
+39.948709246,988.5864280143751,-5464.423706537313,17.126474112422592
+39.998709247,988.5264790552395,-5463.459107210811,16.00859205783204
+40.048709248,988.6620137220482,-5462.5642386043855,14.753305647696493
+40.098709248,988.8086021352887,-5461.8850930666285,13.436842075852454
+40.148709249,988.9144072636033,-5460.9612568621515,12.101530984041089
+40.19870925,988.7778214408406,-5460.130555791658,10.752066292579478
+40.248709251,988.8496726097001,-5459.2443358605,9.387690951327716
+40.298709251,988.9943586079705,-5458.504765009478,8.012752168819702
+40.348709252,989.0032098016361,-5457.891292133314,6.6289815384286195
+40.398709253,989.1281252727892,-5457.215809368402,5.2421578556284
+40.448709254,989.1271816699948,-5456.601425238764,3.8659155341296314
+40.498709254,989.1721031684323,-5456.187606560714,2.4806029141778523
+40.548709255,989.1378249843021,-5455.900412081551,1.095123249803875

code/acting/src/acting/Test_Data/EmergencyBreaks/data_170.csv

@@ -0,0 +1,17 @@
+30.604615502,975.3876779242216,-5486.125669495421,20.40898440326724
+30.654615503,975.3071410023374,-5487.232175543362,20.55668141329046
+30.704615504,975.2215027390369,-5488.159997212479,19.873299095514174
+30.754615504,975.3218579658954,-5489.2479443440125,18.758563504252216
+30.804615505,975.3880912252307,-5490.328881957482,17.51218114274025
+30.854615506000002,975.4629747277249,-5491.282896751434,16.209350499935212
+30.904615506,975.5841709755193,-5492.257539565526,14.888403187747006
+30.954615507,975.5835315139315,-5493.25194098295,13.55052550513003
+31.004615508,975.6508656486906,-5494.075518904299,12.193871975966868
+31.054615509,975.5496021661726,-5494.850429704119,10.822596578206527
+31.104615509,975.593133409359,-5495.686611794357,9.436865906033168
+31.15461551,975.6603573887342,-5496.300888227886,8.049284261558302
+31.204615511,975.6388529523658,-5497.113060180707,6.660726431438673
+31.254615512,975.4914122308843,-5497.599595958284,5.273973778025009
+31.304615512,975.4623768009333,-5498.04676273403,3.944498919407128
+31.354615513,975.4220396421682,-5498.570634291627,2.5400818008153485
+31.404615514,975.3075315056342,-5498.955946458818,1.1433731751478977

code/acting/src/acting/Test_Data/EmergencyBreaks/data_171.csv

@@ -0,0 +1,20 @@
+31.343974094,975.1589734922039,-5507.594921272756,23.41305511148101
+31.393974095,974.9789193802459,-5508.627243405508,23.564218015546555
+31.443974095,974.9548326222866,-5509.8774548230795,22.88683742621307
+31.493974096,974.9863654413299,-5510.906279571604,21.777352654035315
+31.543974097,975.0813604839889,-5512.050207669057,20.528695321033986
+31.593974098,975.0657491547847,-5513.285573027628,19.221661145877007
+31.643974098,975.074541963585,-5514.385229616691,17.888252386311105
+31.693974099,975.1465961148546,-5515.448332206605,16.537679165625377
+31.7439741,975.1299932639662,-5516.411896162779,15.16513502752404
+31.793974101,975.0592353741528,-5517.4981181103085,13.778472464032067
+31.843974101,974.9838693115556,-5518.382300617908,12.386966639019402
+31.893974102,974.9845091791979,-5519.324061248352,10.996490840566748
+31.943974103,974.9742079228108,-5520.056783537454,9.608298635309522
+31.993974104,974.8751604928766,-5520.996968082695,8.255759041654061
+32.043974104,974.8031517133668,-5521.739743847953,6.955002403080414
+32.093974105,974.76103125238,-5522.290330090848,5.552074083002885
+32.143974106,974.7163373911778,-5522.80123306336,4.227842404001307
+32.193974107,974.6929637274185,-5523.380270168709,2.8374770682465127
+32.243974107,974.6666605927463,-5523.850096043085,1.4742053420292451
+32.293974108,974.699796728902,-5524.1955704691145,0.16253506312797675

code/acting/src/acting/Test_Data/EmergencyBreaks/data_172.csv

@@ -0,0 +1,22 @@
+32.826638912,975.4030085789585,-5535.240638708415,26.4657536355697
+32.876638913,975.3429575461653,-5536.523987870161,26.60201529646476
+32.926638914,975.3950082780705,-5537.961441279362,25.928985808928342
+32.976638915,975.3012768344897,-5539.276963000697,24.810452219025798
+33.026638915,975.184556818616,-5540.478883147377,23.573023975931605
+33.076638916,975.0846622765566,-5541.682302973952,22.25565416184524
+33.126638917,975.1416771142924,-5542.830002941896,20.93520389268133
+33.176638917,975.1915366454423,-5543.951510322053,19.58590617173647
+33.226638918,975.1328683551603,-5545.03310437439,18.213062363827877
+33.276638919,975.0855152143513,-5546.139316217877,16.821989321518608
+33.32663892,975.1362091651987,-5547.3783447938495,15.427257315225424
+33.37663892,975.1615331670944,-5548.449991649648,14.032409286446452
+33.426638921,975.0531420732856,-5549.251272107738,12.633933209051966
+33.476638922,975.0234698278557,-5550.216319430053,11.346380402292986
+33.526638923,975.0327662073772,-5551.069767204186,9.980565982458653
+33.576638923,975.1011283699096,-5551.850891947582,8.613135824207326
+33.626638924,974.9667556334831,-5552.636439864188,7.261492723190242
+33.676638925,974.9163660226726,-5553.414932181464,5.923602112448675
+33.726638926,974.8918656990141,-5553.987507542026,4.5961073274738515
+33.776638926,974.7276905964245,-5554.459997420486,3.216148170962101
+33.826638927,974.809785055875,-5554.8323678387005,1.8680042322572596
+33.876638928,974.873849845268,-5555.196989712757,0.5226280227461744

code/acting/src/acting/Test_Data/EmergencyBreaks/data_173.csv

@@ -0,0 +1,26 @@
+32.931542424,975.5205266375558,-5576.2964072632785,29.501551315879727
+32.981542424,975.432613785377,-5577.699230297547,29.613875989293334
+33.031542425,975.424599362869,-5579.2403485434,28.91054504245132
+33.081542426,975.448235664065,-5580.652228865889,27.699112158342583
+33.131542426,975.2954552066458,-5582.005734626734,27.005613583701574
+33.181542427,975.4489148569144,-5583.433820739485,26.268290291590187
+33.231542428,975.5508432656168,-5584.960246533184,25.60488136539823
+33.281542429,975.5526543252648,-5586.348307655464,24.902309743218222
+33.331542429,975.5951850330932,-5587.663665667693,24.039107373849905
+33.38154243,975.5921404255147,-5589.077481347743,22.985417617044664
+33.431542431,975.6094684111542,-5590.356750042915,21.64723137510353
+33.481542432,975.5878081064711,-5591.6077292146765,20.21784702009804
+33.531542432,975.508360758345,-5592.745506430992,18.750410025965454
+33.581542433,975.511947905639,-5593.873234062043,17.25809620909478
+33.631542433999996,975.5622741079658,-5594.860482023955,15.796764191441829
+33.681542435,975.3663600455706,-5595.884431604731,14.340872025578271
+33.731542435,975.2914385633069,-5596.799758679682,12.96097953224908
+33.781542436,975.2736881644851,-5597.744041596381,11.653990851742376
+33.831542437,975.3015182419889,-5598.6494597933,10.266812695749003
+33.881542438,975.2482775884807,-5599.420576109833,8.795994421657193
+33.931542438,975.1864650564006,-5600.153009721024,7.435163448443343
+33.981542439,975.2334046891077,-5600.775993181181,6.034839857468698
+34.03154244,975.0986750298835,-5601.326015124316,4.663573090712611
+34.081542441,975.1630671416435,-5601.804343232823,3.3065880513585593
+34.131542441,975.1634643225277,-5602.319455595705,1.9582086274688442
+34.181542442,975.2468849724355,-5602.709462788366,0.791764863117708

code/acting/src/acting/vehicle_controller.py

@@ -210,7 +210,7 @@ def __map_steering(self, steering_angle: float) -> float:
         :param steering_angle: calculated by a controller in [-pi/2 , pi/2]
         :return: float for steering in [-1, 1]
         """
-        tune_k = 1  # factor for tuning TODO: tune but why?
+        tune_k = -1  # factor for tuning TODO: tune but why?
         # negative because carla steer and our steering controllers are flipped
         r = 1 / (math.pi / 2)
         steering_float = steering_angle * r * tune_k

code/perception/launch/perception.launch

@@ -7,33 +7,40 @@
   <node pkg="perception" type="sensor_filter_debug.py" name="sensor_filter_debug" output="screen">
     <param name="control_loop_rate" value="0.001" />
     <param name="role_name" value="$(arg role_name)" />
-  </node>
-  <node pkg="rqt_plot" type="rqt_plot" output="screen" name="rqt_plot" args="/paf/hero/location_error/data[0] /paf/hero/location_error/data[1]" />
+  </node> -->
+  <!--node pkg="rqt_plot" type="rqt_plot" output="screen" name="rqt_plot" args="/paf/hero/location_error/data[0] /paf/hero/location_error/data[1]" />
   <node pkg="rqt_plot" type="rqt_plot" output="screen" name="x_plot" args="/paf/hero/ideal_x/data[0] /paf/hero/ideal_x/data[1]" />
   <node pkg="rqt_plot" type="rqt_plot" output="screen" name="y_plot" args="/paf/hero/ideal_y/data[0] /paf/hero/ideal_y/data[1]" /-->
-  <!--node pkg="rqt_plot" type="rqt_plot" output="screen" name="error_plot" args="/paf/hero/ideal_x/data[2]" />
-  -->
-
-  <!-- does not publish data yet. Uncomment to use and see plots
+  <!--node pkg="rqt_plot" type="rqt_plot" output="screen" name="error_plot" args="/paf/hero/ideal_x/data[2]" /-->
+
   <node pkg="perception" type="kalman_filter.py" name="kalman_filter_node" output="screen">
-    <param name="control_loop_rate" value="0.1" />
+    <param name="control_loop_rate" value="0.05" />
     <param name="role_name" value="$(arg role_name)" />
   </node>
-  <node pkg="rqt_plot" type="rqt_plot" output="screen" name="x_plot" args="/paf/hero/current_pos/data[0] /paf/hero/kalman_pos/data[0]" />
-  <node pkg="rqt_plot" type="rqt_plot" output="screen" name="y_plot" args="/paf/hero/current_pos/data[1] /paf/hero/kalman_pos/data[1]" />
-  <node pkg="rqt_plot" type="rqt_plot" output="screen" name="heading_plot" args="/paf/hero/current_heading/data /paf/hero/kalman_heading/data" />
-  -->
-
+
   <node pkg="perception" type="Position_Publisher_Node.py" name="Position_Publisher_Node" output="screen">
     <param name="control_loop_rate" value="0.1" />
     <param name="role_name" value="$(arg role_name)" />
+    <param name="pos_filter" value="Kalman" />
+    <param name="heading_filter" value="Kalman" />
+      <!--
+        Position Filter values:
+        - "Kalman" (Default)
+        - "RunningAvg"
+        - "None"
+        Heading Filter values:
+        - "Kalman" (Default)
+        - "None"
+        - "Old" (Buggy for demonstration purposes only)
+      -->
   </node>
 
   <node pkg="perception" type="vision_node.py" name="VisionNode" output="screen">
     <param name="role_name" value="$(arg role_name)" />
     <param name="side" value="Center" />
-
-     <!-- Object-Detection: 
+
+    <!--
+      Object-Detection: 
       - fasterrcnn_resnet50_fpn_v2 
       - fasterrcnn_mobilenet_v3_large_320_fpn
       - yolov8n
@@ -51,9 +58,8 @@
 
       Image-Segmentation:
       - deeplabv3_resnet101
-      - yolov8x-seg
+      - yolov8x-seg 
     -->
-
     <param name="model" value="rtdetr-x" />
   </node>
 
@@ -62,7 +68,7 @@
     <param name="side" value="Center" />
     <param name="model" value="/workspace/code/perception/src/traffic_light_detection/models/model_acc_92.48_val_91.88.pt" />
   </node>
-      
+
 
   <node pkg="perception" type="global_plan_distance_publisher.py" name="GlobalPlanDistance" output="screen">
     <param name="control_loop_rate" value="0.1" />
@@ -79,7 +85,7 @@
     <param name="range_topic" value="/carla/hero/LIDAR_range"/>
   </node>
 
- <!-- <node pkg="perception" type="lidar_distance.py" name="lidar_distance_rear_right" output="screen">
+ <node pkg="perception" type="lidar_distance.py" name="lidar_distance_rear_right" output="screen">
     <param name="min_y" value="-5"/>
     <param name="max_y" value="-2.5"/>
     <param name="max_x" value="0"/>
@@ -97,6 +103,6 @@
     <param name="max_z" value="0"/>
     <param name="point_cloud_topic" value="/carla/hero/LIDAR_filtered_rear_left"/>
     <param name="range_topic" value="/carla/hero/LIDAR_range_rear_left"/>
-  </node>-->
+  </node>
 
 </launch>