Adjust speed limits, add lidar data parameters, and refine obstacle detection logic

Author: Danura30082

src/HL/Autotech_constant.py

@@ -1,15 +1,19 @@
 import os
 import numpy as np
 
-MAX_SOFT_SPEED = 1
+MAX_SOFT_SPEED = 0.5
 MIN_SOFT_SPEED = 0.1
 MAX_ANGLE = 18
 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
+LIDAR_DATA_SIGMA = 45
+LIDAR_DATA_OFFSET = 0.5
 
 script_dir = os.path.dirname(os.path.abspath(__file__))
 MODEL_PATH = os.path.join(script_dir, "model_CNN1D.onnx")  # Allows the model to be loaded from the same directory as the script regardless of the current working directory (aka where the script is run from)
 
+
 PWM_PROP = {
     "direction_prop": 1,
     "pwm_stop_prop": 7.37,
@@ -32,6 +36,6 @@
 ANGLE_LOOKUP = np.linspace(-MAX_ANGLE, MAX_ANGLE, 16)
 SPEED_LOOKUP = np.linspace(MIN_SOFT_SPEED, MAX_SOFT_SPEED, 16)
 
-Temperature = 1  # Temperature parameter for softmax function, used to control the sharpness of the distribution resols around 1
+Temperature = 0.6  # Temperature parameter for softmax function, used to control the sharpness of the distribution resols around 1
 # the higher the temperature the more unprobalbe actions become probable, the lower the temperature the more probable actions become probable.
 # In our case Higher temperature means less agressive driving and lower temperature means more aggressive driving.

src/HL/Camera.py

@@ -5,6 +5,7 @@
 import logging as log
 import threading
 import shutil
+import scipy as sp
 
 N_IMAGES = 100  # Number of images to capture
 SAVE_DIR = "Captured_Frames"  # Directory to save frames
@@ -152,16 +153,18 @@ def recreate_image_from_matrix(self, image, matrix, adjusted_width, vector_size=
         self.debug_counter += 1
         return Image.fromarray(combined_image).convert("RGB")
 
-    def is_green_or_red(self):
+    def is_green_or_red(self,lidar):
         """
         Check if the car is facing a green or red wall by analyzing the bottom half of the image.
         """
         image = self.get_last_image()
         height, _, _ = image.shape
         bottom_half = image[height // 2:, :, :]  # Slice the bottom half of the image
-
-        red_intensity = np.mean(bottom_half[:, :, 0])  # Red channel in RGB
-        green_intensity = np.mean(bottom_half[:, :, 1])  # Green channel in RGB
+        lidar= np.max(sp.ndimage.zoom(lidar[595:855], image.shape[1]/len(lidar[595:855]),mode="nearest")[None,:],0) # Resize lidar data to match the image size
+        print((lidar < 0.5).sum())
+        print(f"min lidar: {lidar.min()}, max lidar: {lidar.max()}")
+        red_intensity = np.mean(bottom_half[:, :, 0]*(lidar < 0.5))  # Red channel in RGB
+        green_intensity = np.mean(bottom_half[:, :, 1]*(lidar < 0.5))  # Green channel in RGB
 
         if green_intensity > red_intensity + COLOR_THRESHOLD:
             return COLOUR_KEY["green"]

src/HL/Car.py

@@ -8,7 +8,7 @@
 
 
 # Import constants from HL.Autotech_constant to share them between files and ease of use
-from Autotech_constant import MAX_SOFT_SPEED, MAX_ANGLE, CRASH_DIST, MODEL_PATH, PWM_DIR, PWM_PROP, SOCKET_ADRESS, REAR_BACKUP_DIST
+from Autotech_constant import MAX_SOFT_SPEED, MAX_ANGLE, CRASH_DIST, MODEL_PATH, PWM_DIR, PWM_PROP, SOCKET_ADRESS, REAR_BACKUP_DIST,  LIDAR_DATA_SIGMA, LIDAR_DATA_AMPLITUDE, LIDAR_DATA_OFFSET
 from Driver import Driver
 from Lidar import Lidar
 from Camera import Camera
@@ -149,7 +149,7 @@ def recule(self,angle,duration=0.5):
         time.sleep(0.2)
         self.set_vitesse_m_s(0)
         time.sleep(0.2)
-        self.set_vitesse_m_s(-2)
+        self.set_vitesse_m_s(-4)
         time.sleep(duration+0.3)
         if angle != 0:
             self.set_direction_degre(-angle)
@@ -169,7 +169,7 @@ def stop(self):
 
     def has_Crashed(self):
 
-        small_distances = [d for d in self.lidar.rDistance[300:780] if 0 < d < CRASH_DIST] # 360 to 720 is the front of the car. 1/3 of the fov of the lidar
+        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
         log.debug(f"Distances: {small_distances}")
         if len(small_distances) > 2:
             # min_index = self.lidar.rDistance.index(min(small_distances))
@@ -196,20 +196,33 @@ def turn_around(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]/1000
-        angle, vitesse = self.driving(lidar_data) #l'ai prend des distance en mètre et non en mm
+        lidar_data = (self.lidar.rDistance[:1080]/1000)
+        lidar_data_ai= (lidar_data-0.5)*(
+            LIDAR_DATA_OFFSET + LIDAR_DATA_AMPLITUDE * np.exp(-1/2*((np.arange(1080) - 135) / LIDAR_DATA_SIGMA**2))
+        ) #convertir en mètre et ajouter un bruit gaussien
+        angle, vitesse = self.driving(lidar_data_ai) #l'ai prend des distance en mètre et non en mm
         log.debug(f"Min Lidar: {min(lidar_data)}, Max Lidar: {max(lidar_data)}")
         self.set_direction_degre(angle)
         self.set_vitesse_m_s(vitesse)
         if self.camera.is_running_in_reversed():
             self.reverse_count += 1
         else:
             self.reverse_count = 0
-        if self.reverse_count > 5:
+        if self.reverse_count > 2:
             self.turn_around()
             self.reverse_count = 0
         if self.has_Crashed():
-            color= self.camera.is_green_or_red()
+            print("Obstacle détecté")
+            color= self.camera.is_green_or_red(lidar_data)
+            if color == 0:
+                small_distances = [d for d in self.lidar.rDistance if 0 < d < CRASH_DIST]
+                if len(small_distances) == 0:
+                    log.info("Aucun obstacle détecté")
+                    return
+                min_index = np.argmin(small_distances)
+                direction = MAX_ANGLE if min_index < 540 else -MAX_ANGLE #540 is the middle of the lidar
+                color = direction/direction
+                log.info("Obstacle détecté, Lidar Fallback")
             if color == -1:
                 log.info("Obstacle rouge détecté")
             if color == 1: