Merge branch 'wrapper3'

Author: Danura30082

src/HL/Autotech_constant.py

@@ -3,7 +3,7 @@
 
 MAX_SOFT_SPEED = 0.5
 MIN_SOFT_SPEED = 0.1
-MAX_ANGLE = 18
+MAX_ANGLE = 14
 CRASH_DIST = 110
 REAR_BACKUP_DIST = 100  #mm Distance at which the car will NOT reverse due to the obstacle behind it
 LIDAR_DATA_AMPLITUDE = 1

src/HL/Car.py

@@ -15,9 +15,9 @@
 from ToF import ToF
 
 class Car:
-    def __init__(self, driving_strategy=Driver().farthest_distants):
+    def __init__(self, driver):
         """Initialize the car's components."""
-        
+
         def _initialize_speed_limits():
             """Set the car's speed limits."""
             self.vitesse_max_m_s_hard = 8  # Maximum hardware speed
@@ -70,7 +70,7 @@ def _initialize_lidar():
             except Exception as e:
                 log.error(f"Error initializing Lidar: {e}")
                 raise
-        
+
         def _initialize_camera():
             """Initialize the camera."""
             try:
@@ -103,7 +103,7 @@ def _initialize_tof():
 
         # Initialize Lidar
         _initialize_lidar()
-        
+
         _initialize_camera()
 
         _initialize_tof()
@@ -117,25 +117,25 @@ def _initialize_tof():
     def set_vitesse_m_s(self, vitesse_m_s):
         """Set the car's speed in meters per second."""
         # Clamp the speed to the maximum and minimum speed
-        vitesse_m_s = max(-self.vitesse_max_m_s_hard, min(vitesse_m_s, self.vitesse_max_m_s_soft)) 
+        vitesse_m_s = max(-self.vitesse_max_m_s_hard, min(vitesse_m_s, self.vitesse_max_m_s_soft))
         vitesse_pwm = vitesse_m_s * (self.delta_pwm_max_prop)/self.vitesse_max_m_s_hard
-        
+
         if vitesse_m_s == 0:
             pwm = self.pwm_stop_prop
         elif vitesse_m_s > 0:
             pwm= self.pwm_stop_prop + self.direction_prop*(self.point_mort_prop + vitesse_pwm)
-            
+
         elif vitesse_m_s < 0:
             pwm= self.pwm_stop_prop - self.direction_prop*(self.point_mort_prop - vitesse_pwm)
-            
+
         self.pwm_prop.change_duty_cycle(pwm)
         # log.debug(f"Vitesse: {vitesse_m_s} m/s, PWM: {pwm}")
 
 
     def set_direction_degre(self, angle_degre):
         """Set the car's steering angle in degrees."""
         angle_pwm = self.angle_pwm_centre + self.direction * ((self.angle_pwm_max - self.angle_pwm_min) * angle_degre / (2 * MAX_ANGLE))
-        
+
         # Clamp the angle to the maximum and minimum angle
         angle_pwm = max(self.angle_pwm_min, min(angle_pwm, self.angle_pwm_max))
         # log.debug(f"Angle: {angle_degre}°, PWM: {angle_pwm}")
@@ -166,7 +166,7 @@ def stop(self):
         log.info("Arrêt du moteur")
         self.lidar.stop()
         # exit() #not to be used in prodution/library? https://www.geeksforgeeks.org/python-exit-commands-quit-exit-sys-exit-and-os-_exit/
-    
+
     def has_Crashed(self):
 
         small_distances = [d for d in self.lidar.rDistance[200:880] if 0 < d < CRASH_DIST] # 360 to 720 is the front of the car. 1/3 of the fov of the lidar
@@ -179,7 +179,7 @@ def has_Crashed(self):
                 time.sleep(0.1)
             return True
         return False
-    
+
     def turn_around(self):
         """Turn the car around."""
         log.info("Turning around")
@@ -241,7 +241,7 @@ def main(self):
     bp2 = Button("GPIO6")
     try:
         Schumacher = Driver(128, 128)
-        GR86 = Car(Schumacher.ai)
+        GR86 = Car(Schumacher)
         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:
             log.info("Depart")
@@ -252,9 +252,9 @@ def main(self):
     except KeyboardInterrupt:
         GR86.stop()
         log.info("Le programme a été arrêté par l'utilisateur")
-    
+
     except Exception as e: # catch all exceptions to stop the car
         GR86.stop()
         log.error("Erreur inconnue")
         raise e # re-raise the exception to see the error message
-    
+    

src/HL/Driver.py

@@ -1,18 +1,55 @@
 import math
