Merge branch 'main' into 155-feature-tune-pure-pursuit-steering-algor…

…ithm

.flake8

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

.gitignore

@@ -11,3 +11,5 @@ code/output
 
 # Byte-compiled / optimized / DLL files
 __pycache__/
+
+*.tsv

build/docker-compose.yml

@@ -26,6 +26,10 @@ services:
     # image: carlasim/carla:0.9.14
     image: ghcr.io/una-auxme/paf23:leaderboard-2.0
     init: true
+    deploy:
+      resources:
+        limits:
+          memory: 16G
     expose:
       - 2000
       - 2001
@@ -57,9 +61,8 @@ services:
     tty: true
     shm_size: 2gb
     #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 && 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:

build/docker/agent/Dockerfile

@@ -21,7 +21,8 @@ ARG DEBIAN_FRONTEND=noninteractive
 RUN apt-get update \
   && apt-get install -y -qq --no-install-recommends \
     libxext6 libx11-6 libglvnd0 libgl1 \
-    libglx0 libegl1 freeglut3-dev
+    libglx0 libegl1 freeglut3-dev apt-utils \
+    fprintd libfprint-2-2 libpam-fprintd
 
 # install dependencies for libgit2 and Carla PythonAPI
 RUN apt-get install wget unzip
@@ -34,34 +35,29 @@ RUN wget https://github.com/una-auxme/paf23/releases/download/v0.0.1/PythonAPI_L
   && mkdir -p /opt/carla \
   && mv PythonAPI /opt/carla/PythonAPI
 
-# build libgit2
-RUN wget https://github.com/libgit2/libgit2/archive/refs/tags/v1.5.0.tar.gz -O libgit2-1.5.0.tar.gz \
-    && tar xzf libgit2-1.5.0.tar.gz \
-    && cd libgit2-1.5.0/ \
-    && cmake . \
-    && make -j$(nproc --ignore=2) \
-    && make install \
-    && cd ../ \
-    && rm libgit2-1.5.0.tar.gz \
-    && rm -r libgit2-1.5.0/
+# Workaround/fix for using dpkg for cuda installation
+# Only required for the lab PCs
+RUN rm -f /var/lib/dpkg/info/fprintd.postinst \
+    && rm -f /var/lib/dpkg/info/libfprint*.postinst \
+    && rm -f /var/lib/dpkg/info/libpam-fprintd*.postinst \
+    && dpkg --configure -a
 
 # CUDA installation
 RUN wget https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/cuda-ubuntu2004.pin \
     && mv cuda-ubuntu2004.pin /etc/apt/preferences.d/cuda-repository-pin-600 \
-    && wget https://developer.download.nvidia.com/compute/cuda/11.7.0/local_installers/cuda-repo-ubuntu2004-11-7-local_11.7.0-515.43.04-1_amd64.deb \
-    && dpkg -i cuda-repo-ubuntu2004-11-7-local_11.7.0-515.43.04-1_amd64.deb \
-    && cp /var/cuda-repo-ubuntu2004-11-7-local/cuda-*-keyring.gpg /usr/share/keyrings/ \
+    && wget https://developer.download.nvidia.com/compute/cuda/11.8.0/local_installers/cuda-repo-ubuntu2004-11-8-local_11.8.0-520.61.05-1_amd64.deb \
+    && dpkg -i cuda-repo-ubuntu2004-11-8-local_11.8.0-520.61.05-1_amd64.deb \
+    && cp /var/cuda-repo-ubuntu2004-11-8-local/cuda-*-keyring.gpg /usr/share/keyrings/ \
     && apt-get update \
-    && DEBIAN_FRONTEND=noninteractive apt-get -y install cuda-nvcc-11-7 \
-    && rm cuda-repo-ubuntu2004-11-7-local_11.7.0-515.43.04-1_amd64.deb \
-    && apt-get -y remove cuda-repo-ubuntu2004-11-7-local \
-    && cat /etc/apt/sources.list \
+    && DEBIAN_FRONTEND=noninteractive apt-get -y install cuda \
+    && rm cuda-repo-ubuntu2004-11-8-local_11.8.0-520.61.05-1_amd64.deb \
+    && apt-get -y remove cuda-repo-ubuntu2004-11-8-local \
     && rm /etc/apt/preferences.d/cuda-repository-pin-600 \
