dexcontrol 0.2.1__py3-none-any.whl → 0.2.3__py3-none-any.whl

dexcontrol/robot.py

@@ -1,7 +1,12 @@
-# Copyright (c) 2025 Dexmate CORPORATION & AFFILIATES. All rights reserved.
+# Copyright (C) 2025 Dexmate Inc.
 #
-# Licensed under the Apache License, Version 2.0 with Commons Clause License
-# Condition v1.0 [see LICENSE for details].
+# This software is dual-licensed:
+#
+# 1. GNU Affero General Public License v3.0 (AGPL-3.0)
+#    See LICENSE-AGPL for details
+#
+# 2. Commercial License
+#    For commercial licensing terms, contact: contact@dexmate.ai
 
 """Main robot interface module.
 
@@ -36,6 +41,7 @@ import omegaconf
 import zenoh
 from loguru import logger
 from rich.console import Console
+from rich.table import Table
 
 import dexcontrol
 from dexcontrol.config.vega import VegaConfig, get_vega_config
@@ -106,7 +112,6 @@ def _register_signal_handlers() -> None:
         signal.signal(signal.SIGHUP, _signal_handler)
 
     _signal_handlers_registered = True
-    logger.debug("Signal handlers registered for graceful robot shutdown")
 
 
 class ComponentConfig(omegaconf.DictConfig):
@@ -178,7 +183,7 @@ class Robot:
         """Initializes the Robot with the given configuration.
 
         Args:
-            robot_model: Optional robot variant name (e.g., "vega_rc2", "vega_1").
+            robot_model: Optional robot variant name (e.g., "vega-rc2", "vega-1").
                 If configs is None, this will be used to get the appropriate config.
                 Ignored if configs is provided.
             configs: Configuration parameters for all robot components.
@@ -192,6 +197,8 @@ class Robot:
             RuntimeError: If any critical component fails to become active within timeout.
             ValueError: If robot_model is invalid or configs cannot be loaded.
         """
+        self._components: list[RobotComponent] = []
+
         if robot_model is None:
             robot_model = get_robot_model()
         self._robot_model: Final[str] = robot_model
@@ -208,7 +215,6 @@ class Robot:
         except Exception as e:
             raise RuntimeError(f"Failed to initialize zenoh session: {e}") from e
 
-        self._components: list[RobotComponent] = []
         self._robot_name: Final[str] = os.getenv(ROBOT_NAME_ENV_VAR, "robot")
         self._pv_components: Final[list[str]] = [
             "left_hand",
@@ -234,18 +240,19 @@ class Robot:
             self.shutdown()  # Clean up on initialization failure
             raise RuntimeError(f"Failed to initialize robot components: {e}") from e
 
+        # Ensure all components are active
         try:
-            self.sensors = Sensors(self._configs.sensors, self._zenoh_session)
+            self._wait_for_components()
         except Exception as e:
             self.shutdown()  # Clean up on initialization failure
-            raise RuntimeError(f"Failed to initialize sensors: {e}") from e
+            raise RuntimeError(f"Failed to activate components: {e}") from e
 
-        # Ensure all components are active
         try:
-            self._wait_for_components()
+            self.sensors = Sensors(self._configs.sensors, self._zenoh_session)
+            self.sensors.wait_for_all_active()
         except Exception as e:
             self.shutdown()  # Clean up on initialization failure
-            raise RuntimeError(f"Failed to activate components: {e}") from e
+            raise RuntimeError(f"Failed to initialize sensors: {e}") from e
 
         # Set default modes
         try:
@@ -306,12 +313,17 @@ class Robot:
         Args:
             robot_model: The robot model being initialized.
         """
-        print("=" * 50)
-        print(f"Robot name: {self._robot_name}")
+        console = Console()
+        table = Table(show_header=False)
+        table.add_column(style="cyan", no_wrap=True)
+        table.add_column(style="white")
+
+        table.add_row("Robot Name", str(self._robot_name))
         if robot_model:
