dexcontrol 0.2.4__py3-none-any.whl → 0.2.10__py3-none-any.whl

dexcontrol/proto/dexcontrol_query_pb2.py

@@ -25,7 +25,7 @@ _sym_db = _symbol_database.Default()
 
 
 
-DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x16\x64\x65xcontrol_query.proto\x12\ndexcontrol\"Z\n\nSetArmMode\x12)\n\x04mode\x18\x01 \x01(\x0e\x32\x1b.dexcontrol.SetArmMode.Mode\"!\n\x04Mode\x12\x0c\n\x08POSITION\x10\x00\x12\x0b\n\x07\x44ISABLE\x10\x01\"Z\n\x0bSetHeadMode\x12*\n\x04mode\x18\x01 \x01(\x0e\x32\x1c.dexcontrol.SetHeadMode.Mode\"\x1f\n\x04Mode\x12\n\n\x06\x45NABLE\x10\x00\x12\x0b\n\x07\x44ISABLE\x10\x01\"\x1a\n\x08SetEstop\x12\x0e\n\x06\x65nable\x18\x01 \x01(\x08\"\x18\n\x06SetLed\x12\x0e\n\x06\x65nable\x18\x01 \x01(\x08\"\x82\x01\n\nClearError\x12\x33\n\tcomponent\x18\x01 \x01(\x0e\x32 .dexcontrol.ClearError.Component\"?\n\tComponent\x12\x0c\n\x08LEFT_ARM\x10\x00\x12\r\n\tRIGHT_ARM\x10\x01\x12\x08\n\x04HEAD\x10\x02\x12\x0b\n\x07\x43HASSIS\x10\x03\"y\n\x0fRebootComponent\x12\x38\n\tcomponent\x18\x01 \x01(\x0e\x32%.dexcontrol.RebootComponent.Component\",\n\tComponent\x12\x07\n\x03\x41RM\x10\x00\x12\t\n\x05TORSO\x10\x01\x12\x0b\n\x07\x43HASSIS\x10\x02\"X\n\x0f\x46irmwareVersion\x12\x18\n\x10hardware_version\x18\x01 \x01(\x05\x12\x18\n\x10software_version\x18\x02 \x01(\x05\x12\x11\n\tmain_hash\x18\x03 \x01(\t\"\xb2\x01\n\x0fSoftwareVersion\x12J\n\x10\x66irmware_version\x18\x01 \x03(\x0b\x32\x30.dexcontrol.SoftwareVersion.FirmwareVersionEntry\x1aS\n\x14\x46irmwareVersionEntry\x12\x0b\n\x03key\x18\x01 \x01(\t\x12*\n\x05value\x18\x02 \x01(\x0b\x32\x1b.dexcontrol.FirmwareVersion:\x02\x38\x01\"\x9d\x01\n\x14SingleComponentState\x12\x11\n\tconnected\x18\x01 \x01(\x08\x12,\n\x07\x65nabled\x18\x02 \x01(\x0e\x32\x1b.dexcontrol.ComponentStatus\x12\x30\n\x0b\x65rror_state\x18\x03 \x01(\x0e\x32\x1b.dexcontrol.ComponentStatus\x12\x12\n\nerror_code\x18\x04 \x01(\t\"\x9b\x01\n\x0f\x43omponentStates\x12\x37\n\x06states\x18\x01 \x03(\x0b\x32\'.dexcontrol.ComponentStates.StatesEntry\x1aO\n\x0bStatesEntry\x12\x0b\n\x03key\x18\x01 \x01(\t\x12/\n\x05value\x18\x02 \x01(\x0b\x32 .dexcontrol.SingleComponentState:\x02\x38\x01*0\n\x0f\x43omponentStatus\x12\n\n\x06NORMAL\x10\x00\x12\x06\n\x02NA\x10\x01\x12\t\n\x05\x45RROR\x10\x02\x62\x06proto3')
+DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x16\x64\x65xcontrol_query.proto\x12\ndexcontrol\"Z\n\nSetArmMode\x12)\n\x04mode\x18\x01 \x01(\x0e\x32\x1b.dexcontrol.SetArmMode.Mode\"!\n\x04Mode\x12\x0c\n\x08POSITION\x10\x00\x12\x0b\n\x07\x44ISABLE\x10\x01\"Z\n\x0bSetHeadMode\x12*\n\x04mode\x18\x01 \x01(\x0e\x32\x1c.dexcontrol.SetHeadMode.Mode\"\x1f\n\x04Mode\x12\n\n\x06\x45NABLE\x10\x00\x12\x0b\n\x07\x44ISABLE\x10\x01\"\x1a\n\x08SetEstop\x12\x0e\n\x06\x65nable\x18\x01 \x01(\x08\"\x18\n\x06SetLed\x12\x0e\n\x06\x65nable\x18\x01 \x01(\x08\"\x82\x01\n\nClearError\x12\x33\n\tcomponent\x18\x01 \x01(\x0e\x32 .dexcontrol.ClearError.Component\"?\n\tComponent\x12\x0c\n\x08LEFT_ARM\x10\x00\x12\r\n\tRIGHT_ARM\x10\x01\x12\x08\n\x04HEAD\x10\x02\x12\x0b\n\x07\x43HASSIS\x10\x03\"y\n\x0fRebootComponent\x12\x38\n\tcomponent\x18\x01 \x01(\x0e\x32%.dexcontrol.RebootComponent.Component\",\n\tComponent\x12\x07\n\x03\x41RM\x10\x00\x12\t\n\x05TORSO\x10\x01\x12\x0b\n\x07\x43HASSIS\x10\x02\"\x80\x01\n\x0f\x46irmwareVersion\x12\x18\n\x10hardware_version\x18\x01 \x01(\x05\x12\x18\n\x10software_version\x18\x02 \x01(\x05\x12\x14\n\x0c\x63ompile_time\x18\x03 \x01(\x05\x12\x11\n\tmain_hash\x18\x04 \x01(\t\x12\x10\n\x08sub_hash\x18\x05 \x01(\t\"\xb2\x01\n\x0fSoftwareVersion\x12J\n\x10\x66irmware_version\x18\x01 \x03(\x0b\x32\x30.dexcontrol.SoftwareVersion.FirmwareVersionEntry\x1aS\n\x14\x46irmwareVersionEntry\x12\x0b\n\x03key\x18\x01 \x01(\t\x12*\n\x05value\x18\x02 \x01(\x0b\x32\x1b.dexcontrol.FirmwareVersion:\x02\x38\x01\"\x9d\x01\n\x14SingleComponentState\x12\x11\n\tconnected\x18\x01 \x01(\x08\x12,\n\x07\x65nabled\x18\x02 \x01(\x0e\x32\x1b.dexcontrol.ComponentStatus\x12\x30\n\x0b\x65rror_state\x18\x03 \x01(\x0e\x32\x1b.dexcontrol.ComponentStatus\x12\x12\n\nerror_code\x18\x04 \x01(\t\"\x9b\x01\n\x0f\x43omponentStates\x12\x37\n\x06states\x18\x01 \x03(\x0b\x32\'.dexcontrol.ComponentStates.StatesEntry\x1aO\n\x0bStatesEntry\x12\x0b\n\x03key\x18\x01 \x01(\t\x12/\n\x05value\x18\x02 \x01(\x0b\x32 .dexcontrol.SingleComponentState:\x02\x38\x01*0\n\x0f\x43omponentStatus\x12\n\n\x06NORMAL\x10\x00\x12\x06\n\x02NA\x10\x01\x12\t\n\x05\x45RROR\x10\x02\x62\x06proto3')
 
 _globals = globals()
 _builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
