Lidar

src/high_level/scripts/Live_display_lidar.py

@@ -1,15 +1,15 @@
 import time
-from actionneur_capteur.lidar import Lidar
+from drivers.lidar import Lidar
 
 
-IP = '192.168.0.20'
+IP = "192.168.0.20"
 PORT = 10940
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     sensor = Lidar(IP, PORT)
     sensor.stop()
     # sensor.singleRead(0, 1080)
     time.sleep(2)
 
-    sensor.startContinuous(0, 1080)
-    sensor.startPlotter()
+    sensor.start_continuous(0, 1080)
+    sensor.start_plotter()

src/high_level/scripts/launch_serveur.py

@@ -20,7 +20,7 @@
     log_serveur = logging.getLogger("src.HL")
     log_serveur.setLevel(level=logging.DEBUG)
 
-    log_lidar = logging.getLogger("src.HL.actionneur_capteur.Lidar")
+    log_lidar = logging.getLogger("src.HL.driver.Lidar")
     log_lidar.setLevel(level=logging.INFO)
 
     boot = Server()

src/high_level/scripts/live_display_lidar.py

@@ -1,6 +1,6 @@
 import time
 
-from actionneur_capteur import Lidar
+from drivers import Lidar
 
 IP = "192.168.0.10"
 PORT = 10940

src/high_level/scripts/min_lidar.py

@@ -5,7 +5,7 @@
 
 import numpy as np
 
-from src.actionneur_capteur.lidar import Lidar
+from src.driver.lidar import Lidar
 
 IP = "192.168.0.10"
 PORT = 10940

src/high_level/scripts/onetime_lidar.py

@@ -1,4 +1,4 @@
-from actionneur_capteur import Lidar
+from drivers import Lidar
 
 IP = "192.168.0.10"
 PORT = 10940

src/high_level/scripts/simple.py

@@ -2,7 +2,7 @@
 
 from rpi_hardware_pwm import HardwarePWM
 
-from actionneur_capteur import Lidar
+from drivers import Lidar
 
 IP = "192.168.0.10"
 PORT = 10940

src/high_level/src/high_level/autotech_constant.py

@@ -3,69 +3,81 @@
 import logging
 
 # Car control
-MAX_IA_SPEED = 1000 # maximum speed for ia in centimeter per second
-MIN_IA_SPEED = -500 # minimum speed for ia in centimeter per second
-MAX_CONTROL_SPEED = 2 # maximum speed for controling devices
-MIN_CONTROL_SPEED = -2 # minimum speed for controlig devices
-MAX_ANGLE = 18 # angle between the two extrem position
-
-
-#I2C
-I2C_NUMBER_DATA_RECEIVED = 3 # the number of info data sent by the arduino
-I2C_SLEEP_RECEIVED = 0.1 # the time between two demand of info data to the arduino
-I2C_SLEEP_ERROR_LOOP = 1 # In seconds its the time bettween two try of i2C if an error occurd
-SLAVE_ADDRESS = 0x08 # Adresse of the arduino i2c port
-
-#Remote control
-PORT_REMOTE_CONTROL = 5556 # Port to send data for remote control on <IP>:PORT_REMOTE_CONTROL
-
-#Camera
-PORT_STREAMING_CAMERA = 8889 # adresse where to see the stream of the camera if activate is <IP>:PORT_STREAMIN_CAMERA/STREAM_PATH/cam
+MAX_IA_SPEED = 1000  # maximum speed for ia in centimeter per second
+MIN_IA_SPEED = -500  # minimum speed for ia in centimeter per second
+MAX_CONTROL_SPEED = 2  # maximum speed for controling devices
+MIN_CONTROL_SPEED = -2  # minimum speed for controlig devices
+MAX_ANGLE = 18  # angle between the two extrem position
+
+
+# I2C
+I2C_NUMBER_DATA_RECEIVED = 3  # the number of info data sent by the arduino
+I2C_SLEEP_RECEIVED = 0.1  # the time between two demand of info data to the arduino
+I2C_SLEEP_ERROR_LOOP = (
+    1  # In seconds its the time bettween two try of i2C if an error occurd
+)
+SLAVE_ADDRESS = 0x08  # Adresse of the arduino i2c port
+
+# Remote control
+PORT_REMOTE_CONTROL = (
+    5556  # Port to send data for remote control on <IP>:PORT_REMOTE_CONTROL
+)
+
+# Camera
+PORT_STREAMING_CAMERA = 8889  # adresse where to see the stream of the camera if activate is <IP>:PORT_STREAMIN_CAMERA/STREAM_PATH/cam
 STREAM_PATH = "map"
 SIZE_CAMERA_X = 1280
 SIZE_CAMERA_Y = 720
