Bug fix: the robot daemon should function even when a camera is not a…

…ttached (#585)

docs/source/physical_robot_core_setup/index.rst

@@ -39,7 +39,7 @@ On the RPi adjust the config in `/boot/config.txt` or on newer systems `/boot/fi
 ------------------
 Setting up the RPi
 ------------------
-**Note**: For students in the CI Group, the RPi is already set up. All RPis are flashed with the same image, so the following steps are not necessary. Additionally, there should be an IP address on the head, allowing you to SSH into it under the *ThymioNet* Wi-Fi. However, be aware that the IP might change from time to time. If you find the IP doesn't work, use the serial connection to log in and obtain the correct IP. For instructions on how to establish a serial connection, please refer to the section below.
+**Note**: For students in the CI Group, the RPi is already set up. If the heads are labeled as `flashed`, it means they are already flashed with the setup image, so the following steps are unnecessary. Additionally, the flashed heads are already connected to the *ThymioNet* Wi-Fi. However, the IP address on the head changes from time to time, so you should use the serial connection to log in and obtain the correct IP address. For instructions on how to establish a serial connection, please refer to the section below.
 Also, note that ongoing development changes will continue in revolve2-modular-robot_physical and revolve2-robohat packages, so make sure to pip install the latest version in your virtual environment.
 
 This step is the same for all types of hardware.

modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/_physical_interface.py

@@ -1,5 +1,5 @@
 from abc import ABC, abstractmethod
-from typing import Sequence
+from typing import Optional, Sequence
 
 import numpy as np
 from numpy.typing import NDArray
@@ -79,7 +79,7 @@ def get_imu_specific_force(self) -> Vector3:
         """
 
     @abstractmethod
-    def get_camera_view(self) -> NDArray[np.uint8]:
+    def get_camera_view(self) -> Optional[NDArray[np.uint8]]:
         """
         Get the current view from the camera.
 

modular_robot_physical/revolve2/modular_robot_physical/physical_interfaces/v2/_v2_physical_interface.py

@@ -1,5 +1,5 @@
 import math
-from typing import Sequence
+from typing import Optional, Sequence
 
 import numpy as np
 from numpy.typing import NDArray
@@ -178,11 +178,18 @@ def get_imu_specific_force(self) -> Vector3:
             raise RuntimeError("Could not get IMU acceleration reading!")
         return Vector3(accel)
 
-    def get_camera_view(self) -> NDArray[np.uint8]:
+    def get_camera_view(self) -> Optional[NDArray[np.uint8]]:
         """
         Get the current view from the camera.
 
         :returns: An image captured from robohatlib.
         """
-        image = self.cam.get_capture_array().astype(np.uint8)
-        return image
+        try:
+            image = self.cam.get_capture_array()
+            if image is None:
+                print("No image captured (camera may not be available).")
+                return None
+            return image.astype(np.uint8)
+        except RuntimeError as e:
+            print(f"Runtime error encountered: {e}")
+            return None

modular_robot_physical/revolve2/modular_robot_physical/remote/_remote.py

@@ -327,9 +327,10 @@ def _get_camera_sensor_state(
         return {}
     else:
         image = sensor_readings.cameraView
-        if list(image.r) == [0] and list(image.g) == [0] and list(image.b) == [0]:
+        if len(image.r) == 0 and len(image.g) == 0 and len(image.b) == 0:
             raise RuntimeError(
-                "Camera image is emtpy. Are you sure you have attached a camera?"
+                "Camera image is empty. Are you sure you have attached a camera? "
+                "If you don't want to get the camera state, don't add it to the body."
             )
         return {
             UUIDKey(camera_sensor): CameraSensorStateImpl(
@@ -352,13 +353,13 @@ def _display_camera_view(
     :raises RuntimeError: If the camera image is empty.
     """
     if camera_sensor is None:
-        print("No camera added in the body.")
+        raise RuntimeError(
+            "Can't display camera because there is no camera added in the body"
+        )
     else:
         image = sensor_readings.cameraView
-        if list(image.r) == [0] and list(image.g) == [0] and list(image.b) == [0]:
-            raise RuntimeError(
-                "Camera image is emtpy. Are you sure you have attached a camera?"
-            )
+        if len(image.r) == 0 and len(image.g) == 0 and len(image.b) == 0:
+            raise RuntimeError("Image is emtpy so nothing can be displayed")
         rgb_image = _capnp_to_camera_view(
             sensor_readings.cameraView, camera_sensor.camera_size
         )

modular_robot_physical/revolve2/modular_robot_physical/robot_daemon/_robo_server_impl.py

@@ -256,7 +256,6 @@ def _get_sensor_readings(
                 pins_readings.append(value)
 
             battery = self._physical_interface.get_battery_level()
-
             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()
@@ -284,7 +283,7 @@ def _vector3_to_capnp(vector: Vector3) -> capnpVector3:
         )
 
     @staticmethod
-    def _camera_view_to_capnp(image: NDArray[np.uint8]) -> capnpImage:
+    def _camera_view_to_capnp(image: NDArray[np.uint8] | None) -> capnpImage:
         """
         Convert an image as an NDArray into an capnp compatible Image.
 
@@ -294,8 +293,15 @@ def _camera_view_to_capnp(image: NDArray[np.uint8]) -> capnpImage:
         :return: The capnp Image object.
         """
         # Convert each channel to a list of Int32 for Cap'n Proto
-        return robot_daemon_protocol_capnp.Image(
-            r=image[:, :, 0].astype(np.int32).flatten().tolist(),
-            g=image[:, :, 1].astype(np.int32).flatten().tolist(),
-            b=image[:, :, 2].astype(np.int32).flatten().tolist(),
-        )
+        if image is None:
+            return robot_daemon_protocol_capnp.Image(
+                r=[],
+                g=[],
+                b=[],
+            )
+        else:
+            return robot_daemon_protocol_capnp.Image(
+                r=image[:, :, 0].astype(np.int32).flatten().tolist(),
+                g=image[:, :, 1].astype(np.int32).flatten().tolist(),
+                b=image[:, :, 2].astype(np.int32).flatten().tolist(),
+            )