@@ -36,8 +36,8 @@ if not _descriptor._USE_C_DESCRIPTORS:
   _globals['_SOFTWAREVERSION_FIRMWAREVERSIONENTRY']._serialized_options = b'8\001'
   _globals['_COMPONENTSTATES_STATESENTRY']._loaded_options = None
   _globals['_COMPONENTSTATES_STATESENTRY']._serialized_options = b'8\001'
-  _globals['_COMPONENTSTATUS']._serialized_start=1121
-  _globals['_COMPONENTSTATUS']._serialized_end=1169
+  _globals['_COMPONENTSTATUS']._serialized_start=1162
+  _globals['_COMPONENTSTATUS']._serialized_end=1210
   _globals['_SETARMMODE']._serialized_start=38
   _globals['_SETARMMODE']._serialized_end=128
   _globals['_SETARMMODE_MODE']._serialized_start=95
@@ -58,16 +58,16 @@ if not _descriptor._USE_C_DESCRIPTORS:
   _globals['_REBOOTCOMPONENT']._serialized_end=530
   _globals['_REBOOTCOMPONENT_COMPONENT']._serialized_start=486
   _globals['_REBOOTCOMPONENT_COMPONENT']._serialized_end=530
-  _globals['_FIRMWAREVERSION']._serialized_start=532
-  _globals['_FIRMWAREVERSION']._serialized_end=620
-  _globals['_SOFTWAREVERSION']._serialized_start=623
-  _globals['_SOFTWAREVERSION']._serialized_end=801
-  _globals['_SOFTWAREVERSION_FIRMWAREVERSIONENTRY']._serialized_start=718
-  _globals['_SOFTWAREVERSION_FIRMWAREVERSIONENTRY']._serialized_end=801
-  _globals['_SINGLECOMPONENTSTATE']._serialized_start=804
-  _globals['_SINGLECOMPONENTSTATE']._serialized_end=961
-  _globals['_COMPONENTSTATES']._serialized_start=964
-  _globals['_COMPONENTSTATES']._serialized_end=1119
-  _globals['_COMPONENTSTATES_STATESENTRY']._serialized_start=1040
-  _globals['_COMPONENTSTATES_STATESENTRY']._serialized_end=1119
+  _globals['_FIRMWAREVERSION']._serialized_start=533
+  _globals['_FIRMWAREVERSION']._serialized_end=661
+  _globals['_SOFTWAREVERSION']._serialized_start=664
+  _globals['_SOFTWAREVERSION']._serialized_end=842
+  _globals['_SOFTWAREVERSION_FIRMWAREVERSIONENTRY']._serialized_start=759
+  _globals['_SOFTWAREVERSION_FIRMWAREVERSIONENTRY']._serialized_end=842
+  _globals['_SINGLECOMPONENTSTATE']._serialized_start=845
+  _globals['_SINGLECOMPONENTSTATE']._serialized_end=1002
+  _globals['_COMPONENTSTATES']._serialized_start=1005
+  _globals['_COMPONENTSTATES']._serialized_end=1160
+  _globals['_COMPONENTSTATES_STATESENTRY']._serialized_start=1081
+  _globals['_COMPONENTSTATES_STATESENTRY']._serialized_end=1160
 # @@protoc_insertion_point(module_scope)