-FRAME_RATE = 30 # frame rate of the camera
-CAMERA_QUALITY = 10 # the more the better but slow the speed of the stream (10 its passable)
+FRAME_RATE = 30  # frame rate of the camera
+CAMERA_QUALITY = (
+    10  # the more the better but slow the speed of the stream (10 its passable)
+)
 
 
-#Car
+# Car
 CRASH_DIST = 110
-REAR_BACKUP_DIST = 100  #mm Distance at which the car will NOT reverse due to the obstacle behind it
+REAR_BACKUP_DIST = (
+    100  # mm Distance at which the car will NOT reverse due to the obstacle behind it
+)
 
-#Lidar
+# Lidar
 LIDAR_DATA_AMPLITUDE = 1
 LIDAR_DATA_SIGMA = 45
 LIDAR_DATA_OFFSET = 0.5
 
 # Screen car
 TEXT_HEIGHT = 11
-TEXT_LEFT_OFFSET = 3 # Offset from the left of the screen to ensure no cuttoff
-
+TEXT_LEFT_OFFSET = 3  # Offset from the left of the screen to ensure no cuttoff
 
 
 script_dir = os.path.dirname(os.path.abspath(__file__))
 MODEL_PATH = "/home/intech/CoVAPSy/src/high_level/models/"  # 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)
 
 
-SOCKET_ADRESS = {
-    "IP": '192.168.0.10',
-    "PORT": 10940
-}
+SOCKET_ADRESS = {"IP": "192.168.0.10", "PORT": 10940}
 
 ANGLE_LOOKUP = np.linspace(-MAX_ANGLE, MAX_ANGLE, 16)
 SPEED_LOOKUP = np.linspace(0, MAX_IA_SPEED, 16)
 
-Temperature = 0.7 # Temperature parameter for softmax function, used to control the sharpness of the distribution resols around 1
+Temperature = 0.7  # 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.
 
 
-LOGGING_LEVEL = logging.DEBUG # can be either NOTSET, DEBUG, INFO, WARNING, ERROR, CRITICAL
+LOGGING_LEVEL = (
+    logging.DEBUG
+)  # can be either NOTSET, DEBUG, INFO, WARNING, ERROR, CRITICAL
 
-#Startup
-CAMERA_STREAM_ON_START = True #If True the camera stream will start at the launch of the car
-BACKEND_ON_START = True #If True the backend will start at the launch of the car
-LIDAR_STREAM_ON_START = True #If True the lidar stream will start at the launch of the car
+# Startup
+CAMERA_STREAM_ON_START = (
+    True  # If True the camera stream will start at the launch of the car
+)
+BACKEND_ON_START = True  # If True the backend will start at the launch of the car
+LIDAR_STREAM_ON_START = (
+    True  # If True the lidar stream will start at the launch of the car
+)
+LIMIT_CRASH_POINT = 10  # number of lidar points that must be under the crash border distance to consider that the car is in a crash situation,
+FREQUENCY_CRASH_DETECTION = 0.1  # in seconds, the time between two crash detection
 
-#Backend
-SITE_DIR_BACKEND = "/home/intech/CoVAPSy/src/high_level/src/site_controle" # the directory where the backend will look for the site to serve
+# Backend
+SITE_DIR_BACKEND = "/home/intech/CoVAPSy/src/high_level/src/site_controle"  # the directory where the backend will look for the site to serve

