Updates to use process for camera stream

raspberry_pi/simulate.py

@@ -27,8 +27,9 @@ def main():
     )
 
     kinematics = SpotModel()
-    camera = WebCamera()
+    # camera = WebCamera()
-    camera = PyBulletCamera(env.spot.quadruped)
+    frame_queue = Queue(maxsize=10) 
+    camera = PyBulletCamera(env.spot.quadruped, frame_queue=frame_queue)
     imu = IMU()
     hardware_interface = PyBulletHardwareInterface(env)
     # shared_controller_state = SharedState()
@@ -37,6 +38,7 @@ def main():
     spot = Spot(
         kinematics, 
         camera,
+        frame_queue,
         imu, 
         hardware_interface, 
         controller_interface, 

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:

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,7 +13,9 @@ class StreamingHandler(BaseHTTPRequestHandler):
             self.end_headers()
             try:
                 while True:
-                    frame = self.server.camera.get_frame()
+                    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')
@@ -20,34 +23,42 @@ class StreamingHandler(BaseHTTPRequestHandler):
                         self.end_headers()
                         self.wfile.write(jpeg.tobytes())
                         self.wfile.write(b'\r\n')
-                    time.sleep(0.5)
+                    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):
+    def __init__(self, frame_queue, port=8080):
-        self.camera = camera
+        self.frame_queue = frame_queue
         self.port = port
         self.server_thread = None
 
-    def start(self):
+    def run_server(self, frame_queue):
-        def run_server():
         address = ('', self.port)
-            server = StreamingServer(address, self.camera)
+        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):
+        if self.server_thread:
             self.server_thread.join()
+        if self.server_process:
+            self.server_process.join()

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)

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
+        self.frame_queue = frame_queue
-
-    def get_frame(self):
-        self._last_frame
 
     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