<filename>src/vehicle/stereo_camera.py
from .vehicle_peripheral import VehiclePeripheral
import threading
import cv2
import logging
import base64


class USBCameraCaptureThread(threading.Thread):

    def __init__(self, cam_no: int, dev_no: int):
        super(USBCameraCaptureThread, self).__init__()
        self._cam_no = cam_no
        self._dev_no = dev_no
        self._stop_event = threading.Event()
        self._capture_event = threading.Event()
        self._cond_var = threading.Condition()
        self._data_lock = threading.Lock()

        self._height = 120  # px
        self._width = 160  # px

        self._image_ready = False
        self._image = None

    def run(self):
        while not self.stopped():

            # Start a wait loop until the condition variable is set, upon which we will capture a new image
            self._cond_var.acquire()
            while not self._capture_event.is_set() and not self.stopped():
                # Don't capture the image until the the capture event is set
                self._cond_var.wait()
            self._cond_var.release()

            # We've been released from the wait loop, if we're supposed to capture do that now
            if self._capture_event.is_set():
                self._capture_event.clear()

                # Have only been able to get this to work by instantiating the camera every capture. This is obviously
                # ridiculously wasteful
                cam = cv2.VideoCapture(self._dev_no)

                # Read from the camera into a numpy array
                ret, img_arr = cam.read()

                # Resize the image to a more manageable size
                img_arr = cv2.resize(img_arr, (self._width, self._height))

                if ret:

                    # Encode the image to jpeg
                    ret, encoded_img_buf = cv2.imencode('.jpg', img_arr)
                    # logging.info(f"Camera {self._dev_no} captured image, buffer shape: {encoded_img_buf.shape}")
                    if ret:
                        with self._data_lock:
                            self._image = encoded_img_buf.tobytes()
                        self._image_ready = True
                    else:
                        logging.error(f"Camera {self._cam_no} failed to encode image")
                else:
                    logging.error(f"Camera {self._cam_no} failed to capture image")

                cam.release()

    def stop(self):
        self._cond_var.acquire()
        self._stop_event.set()
        self._cond_var.notifyAll()
        self._cond_var.release()

    def stopped(self):
        return self._stop_event.is_set()

    def queue_capture_image(self):
        self._cond_var.acquire()
        self._capture_event.set()
        self._cond_var.notifyAll()
        self._cond_var.release()

    def get_latest_image(self):
        if not self._image_ready:
            return None
        else:
            with self._data_lock:
                self._image_ready = False
                return self._image

    def camera_number(self):
        return self._cam_no


class StereoCamera(VehiclePeripheral):
    """
    The StereoCamera class owns the two cameras which operate on independent threads. This is to try to avoid latency
    between the two captured images. A condition variable is used to try to synchronize image captures
    """

    def __init__(self):
        super().__init__()
        # List of cameras that we will be capturing images from. On my raspberry pi it needs to capture from devices
        # 0 and 2, but the cameras are named 1 and 2 for more convenient logging and labelling
        self._cameras = [USBCameraCaptureThread(1, 0), USBCameraCaptureThread(2, 2)]

        # Start the camera thread
        for camera in self._cameras:
            camera.start()

    def behave(self):
        # Try to capture images at the same time. Can't guarantee that they will actually be simultaneous, but using
        # async/await syntax gets us a decent approximation
        for camera in self._cameras:
            camera.queue_capture_image()

    def cleanup(self):
        for camera in self._cameras:
            camera.stop()
            camera.join()

    def loop_delay(self):
        # Fairly substantial loop delay since the image capture actually takes a while
        return 0.5

    def get_data(self) -> dict:

        ret = {}
        for camera in self._cameras:
            image = camera.get_latest_image()
            if image is None:
                # At least one of the images is not ready, return empty data
                return {}
            else:
                # Encode the image buffer into a base64 so it can be sent via standard json
                ret[f"camera_{camera.camera_number()}"] = base64.b64encode(image)

        return ret

<filename>src/vehicle/stereo_camera.py
from .vehicle_peripheral import VehiclePeripheral
import threading
import cv2
import logging
import base64