dexcontrol/proto/dexcontrol_query_pb2.pyi

@@ -86,14 +86,18 @@ class RebootComponent(_message.Message):
     def __init__(self, component: _Optional[_Union[RebootComponent.Component, str]] = ...) -> None: ...
 
 class FirmwareVersion(_message.Message):
-    __slots__ = ("hardware_version", "software_version", "main_hash")
+    __slots__ = ("hardware_version", "software_version", "compile_time", "main_hash", "sub_hash")
     HARDWARE_VERSION_FIELD_NUMBER: _ClassVar[int]
     SOFTWARE_VERSION_FIELD_NUMBER: _ClassVar[int]
+    COMPILE_TIME_FIELD_NUMBER: _ClassVar[int]
     MAIN_HASH_FIELD_NUMBER: _ClassVar[int]
+    SUB_HASH_FIELD_NUMBER: _ClassVar[int]
     hardware_version: int
     software_version: int
+    compile_time: int
     main_hash: str
-    def __init__(self, hardware_version: _Optional[int] = ..., software_version: _Optional[int] = ..., main_hash: _Optional[str] = ...) -> None: ...
+    sub_hash: str
+    def __init__(self, hardware_version: _Optional[int] = ..., software_version: _Optional[int] = ..., compile_time: _Optional[int] = ..., main_hash: _Optional[str] = ..., sub_hash: _Optional[str] = ...) -> None: ...
 
 class SoftwareVersion(_message.Message):
     __slots__ = ("firmware_version",)

dexcontrol/robot.py

@@ -51,6 +51,7 @@ from dexcontrol.sensors import Sensors
 from dexcontrol.utils.constants import ROBOT_NAME_ENV_VAR
 from dexcontrol.utils.os_utils import get_robot_model, resolve_key_name
 from dexcontrol.utils.pb_utils import (
+    TYPE_SOFTWARE_VERSION,
     ComponentStatus,
     software_version_to_dict,
     status_to_dict,
@@ -416,9 +417,12 @@ class Robot:
         timeout_sec: Final[int] = 8
         check_interval: Final[float] = 0.1  # Check every 100ms
 
-        with console.status(
+        status = console.status(
             "[bold green]Waiting for components to become active..."
-        ) as status:
+        )
+        status.start()
+
+        try:
             for name in component_names:
                 # Check if shutdown was triggered
                 if self._shutdown_called:
@@ -447,6 +451,8 @@ class Robot:
 
                         # Wait a short interval before checking again
                         time.sleep(check_interval)
+        finally:
+            status.stop()
 
         if not all(actives):
             self.shutdown()
@@ -527,34 +533,30 @@ class Robot:
                         f"Error stopping component {component.__class__.__name__}: {e}"
                     )
 
-        # Longer delay to ensure all stop operations complete and background threads settle
-        time.sleep(0.3)
+        # Brief delay to ensure all stop operations complete
+        time.sleep(0.1)
 
         # Shutdown sensors first (they may have background threads)
         try:
-            self.sensors.shutdown()
-            # Give extra time for all sensor subscribers to undeclare cleanly
-            time.sleep(0.5)
+            if hasattr(self, "sensors") and self.sensors is not None:
+                self.sensors.shutdown()
+                # Give time for sensor subscribers to undeclare cleanly
+                time.sleep(0.2)
         except Exception as e:  # pylint: disable=broad-except
             logger.error(f"Error shutting down sensors: {e}")
 
-        # Additional delay for sensor shutdown to complete
-        time.sleep(0.2)
-
         # Shutdown components in reverse order
         for component in reversed(self._components):
             if component is not None:
                 try:
                     component.shutdown()
-                    # Small delay between each component shutdown to prevent race conditions
-                    time.sleep(0.05)
                 except Exception as e:  # pylint: disable=broad-except
                     logger.error(
                         f"Error shutting down component {component.__class__.__name__}: {e}"
                     )
 
-        # Longer delay to allow all component shutdown to complete
-        time.sleep(0.5)
+        # Brief delay to allow component shutdown to complete
+        time.sleep(0.1)
 
         # Enhanced Zenoh session close with better synchronization
         zenoh_close_success = False
@@ -563,8 +565,8 @@ class Robot:
             """Close zenoh session in a separate thread."""
             nonlocal zenoh_close_success
             try:
-                # Give Zenoh even more time to clean up internal resources and settle threads
-                time.sleep(0.5)
+                # Brief delay for Zenoh cleanup
+                time.sleep(0.1)
                 self._zenoh_session.close()
                 zenoh_close_success = True
             except Exception as e:  # pylint: disable=broad-except
@@ -574,20 +576,19 @@ class Robot:
         close_thread = threading.Thread(target=_close_zenoh_session, daemon=False)
         close_thread.start()
 
-        # Wait for close with extended timeout to avoid race conditions
+        # Wait for close with reasonable timeout
         close_thread.join(timeout=3.0)
 
         if close_thread.is_alive():
             logger.warning(
-                "Zenoh session close timed out after 5s, continuing shutdown"
+                "Zenoh session close timed out after 1s, continuing shutdown"
             )
-            # Force daemon mode so it doesn't block process exit
-            close_thread.daemon = True
+            # Thread will be left to finish in background, but won't block shutdown
         elif zenoh_close_success:
             logger.debug("Zenoh session closed cleanly")
 
-        # Extended final delay to allow any remaining Zenoh background threads to finish
-        time.sleep(0.8)
+        # Brief final delay for cleanup
+        time.sleep(0.5)
 
         logger.info("Robot shutdown complete")
 
