Updates camera and mocks

raspberry_pi/simulate.py

@@ -1,38 +1,66 @@
 from os import sys
+import time
 
 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
+from src.kinematics.SpotKinematics import SpotModel
+from src.camera.WebCamera import WebCamera
+from src.imu.IMU import IMU
+from src.controller_interface.WebsocketController import WebsocketController
+from src.controller_interface.SharedControllerState import SharedState, shared_state
+
+# from src.camera_server import StreamingServerThread
 
 sys.path.append('../')
 
 from simulation.simulator import Simulator
 from simulation.GymEnvs.spot_bezier_env import spotBezierEnv
-
 from simulation.sensors.camera import PyBulletCamera
+from simulation.sensors.hardware_interface import PyBulletHardwareInterface
 
-camera = PyBulletCamera()
+def main():
+    env = spotBezierEnv(
+        render=True,
+        on_rack=False,
+        height_field=False,
+        draw_foot_path=False,
+        urdf_root="..\simulation\pybullet_data"
+    )
 
-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)
+    kinematics = SpotModel()
+    camera = WebCamera()
+    camera = PyBulletCamera(env.spot.quadruped)
+    imu = IMU()
+    hardware_interface = PyBulletHardwareInterface(env)
+    # shared_controller_state = SharedState()
+    controller_interface = WebsocketController(shared_state)
 
-server = StreamingServerThread(camera)
-sim = Simulator(env, spot)
+    spot = Spot(
+        kinematics, 
+        camera, 
+        imu, 
+        hardware_interface, 
+        controller_interface, 
+        shared_state
+    )
 
-server.start()
+    simulator = Simulator(env, spot)
+
+    spot.start()
+
+    last_time = time.time()
+
+    try:
+        while True:
+            contacts, done = simulator.step()
+            spot.gait_state.contacts = contacts
+            spot.run() # time.time() - last_time
+            last_time = time.time()
+            if done:
+                break
+    except KeyboardInterrupt:
+        print("Program was interrupted")
+    spot.stop()
+
+if __name__ == '__main__':
+    main()
 
-while True:
-    camera.update_view_matrix(sim.env.spot.quadruped)
-    camera.get_image()
-    sim.step(0.01)