From d4df7957b4c1c9804bc5aa6a98a0bf3dd9099a14 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?oliver=20wei=C3=9Fl?= <oliver.weissl@outlook.com>
Date: Tue, 19 Mar 2024 13:24:49 +0100
Subject: [PATCH 1/3] added support for camera in physical robot

---
 .../revolve2/modular_robot/body/_module.py    |   8 ++
 .../_protocol_version.py                      |   2 +-
 .../_physical_interface.py                    |  11 ++
 .../v1/_v1_physical_interface.py              |  10 ++
 .../v2/_v2_physical_interface.py              |  10 ++
 .../remote/_camera_sensor_state_impl.py       |  27 ++++
 .../_modular_robot_sensor_state_impl_v2.py    |  15 ++-
 .../modular_robot_physical/remote/_remote.py  | 117 ++++++++++++++----
 .../robot_daemon/_robo_server_impl.py         |  30 ++++-
 .../robot_daemon_protocol.capnp               |   7 ++
 .../robot_daemon_protocol_capnp.py            |  10 +-
 .../robot_daemon_protocol_capnp.pyi           |  69 +++++++++--
 12 files changed, 268 insertions(+), 48 deletions(-)
 create mode 100644 modular_robot_physical/revolve2/modular_robot_physical/remote/_camera_sensor_state_impl.py

diff --git a/modular_robot/revolve2/modular_robot/body/_module.py b/modular_robot/revolve2/modular_robot/body/_module.py
index 809f1d507..c1cbbea02 100644
--- a/modular_robot/revolve2/modular_robot/body/_module.py
+++ b/modular_robot/revolve2/modular_robot/body/_module.py
@@ -251,6 +251,14 @@ def color(self) -> Color:
         """
         return self._color
 
+    def set_color(self, color: Color) -> None:
+        """
+        Set the color of a module.
+
+        :param color: The color
+        """
+        self._color = color
+
     @property
     def attachment_points(self) -> dict[int, AttachmentPoint]:
         """
diff --git a/modular_robot_physical/revolve2/modular_robot_physical/_protocol_version.py b/modular_robot_physical/revolve2/modular_robot_physical/_protocol_version.py
index db316f2aa..84eeb48a1 100644
--- a/modular_robot_physical/revolve2/modular_robot_physical/_protocol_version.py
+++ b/modular_robot_physical/revolve2/modular_robot_physical/_protocol_version.py
@@ -1 +1 @@
-PROTOCOL_VERSION = "1.0.0"
+PROTOCOL_VERSION = "1.0.2"
diff --git a/modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/_physical_interface.py b/modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/_physical_interface.py
index 0be69f254..c3b249a62 100644
--- a/modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/_physical_interface.py
+++ b/modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/_physical_interface.py
@@ -1,6 +1,8 @@
 from abc import ABC, abstractmethod
 from typing import Sequence
 
+import numpy as np
+from numpy.typing import NDArray
 from pyrr import Vector3
 
 
@@ -75,3 +77,12 @@ def get_imu_specific_force(self) -> Vector3:
         :returns: The specific force.
         :raises NotImplementedError: If the IMU is not supported on this hardware.
         """
+
+    @abstractmethod
+    def get_camera_view(self) -> NDArray[np.uint8]:
+        """
+        Get the current view from the camera.
+
+        :return: The camera view.
+        :raises NotImplementedError: If the Camera is not supported on this hardware.
+        """
diff --git a/modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/v1/_v1_physical_interface.py b/modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/v1/_v1_physical_interface.py
index c061d2d36..f1717fc54 100644
--- a/modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/v1/_v1_physical_interface.py
+++ b/modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/v1/_v1_physical_interface.py
@@ -2,7 +2,9 @@
 import time
 from typing import Sequence
 
+import numpy as np
 import pigpio
+from numpy.typing import NDArray
 from pyrr import Vector3
 
 from .._physical_interface import PhysicalInterface
@@ -126,3 +128,11 @@ def get_imu_specific_force(self) -> Vector3:
         :raises NotImplementedError: Always.
         """
         raise NotImplementedError()
