🎍 Adds virtual sensors and camera mjpeg stream

raspberry_pi/simulate.py

@@ -0,0 +1,38 @@
+from os import sys
+
+from src.spot import Spot
+from src.IMU.IMU import IMU
+from src.Kinematics.SpotKinematics import SpotModel
+from src.camera_server import StreamingServerThread
+from src.Camera.WebCamera import WebCamera
+
+sys.path.append('../')
+
+from simulation.simulator import Simulator
+from simulation.GymEnvs.spot_bezier_env import spotBezierEnv
+
+from simulation.sensors.camera import PyBulletCamera
+
+camera = PyBulletCamera()
+
+env = spotBezierEnv(
+    render=True,
+    on_rack=False,
+    height_field=False,
+    draw_foot_path=False,
+    urdf_root="..\simulation\pybullet_data"
+)
+kinematics = SpotModel()
+imu = IMU()
+camera = PyBulletCamera() # WebCamera()
+spot = Spot(kinematics, imu)
+
+server = StreamingServerThread(camera)
+sim = Simulator(env, spot)
+
+server.start()
+
+while True:
+    camera.update_view_matrix(sim.env.spot.quadruped)
+    camera.get_image()
+    sim.step(0.01)

raspberry_pi/src/Camera/CameraBase.py

@@ -0,0 +1,11 @@
+from abc import abstractmethod
+from typing import Any
+
+
+class CameraBase():
+    def __init__(self) -> None:
+        pass
+
+    @abstractmethod
+    def get_image(self) -> Any:
+        raise NotImplementedError

raspberry_pi/src/Camera/WebCamera.py

@@ -0,0 +1,18 @@
+import cv2
+from .CameraBase import CameraBase
+
+
+class WebCamera(CameraBase):
+    def __init__(self, camera_id=0):
+        self.cap = cv2.VideoCapture(camera_id)
+        # super().__init__()
+
+    def get_image(self):
+        ret, frame = self.cap.read()
+        if ret:
+            return frame
+        else:
+            return None  # Return None if the frame could not be captured
+
+    def __del__(self):
+        self.cap.release()

raspberry_pi/src/IMU/IMU.py

@@ -1,7 +1,7 @@
 import numpy as np
+from .IMUBase import IMUBase
 
-
-class IMU:
+class IMU(IMUBase):
     def __init__(self) -> None:
         pass
 

raspberry_pi/src/IMU/IMUBase.py

@@ -0,0 +1,6 @@
+class IMUBase():
+    def __init__(self) -> None:
+        pass
+
+    def read_orientation(self):
+        raise NotImplementedError()

raspberry_pi/src/camera_server.py

@@ -0,0 +1,51 @@
+from http.server import BaseHTTPRequestHandler, HTTPServer
+from threading import Thread
+import cv2
+import time
+
+
+class StreamingHandler(BaseHTTPRequestHandler):
+    def do_GET(self):
+        if self.path == '/stream.mjpg':
+            self.send_response(200)
+            self.send_header('Content-type', 'multipart/x-mixed-replace; boundary=frame')
+            self.end_headers()
+            try:
+                while True:
+                    frame = self.server.camera.get_image()
+                    _, 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)
+            except Exception as e:
+                print(f"Stream stopped: {e}")
+        else:
+            self.send_error(404)
+            self.end_headers()
+
+class StreamingServer(HTTPServer):
+    def __init__(self, server_address, camera):
+        super().__init__(server_address, StreamingHandler)
+        self.camera = camera
+
+class StreamingServerThread:
+    def __init__(self, camera, port=8080):
+        self.camera = camera
+        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()
+
+        self.server_thread = Thread(target=run_server)
+        self.server_thread.daemon = True
+        self.server_thread.start()
+