-            print(f"Robot model: {robot_model}")
-        print(f"Communication config file: {dexcontrol.COMM_CFG_PATH}")
-        print("=" * 50)
+            table.add_row("Robot Model", str(robot_model))
+        table.add_row("Communication Config", str(dexcontrol.COMM_CFG_PATH))
+
+        console.print(table)
 
     def _init_components(self, config_dict: dict[str, Any]) -> None:
         """Initialize robot components from configuration.
@@ -502,38 +514,80 @@ class Robot:
         except Exception:  # pylint: disable=broad-except
             pass  # WeakSet may already have removed it
 
-        # Shutdown components in reverse order
-        for component in reversed(self._components):
+        # First, stop all components that have stop methods to halt ongoing operations
+        for component in self._components:
             if component is not None:
                 try:
-                    component.shutdown()
+                    if hasattr(component, "stop"):
+                        method = getattr(component, "stop")
+                        if callable(method):
+                            method()
                 except Exception as e:  # pylint: disable=broad-except
                     logger.error(
-                        f"Error shutting down component {component.__class__.__name__}: {e}"
+                        f"Error stopping component {component.__class__.__name__}: {e}"
                     )
 
+        # Longer delay to ensure all stop operations complete and background threads settle
+        time.sleep(0.3)
+
+        # 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)
         except Exception as e:  # pylint: disable=broad-except
             logger.error(f"Error shutting down sensors: {e}")
 
-        # Use timeout-based zenoh session close to avoid long waits
+        # 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)
+
+        # Enhanced Zenoh session close with better synchronization
+        zenoh_close_success = False
+
         def _close_zenoh_session():
             """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)
                 self._zenoh_session.close()
+                zenoh_close_success = True
             except Exception as e:  # pylint: disable=broad-except
-                logger.debug(f"Zenoh session close error: {e}")
+                logger.warning(f"Zenoh session close error: {e}")
 
-        close_thread = threading.Thread(target=_close_zenoh_session, daemon=True)
+        # Use non-daemon thread for proper cleanup
+        close_thread = threading.Thread(target=_close_zenoh_session, daemon=False)
         close_thread.start()
 
-        # Wait for close with timeout
-        close_thread.join(timeout=0.5)  # 500ms timeout
+        # Wait for close with extended timeout to avoid race conditions
+        close_thread.join(timeout=3.0)
+
         if close_thread.is_alive():
-            logger.warning("Zenoh session close timed out, continuing shutdown")
-        else:
-            logger.debug("Zenoh session closed successfully")
+            logger.warning(
+                "Zenoh session close timed out after 5s, continuing shutdown"
+            )
+            # Force daemon mode so it doesn't block process exit
+            close_thread.daemon = True
+        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)
 
         logger.info("Robot shutdown complete")
 

dexcontrol/sensors/__init__.py

@@ -1,7 +1,12 @@
-# Copyright (c) 2025 Dexmate CORPORATION & AFFILIATES. All rights reserved.
+# Copyright (C) 2025 Dexmate Inc.
 #
-# Licensed under the Apache License, Version 2.0 with Commons Clause License
-# Condition v1.0 [see LICENSE for details].
+# This software is dual-licensed:
+#
+# 1. GNU Affero General Public License v3.0 (AGPL-3.0)
+#    See LICENSE-AGPL for details
+#
+# 2. Commercial License
+#    For commercial licensing terms, contact: contact@dexmate.ai
 
 """Sensor implementations for dexcontrol.
 
@@ -10,10 +15,10 @@ using Zenoh subscribers for data communication.
 """
 
 # Import camera sensors
-from .camera import GeminiCameraSensor, RGBCameraSensor
+from .camera import RGBCameraSensor, ZedCameraSensor
 
 # Import IMU sensors
-from .imu import GeminiIMUSensor, NineAxisIMUSensor
+from .imu import ChassisIMUSensor, ZedIMUSensor
 
 # Import other sensors
 from .lidar import RPLidarSensor
