Merge pull request #189 from utn-mi/juelg/refactor-camera

Refactor the Camera Stack

Author: juelg

pyproject.toml

@@ -10,9 +10,7 @@ dependencies = ["websockets>=11.0",
     "requests~=2.31",
     "numpy",
     "typer~=0.9",
-    "pydantic~=2.6",
     "gymnasium~=0.29.1",
-    "pydantic_yaml~=1.3",
     "absl-py~=2.1",
     "etils[epath]>=1.7.0",
     "glfw~=2.7",

python/examples/env_cartesian_control.py

@@ -67,7 +67,7 @@ def main():
                 robot_cfg=default_fr3_sim_robot_cfg(),
                 collision_guard=False,
                 gripper_cfg=default_fr3_sim_gripper_cfg(),
-                camera_set_cfg=default_mujoco_cameraset_cfg(),
+                cameras=default_mujoco_cameraset_cfg(),
                 max_relative_movement=0.5,
                 relative_to=RelativeTo.LAST_STEP,
             )

python/examples/env_cartesian_control_digit.py

@@ -13,6 +13,8 @@
     default_mujoco_cameraset_cfg,
 )
 
+from python.rcs.camera.hw import HardwareCameraSet
+
 logger = logging.getLogger(__name__)
 logger.setLevel(logging.INFO)
 
@@ -57,8 +59,8 @@ def main():
                 control_mode=ControlMode.CARTESIAN_TQuat,
                 robot_cfg=default_fr3_hw_robot_cfg(),
                 collision_guard="lab",
+                camera_set=HardwareCameraSet(default_digit({"digit_0": "D21182"})),
                 gripper_cfg=default_fr3_hw_gripper_cfg(),
-                digit_set=default_digit({"digit_0": "D21182"}),
                 max_relative_movement=0.5,
                 relative_to=RelativeTo.LAST_STEP,
             )
@@ -68,7 +70,7 @@ def main():
                 robot_cfg=default_fr3_sim_robot_cfg(),
                 collision_guard=False,
                 gripper_cfg=default_fr3_sim_gripper_cfg(),
-                camera_set_cfg=default_mujoco_cameraset_cfg(),
+                cameras=default_mujoco_cameraset_cfg(),
                 max_relative_movement=0.5,
                 relative_to=RelativeTo.LAST_STEP,
             )

python/examples/env_cartesian_control_tilburg.py

@@ -66,7 +66,7 @@ def main():
                 robot_cfg=default_fr3_sim_robot_cfg(),
                 collision_guard=False,
                 gripper_cfg=default_fr3_sim_gripper_cfg(),
-                camera_set_cfg=default_mujoco_cameraset_cfg(),
+                cameras=default_mujoco_cameraset_cfg(),
                 max_relative_movement=0.5,
                 relative_to=RelativeTo.LAST_STEP,
             )

python/examples/env_joint_control.py

@@ -68,7 +68,7 @@ def main():
                 collision_guard=False,
                 robot_cfg=default_fr3_sim_robot_cfg(),
                 gripper_cfg=default_fr3_sim_gripper_cfg(),
-                camera_set_cfg=default_mujoco_cameraset_cfg(),
+                cameras=default_mujoco_cameraset_cfg(),
                 max_relative_movement=np.deg2rad(5),
                 relative_to=RelativeTo.LAST_STEP,
             )

python/examples/fr3.py

@@ -7,7 +7,7 @@
 from rcs._core.common import RobotPlatform
 from rcs._core.hw import FR3Config, IKSolver
 from rcs._core.sim import CameraType
-from rcs.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig
+from rcs.camera.sim import SimCameraConfig, SimCameraSet
 from rcs.control.fr3_desk import FCI, ContextManager, Desk, load_creds_fr3_desk
 from rcs.envs.creators import get_urdf_path
 
@@ -80,11 +80,22 @@ def main():
 
             # add camera to have a rendering gui
             cameras = {
-                "default_free": SimCameraConfig(identifier="", type=int(CameraType.default_free)),
-                "wrist": SimCameraConfig(identifier="wrist_0", type=int(CameraType.fixed)),
+                "default_free": SimCameraConfig(
+                    identifier="",
+                    type=CameraType.default_free,
+                    resolution_width=1280,
+                    resolution_height=720,
+                    frame_rate=20,
+                ),
+                "wrist": SimCameraConfig(
+                    identifier="wrist_0",
+                    type=CameraType.fixed,
+                    resolution_width=640,
+                    resolution_height=480,
+                    frame_rate=30,
+                ),
             }
-            cam_cfg = SimCameraSetConfig(cameras=cameras, resolution_width=1280, resolution_height=720, frame_rate=20)
-            camera_set = SimCameraSet(simulation, cam_cfg)  # noqa: F841
+            camera_set = SimCameraSet(simulation, cameras)  # noqa: F841
             simulation.open_gui()
 
         else:

python/rcs/_core/common.pyi