src/high_level/src/high_level/backend.py

@@ -117,7 +117,6 @@ def _get_telemetry(self) -> Dict[str, Any]:
 
         target_speed = float(getattr(self.server, "target_speed", 0.0))
         direction = float(getattr(self.server, "direction", 0.0))
-
         return {
             "battery": {"lipo": voltage_lipo, "nimh": voltage_nimh},
             "car": {
@@ -127,6 +126,7 @@ def _get_telemetry(self) -> Dict[str, Any]:
                 "car_control": prog_name,
                 "program_id": last_ctrl,
                 "tof": self.server.tof.distance,
+                "crashed": self.server.crash_car.crashed,
             },
             "timestamp": time.time(),
         }

src/high_level/src/high_level/server.py

@@ -9,12 +9,12 @@
 from luma.oled.device import ssd1306
 from PIL import Image, ImageDraw, ImageFont
 
-from actionneur_capteur.camera import Camera
-from actionneur_capteur.lidar import Lidar
-from actionneur_capteur.master_i2c import I2CArduino
-from actionneur_capteur.tof import ToF
+from drivers.camera import Camera
+from drivers.lidar import Lidar
+from drivers.master_i2c import I2CArduino
+from drivers.tof import ToF
 from high_level.autotech_constant import SITE_DIR_BACKEND, TEXT_HEIGHT
-from programs.car import AIProgram
+from programs.car import AIProgram, CrashCar
 from programs.initialization import Initialization
 from programs.poweroff import Poweroff
 from programs.ps4_controller_program import PS4ControllerProgram
@@ -56,7 +56,7 @@ def __init__(self):
         self.temp = None
 
         self.initialization_module = Initialization(self)
-
+        self.crash_car = CrashCar(self)
         self.programs = [
             SshProgram(),
             self.initialization_module,
@@ -75,19 +75,19 @@ def __init__(self):
         self.scroll_offset = 3
 
     @property
-    def camera(self) -> Camera:
+    def camera(self) -> Camera | None:
         return self.initialization_module.camera
 
     @property
-    def lidar(self) -> Lidar:
+    def lidar(self) -> Lidar | None:
         return self.initialization_module.lidar
 
     @property
-    def tof(self) -> ToF:
+    def tof(self) -> ToF | None:
         return self.initialization_module.tof
 
     @property
-    def arduino_I2C(self) -> I2CArduino:
+    def arduino_I2C(self) -> I2CArduino | None:
         return self.initialization_module.arduino_I2C
 
     @property

src/high_level/src/programs/__init__.py

@@ -18,4 +18,5 @@
     "StreamHandler",
     "StreamOutput",
     "frame_buffer",
+    "CrashCar",
 ]

src/high_level/src/programs/car.py

@@ -7,25 +7,80 @@
 import numpy as np
 import onnxruntime as ort
 
-from actionneur_capteur import Lidar
-from actionneur_capteur.camera import Camera
-from actionneur_capteur.tof import ToF
+from drivers import Lidar
+from drivers.camera import Camera
+from drivers.tof import ToF
 
 # Import constants from HL.Autotech_constant to share them between files and ease of use
 from high_level.autotech_constant import (
-    CRASH_DIST,
     LIDAR_DATA_AMPLITUDE,
     LIDAR_DATA_OFFSET,
     LIDAR_DATA_SIGMA,
     MAX_ANGLE,
     MODEL_PATH,
-    REAR_BACKUP_DIST,
+    LIMIT_CRASH_POINT,
+    FREQUENCY_CRASH_DETECTION,
 )
 
 from .program import Program
 from .utils import Driver
 
 