+from matplotlib import pyplot as plt
 import scipy as sp
 from scipy.special import softmax
 import numpy as np
 import onnxruntime as ort
+import logging as log
 
 from Autotech_constant import SPEED_LOOKUP, ANGLE_LOOKUP, MODEL_PATH, Temperature
 
 
 
 class Driver:
     def __init__(self, context_size=0, horizontal_size=0):
+        self.context_size = context_size
+        self.horizontal_size = horizontal_size
         self.ai_session = ort.InferenceSession(MODEL_PATH)
         self.context = np.zeros([2, context_size, horizontal_size], dtype=np.float32)
 
+        if log.getLogger().isEnabledFor(log.DEBUG):
+            self.fig, self.ax = plt.subplots(4, 1, figsize=(10, 8))
+            self.steering_bars = self.ax[0].bar(range(16), np.zeros(16), color='blue')
+            self.steering_avg = [
+                self.ax[0].plot([0, 0], [0,  1], color=(i/3, 1 - i/3, 0), label='Average')[0]
+                for i in range(4)
+            ]
+            self.ax[0].set_ylim(0, 1) # Probabilities range from 0 to 1
+            self.ax[0].set_title('Steering Action Probabilities')
+
+            # Speed bars
+            self.speed_bars = self.ax[1].bar(range(16), np.zeros(16), color='blue')
+            self.speed_avg = self.ax[1].plot([0, 0], [0,  1], color='red', label='Average')[0]
+            self.ax[1].set_ylim(0, 1)  # Probabilities range from 0 to 1
+            self.ax[1].set_title('Speed Action Probabilities')
+
+            # LiDAR img
+            self.lidar_img = self.ax[2].imshow(
+                np.zeros((128, 128)),
+                cmap='gray', vmin=0, vmax=np.log(31)
+            )
+            self.ax[2].set_title('LiDAR Image')
+
+            # Camera img
+            self.camera_img = self.ax[3].imshow(
+                np.zeros((128, 128, 3)),
+                cmap='RdYlGn', vmin=-1, vmax=1
+            )
+            self.ax[3].set_title('Camera Image')
+
+    def reset(self):
+        self.context = np.zeros([2, self.context_size, self.horizontal_size], dtype=np.float32)
+
     def omniscent(self, lidar_data, camera_data):
         return self.ai_update_lidar_camera(lidar_data, camera_data)
 
@@ -23,10 +60,12 @@ def simple_minded(self, lidar_data):
         return self.farthest_distants(lidar_data)
 
     def ai_update_lidar_camera(self, lidar_data, camera_data):
+        log.info(f"MIN MAX lidar_data: {(min(lidar_data), max(lidar_data))}")
+
         lidar_data = sp.ndimage.zoom(
             np.array(lidar_data, dtype=np.float32),
             128/len(lidar_data)
-        )
+        ) / 1000 * 0.8
         camera_data = sp.ndimage.zoom(
             np.array(camera_data, dtype=np.float32),
             128/len(camera_data)
@@ -38,15 +77,40 @@ def ai_update_lidar_camera(self, lidar_data, camera_data):
         ], 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 = self.ai_session.run(None, {'input': self.context[None]})[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
+        if log.getLogger().isEnabledFor(log.DEBUG):
+            log.info(f"MIN MAX lidar_data: {(min(lidar_data), max(lidar_data))}")
+            self.lidar_img.set_array(np.log(1 + self.context[0]))
+            self.camera_img.set_array(self.context[1])
+
+            for i, bar in enumerate(self.steering_bars):
+                bar.set_height(vect_dir[i].item())
+
+            for i in range(4):
+                steering_avg = (vect_dir / vect_dir.sum() * np.arange(16)).sum().item()
+                self.steering_avg[i].set_xdata([steering_avg, steering_avg])
+
+            for i, bar in enumerate(self.speed_bars):
+                bar.set_height(vect_dir[i].item())
+
+            speed_avg = (vect_dir * np.arange(16)).sum().item()
+            self.speed_avg.set_xdata([speed_avg, speed_avg])
+
+            plt.draw()
+            plt.pause(1e-8)
+
+
+        print(" ".join([f"{x:.1f}" for x in vect_dir]))
+        print(" ".join([f"{x:.1f}" for x in vect_prop]), flush=True)
+
+        angle = sum(ANGLE_LOOKUP*vect_dir)  # moyenne pondérée des angles
         # moyenne pondérée des vitesses
-        vitesse = sum(ANGLE_LOOKUP*vect_prop)
+        vitesse = sum(SPEED_LOOKUP*vect_prop)
 
         return angle, vitesse