added camera and temporal data support

Author: MatthiasBienvenu

src/HL/Car.py

@@ -142,8 +142,10 @@ def has_Crashed(self):
     def main(self):
         # récupération des données du lidar. On ne prend que les 1080 premières valeurs et on ignore la dernière par facilit" pour l'ia
 
-        lidar_data = self.lidar.rDistance[:1080]
-        angle, vitesse = self.driving(lidar_data)
+        lidar_data = self.lidar.rDistance
+        raise NotImplementedError("Camera data is not implemented")
+        camera_data = ...
+        angle, vitesse = self.driving(lidar_data, camera_data)
         self.set_direction_degre(angle)
         self.set_vitesse_m_s(vitesse)
         if self.has_Crashed():
@@ -155,7 +157,7 @@ def main(self):
     log.basicConfig(level=log.INFO)
     bp2 = Button("GPIO6")
     try:
-        Schumacher = Driver()
+        Schumacher = Driver(128, 128)
         GR86 = Car(Schumacher.ai)
         log.info("Initialisation terminée")
         if input("Appuyez sur D pour démarrer ou tout autre touche pour quitter") in ("D", "d") or bp2.is_pressed:

src/HL/Driver.py

@@ -1,4 +1,5 @@
 import math
+import scipy as sp
 from scipy.special import softmax
 import numpy as np
 import onnxruntime as ort
@@ -8,19 +9,49 @@
 
 
 class Driver:
-    def __init__(self):
+    def __init__(self, context_size, horizontal_size):
         self.ai_session = ort.InferenceSession(MODEL_PATH)
-        
+        self.context = np.zeros([2, context_size, horizontal_size], dtype=np.float32)
+
     def omniscent(self, lidar_data, camera_data):
         pass
-        
-    def ai(self, lidar_data):
-        return self.ai_update_lidar(lidar_data)
-    
+
+    def ai(self, lidar_data, camera_data):
+        return self.ai_update_lidar_camera(lidar_data, camera_data)
+
     def simple_minded(self, lidar_data):
         return self.farthest_distants(lidar_data)
-    
+
+    def ai_update_lidar_camera(self, lidar_data, camera_data):
+        lidar_data = sp.ndimage.zoom(
+            np.array(lidar_data, dtype=np.float32),
+            128/len(lidar_data)
+        )
+        camera_data = sp.ndimage.zoom(
+            np.array(camera_data, dtype=np.float32),
+            128/len(camera_data)
+        )
+
+        self.context = np.concatenate([
+            self.context[:, 1:],
+            [lidar_data[None], camera_data[None]]
+        ], axis=1)
+
+        # 2 vectors direction and speed. direction is between hard left at index 0 and hard right at index 1. speed is between min speed at index 0 and max speed at index 1
+        vect = self.ai_session.run(None, {'input': self.context})[0][0]
+
+        vect_dir, vect_prop = vect[:16], vect[16:]  # split the vector in 2
+        vect_dir = softmax(vect_dir)  # distribution de probabilité
+        vect_prop = softmax(vect_prop)
+
+        angle = sum(SPEED_LOOKUP*vect_dir)  # moyenne pondérée des angles
+        # moyenne pondérée des vitesses
+        vitesse = sum(ANGLE_LOOKUP*vect_prop)
+
+        return angle, vitesse
+
     def ai_update_lidar(self, lidar_data):
+        raise NotImplementedError("This method doesn't work anymore")
         lidar_data = np.array(lidar_data, dtype=np.float32)
         # 2 vectors direction and speed. direction is between hard left at index 0 and hard right at index 1. speed is between min speed at index 0 and max speed at index 1
         vect = self.ai_session.run(None, {'input': lidar_data[None]})[0][0]
@@ -88,5 +119,5 @@ def farthest_distants(self, lidar_data):
                 print("error calculating angle:", e)
         else:
             steering_angle = 0
-        
-        return steering_angle, speed
+
+        return steering_angle, speed