+
+    def get_camera_view(self) -> NDArray[np.uint8]:
+        """
+        Get the current view from the camera.
+
+        :raises NotImplementedError: If the Camera is not supported on this hardware.
+        """
+        raise NotImplementedError()
diff --git a/modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/v2/_v2_physical_interface.py b/modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/v2/_v2_physical_interface.py
index 4353d4167..ec59cb595 100644
--- a/modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/v2/_v2_physical_interface.py
+++ b/modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/v2/_v2_physical_interface.py
@@ -1,6 +1,8 @@
 import math
 from typing import Sequence
 
+import numpy as np
+from numpy.typing import NDArray
 from pyrr import Vector3
 from robohatlib.hal.assemblyboard.PwmPlug import PwmPlug
 from robohatlib.hal.assemblyboard.servo.ServoData import ServoData
@@ -173,3 +175,11 @@ def get_imu_specific_force(self) -> Vector3:
         if accel is None:
             raise RuntimeError("Could not get IMU acceleration reading!")
         return Vector3(accel)
+
+    def get_camera_view(self) -> NDArray[np.uint8]:
+        """
+        Get the current view from the camera.
+
+        :raises NotImplementedError: If the Camera is not supported on this hardware.
+        """
+        raise NotImplementedError()
diff --git a/modular_robot_physical/revolve2/modular_robot_physical/remote/_camera_sensor_state_impl.py b/modular_robot_physical/revolve2/modular_robot_physical/remote/_camera_sensor_state_impl.py
new file mode 100644
index 000000000..d50867ff7
--- /dev/null
+++ b/modular_robot_physical/revolve2/modular_robot_physical/remote/_camera_sensor_state_impl.py
@@ -0,0 +1,27 @@
+import numpy as np
+from numpy.typing import NDArray
+
+from revolve2.modular_robot.sensor_state import CameraSensorState
+
+
+class CameraSensorStateImpl(CameraSensorState):
+    """CameraSensorState implementation for physical robots."""
+
+    _image: NDArray[np.uint8]
+
+    def __init__(self, image: NDArray[np.uint8]) -> None:
+        """
+        Initialize this object.
+
+        :param image: The current image.
+        """
+        self._image = image
+
+    @property
+    def image(self) -> NDArray[np.uint8]:
+        """
+        Get the current image.
+
+        :returns: The image.
+        """
+        return self._image
diff --git a/modular_robot_physical/revolve2/modular_robot_physical/remote/_modular_robot_sensor_state_impl_v2.py b/modular_robot_physical/revolve2/modular_robot_physical/remote/_modular_robot_sensor_state_impl_v2.py
index 732b7f608..bb6ebb4e1 100644
--- a/modular_robot_physical/revolve2/modular_robot_physical/remote/_modular_robot_sensor_state_impl_v2.py
+++ b/modular_robot_physical/revolve2/modular_robot_physical/remote/_modular_robot_sensor_state_impl_v2.py
@@ -12,6 +12,7 @@
 
 from .._uuid_key import UUIDKey
 from ._active_hinge_sensor_state_impl import ActiveHingeSensorStateImpl
+from ._camera_sensor_state_impl import CameraSensorStateImpl
 from ._imu_sensor_state_impl import IMUSensorStateImpl
 
 
@@ -22,12 +23,14 @@ class ModularRobotSensorStateImplV2(ModularRobotSensorState):
     _hinge_positions: dict[int, float]
 
     _imu_sensor_states: dict[UUIDKey[IMUSensor], IMUSensorStateImpl]
+    _camera_sensor_states: dict[UUIDKey[CameraSensor], CameraSensorStateImpl]
 
     def __init__(
         self,
         hinge_sensor_mapping: dict[UUIDKey[ActiveHingeSensor], int],
         hinge_positions: dict[int, float],
         imu_sensor_states: dict[UUIDKey[IMUSensor], IMUSensorStateImpl],
+        camera_sensor_states: dict[UUIDKey[CameraSensor], CameraSensorStateImpl],
     ) -> None:
         """
         Initialize this object.
@@ -35,10 +38,12 @@ def __init__(
         :param hinge_sensor_mapping: Mapping from active hinge sensors to pin ids.
         :param hinge_positions: Position of hinges accessed by pin id.
         :param imu_sensor_states: State of the IMU sensors.
+        :param camera_sensor_states: The states of the Camera sensors.
         """
         self._hinge_sensor_mapping = hinge_sensor_mapping
         self._hinge_positions = hinge_positions
         self._imu_sensor_states = imu_sensor_states