-    && rm -rf /var/cuda-repo-ubuntu1804-11-7-local/ \
-    && rm /etc/apt/sources.list.d/cuda-ubuntu2004-11-7-local.list \
+    && rm -rf /var/cuda-repo-ubuntu1804-11-8-local/ \
+    && rm /etc/apt/sources.list.d/cuda-ubuntu2004-11-8-local.list \
     && apt-get clean
 
-ENV CUDA_HOME=/usr/local/cuda-11.7
+ENV CUDA_HOME=/usr/local/cuda-11.8
 
 # override python path, carla pip package path didn't exist and was using Python 3.7 instead of 2.7
 ENV PYTHONPATH=/opt/ros/noetic/lib/python3/dist-packages
@@ -158,7 +154,7 @@ RUN echo "export CUDA_HOME=/usr/local/cuda-11" >> ~/.bashrc \
     && echo "export PATH=$PATH:$CUDA_HOME/bin" >> ~/.bashrc
 
 COPY --chown=$USERNAME:$USERNAME ./code/requirements.txt /workspace/
-ENV TORCH_CUDA_ARCH_LIST="5.2 6.0 6.1 7.0 7.5 8.0 8.6+PTX"
+ENV TORCH_CUDA_ARCH_LIST="5.2 6.0 6.1 7.0 7.5 8.0 8.6 8.7 8.9+PTX"
 ENV IABN_FORCE_CUDA=1
 
 RUN source ~/.bashrc && pip install -r /workspace/requirements.txt

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

@@ -209,7 +209,6 @@ 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]
         """
-        # No tuning needed tune_k = 1 factor for tuning TODO: tune but why?
         r = - 1 / (math.pi / 2)  # -1 to invert for carla steering
         steering_float = steering_angle * r  # No Tuning needed * tune_k
         self.pidpoint_publisher.publish(steering_float)  # TODO needed?

code/agent/launch/agent.launch

@@ -7,7 +7,7 @@
     </include>
 
     <!-- planning -->
-    <include file="$(find planning_runner)/launch/planning_runner.launch">
+    <include file="$(find planning)/launch/planning.launch">
     </include>
 
     <include file="$(find mock)/launch/mock.launch">

code/agent/launch/agent_manual.launch

@@ -0,0 +1,31 @@
+<launch>
+  <arg name="role_name" default="hero" />
+  <arg name="control_loop_rate" default="0.05" />
+
+  <!-- perception -->
+  <include file="$(find perception)/launch/perception.launch">
+  </include>
+
+  <!--
+  <include file="$(find planning_runner)/launch/planning_runner.launch">
+  </include>
+
+  <include file="$(find mock)/launch/mock.launch">
+      <arg name="control_loop_rate" value="$(arg control_loop_rate)"/>
+      <arg name="role_name" value="$(arg role_name)"/>
+  </include>
+
+  <include file="$(find acting)/launch/acting.launch">
+      <arg name="control_loop_rate" value="$(arg control_loop_rate)"/>
+      <arg name="role_name" value="$(arg role_name)"/>
+  </include>
+  -->
+
+  <node pkg="carla_manual_control" type="carla_manual_control.py" name="carla_manual_control_$(arg role_name)" output="screen">
+    <param name="role_name" value="$(arg role_name)"/>
+  </node>
+
+
+  <node pkg="rqt_gui" type="rqt_gui" name="rqt_gui"/>
+  <node type="rviz" name="rviz" pkg="rviz" args="-d $(find agent)/config/rviz_config.rviz" />
+</launch>

code/perception/CMakeLists.txt

@@ -52,6 +52,7 @@ add_message_files(
   FILES
   Waypoint.msg
   LaneChange.msg
+  TrafficLightState.msg
   MinDistance.msg
 )