@@ -10,6 +10,7 @@ import numpy
 import pybind11_stubgen.typing_ext
 
 __all__ = [
+    "BaseCameraConfig",
     "FR3",
     "FrankaHandTCPOffset",
     "Gripper",
@@ -35,6 +36,13 @@ __all__ = [
 M = typing.TypeVar("M", bound=int)
 N = typing.TypeVar("N", bound=int)
 
+class BaseCameraConfig:
+    frame_rate: int
+    identifier: str
+    resolution_height: int
+    resolution_width: int
+    def __init__(self, identifier: str, frame_rate: int, resolution_width: int, resolution_height: int) -> None: ...
+
 class Gripper:
     def get_normalized_width(self) -> float: ...
     def get_parameters(self) -> GripperConfig: ...

python/rcs/_core/sim.pyi

@@ -15,7 +15,6 @@ __all__ = [
     "Sim",
     "SimCameraConfig",
     "SimCameraSet",
-    "SimCameraSetConfig",
     "SimGripper",
     "SimGripperConfig",
     "SimGripperState",
@@ -83,35 +82,21 @@ class Sim:
     def step(self, k: int) -> None: ...
     def step_until_convergence(self) -> None: ...
 
-class SimCameraConfig:
-    identifier: str
+class SimCameraConfig(rcs._core.common.BaseCameraConfig):
     type: CameraType
-    def __init__(self) -> None: ...
+    def __init__(
+        self, identifier: str, frame_rate: int, resolution_width: int, resolution_height: int, type: CameraType = ...
+    ) -> None: ...
 
 class SimCameraSet:
-    def __init__(self, sim: Sim, cfg: SimCameraSetConfig) -> None: ...
+    def __init__(self, sim: Sim, cameras: dict[str, SimCameraConfig], render_on_demand: bool = True) -> None: ...
     def buffer_size(self) -> int: ...
     def clear_buffer(self) -> None: ...
     def get_latest_frameset(self) -> FrameSet | None: ...
     def get_timestamp_frameset(self, ts: float) -> FrameSet | None: ...
     @property
     def _sim(self) -> Sim: ...
 
-class SimCameraSetConfig:
-    cameras: dict[str, SimCameraConfig]
-    max_buffer_frames: int
-    resolution_height: int
-    resolution_width: int
-    def __init__(self) -> None: ...
-    @property
-    def frame_rate(self) -> int:
-        """
-        The frame rate in which the cameras render in Hz. If set to zero, the camera frames will render on demand and without fixed rate which takes away compute effort.
-        """
-
-    @frame_rate.setter
-    def frame_rate(self, arg0: int) -> None: ...
-
 class SimGripper(rcs._core.common.Gripper):
     def __init__(self, sim: Sim, cfg: SimGripperConfig) -> None: ...
     def get_parameters(self) -> SimGripperConfig: ...

python/rcs/camera/digit_cam.py

@@ -1,40 +1,36 @@
 from digit_interface.digit import Digit
-from rcs.camera.hw import BaseHardwareCameraSet, HWCameraSetConfig
+from rcs._core.common import BaseCameraConfig
+from rcs.camera.hw import HardwareCamera
 from rcs.camera.interface import CameraFrame, DataFrame, Frame
 
 
-class DigitConfig(HWCameraSetConfig):
-    """
-    Configuration for the DIGIT device.
-    This class is used to define the settings for the DIGIT device.
-    """
-
-    stream_name: str = "QVGA"  # options: "QVGA" (60 and 30 fps), "VGA" (30 and 15 fps)
-
-
-class DigitCam(BaseHardwareCameraSet):
+class DigitCam(HardwareCamera):
     """
     This module provides an interface to interact with the DIGIT device.
     It allows for connecting to the device, changing settings, and retrieving information.
     """
 
-    def __init__(self, cfg: DigitConfig):
-        self._cfg = cfg
-        super().__init__()
+    def __init__(self, cameras: dict[str, BaseCameraConfig]):
+        self.cameras = cameras
+        self._camera_names = list(self.cameras.keys())
         self._cameras: dict[str, Digit] = {}
-        self.initalize(self.config)
 
-    def initalize(self, cfg: HWCameraSetConfig):
+    def open(self):
         """
         Initialize the digit interface with the given configuration.
         :param cfg: Configuration for the DIGIT device.
         """
-        for name, serial in cfg.name_to_identifier.items():
-            digit = Digit(serial, name)
+        for name, camera in self.cameras.items():
+            digit = Digit(camera.identifier, name)
             digit.connect()
             self._cameras[name] = digit
 
-    def _poll_frame(self, camera_name: str) -> Frame:
+    @property
+    def camera_names(self) -> list[str]:
+        """Returns the names of the cameras in this set."""
+        return self._camera_names
+
+    def poll_frame(self, camera_name: str) -> Frame:
         """Polls the frame from the camera with the given name."""
         digit = self._cameras[camera_name]
         frame = digit.get_frame()
@@ -45,6 +41,13 @@ def _poll_frame(self, camera_name: str) -> Frame:
 
         return Frame(camera=cf)
 
-    @property
-    def config(self) -> DigitConfig:
-        return self._cfg
+    def close(self):
+        """
+        Closes the connection to the DIGIT device.
+        """
+        for digit in self._cameras.values():
+            digit.disconnect()
+        self._cameras = {}
+
+    def config(self, camera_name) -> BaseCameraConfig:
+        return self.cameras[camera_name]