@@ -601,13 +602,7 @@ class Robot:
 
     def get_software_version(
         self, show: bool = True
-    ) -> dict[
-        str,
-        dict[
-            Literal["hardware_version", "software_version", "main_hash"],
-            int | str,
-        ],
-    ]:
+    ) -> dict[str, TYPE_SOFTWARE_VERSION]:
         """Retrieve software version information for all components.
 
         Args:
@@ -744,14 +739,18 @@ class Robot:
 
     def get_joint_pos_dict(
         self,
-        component: Literal["left_arm", "right_arm", "torso", "head"]
-        | list[Literal["left_arm", "right_arm", "torso", "head"]],
+        component: Literal[
+            "left_arm", "right_arm", "torso", "head", "left_hand", "right_hand"
+        ]
+        | list[
+            Literal["left_arm", "right_arm", "torso", "head", "left_hand", "right_hand"]
+        ],
     ) -> dict[str, float]:
         """Get the joint positions of one or more robot components.
 
         Args:
             component: Component name or list of component names to get joint positions for.
-                Valid components are "left_arm", "right_arm", "torso", and "head".
+                Valid components are "left_arm", "right_arm", "torso", "head", "left_hand", and "right_hand".
 
         Returns:
             Dictionary mapping joint names to joint positions.
@@ -766,6 +765,8 @@ class Robot:
             "right_arm": self.right_arm,
             "torso": self.torso,
             "head": self.head,
+            "left_hand": self.left_hand,
+            "right_hand": self.right_hand,
         }
 
         try:
@@ -831,6 +832,8 @@ class Robot:
         relative: bool = False,
         wait_time: float = 0.0,
         wait_kwargs: dict[str, Any] | None = None,
+        exit_on_reach: bool = False,
+        exit_on_reach_kwargs: dict[str, Any] | None = None,
     ) -> None:
         """Set the joint positions of the robot.
 
@@ -841,7 +844,9 @@ class Robot:
             wait_time: Time to wait for movement completion in seconds.
             wait_kwargs: Additional parameters for trajectory generation.
                 control_hz: Control frequency in Hz (default: 100).
-                max_vel: Maximum velocity for trajectory (default: 0.5).
+                max_vel: Maximum velocity for trajectory (default: 3.).
+            exit_on_reach: If True, the function will exit when the joint positions are reached.
+            exit_on_reach_kwargs: Optional parameters for exit when the joint positions are reached.
 
         Raises:
             ValueError: If any component name is invalid.
@@ -851,6 +856,7 @@ class Robot:
             wait_kwargs = {}
 
         try:
+            start_time = time.time()
             component_map = self._get_component_map()
 
             # Validate component names
@@ -876,11 +882,69 @@ class Robot:
                     relative,
                     wait_time,
                     wait_kwargs,
+                    exit_on_reach=exit_on_reach,
+                    exit_on_reach_kwargs=exit_on_reach_kwargs,
                 )
+            remaining_time = wait_time - (time.time() - start_time)
+            if remaining_time <= 0:
+                return
+
+            self._wait_for_multi_component_positions(
+                component_map,
+                pv_components,
+                joint_pos,
+                start_time,
+                wait_time,
+                exit_on_reach,
+                exit_on_reach_kwargs,
+            )
 
         except Exception as e:
             raise RuntimeError(f"Failed to set joint positions: {e}") from e
 
+    def _wait_for_multi_component_positions(
+        self,
+        component_map: dict[str, Any],
+        components: list[str],
+        joint_pos: dict[str, list[float] | np.ndarray],
+        start_time: float,
+        wait_time: float,
+        exit_on_reach: bool = False,
+        exit_on_reach_kwargs: dict[str, Any] | None = None,
+    ) -> None:
+        """Wait for multiple components to reach target positions.
+
+        Args:
+            component_map: Mapping of component names to component instances.
+            components: List of component names to check.
+            joint_pos: Target joint positions for each component.
+            start_time: Time when the operation started.
+            wait_time: Maximum time to wait.
+            exit_on_reach: If True, exit early when all positions are reached.
+            exit_on_reach_kwargs: Optional parameters for position checking.
+        """
+        sleep_interval = 0.01  # Use consistent sleep interval
+
+        if exit_on_reach:
+            if components:
+                # Set default tolerance if not provided
+                exit_on_reach_kwargs = exit_on_reach_kwargs or {}
+
+                # Wait until all positions are reached or timeout
+                while time.time() - start_time < wait_time:
+                    if all(
+                        component_map[c].is_joint_pos_reached(
+                            joint_pos[c], **exit_on_reach_kwargs
+                        )
+                        for c in components
+                    ):
+                        break
+                    time.sleep(sleep_interval)
+        else:
+            # Simple wait without position checking
+            while time.time() - start_time < wait_time:
+                time.sleep(sleep_interval)
+
     def compensate_torso_pitch(self, joint_pos: np.ndarray, part: str) -> np.ndarray:
         """Compensate for torso pitch in joint positions.
 