+        self._camera_sensor_states = camera_sensor_states
 
     def get_active_hinge_sensor_state(
         self, sensor: ActiveHingeSensor
@@ -73,6 +78,12 @@ def get_camera_sensor_state(self, sensor: CameraSensor) -> CameraSensorState:
         Get the state of the provided camera sensor.
 
         :param sensor: The sensor.
-        :raises NotImplementedError: Always.
+        :raises ValueError: If sensor is not found on the robot.
+        :returns: The camera sensor state.
         """
-        raise NotImplementedError()
+        state = self._camera_sensor_states.get(UUIDKey(sensor))
+        if state is None:
+            raise ValueError(
+                "State for camera sensor not found. Does it exist in the robot definition?"
+            )
+        return state
diff --git a/modular_robot_physical/revolve2/modular_robot_physical/remote/_remote.py b/modular_robot_physical/revolve2/modular_robot_physical/remote/_remote.py
index 8f5bedaae..6bbf0feca 100644
--- a/modular_robot_physical/revolve2/modular_robot_physical/remote/_remote.py
+++ b/modular_robot_physical/revolve2/modular_robot_physical/remote/_remote.py
@@ -3,9 +3,12 @@
 from typing import Callable
 
 import capnp
+import numpy as np
+from numpy.typing import NDArray
 from pyrr import Vector3
 
 from revolve2.modular_robot.body.base import ActiveHinge
+from revolve2.modular_robot.body.sensors import CameraSensor, IMUSensor
 from revolve2.modular_robot.sensor_state import ModularRobotSensorState
 
 from .._config import Config
@@ -14,6 +17,7 @@
 from .._standard_port import STANDARD_PORT
 from .._uuid_key import UUIDKey
 from ..robot_daemon_api import robot_daemon_protocol_capnp
+from ._camera_sensor_state_impl import CameraSensorStateImpl
 from ._imu_sensor_state_impl import IMUSensorStateImpl
 from ._modular_robot_control_interface_impl import ModularRobotControlInterfaceImpl
 from ._modular_robot_sensor_state_impl_v1 import ModularRobotSensorStateImplV1
@@ -105,7 +109,7 @@ async def _run_remote_impl(
                     )
                 )
             ).response
-            print(f"Battery level is at {sensor_readings.battery*100.0}%.")
+            print(f"Battery level is at {sensor_readings.battery * 100.0}%.")
 
     # Fire prepared callback
     on_prepared()
@@ -154,18 +158,14 @@ async def _run_remote_impl(
                         robot_daemon_protocol_capnp.ReadSensorsArgs(readPins=pins)
                     )
                 ).response
-                if config.modular_robot.body.core.sensors.imu_sensor is None:
-                    imu_sensor_states = {}
-                else:
-                    imu_sensor_states = {
-                        UUIDKey(
-                            config.modular_robot.body.core.sensors.imu_sensor
-                        ): IMUSensorStateImpl(
-                            _capnp_to_vector3(sensor_readings.imuSpecificForce),
-                            _capnp_to_vector3(sensor_readings.imuAngularRate),
-                            _capnp_to_vector3(sensor_readings.imuOrientation),
-                        )
-                    }
+                imu_sensor_states = _get_imu_sensor_state(
+                    config.modular_robot.body.core.sensors.imu_sensor, sensor_readings
+                )
+                camera_sensor_states = _get_camera_sensor_state(
+                    config.modular_robot.body.core.sensors.camera_sensor,
+                    sensor_readings,
+                )
+
                 sensor_state = ModularRobotSensorStateImplV2(
                     hinge_sensor_mapping=active_hinge_sensor_to_pin,
                     hinge_positions={
@@ -173,6 +173,7 @@ async def _run_remote_impl(
                         for pin, position in zip(pins, sensor_readings.pins)
                     },
                     imu_sensor_states=imu_sensor_states,
+                    camera_sensor_states=camera_sensor_states,
                 )
             case _:
                 raise NotImplementedError("Hardware type not supported.")
@@ -221,18 +222,16 @@ async def _run_remote_impl(
                             )
                         )
                     ).response
-                    if config.modular_robot.body.core.sensors.imu_sensor is None:
-                        imu_sensor_states = {}
-                    else:
-                        imu_sensor_states = {
-                            UUIDKey(
-                                config.modular_robot.body.core.sensors.imu_sensor
-                            ): IMUSensorStateImpl(
-                                _capnp_to_vector3(sensor_readings.imuSpecificForce),
-                                _capnp_to_vector3(sensor_readings.imuAngularRate),
-                                _capnp_to_vector3(sensor_readings.imuOrientation),
-                            )
-                        }
+
+                    imu_sensor_states = _get_imu_sensor_state(
+                        config.modular_robot.body.core.sensors.imu_sensor,
+                        sensor_readings,
+                    )
+                    camera_sensor_states = _get_camera_sensor_state(
+                        config.modular_robot.body.core.sensors.camera_sensor,
+                        sensor_readings,
+                    )
+
                     sensor_state = ModularRobotSensorStateImplV2(
                         hinge_sensor_mapping=active_hinge_sensor_to_pin,
                         hinge_positions={
@@ -240,10 +239,13 @@ async def _run_remote_impl(
                             for pin, position in zip(pins, sensor_readings.pins)
                         },
                         imu_sensor_states=imu_sensor_states,
+                        camera_sensor_states=camera_sensor_states,
                     )
 
                     if battery_print_timer > 5.0:
-                        print(f"Battery level is at {sensor_readings.battery*100.0}%.")
+                        print(
+                            f"Battery level is at {sensor_readings.battery * 100.0}%."
+                        )
                         battery_print_timer = 0.0
                 case _:
                     raise NotImplementedError("Hardware type not supported.")
@@ -253,6 +255,69 @@ def _capnp_to_vector3(vector: robot_daemon_protocol_capnp.Vector3) -> Vector3:
     return Vector3([vector.x, vector.y, vector.z])
 
 
+def _capnp_to_camera_view(
+    image: robot_daemon_protocol_capnp.Image, camera_size: tuple[int, int]
+) -> NDArray[np.uint8]:
+    """
+    Convert a capnp compatible Image into an NDArray.
+
+    :param image: The capnp Image.
+    :param camera_size: The camera size to reconstruct the image.
+    :return: The NDArray imag.
+    """
+    np_image = np.zeros(shape=(3, *camera_size), dtype=np.uint8)
+    np_image[0] = np.array(image.r).reshape(camera_size).astype(np.uint8)
+    np_image[1] = np.array(image.g).reshape(camera_size).astype(np.uint8)
+    np_image[2] = np.array(image.b).reshape(camera_size).astype(np.uint8)
+    return np_image
+
+
+def _get_imu_sensor_state(
+    imu_sensor: IMUSensor | None,
+    sensor_readings: robot_daemon_protocol_capnp.SensorReadings,
+) -> dict[UUIDKey[IMUSensor], IMUSensorStateImpl]:
+    """
+    Get the IMU sensor state.
+
+    :param imu_sensor: The sensor in question.
+    :param sensor_readings: The sensor readings.
+    :return: The Sensor state.
+    """
+    if imu_sensor is None:
+        return {}
+    else:
+        return {
+            UUIDKey(imu_sensor): IMUSensorStateImpl(
+                _capnp_to_vector3(sensor_readings.imuSpecificForce),
+                _capnp_to_vector3(sensor_readings.imuAngularRate),
+                _capnp_to_vector3(sensor_readings.imuOrientation),
+            )
+        }
+
+
+def _get_camera_sensor_state(
+    camera_sensor: CameraSensor | None,
+    sensor_readings: robot_daemon_protocol_capnp.SensorReadings,
+) -> dict[UUIDKey[CameraSensor], CameraSensorStateImpl]:
+    """
+    Get the camera sensor state.
+
+    :param camera_sensor: The sensor in question.
+    :param sensor_readings: The sensor readings.
+    :return: The Sensor state.
+    """
+    if camera_sensor is None:
+        return {}
+    else:
+        return {
+            UUIDKey(camera_sensor): CameraSensorStateImpl(
+                _capnp_to_camera_view(
+                    sensor_readings.cameraView, camera_sensor.camera_size
+                )
+            )
+        }
+
+
 def run_remote(
     config: Config,
     hostname: str,
diff --git a/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon/_robo_server_impl.py b/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon/_robo_server_impl.py
index 0d49e97a0..fe10e9a31 100644
--- a/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon/_robo_server_impl.py
+++ b/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon/_robo_server_impl.py
@@ -2,12 +2,16 @@
 import time
 from typing import Any, Sequence
 
+import numpy as np
+from numpy.typing import NDArray
 from pyrr import Vector3
 
 from .._hardware_type import HardwareType
 from .._protocol_version import PROTOCOL_VERSION
 from ..physical_interfaces import PhysicalInterface
 from ..robot_daemon_api import robot_daemon_protocol_capnp
+from ..robot_daemon_api.robot_daemon_protocol_capnp import Image as capnpImage
+from ..robot_daemon_api.robot_daemon_protocol_capnp import Vector3 as capnpVector3
 
 
 class RoboServerImpl(robot_daemon_protocol_capnp.RoboServer.Server):  # type: ignore
@@ -253,6 +257,7 @@ def _get_sensor_readings(
             imu_orientation = self._physical_interface.get_imu_orientation()
             imu_specific_force = self._physical_interface.get_imu_specific_force()
             imu_angular_rate = self._physical_interface.get_imu_angular_rate()
+            camera_view = self._physical_interface.get_camera_view()
 
         return robot_daemon_protocol_capnp.SensorReadings(
             pins=pins_readings,
@@ -260,10 +265,33 @@ def _get_sensor_readings(
             imuOrientation=self._vector3_to_capnp(imu_orientation),
             imuSpecificForce=self._vector3_to_capnp(imu_specific_force),
             imuAngularRate=self._vector3_to_capnp(imu_angular_rate),
+            cameraView=self._camera_view_to_capnp(camera_view),
         )
 
     @staticmethod
-    def _vector3_to_capnp(vector: Vector3) -> Vector3:
+    def _vector3_to_capnp(vector: Vector3) -> capnpVector3:
+        """
+        Convert a pyrr Vector3 object into a capnp compatible Vector3.
+
+        :param vector: The pyrr Vector3.
+        :return: The capnp Vector3.
+        """
         return robot_daemon_protocol_capnp.Vector3(
             x=float(vector.x), y=float(vector.y), z=float(vector.z)
         )
+
+    @staticmethod
+    def _camera_view_to_capnp(image: NDArray[np.uint8]) -> capnpImage:
+        """
+        Convert an image as an NDArray into an capnp compatible Image.
+
+        Not that we flatten the channels so they have to be reconstructed later on.
+
+        :param image: The NDArray image.
+        :return: The capnp Image object.
+        """
+        return robot_daemon_protocol_capnp.Image(
+            r=list(image[0].flatten().astype(np.uint8)),
+            g=list(image[1].flatten().astype(np.uint8)),
+            b=list(image[2].flatten().astype(np.uint8)),
+        )
diff --git a/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol.capnp b/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol.capnp
index e2d9fa269..241d02080 100644
--- a/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol.capnp
+++ b/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol.capnp
@@ -39,12 +39,19 @@ struct Vector3 {
   z @2 :Float32;
 }
 
+struct Image {
+  r @0 :List(Int32);
+  g @1 :List(Int32);
+  b @2 :List(Int32);
+}
+
 struct SensorReadings {
   pins @0 :List(Float32);
   battery @1 :Float32;
   imuOrientation @2 :Vector3;
   imuSpecificForce @3 :Vector3;
   imuAngularRate @4 :Vector3;
+  cameraView @5 :Image;
 }
 
 interface RoboServer {
diff --git a/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol_capnp.py b/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol_capnp.py
index 38c8d2b99..ffb348732 100644
--- a/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol_capnp.py
+++ b/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol_capnp.py
@@ -1,8 +1,4 @@
-"""
-This is an automatically generated stub for `robot_daemon_protocol.capnp`.
-
-This file was manually edited to contain the RoboServer interface.
-"""
+"""This is an automatically generated stub for `robot_daemon_protocol.capnp`."""
 import os
 
 import capnp  # type: ignore
@@ -31,7 +27,9 @@
 Vector3 = capnp.load(module_file).Vector3
 Vector3Builder = Vector3
 Vector3Reader = Vector3
+Image = capnp.load(module_file).Image
+ImageBuilder = Image
+ImageReader = Image
 SensorReadings = capnp.load(module_file).SensorReadings
 SensorReadingsBuilder = SensorReadings
 SensorReadingsReader = SensorReadings
-RoboServer = capnp.load(module_file).RoboServer
diff --git a/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol_capnp.pyi b/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol_capnp.pyi
index a5532de14..25a535ce2 100644
--- a/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol_capnp.pyi
+++ b/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol_capnp.pyi
@@ -1,19 +1,9 @@
-"""
-This is an automatically generated stub for `robot_daemon_protocol.capnp`.
-
-This file was manually edited to work better with the code tools.
-
-Added __init__ functions
-Made `dict` into `dict[Any,Any]`
-Formatted file with black
-RoboServer
-"""
-
+"""This is an automatically generated stub for `robot_daemon_protocol.capnp`."""
 from __future__ import annotations
 
 from contextlib import contextmanager
 from io import BufferedWriter
-from typing import Any, Iterator, Literal, Sequence
+from typing import Any, Iterator, Literal, Sequence, overload
 
 class SetupArgs:
     version: str
@@ -98,6 +88,7 @@ class SetupResponseBuilder(SetupResponse):
 class PinControl:
     pin: int
     target: float
+
     def __init__(self, pin: int, target: float) -> None: ...
     @staticmethod
     @contextmanager
@@ -134,6 +125,7 @@ class PinControlBuilder(PinControl):
 
 class ControlArgs:
     setPins: Sequence[PinControl | PinControlBuilder | PinControlReader]
+
     def __init__(
         self, setPins: Sequence[PinControl | PinControlBuilder | PinControlReader]
     ) -> None: ...
@@ -174,6 +166,7 @@ class ControlArgsBuilder(ControlArgs):
 
 class ReadSensorsArgs:
     readPins: Sequence[int]
+
     def __init__(self, readPins: Sequence[int]) -> None: ...
     @staticmethod
     @contextmanager
@@ -211,6 +204,7 @@ class ReadSensorsArgsBuilder(ReadSensorsArgs):
 class ControlAndReadSensorsArgs:
     setPins: Sequence[PinControl | PinControlBuilder | PinControlReader]
     readPins: Sequence[int]
+
     def __init__(
         self,
         setPins: Sequence[PinControl | PinControlBuilder | PinControlReader],
@@ -255,6 +249,7 @@ class Vector3:
     x: float
     y: float
     z: float
+
     def __init__(self, x: float, y: float, z: float) -> None: ...
     @staticmethod
     @contextmanager
@@ -289,12 +284,53 @@ class Vector3Builder(Vector3):
     @staticmethod
     def write_packed(file: BufferedWriter) -> None: ...
 
+class Image:
+    r: list[int]
+    g: list[int]
+    b: list[int]
+
+    def __init__(self, r: list[int], g: list[int], b: list[int]) -> None: ...
+    @staticmethod
+    @contextmanager
+    def from_bytes(
+        data: bytes,
+        traversal_limit_in_words: int | None = ...,
+        nesting_limit: int | None = ...,
+    ) -> Iterator[ImageReader]: ...
+    @staticmethod
+    def from_bytes_packed(
+        data: bytes,
+        traversal_limit_in_words: int | None = ...,
+        nesting_limit: int | None = ...,
+    ) -> ImageReader: ...
+    @staticmethod
+    def new_message() -> ImageBuilder: ...
+    def to_dict(self) -> dict[Any, Any]: ...
+
+class ImageReader(Image):
+    def as_builder(self) -> ImageBuilder: ...
+
+class ImageBuilder(Image):
+    @staticmethod
+    def from_dict(dictionary: dict[Any, Any]) -> ImageBuilder: ...
+    def copy(self) -> ImageBuilder: ...
+    def to_bytes(self) -> bytes: ...
+    def to_bytes_packed(self) -> bytes: ...
+    def to_segments(self) -> list[bytes]: ...
+    def as_reader(self) -> ImageReader: ...
+    @staticmethod
+    def write(file: BufferedWriter) -> None: ...
+    @staticmethod
+    def write_packed(file: BufferedWriter) -> None: ...
+
 class SensorReadings:
     pins: Sequence[float]
     battery: float
     imuOrientation: Vector3 | Vector3Builder | Vector3Reader
     imuSpecificForce: Vector3 | Vector3Builder | Vector3Reader
     imuAngularRate: Vector3 | Vector3Builder | Vector3Reader
+    cameraView: Image | ImageBuilder | ImageReader
+
     def __init__(
         self,
         pins: Sequence[float],
@@ -302,6 +338,7 @@ class SensorReadings:
         imuOrientation: Vector3 | Vector3Builder | Vector3Reader,
         imuSpecificForce: Vector3 | Vector3Builder | Vector3Reader,
         imuAngularRate: Vector3 | Vector3Builder | Vector3Reader,
+        cameraView: Image | ImageBuilder | ImageReader,
     ) -> None: ...
     @staticmethod
     @contextmanager
@@ -321,9 +358,17 @@ class SensorReadings:
     def to_dict(self) -> dict[Any, Any]: ...
 
 class SensorReadingsReader(SensorReadings):
+    imuOrientation: Vector3Reader
+    imuSpecificForce: Vector3Reader
+    imuAngularRate: Vector3Reader
+    cameraView: ImageReader
     def as_builder(self) -> SensorReadingsBuilder: ...
 
 class SensorReadingsBuilder(SensorReadings):
+    imuOrientation: Vector3 | Vector3Builder | Vector3Reader
+    imuSpecificForce: Vector3 | Vector3Builder | Vector3Reader
+    imuAngularRate: Vector3 | Vector3Builder | Vector3Reader
+    cameraView: Image | ImageBuilder | ImageReader
     @staticmethod
     def from_dict(dictionary: dict[Any, Any]) -> SensorReadingsBuilder: ...
     def copy(self) -> SensorReadingsBuilder: ...

From 258cce19e44c6b12d36adef5409ae95569acb1d2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?oliver=20wei=C3=9Fl?= <oliver.weissl@outlook.com>
Date: Wed, 20 Mar 2024 09:36:48 +0100
Subject: [PATCH 2/3] upd capnp files

---
 .../robot_daemon_protocol_capnp.py            |  7 ++++-
 .../robot_daemon_protocol_capnp.pyi           | 27 +++++++------------
 2 files changed, 16 insertions(+), 18 deletions(-)

diff --git a/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol_capnp.py b/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol_capnp.py
index ffb348732..36059c52a 100644
--- a/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol_capnp.py
+++ b/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol_capnp.py
@@ -1,4 +1,8 @@
-"""This is an automatically generated stub for `robot_daemon_protocol.capnp`."""
+"""
+This is an automatically generated stub for `robot_daemon_protocol.capnp`.
+
+This file was manually edited to contain the RoboServer interface.
+"""
 import os
 
 import capnp  # type: ignore
@@ -33,3 +37,4 @@
 SensorReadings = capnp.load(module_file).SensorReadings
 SensorReadingsBuilder = SensorReadings
 SensorReadingsReader = SensorReadings
+RoboServer = capnp.load(module_file).RoboServer
diff --git a/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol_capnp.pyi b/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol_capnp.pyi
index 25a535ce2..f4fe9d324 100644
--- a/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol_capnp.pyi
+++ b/modular_robot_physical/revolve2/modular_robot_physical/robot_daemon_api/robot_daemon_protocol_capnp.pyi
@@ -1,9 +1,17 @@
-"""This is an automatically generated stub for `robot_daemon_protocol.capnp`."""
+"""
+This is an automatically generated stub for `robot_daemon_protocol.capnp`.
+
+This file was manually edited to work better with the code tools.
+Added __init__ functions
+Made `dict` into `dict[Any,Any]`
+Formatted file with black
+RoboServer
+"""
 from __future__ import annotations
 
 from contextlib import contextmanager
 from io import BufferedWriter
-from typing import Any, Iterator, Literal, Sequence, overload
+from typing import Any, Iterator, Literal, Sequence
 
 class SetupArgs:
     version: str
@@ -88,7 +96,6 @@ class SetupResponseBuilder(SetupResponse):
 class PinControl:
     pin: int
     target: float
-
     def __init__(self, pin: int, target: float) -> None: ...
     @staticmethod
     @contextmanager
@@ -125,7 +132,6 @@ class PinControlBuilder(PinControl):
 
 class ControlArgs:
     setPins: Sequence[PinControl | PinControlBuilder | PinControlReader]
-
     def __init__(
         self, setPins: Sequence[PinControl | PinControlBuilder | PinControlReader]
     ) -> None: ...
@@ -166,7 +172,6 @@ class ControlArgsBuilder(ControlArgs):
 
 class ReadSensorsArgs:
     readPins: Sequence[int]
-
     def __init__(self, readPins: Sequence[int]) -> None: ...
     @staticmethod
     @contextmanager
@@ -204,7 +209,6 @@ class ReadSensorsArgsBuilder(ReadSensorsArgs):
 class ControlAndReadSensorsArgs:
     setPins: Sequence[PinControl | PinControlBuilder | PinControlReader]
     readPins: Sequence[int]
-
     def __init__(
         self,
         setPins: Sequence[PinControl | PinControlBuilder | PinControlReader],
@@ -249,7 +253,6 @@ class Vector3:
     x: float
     y: float
     z: float
-
     def __init__(self, x: float, y: float, z: float) -> None: ...
     @staticmethod
     @contextmanager
@@ -288,7 +291,6 @@ class Image:
     r: list[int]
     g: list[int]
     b: list[int]
-
     def __init__(self, r: list[int], g: list[int], b: list[int]) -> None: ...
     @staticmethod
     @contextmanager
@@ -330,7 +332,6 @@ class SensorReadings:
     imuSpecificForce: Vector3 | Vector3Builder | Vector3Reader
     imuAngularRate: Vector3 | Vector3Builder | Vector3Reader
     cameraView: Image | ImageBuilder | ImageReader
-
     def __init__(
         self,
         pins: Sequence[float],
@@ -358,17 +359,9 @@ class SensorReadings:
     def to_dict(self) -> dict[Any, Any]: ...
 
 class SensorReadingsReader(SensorReadings):
-    imuOrientation: Vector3Reader
-    imuSpecificForce: Vector3Reader
-    imuAngularRate: Vector3Reader
-    cameraView: ImageReader
     def as_builder(self) -> SensorReadingsBuilder: ...
 
 class SensorReadingsBuilder(SensorReadings):
-    imuOrientation: Vector3 | Vector3Builder | Vector3Reader
-    imuSpecificForce: Vector3 | Vector3Builder | Vector3Reader
-    imuAngularRate: Vector3 | Vector3Builder | Vector3Reader
-    cameraView: Image | ImageBuilder | ImageReader
     @staticmethod
     def from_dict(dictionary: dict[Any, Any]) -> SensorReadingsBuilder: ...
     def copy(self) -> SensorReadingsBuilder: ...

From e1662a299fa06955c69715c3463a6df59ba2eb5b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?oliver=20wei=C3=9Fl?= <oliver.weissl@outlook.com>
Date: Wed, 20 Mar 2024 09:38:50 +0100
Subject: [PATCH 3/3] changed setter

---
 modular_robot/revolve2/modular_robot/body/_module.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/modular_robot/revolve2/modular_robot/body/_module.py b/modular_robot/revolve2/modular_robot/body/_module.py
index c1cbbea02..51cc0e702 100644
--- a/modular_robot/revolve2/modular_robot/body/_module.py
+++ b/modular_robot/revolve2/modular_robot/body/_module.py
@@ -251,7 +251,8 @@ def color(self) -> Color:
         """
         return self._color
 
-    def set_color(self, color: Color) -> None:
+    @color.setter
+    def color(self, color: Color) -> None:
         """
         Set the color of a module.