+class Border_zone:
+    ZONE1 = [0, 370]
+    ZONE2 = [371, 750]
+    ZONE3 = [751, 1080]
+
+
+class CrashCar:
+    def __init__(self, serveur) -> None:
+        self.log = logging.getLogger(__name__)
+        self.serveur = serveur
+        self.crashed = False
+        # Load reference lidar contour once
+        try:
+            self.reference_lidar = np.load(
+                "/home/intech/CoVAPSy/src/high_level/src/programs/data/min_lidar.npy"
+            )[
+                :1080
+            ]  # Load only the first 1080 values to match the lidar data used by the AI
+            self.log.info("Reference lidar contour loaded")
+            Thread(target=self.has_Crashed, daemon=True).start()
+        except Exception as e:
+            self.log.error(f"Unable to load reference lidar contour: {e}")
+            raise
+
+    def has_Crashed(self) -> bool:
+
+        while self.serveur.lidar is None:
+            self.log.debug("Lidar not yet ready for crash detection")
+            time.sleep(0.1)
+        while True:
+            current = self.serveur.lidar.r_distance[:1080]
+
+            if current is None or len(current) != len(self.reference_lidar):
+                self.log.warning(
+                    str(len(current) if current is not None else "None")
+                    + " lidar points received, expected "
+                    + str(len(self.reference_lidar))
+                )
+                self.crashed = False
+            else:
+                # Points that are inside the vehicle contour
+                penetration_mask = (current > 0) & (current < self.reference_lidar)
+
+                penetration_count = np.sum(penetration_mask)
+
+                self.log.debug(f"Penetration points: {penetration_count}")
+
+                if penetration_count >= LIMIT_CRASH_POINT:
+                    self.log.info("Crash detected via contour penetration")
+                    self.crashed = True
+                else:
+                    self.crashed = False
+            time.sleep(FREQUENCY_CRASH_DETECTION)  # time between two crash detection
+
+
 class Car:
     def __init__(self, driving_strategy, serveur, model) -> None:
         self.log = logging.getLogger(__name__)
@@ -64,21 +119,6 @@ def stop(self) -> None:
         self.direction = 0
         self.log.info("Motor stop")
 
-    def has_Crashed(self) -> bool:
-
-        small_distances = [
-            d for d in self.lidar.r_distance[200:880] if 0 < d < CRASH_DIST
-        ]  # 360 to 720 is the front of the car. 1/3 of the fov of the lidar
-        self.log.debug(f"Distances: {small_distances}")
-        if len(small_distances) > 2:
-            # min_index = self.lidar.rDistance.index(min(small_distances))
-            while self.tof.distance < REAR_BACKUP_DIST:
-                self.log.info(f"Rear obstacle detected {self.tof.distance}")
-                self.target_speed = 0
-                time.sleep(0.1)
-            return True
-        return False
-
     def turn_around(self) -> None:
         """Turn the car around."""
         self.log.info("Turning around")
@@ -117,9 +157,9 @@ def main(self) -> None:
             self.turn_around()
             self.reverse_count = 0
 
-        if self.has_Crashed():
+        if self.serveur.crash_car.crashed:
             self.log.info("Obstacle detected")
-            color= self.camera.is_green_or_red(lidar_data)
+            color = self.camera.is_green_or_red(lidar_data)
             if color == 0:
                 small_distances = [
                     d for d in self.lidar.r_distance if 0 < d < CRASH_DIST

src/high_level/src/programs/initialization.py

@@ -2,7 +2,7 @@
 import threading
 from enum import Enum
 
-from actionneur_capteur import Camera, I2CArduino, Lidar, ToF
+from drivers import Camera, I2CArduino, Lidar, ToF
 from high_level.autotech_constant import SOCKET_ADRESS
 
 from .program import Program
@@ -23,6 +23,11 @@ def __init__(self, server) -> None:
         self.lidar_init = ProgramState.INITIALIZATION
         self.tof_init = ProgramState.INITIALIZATION
 
+        self.arduino_I2C = None
+        self.camera = None
+        self.lidar = None
+        self.tof = None
+
         threading.Thread(target=self.init_camera, daemon=True).start()
         threading.Thread(target=self.init_lidar, daemon=True).start()
         threading.Thread(target=self.init_tof, daemon=True).start()