From 9e40e01065aeb84a228fd967c5e211b86b9cca3d Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Thu, 7 Mar 2024 15:14:10 +0100 Subject: [PATCH] Updates to use process for camera stream --- raspberry_pi/simulate.py | 8 +++-- raspberry_pi/src/Camera/WebCamera.py | 2 +- raspberry_pi/src/camera_server.py | 53 +++++++++++++++++----------- raspberry_pi/src/spot.py | 9 +++-- simulation/sensors/camera.py | 18 ++++++---- 5 files changed, 54 insertions(+), 36 deletions(-) diff --git a/raspberry_pi/simulate.py b/raspberry_pi/simulate.py index 975ab15..1315c32 100644 --- a/raspberry_pi/simulate.py +++ b/raspberry_pi/simulate.py @@ -27,8 +27,9 @@ def main(): ) kinematics = SpotModel() - camera = WebCamera() - camera = PyBulletCamera(env.spot.quadruped) + # camera = WebCamera() + frame_queue = Queue(maxsize=10) + camera = PyBulletCamera(env.spot.quadruped, frame_queue=frame_queue) imu = IMU() hardware_interface = PyBulletHardwareInterface(env) # shared_controller_state = SharedState() @@ -36,7 +37,8 @@ def main(): spot = Spot( kinematics, - camera, + camera, + frame_queue, imu, hardware_interface, controller_interface, diff --git a/raspberry_pi/src/Camera/WebCamera.py b/raspberry_pi/src/Camera/WebCamera.py index 4931d19..5bf39be 100644 --- a/raspberry_pi/src/Camera/WebCamera.py +++ b/raspberry_pi/src/Camera/WebCamera.py @@ -8,7 +8,7 @@ class WebCamera(CameraBase): self._last_frame = None super().__init__() - def get_frame(self): + def get_image(self): self._last_frame def update(self) -> None: diff --git a/raspberry_pi/src/camera_server.py b/raspberry_pi/src/camera_server.py index 40d1989..468393b 100644 --- a/raspberry_pi/src/camera_server.py +++ b/raspberry_pi/src/camera_server.py @@ -1,5 +1,6 @@ from http.server import BaseHTTPRequestHandler, HTTPServer from threading import Thread +from multiprocessing import Process import cv2 import time @@ -12,42 +13,52 @@ class StreamingHandler(BaseHTTPRequestHandler): self.end_headers() try: while True: - frame = self.server.camera.get_frame() - _, jpeg = cv2.imencode('.jpg', frame) - self.wfile.write(b'--frame\r\n') - self.send_header('Content-Type', 'image/jpeg') - self.send_header('Content-Length', len(jpeg)) - self.end_headers() - self.wfile.write(jpeg.tobytes()) - self.wfile.write(b'\r\n') - time.sleep(0.5) + if not self.server.frame_queue.empty(): + frame = self.server.frame_queue.get() + + _, jpeg = cv2.imencode('.jpg', frame) + self.wfile.write(b'--frame\r\n') + self.send_header('Content-Type', 'image/jpeg') + self.send_header('Content-Length', len(jpeg)) + self.end_headers() + self.wfile.write(jpeg.tobytes()) + self.wfile.write(b'\r\n') + time.sleep(0.1) except Exception as e: print(f"Stream stopped: {e}") + raise e else: self.send_error(404) self.end_headers() class StreamingServer(HTTPServer): - def __init__(self, server_address, camera): + def __init__(self, server_address, frame_queue): super().__init__(server_address, StreamingHandler) - self.camera = camera + self.frame_queue = frame_queue class StreamingServerThread: - def __init__(self, camera, port=8080): - self.camera = camera + def __init__(self, frame_queue, port=8080): + self.frame_queue = frame_queue self.port = port self.server_thread = None - def start(self): - def run_server(): - address = ('', self.port) - server = StreamingServer(address, self.camera) - print(f"Starting server at http://localhost:{self.port}/stream.mjpg") - server.serve_forever() + def run_server(self, frame_queue): + address = ('', self.port) + server = StreamingServer(address, frame_queue) + print(f"Starting server at http://localhost:{self.port}/stream.mjpg") + server.serve_forever() - self.server_thread = Thread(target=run_server) + def start(self): + self.server_thread = Thread(target=self.run_server) self.server_thread.daemon = True self.server_thread.start() + def start_process(self): + self.server_process = Process(target=self.run_server, args=(self.frame_queue,)) + self.server_process.start() + def stop(self): - self.server_thread.join() + if self.server_thread: + self.server_thread.join() + if self.server_process: + self.server_process.join() diff --git a/raspberry_pi/src/spot.py b/raspberry_pi/src/spot.py index 0c495b8..76d6699 100644 --- a/raspberry_pi/src/spot.py +++ b/raspberry_pi/src/spot.py @@ -14,7 +14,7 @@ def map_value(x, from_low, from_high, to_low, to_high): return ((x - from_low) / (from_high - from_low)) * (to_high - to_low) + to_low class Spot: - def __init__(self, kinematic, camera:CameraBase, imu, hardware_interface, controller_interface, shared_controller_state): + def __init__(self, kinematic, camera:CameraBase, frame_queue, imu, hardware_interface, controller_interface, shared_controller_state): self.kinematic = kinematic self.camera = camera self.imu = imu @@ -22,7 +22,7 @@ class Spot: self.controller_interface = controller_interface self.shared_controller_state = shared_controller_state - self.camera_stream = StreamingServerThread(self.camera) + self.camera_stream = StreamingServerThread(frame_queue) self.body_state = BodyState() self.gait_state = GaitState() @@ -31,9 +31,8 @@ class Spot: self.body_state.worldFeetPositions = copy.deepcopy(self.kinematic.WorldToFoot) def start(self): - self.controller_interface.start() - self.camera_stream.start() + self.camera_stream.start_process() def stop(self): self.controller_interface.stop() @@ -52,7 +51,7 @@ class Spot: # self.gait_state.target_yaw_rate = map_value(controller.rx, -127, 128, -2.0, 2.0) def run(self, dt=.01): - # self.camera.update() + self.camera.update() controller = self.shared_controller_state.get_latest_state() self.handle_input(controller) self.gait_state.update_gait_state(dt) diff --git a/simulation/sensors/camera.py b/simulation/sensors/camera.py index c3ff0b6..fac7d5e 100644 --- a/simulation/sensors/camera.py +++ b/simulation/sensors/camera.py @@ -1,4 +1,5 @@ import math +from queue import Empty import cv2 import numpy as np import pybullet as pb @@ -18,7 +19,7 @@ projectionMatrix = pb.computeProjectionMatrixFOV( distance = 100000 class PyBulletCamera(CameraBase): - def __init__(self, model_id, view_matrix=viewMatrix, projection_matrix=projectionMatrix, width=640, height=480): + def __init__(self, model_id, frame_queue, view_matrix=viewMatrix, projection_matrix=projectionMatrix, width=640, height=480): """ Initializes the camera with PyBullet view and projection matrices. :param view_matrix: The view matrix for the camera in PyBullet. @@ -32,10 +33,7 @@ class PyBulletCamera(CameraBase): self.projection_matrix = projection_matrix self.width = width self.height = height - self._last_frame = None - - def get_frame(self): - self._last_frame + self.frame_queue = frame_queue def update(self): """ @@ -44,7 +42,15 @@ class PyBulletCamera(CameraBase): """ self.update_view_matrix() - self._last_frame = self.get_frame() + frame = self.get_frame() + if frame is not None: + # Only keep the latest frame to avoid filling up the queue + while not self.frame_queue.empty(): + try: + self.frame_queue.get_nowait() + except Empty: + pass + self.frame_queue.put(frame) def get_frame(self): # Capture an image from the simulation