dexcontrol 0.2.1__py3-none-any.whl

dexcontrol/sensors/__init__.py

@@ -0,0 +1,40 @@
+# Copyright (c) 2025 Dexmate CORPORATION & AFFILIATES. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 with Commons Clause License
+# Condition v1.0 [see LICENSE for details].
+
+"""Sensor implementations for dexcontrol.
+
+This module provides sensor classes for various robotic sensors
+using Zenoh subscribers for data communication.
+"""
+
+# Import camera sensors
+from .camera import GeminiCameraSensor, RGBCameraSensor
+
+# Import IMU sensors
+from .imu import GeminiIMUSensor, NineAxisIMUSensor
+
+# Import other sensors
+from .lidar import RPLidarSensor
+
+# Import sensor manager
+from .manager import Sensors
+from .ultrasonic import UltrasonicSensor
+
+__all__ = [
+    # Camera sensors
+    "RGBCameraSensor",
+    "GeminiCameraSensor",
+
+    # IMU sensors
+    "NineAxisIMUSensor",
+    "GeminiIMUSensor",
+
+    # Other sensors
+    "RPLidarSensor",
+    "UltrasonicSensor",
+
+    # Sensor manager
+    "Sensors",
+]

dexcontrol/sensors/camera/__init__.py

@@ -0,0 +1,18 @@
+# Copyright (c) 2025 Dexmate CORPORATION & AFFILIATES. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 with Commons Clause License
+# Condition v1.0 [see LICENSE for details].
+
+"""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.
+"""
+
+from .gemini_camera import GeminiCameraSensor
+from .rgb_camera import RGBCameraSensor
+
+__all__ = [
+    "RGBCameraSensor",
+    "GeminiCameraSensor",
+]

dexcontrol/sensors/camera/gemini_camera.py

@@ -0,0 +1,139 @@
+# Copyright (c) 2025 Dexmate CORPORATION & AFFILIATES. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 with Commons Clause License
+# Condition v1.0 [see LICENSE for details].
+
+"""Gemini RGBD camera sensor implementation using Zenoh subscriber."""
+
+import numpy as np
+import zenoh
+
+from dexcontrol.utils.subscribers.camera import RGBDCameraSubscriber
+
+
+class GeminiCameraSensor:
+    """Gemini RGBD camera sensor using Zenoh subscriber.
+
+    This sensor provides both RGB and depth image data from an Orbbec Gemini camera
+    using the RGBDCameraSubscriber for efficient data handling with lazy decoding.
+    """
+
+    def __init__(
+        self,
+        configs,
+        zenoh_session: zenoh.Session,
+    ) -> None:
+        """Initialize the Gemini RGBD camera sensor.
+
+        Args:
+            rgb_topic: Zenoh topic for RGB data.
+            depth_topic: Zenoh topic for depth data.
+            zenoh_session: Active Zenoh session for communication.
+            name: Name for the sensor instance.
+            enable_fps_tracking: Whether to track and log FPS metrics.
+            fps_log_interval: Number of frames between FPS calculations.
+        """
+        self._name = configs.name
+
+        # Create the RGBD camera subscriber
+        self._subscriber = RGBDCameraSubscriber(
+            rgb_topic=configs.rgb_topic,
+            depth_topic=configs.depth_topic,
+            zenoh_session=zenoh_session,
+            name=f"{self._name}_subscriber",
+            enable_fps_tracking=configs.enable_fps_tracking,
+            fps_log_interval=configs.fps_log_interval,
+        )
+
+    def shutdown(self) -> None:
+        """Shutdown the camera sensor."""
+        self._subscriber.shutdown()
+
+    def is_active(self) -> bool:
+        """Check if the camera sensor is actively receiving data.
+
+        Returns:
+            True if both RGB and depth are receiving data, False otherwise.
+        """
+        return self._subscriber.is_active()
+
+    def wait_for_active(self, timeout: float = 5.0) -> bool:
+        """Wait for the camera sensor to start receiving data.
+
+        Args:
+            timeout: Maximum time to wait in seconds.
+
+        Returns:
+            True if both RGB and depth become active, False if timeout is reached.
+        """
+        return self._subscriber.wait_for_active(timeout)
+
+    def get_obs(self, obs_keys: list[str] | None = None) -> dict[str, np.ndarray] | None:
+        """Get the latest RGBD data.
+
+        Returns:
+            Tuple of (rgb, depth) if both available, None otherwise.
+        """
+        if obs_keys is None:
+            obs_keys = ["rgb", "depth"]
+        obs_out = {}
+        for key in obs_keys:
+            if key == "rgb":
+                obs_out[key] = self._subscriber.get_latest_rgb()
+            elif key == "depth":
+                obs_out[key] = self._subscriber.get_latest_depth()
+            else:
+                raise ValueError(f"Invalid observation key: {key}")
+        return obs_out
+
+    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_rgb()
+
+    def get_depth_image(self) -> np.ndarray | None:
+        """Get the latest depth image.
+
+        Returns:
+            Latest depth image as numpy array (HxW) with values in meters if available, None otherwise.
+        """
+        return self._subscriber.get_latest_depth()
+
+    @property
+    def rgb_fps(self) -> float:
+        """Get the RGB stream FPS measurement.
+
+        Returns:
+            Current RGB frames per second measurement.
+        """
+        return self._subscriber.rgb_fps
+
+    @property
+    def depth_fps(self) -> float:
+        """Get the depth stream FPS measurement.
+
+        Returns:
+            Current depth frames per second measurement.
+        """
+        return self._subscriber.depth_fps
+
+    @property
+    def fps(self) -> float:
+        """Get the combined FPS measurement (minimum of RGB and depth).
+
+        Returns:
+            Current frames per second measurement.
+        """
+        return min(self.rgb_fps, self.depth_fps)
+
+    @property
+    def name(self) -> str:
+        """Get the sensor name.
+
+        Returns:
+            Sensor name string.
+        """
+        return self._name

