346 Physical Camera Sensor (#482)

modular_robot/revolve2/modular_robot/body/_module.py

@@ -251,6 +251,15 @@ def color(self) -> Color:
         """
         return self._color
 
+    @color.setter
+    def 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]:
         """

modular_robot_physical/revolve2/modular_robot_physical/_protocol_version.py

@@ -1 +1 @@
-PROTOCOL_VERSION = "1.0.0"
+PROTOCOL_VERSION = "1.0.2"

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.
+        """

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()

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()

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

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,23 +23,27 @@ 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.
 
         :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

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,25 +158,22 @@ 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={
                         pin: position
                         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,29 +222,30 @@ 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={
                             pin: position
                             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,