@@ -1097,6 +1161,8 @@ class Robot:
         relative: bool,
         wait_time: float,
         wait_kwargs: dict[str, Any],
+        exit_on_reach: bool = False,
+        exit_on_reach_kwargs: dict[str, Any] | None = None,
     ) -> None:
         """Set non-PV components with smooth trajectory over wait_time.
 
@@ -1107,9 +1173,11 @@ class Robot:
             relative: Whether positions are relative.
             wait_time: Time to wait for movement completion.
             wait_kwargs: Additional trajectory parameters.
+            exit_on_reach: If True, the function will exit when the joint positions are reached.
+            exit_on_reach_kwargs: Optional parameters for exit when the joint positions are reached.
         """
-        control_hz = wait_kwargs.get("control_hz", 100)
-        max_vel = wait_kwargs.get("max_vel", 0.5)
+        control_hz = wait_kwargs.get("control_hz", self.left_arm._default_control_hz)
+        max_vel = wait_kwargs.get("max_vel", self.left_arm._default_max_vel)
 
         # Generate trajectories for smooth motion during wait_time
         rate_limiter = RateLimiter(control_hz)
@@ -1141,5 +1209,12 @@ class Robot:
                 break
 
         # Wait for any remaining time
-        while time.time() - start_time < wait_time:
-            rate_limiter.sleep()
+        self._wait_for_multi_component_positions(
+            component_map,
+            non_pv_components,
+            joint_pos,
+            start_time,
+            wait_time,
+            exit_on_reach,
+            exit_on_reach_kwargs,
+        )

dexcontrol/sensors/camera/rgb_camera.py

@@ -8,64 +8,94 @@
 # 2. Commercial License
 #    For commercial licensing terms, contact: contact@dexmate.ai
 
-"""RGB camera sensor implementation using RTC subscriber with Zenoh query."""
+"""RGB camera sensor implementation using RTC or Zenoh subscriber."""
 
 import logging
+from typing import Optional, Union
 
 import numpy as np
 import zenoh
 
+from dexcontrol.config.sensors.cameras import RGBCameraConfig
 from dexcontrol.utils.rtc_utils import create_rtc_subscriber_with_config
+from dexcontrol.utils.subscribers.camera import RGBCameraSubscriber
+from dexcontrol.utils.subscribers.rtc import RTCSubscriber
 
 logger = logging.getLogger(__name__)
 
 
 class RGBCameraSensor:
-    """RGB camera sensor using RTC subscriber.
+    """RGB camera sensor that supports both RTC and standard Zenoh subscribers.
 
-    This sensor provides RGB image data from a camera using RTC.
-    It first queries Zenoh for RTC connection information, then creates
-    a RTC subscriber for efficient data handling.
+    This sensor provides RGB image data from a camera. It can be configured to use
+    either a high-performance RTC subscriber for real-time video streams or a
+    standard Zenoh subscriber for raw image topics. The mode is controlled by the
+    `use_rtc` flag in the `RGBCameraConfig`.
     """
 
     def __init__(
         self,
-        configs,
+        configs: RGBCameraConfig,
         zenoh_session: zenoh.Session,
-        *args,
-        **kwargs,
     ) -> None:
-        """Initialize the RGB camera sensor.
+        """Initialize the RGB camera sensor based on the provided configuration.
 
         Args:
-            configs: Configuration for the RGB camera sensor.
+            configs: Configuration object for the RGB camera sensor.
             zenoh_session: Active Zenoh session for communication.
         """
         self._name = configs.name
-
-        # Create the RTC subscriber using the utility function
-        self._subscriber = create_rtc_subscriber_with_config(
-            zenoh_session=zenoh_session,
-            config=configs.subscriber_config.rgb,
-            name=f"{self._name}_subscriber",
-            enable_fps_tracking=configs.enable_fps_tracking,
-            fps_log_interval=configs.fps_log_interval,
-        )
-
-        if self._subscriber is None:
-            logger.warning(f"Failed to create RTC subscriber for {self._name}")
-            # Continue initialization without subscriber
+        self._subscriber: Optional[Union[RTCSubscriber, RGBCameraSubscriber]] = None
+
+        subscriber_config = configs.subscriber_config.get("rgb", {})
+        if not subscriber_config or not subscriber_config.get("enable", False):
+            logger.info(f"RGBCameraSensor '{self._name}' is disabled in config.")
+            return
+
+        try:
+            if configs.use_rtc:
+                logger.info(f"'{self._name}': Using RTC subscriber.")
+                self._subscriber = create_rtc_subscriber_with_config(
+                    zenoh_session=zenoh_session,
+                    config=subscriber_config,
+                    name=f"{self._name}_subscriber",
+                    enable_fps_tracking=configs.enable_fps_tracking,
+                    fps_log_interval=configs.fps_log_interval,
+                )
+            else:
+                logger.info(f"'{self._name}': Using standard Zenoh subscriber.")
+                topic = subscriber_config.get("topic")
+                if topic:
+                    self._subscriber = RGBCameraSubscriber(
+                        topic=topic,
+                        zenoh_session=zenoh_session,
+                        name=f"{self._name}_subscriber",
+                        enable_fps_tracking=configs.enable_fps_tracking,
+                        fps_log_interval=configs.fps_log_interval,
+                    )
+                else:
+                    logger.warning(
+                        f"No 'topic' specified for '{self._name}' in non-RTC mode."
+                    )
+
+            if self._subscriber is None:
+                logger.warning(f"Failed to create subscriber for '{self._name}'.")
+
+        except Exception as e:
+            logger.error(f"Error creating subscriber for '{self._name}': {e}")
+            self._subscriber = None
 
     def shutdown(self) -> None:
-        """Shutdown the camera sensor."""
+        """Shutdown the camera sensor and release all resources."""
         if self._subscriber:
             self._subscriber.shutdown()
+            logger.info(f"'{self._name}' sensor shut down.")
 
     def is_active(self) -> bool:
         """Check if the camera sensor is actively receiving data.
 
         Returns:
-            True if receiving data, False otherwise.
+            True if the subscriber exists and is receiving data, False otherwise.
         """
         return self._subscriber.is_active() if self._subscriber else False
 
@@ -76,15 +106,18 @@ class RGBCameraSensor:
             timeout: Maximum time to wait in seconds.
 
         Returns:
-            True if sensor becomes active, False if timeout is reached.
+            True if the sensor becomes active within the timeout, False otherwise.
         """
-        return self._subscriber.wait_for_active(timeout) if self._subscriber else False
+        if not self._subscriber:
+            logger.warning(f"'{self._name}': Cannot wait, no subscriber initialized.")
+            return False
+        return self._subscriber.wait_for_active(timeout)
 
     def get_obs(self) -> np.ndarray | None:
-        """Get the latest RGB image data.
+        """Get the latest observation (RGB image) from the sensor.
 
         Returns:
-            Latest RGB image as numpy array (HxWxC) if available, None otherwise.
+            The latest RGB image as a numpy array (HxWxC) if available, otherwise None.
         """
         return self._subscriber.get_latest_data() if self._subscriber else None
 
@@ -105,3 +138,39 @@ class RGBCameraSensor:
             Sensor name string.
         """
         return self._name
+
+    @property
+    def height(self) -> int:
+        """Get the height of the camera image.
+
+        Returns:
+            Height of the camera image.
+        """
+        image = self.get_obs()
+        if image is None:
+            return 0
+        return image.shape[0]
+
+    @property
+    def width(self) -> int:
+        """Get the width of the camera image.
+
+        Returns:
+            Width of the camera image.
+        """
+        image = self.get_obs()
+        if image is None:
+            return 0
+        return image.shape[1]
+
+    @property
+    def resolution(self) -> tuple[int, int]:
+        """Get the resolution of the camera image.
+
+        Returns:
+            Resolution of the camera image.
+        """
+        image = self.get_obs()
+        if image is None:
+            return 0, 0
+        return image.shape[0], image.shape[1]