@@ -25,11 +30,11 @@ from .ultrasonic import UltrasonicSensor
 __all__ = [
     # Camera sensors
     "RGBCameraSensor",
-    "GeminiCameraSensor",
+    "ZedCameraSensor",
 
     # IMU sensors
-    "NineAxisIMUSensor",
-    "GeminiIMUSensor",
+    "ChassisIMUSensor",
+    "ZedIMUSensor",
 
     # Other sensors
     "RPLidarSensor",

dexcontrol/sensors/camera/__init__.py

@@ -1,18 +1,23 @@
-# Copyright (c) 2025 Dexmate CORPORATION & AFFILIATES. All rights reserved.
+# Copyright (C) 2025 Dexmate Inc.
 #
-# Licensed under the Apache License, Version 2.0 with Commons Clause License
-# Condition v1.0 [see LICENSE for details].
+# This software is dual-licensed:
+#
+# 1. GNU Affero General Public License v3.0 (AGPL-3.0)
+#    See LICENSE-AGPL for details
+#
+# 2. Commercial License
+#    For commercial licensing terms, contact: contact@dexmate.ai
 
 """Camera sensor implementations using Zenoh subscribers.
 
 This module provides camera sensor classes that use the specialized camera
-subscribers for RGB and RGBD Gemini camera data, matching the dexsensor structure.
+subscribers for RGB and RGBD camera data, matching the dexsensor structure.
 """
 
-from .gemini_camera import GeminiCameraSensor
 from .rgb_camera import RGBCameraSensor
+from .zed_camera import ZedCameraSensor
 
 __all__ = [
     "RGBCameraSensor",
-    "GeminiCameraSensor",
+    "ZedCameraSensor",
 ]

dexcontrol/sensors/camera/rgb_camera.py

@@ -1,27 +1,39 @@
-# Copyright (c) 2025 Dexmate CORPORATION & AFFILIATES. All rights reserved.
+# Copyright (C) 2025 Dexmate Inc.
 #
-# Licensed under the Apache License, Version 2.0 with Commons Clause License
-# Condition v1.0 [see LICENSE for details].
+# This software is dual-licensed:
+#
+# 1. GNU Affero General Public License v3.0 (AGPL-3.0)
+#    See LICENSE-AGPL for details
+#
+# 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 Zenoh subscriber."""
+import logging
 
 import numpy as np
 import zenoh
 
-from dexcontrol.utils.subscribers.camera import RGBCameraSubscriber
+from dexcontrol.utils.rtc_utils import create_rtc_subscriber_with_config
+
+logger = logging.getLogger(__name__)
 
 
 class RGBCameraSensor:
-    """RGB camera sensor using Zenoh subscriber.
+    """RGB camera sensor using RTC subscriber.
 
-    This sensor provides RGB image data from a camera using the
-    RGBCameraSubscriber for efficient data handling with lazy decoding.
+    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.
     """
 
     def __init__(
         self,
         configs,
         zenoh_session: zenoh.Session,
+        *args,
+        **kwargs,
     ) -> None:
         """Initialize the RGB camera sensor.
 
@@ -31,18 +43,23 @@ class RGBCameraSensor:
         """
         self._name = configs.name
 
-        # Create the RGB camera subscriber
-        self._subscriber = RGBCameraSubscriber(
-            topic=configs.topic,
+        # 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
+
     def shutdown(self) -> None:
         """Shutdown the camera sensor."""
-        self._subscriber.shutdown()
+        if self._subscriber:
+            self._subscriber.shutdown()
 
     def is_active(self) -> bool:
         """Check if the camera sensor is actively receiving data.
@@ -50,7 +67,7 @@ class RGBCameraSensor:
         Returns:
             True if receiving data, False otherwise.
         """
-        return self._subscriber.is_active()
+        return self._subscriber.is_active() if self._subscriber else False
 
     def wait_for_active(self, timeout: float = 5.0) -> bool:
         """Wait for the camera sensor to start receiving data.
@@ -61,7 +78,7 @@ class RGBCameraSensor:
         Returns:
             True if sensor becomes active, False if timeout is reached.
         """
-        return self._subscriber.wait_for_active(timeout)
+        return self._subscriber.wait_for_active(timeout) if self._subscriber else False
 
     def get_obs(self) -> np.ndarray | None:
         """Get the latest RGB image data.
@@ -69,15 +86,7 @@ class RGBCameraSensor:
         Returns:
             Latest RGB image as numpy array (HxWxC) if available, None otherwise.
         """
-        return self._subscriber.get_latest_data()
-
-    def get_rgb_image(self) -> np.ndarray | None:
-        """Get the latest RGB image.
-
-        Returns:
-            Latest RGB image as numpy array (HxWxC) if available, None otherwise.
-        """
-        return self._subscriber.get_latest_image()
+        return self._subscriber.get_latest_data() if self._subscriber else None
 
     @property
     def fps(self) -> float:
@@ -86,7 +95,7 @@ class RGBCameraSensor:
         Returns:
             Current frames per second measurement.
         """
-        return self._subscriber.fps
+        return self._subscriber.fps if self._subscriber else 0.0
 
     @property
     def name(self) -> str: