new_thea/Control/Pi/proximity_detector.py

import serial
import logging
import time
import os
import psutil
from threading import Thread

class ProxStream:
    def __init__(self):
        self.log = logging.getLogger("PROXIMITY ARRAY LISTENER")
        self.log.setLevel("DEBUG")
        self.serial_string = '/dev/proximity'
        try:
            self.ser = serial.Serial(self.serial_string, 115200, timeout=1)
            self.alive = True
            self.log.info('Proximity serial open')
            self.ser.flushInput()
        except Exception as e:
            self.log.critical(f"Cannot open serial device, {e}")
            self.log.critical('Exiting')
            current_system_pid = os.getpid()
            ThisSystem = psutil.Process(current_system_pid)
            ThisSystem.terminate()
        self.proximity_buffer = [99, 99, 99, 99, 99, 99, 99, 99, 99, 99, 99]

    def start(self):
        t = Thread(target=self.update, args=())
        t.daemon = True
        self.running = True
        t.start()
        return self

    def update(self):
        self.ser.flushInput()
        while self.running:
            try:
                p1 = p2 = p3 = p4 = p5 = p6 = p7 = p8 = p9 = p10 = p11 = 0
                for _ in range(1):
                    raw = str(self.ser.readline())
                    raw2 = raw.split("**")[1].split("##")[0].split("|")
                    raw3 = raw2
                    p1 += int(raw3[0])
                    p2 += int(raw3[1])
                    p3 += int(raw3[2])
                    p4 += int(raw3[3])
                    p5 += int(raw3[4])
                    p6 += int(raw3[5])
                    p7 += int(raw3[6])
                    p8 += int(raw3[7])
                    p9 += int(raw3[8])
                    p10 += int(raw3[9])
                    p11 += int(raw3[10])
                    self.proximity_buffer = [int(p1/2), int(p2/2), int(p3/2), int(p4/2), int(p5/2), int(p6/2), int(p7/2), int(p8/2), int(p9/2), int(p10/2), int(p11/2)]
            except Exception as e:
                self.log.critical(f"ERROR: {e}, serial input: {raw}")

    def read(self):
        print(self.proximity_buffer)
        return self.proximity_buffer

    def stop(self):
        self.running = False


class Proximity:
    def __init__(self, state_machine, vehicle_ctrl) -> None:
        self.log = logging.getLogger("PROXIMITY ARRAY")
        self.log.setLevel("DEBUG")
        self.serial_string = '/dev/proximity'
        self.alive = False
        self.proximity_stopped = False
        self.limit_cm = 30
        self.failed_reads = 0
        self.failed_reads_thresh = 4
        # self.limit_cm = 5
        self.state_machine = state_machine
        self.vehicle_ctrl = vehicle_ctrl
        self.rear_sensors = [4, 5, 6, 7, 8, 9, 10]  # TODO: change with more sensors
        self.proximity_detects = 0
        self.proximity_detect_thresh = 1
        self.proximity_stream = ProxStream().start()
        self.proximities = [99, 99, 99, 99, 99, 99, 99, 99, 99, 99, 99]

    def get_prox_readings(self):
        self.proximities = self.proximity_stream.read()
        return self.proximities

    def process_system_failure(self):
        self.log.critical(f"Got {self.failed_reads} sequential fails, killing process...")
        self.alive = False

    def check_proximity(self):
        sensor, dist = self.perform_thresh()
        self.log.info(f"Proximities: FLL: {self.proximities[0]} | FLR: {self.proximities[1]} | FRL: {self.proximities[2]} | FRR: {self.proximities[3]} | RRR: {self.proximities[4]} | RRC: {self.proximities[5]} | RR: {self.proximities[6]} | RC: {self.proximities[7]} | RL: {self.proximities[8]} | RLC: {self.proximities[9]} | RLL: {self.proximities[10]}")
        self.log.info(f"Proximities: {self.proximities}")
        if sensor != None:
            self.proximity_detects +=1
            if self.proximity_detects > self.proximity_detect_thresh:
                front_detect = True
                if sensor in self.rear_sensors:
                    front_detect = False
                self.log.warning(f"Detected distance {dist} on sensor {sensor+1} ({'front' if front_detect else 'rear'}), it is below {self.limit_cm}")
                self.proximity_stopped = True
                _, prev_state = self.state_machine.get_state()
                if prev_state == "NAVIGATING_TO_POINT_VISION" and front_detect:
                    self.log.warning("Because we are in vision navigation, we are not stopping as this is likely caused by the pallet")
                else:
                    self.log.warning(f"Stopping!")
                    if front_detect:
                        self.state_machine.set_state("PROXIMITY_STOP_FRONT")
                    else:
                        self.state_machine.set_state("PROXIMITY_STOP_REAR")
                    # self.vehicle_ctrl.clear_override_channels()
                    while self.proximity_stopped:
                        if front_detect:
                            self.vehicle_ctrl.override_stop_drivetrain_fwd()
                        else:
                            self.vehicle_ctrl.override_stop_drivetrain_back()
                        time.sleep(0.3)
                        obstacle_is_clear = self.check_for_no_obstacle()
                        if obstacle_is_clear:
                            self.log.info("Obstacle is clear")
                            self.state_machine.set_state(prev_state)
                            self.proximity_stopped = False
                            self.vehicle_ctrl.clear_override_channels()
        else:
            self.proximity_detects = 0


    def check_for_no_obstacle(self):
        sensor, _ = self.perform_thresh()
        if sensor == None:
            return True
        return False

    def perform_thresh(self):
        self.get_prox_readings()
        for ix, dist in enumerate(self.proximities):
            if dist < self.limit_cm:
                return ix, dist
        return None, None

if __name__ == "__main__":
    test = Proximity()
    while True:
        test.check_proximity()