dexcontrol/sensors/camera/rgb_camera.py

@@ -0,0 +1,98 @@
+# Copyright (c) 2025 Dexmate CORPORATION & AFFILIATES. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 with Commons Clause License
+# Condition v1.0 [see LICENSE for details].
+
+"""RGB camera sensor implementation using Zenoh subscriber."""
+
+import numpy as np
+import zenoh
+
+from dexcontrol.utils.subscribers.camera import RGBCameraSubscriber
+
+
+class RGBCameraSensor:
+    """RGB camera sensor using Zenoh subscriber.
+
+    This sensor provides RGB image data from a camera using the
+    RGBCameraSubscriber for efficient data handling with lazy decoding.
+    """
+
+    def __init__(
+        self,
+        configs,
+        zenoh_session: zenoh.Session,
+    ) -> None:
+        """Initialize the RGB camera sensor.
+
+        Args:
+            configs: Configuration for the RGB camera sensor.
+            zenoh_session: Active Zenoh session for communication.
+        """
+        self._name = configs.name
+
+        # Create the RGB camera subscriber
+        self._subscriber = RGBCameraSubscriber(
+            topic=configs.topic,
+            zenoh_session=zenoh_session,
+            name=f"{self._name}_subscriber",
+            enable_fps_tracking=configs.enable_fps_tracking,
+            fps_log_interval=configs.fps_log_interval,
+        )
+
+    def shutdown(self) -> None:
+        """Shutdown the camera sensor."""
+        self._subscriber.shutdown()
+
+    def is_active(self) -> bool:
+        """Check if the camera sensor is actively receiving data.
+
+        Returns:
+            True if receiving data, False otherwise.
+        """
+        return self._subscriber.is_active()
+
+    def wait_for_active(self, timeout: float = 5.0) -> bool:
+        """Wait for the camera sensor to start receiving data.
+
+        Args:
+            timeout: Maximum time to wait in seconds.
+
+        Returns:
+            True if sensor becomes active, False if timeout is reached.
+        """
+        return self._subscriber.wait_for_active(timeout)
+
+    def get_obs(self) -> np.ndarray | None:
+        """Get the latest RGB image data.
+
+        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()
+
+    @property
+    def fps(self) -> float:
+        """Get the current FPS measurement.
+
+        Returns:
+            Current frames per second measurement.
+        """
+        return self._subscriber.fps
+
+    @property
+    def name(self) -> str:
+        """Get the sensor name.
+
+        Returns:
+            Sensor name string.
+        """
+        return self._name

dexcontrol/sensors/imu/__init__.py

@@ -0,0 +1,22 @@
+# Copyright (c) 2025 Dexmate CORPORATION & AFFILIATES. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 with Commons Clause License
+# Condition v1.0 [see LICENSE for details].
+
+"""IMU sensors package for dexcontrol.
+
+This package provides sensor classes for various IMU (Inertial Measurement Unit)
+sensors using Zenoh subscribers for data communication.
+
+Available sensors:
+    - NineAxisIMUSensor: Standard 9-axis IMU with accelerometer, gyroscope, and magnetometer
+    - GeminiIMUSensor: IMU specific to Gemini hardware (6-axis: accelerometer + gyroscope)
+"""
+
+from .gemini_imu import GeminiIMUSensor
+from .nine_axis_imu import NineAxisIMUSensor
+
+__all__ = [
+    "NineAxisIMUSensor",
+    "GeminiIMUSensor",
+]

dexcontrol/sensors/imu/gemini_imu.py

@@ -0,0 +1,139 @@
+# Copyright (c) 2025 Dexmate CORPORATION & AFFILIATES. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 with Commons Clause License
+# Condition v1.0 [see LICENSE for details].
+
+"""Gemini IMU sensor implementation using Zenoh subscriber."""
+
+import numpy as np
+import zenoh
+
+from dexcontrol.utils.subscribers.imu import IMUSubscriber
+
+
+class GeminiIMUSensor:
+    """Gemini IMU sensor using Zenoh subscriber.
+
+    This sensor provides 6-axis IMU data (accelerometer + gyroscope) from Gemini hardware
+    using the IMUSubscriber for efficient data handling. Note: Gemini IMU does not include
+    magnetometer data.
+    """
+
+    def __init__(
+        self,
+        configs,
+        zenoh_session: zenoh.Session,
+    ) -> None:
+        """Initialize the Gemini IMU sensor.
+
+        Args:
+            topic: Zenoh topic to subscribe to for IMU data.
+            zenoh_session: Active Zenoh session for communication.
+            name: Name for the sensor instance.
+            enable_fps_tracking: Whether to track and log FPS metrics.
+        """
+        self._name = configs.name
+
+        # Create the IMU subscriber
+        self._subscriber = IMUSubscriber(
+            topic=configs.topic,
+            zenoh_session=zenoh_session,
+            name=f"{self._name}_subscriber",
+            enable_fps_tracking=configs.enable_fps_tracking,
+            fps_log_interval=configs.fps_log_interval,
+        )
+
+
+    def shutdown(self) -> None:
+        """Shutdown the IMU sensor."""
+        self._subscriber.shutdown()
+
+    def is_active(self) -> bool:
+        """Check if the IMU sensor is actively receiving data.
+
+        Returns:
+            True if receiving data, False otherwise.
+        """
+        return self._subscriber.is_active()
+
+    def wait_for_active(self, timeout: float = 5.0) -> bool:
+        """Wait for the IMU sensor to start receiving data.
+
+        Args:
+            timeout: Maximum time to wait in seconds.
+
+        Returns:
+            True if sensor becomes active, False if timeout is reached.
+        """
+        return self._subscriber.wait_for_active(timeout)
+
+    def get_obs(self, obs_keys: list[str] | None = None) -> dict[str, np.ndarray] | None:
+        """Get observation data for the Gemini IMU sensor.
+
+        Args:
+            obs_keys: List of observation keys to retrieve. If None, returns available data.
+                     Valid keys: ['ang_vel', 'acc', 'quat'] (no magnetometer for Gemini)
+
+        Returns:
+            Dictionary with observation data including IMU measurements.
+            Note: Magnetometer data is not available for Gemini IMU.
+        """
+        if obs_keys is None:
+            obs_keys = ['ang_vel', 'acc', 'quat']
+
+        obs_out = {}
+        data = self._subscriber.get_latest_data()
+
+        for key in obs_keys:
+            if key == 'ang_vel':
+                obs_out[key] = data['ang_vel']
+            elif key == 'acc':
+                obs_out[key] = data['acc']
+            elif key == 'quat':
+                obs_out[key] = data['quat']
+            else:
+                raise ValueError(f"Invalid observation key: {key}")
+
+        return obs_out
+
+    def get_acceleration(self) -> np.ndarray | None:
+        """Get the latest linear acceleration.
+
+        Returns:
+            Linear acceleration [x, y, z] in m/s² if available, None otherwise.
+        """
+        return self._subscriber.get_acceleration()
+
+    def get_angular_velocity(self) -> np.ndarray | None:
+        """Get the latest angular velocity.
+
+        Returns:
+            Angular velocity [x, y, z] in rad/s if available, None otherwise.
+        """
+        return self._subscriber.get_angular_velocity()
+
+    def get_orientation(self) -> np.ndarray | None:
+        """Get the latest orientation quaternion.
+
+        Returns:
+            Orientation quaternion [x, y, z, w] if available, None otherwise.
+        """
+        return self._subscriber.get_orientation()
+
+    @property
+    def fps(self) -> float:
+        """Get the current FPS measurement.
+
+        Returns:
+            Current frames per second measurement.
+        """
+        return self._subscriber.fps
+
+    @property
+    def name(self) -> str:
+        """Get the IMU name.
+
+        Returns:
+            IMU name string.
+        """
+        return self._name

dexcontrol/sensors/imu/nine_axis_imu.py

@@ -0,0 +1,149 @@
+# Copyright (c) 2025 Dexmate CORPORATION & AFFILIATES. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 with Commons Clause License
+# Condition v1.0 [see LICENSE for details].
+
+"""Nine-Axis IMU sensor implementation using Zenoh subscriber."""
+
+import numpy as np
+import zenoh
+
+from dexcontrol.utils.subscribers.imu import IMUSubscriber
+
+
+class NineAxisIMUSensor:
+    """Nine-Axis IMU sensor using Zenoh subscriber.
+
+    This sensor provides 9-axis IMU data including acceleration, angular velocity,
+    orientation, and magnetometer data using the IMUSubscriber for efficient data handling.
+    """
+
+    def __init__(
+        self,
+        configs,
+        zenoh_session: zenoh.Session,
+    ) -> None:
+        """Initialize the Nine-Axis IMU sensor.
+
+        Args:
+            topic: Zenoh topic to subscribe to for IMU data.
+            zenoh_session: Active Zenoh session for communication.
+            name: Name for the sensor instance.
+            enable_fps_tracking: Whether to track and log FPS metrics.
+        """
+        self._name = configs.name
+
+        # Create the IMU subscriber
+        self._subscriber = IMUSubscriber(
+            topic=configs.topic,
+            zenoh_session=zenoh_session,
+            name=f"{self._name}_subscriber",
+            enable_fps_tracking=configs.enable_fps_tracking,
+            fps_log_interval=configs.fps_log_interval,
+        )
+
+
+    def shutdown(self) -> None:
+        """Shutdown the IMU sensor."""
+        self._subscriber.shutdown()
+
+    def is_active(self) -> bool:
+        """Check if the IMU sensor is actively receiving data.
+
+        Returns:
+            True if receiving data, False otherwise.
+        """
+        return self._subscriber.is_active()
+
+    def wait_for_active(self, timeout: float = 5.0) -> bool:
+        """Wait for the IMU sensor to start receiving data.
+
+        Args:
+            timeout: Maximum time to wait in seconds.
+
+        Returns:
+            True if sensor becomes active, False if timeout is reached.
+        """
+        return self._subscriber.wait_for_active(timeout)
+
+    def get_obs(self, obs_keys: list[str] | None = None) -> dict[str, np.ndarray] | None:
+        """Get observation data for the Nine-Axis IMU sensor.
+
+        Args:
+            obs_keys: List of observation keys to retrieve. If None, returns all available data.
+                     Valid keys: ['ang_vel', 'acc', 'mag', 'quat']
+
+        Returns:
+            Dictionary with observation data including all IMU measurements.
+        """
+        if obs_keys is None:
+            obs_keys = ['ang_vel', 'acc', 'mag', 'quat']
+
+        data = self._subscriber.get_latest_data()
+        if data is None:
+            return None
+
+        obs_out = {}
+        for key in obs_keys:
+            if key == 'ang_vel':
+                obs_out[key] = data['angular_velocity']
+            elif key == 'acc':
+                obs_out[key] = data['acceleration']
+            elif key == 'mag':
+                obs_out[key] = data['magnetometer']
+            elif key == 'quat':
+                obs_out[key] = data['orientation']
+            else:
+                raise ValueError(f"Invalid observation key: {key}")
+
+        return obs_out
+
+    def get_acceleration(self) -> np.ndarray | None:
+        """Get the latest linear acceleration.
+
+        Returns:
+            Linear acceleration [x, y, z] in m/s² if available, None otherwise.
+        """
+        return self._subscriber.get_acceleration()
+
+    def get_angular_velocity(self) -> np.ndarray | None:
+        """Get the latest angular velocity.
+
+        Returns:
+            Angular velocity [x, y, z] in rad/s if available, None otherwise.
+        """
+        return self._subscriber.get_angular_velocity()
+
+    def get_orientation(self) -> np.ndarray | None:
+        """Get the latest orientation quaternion.
+
+        Returns:
+            Orientation quaternion [x, y, z, w] if available, None otherwise.
+        """
+        return self._subscriber.get_orientation()
+
+    def get_magnetometer(self) -> np.ndarray | None:
+        """Get the latest magnetometer reading.
+
+        Returns:
+            Magnetic field [x, y, z] in µT if available, None otherwise.
+        """
+        return self._subscriber.get_magnetometer()
+
+    @property
+    def fps(self) -> float:
+        """Get the current FPS measurement.
+
+        Returns:
+            Current frames per second measurement.
+        """
+        return self._subscriber.fps
+
+    @property
+    def name(self) -> str:
+        """Get the IMU name.
+
+        Returns:
+            IMU name string.
+        """
+        return self._name

dexcontrol/sensors/lidar/__init__.py

@@ -0,0 +1,3 @@
+from .rplidar import RPLidarSensor
+
+__all__ = ['RPLidarSensor']