class USBCameraCaptureThread(threading.Thread):

    def __init__(self, cam_no: int, dev_no: int):
        super(USBCameraCaptureThread, self).__init__()
        self._cam_no = cam_no
        self._dev_no = dev_no
        self._stop_event = threading.Event()
        self._capture_event = threading.Event()
        self._cond_var = threading.Condition()
        self._data_lock = threading.Lock()

        self._height = 120  # px
        self._width = 160  # px

        self._image_ready = False
        self._image = None

    def run(self):
        while not self.stopped():

            # Start a wait loop until the condition variable is set, upon which we will capture a new image
            self._cond_var.acquire()
            while not self._capture_event.is_set() and not self.stopped():
                # Don't capture the image until the the capture event is set
                self._cond_var.wait()
            self._cond_var.release()

            # We've been released from the wait loop, if we're supposed to capture do that now
            if self._capture_event.is_set():
                self._capture_event.clear()

                # Have only been able to get this to work by instantiating the camera every capture. This is obviously
                # ridiculously wasteful
                cam = cv2.VideoCapture(self._dev_no)

                # Read from the camera into a numpy array
                ret, img_arr = cam.read()

                # Resize the image to a more manageable size
                img_arr = cv2.resize(img_arr, (self._width, self._height))

                if ret:

                    # Encode the image to jpeg
                    ret, encoded_img_buf = cv2.imencode('.jpg', img_arr)
                    # logging.info(f"Camera {self._dev_no} captured image, buffer shape: {encoded_img_buf.shape}")
                    if ret:
                        with self._data_lock:
                            self._image = encoded_img_buf.tobytes()
                        self._image_ready = True
                    else:
                        logging.error(f"Camera {self._cam_no} failed to encode image")
                else:
                    logging.error(f"Camera {self._cam_no} failed to capture image")

                cam.release()

    def stop(self):
        self._cond_var.acquire()
        self._stop_event.set()
        self._cond_var.notifyAll()
        self._cond_var.release()

    def stopped(self):
        return self._stop_event.is_set()

    def queue_capture_image(self):
        self._cond_var.acquire()
        self._capture_event.set()
        self._cond_var.notifyAll()
        self._cond_var.release()

    def get_latest_image(self):
        if not self._image_ready:
            return None
        else:
            with self._data_lock:
                self._image_ready = False
                return self._image

    def camera_number(self):
        return self._cam_no


class StereoCamera(VehiclePeripheral):
    """
    The StereoCamera class owns the two cameras which operate on independent threads. This is to try to avoid latency
    between the two captured images. A condition variable is used to try to synchronize image captures
    """

    def __init__(self):
        super().__init__()
        # List of cameras that we will be capturing images from. On my raspberry pi it needs to capture from devices
        # 0 and 2, but the cameras are named 1 and 2 for more convenient logging and labelling
        self._cameras = [USBCameraCaptureThread(1, 0), USBCameraCaptureThread(2, 2)]

        # Start the camera thread
        for camera in self._cameras:
            camera.start()

    def behave(self):
        # Try to capture images at the same time. Can't guarantee that they will actually be simultaneous, but using
        # async/await syntax gets us a decent approximation
        for camera in self._cameras:
            camera.queue_capture_image()

    def cleanup(self):
        for camera in self._cameras:
            camera.stop()
            camera.join()

    def loop_delay(self):
        # Fairly substantial loop delay since the image capture actually takes a while
        return 0.5

    def get_data(self) -> dict:

        ret = {}
        for camera in self._cameras:
            image = camera.get_latest_image()
            if image is None:
                # At least one of the images is not ready, return empty data
                return {}
            else:
                # Encode the image buffer into a base64 so it can be sent via standard json
                ret[f"camera_{camera.camera_number()}"] = base64.b64encode(image)

        return ret

# px # px # Start a wait loop until the condition variable is set, upon which we will capture a new image # Don't capture the image until the the capture event is set # We've been released from the wait loop, if we're supposed to capture do that now # Have only been able to get this to work by instantiating the camera every capture. This is obviously # ridiculously wasteful # Read from the camera into a numpy array # Resize the image to a more manageable size # Encode the image to jpeg # logging.info(f"Camera {self._dev_no} captured image, buffer shape: {encoded_img_buf.shape}") The StereoCamera class owns the two cameras which operate on independent threads. This is to try to avoid latency     between the two captured images. A condition variable is used to try to synchronize image captures # List of cameras that we will be capturing images from. On my raspberry pi it needs to capture from devices # 0 and 2, but the cameras are named 1 and 2 for more convenient logging and labelling # Start the camera thread # Try to capture images at the same time. Can't guarantee that they will actually be simultaneous, but using # async/await syntax gets us a decent approximation # Fairly substantial loop delay since the image capture actually takes a while # At least one of the images is not ready, return empty data # Encode the image buffer into a base64 so it can be sent via standard json