dexcontrol 0.2.12__py3-none-any.whl → 0.3.1__py3-none-any.whl

dexcontrol/utils/subscribers/camera.py

@@ -1,332 +0,0 @@
-# Copyright (C) 2025 Dexmate Inc.
-#
-# 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 Zenoh subscribers for RGB and depth data.
-
-This module provides specialized subscribers for camera data including RGB images
-and depth images, using the serialization formats from dexsensor.
-"""
-
-import numpy as np
-import zenoh
-from loguru import logger
-
-from .base import BaseZenohSubscriber, CustomDataHandler
-
-# Import camera serialization functions from dexsensor
-try:
-    from dexsensor.serialization.camera import decode_depth, decode_image
-except ImportError:
-    logger.error(
-        "Failed to import dexsensor camera serialization functions. Please install dexsensor."
-    )
-    decode_image = None
-    decode_depth = None
-
-
-class RGBCameraSubscriber(BaseZenohSubscriber):
-    """Zenoh subscriber for RGB camera data.
-
-    This subscriber handles RGB image data encoded using the dexsensor
-    camera serialization format with JPEG compression.
-    Uses lazy decoding - data is only decoded when requested.
-    """
-
-    def __init__(
-        self,
-        topic: str,
-        zenoh_session: zenoh.Session,
-        name: str = "rgb_camera_subscriber",
-        enable_fps_tracking: bool = True,
-        fps_log_interval: int = 30,
-        custom_data_handler: CustomDataHandler | None = None,
-    ) -> None:
-        """Initialize the RGB camera subscriber.
-
-        Args:
-            topic: Zenoh topic to subscribe to for RGB data.
-            zenoh_session: Active Zenoh session for communication.
-            name: Name for logging purposes.
-            enable_fps_tracking: Whether to track and log FPS metrics.
-            fps_log_interval: Number of frames between FPS calculations.
-            custom_data_handler: Optional custom function to handle incoming data.
-                                If provided, this will replace the default data
-                                handling logic entirely.
-        """
-        super().__init__(
-            topic,
-            zenoh_session,
-            name,
-            enable_fps_tracking,
-            fps_log_interval,
-            custom_data_handler,
-        )
-        self._latest_raw_data: bytes | None = None
-
-    def _data_handler(self, sample: zenoh.Sample) -> None:
-        """Handle incoming RGB image data.
-
-        Args:
-            sample: Zenoh sample containing encoded RGB image data.
-        """
-        with self._data_lock:
-            self._latest_raw_data = sample.payload.to_bytes()
-            self._active = True
-
-        self._update_fps_metrics()
-
-    def get_latest_data(self) -> np.ndarray | None:
-        """Get the latest RGB image.
-
-        Returns:
-            Latest RGB image as numpy array (HxWxC) if available, None otherwise.
-        """
-        with self._data_lock:
-            if self._latest_raw_data is None:
-                return None
-
-            if decode_image is None:
-                logger.error(
-                    f"Cannot decode RGB image for {self._name}: dexsensor not available"
-                )
-                return None
-
-            try:
-                # Decode the image, which is typically in BGR format
-                image = decode_image(self._latest_raw_data)
-                return image
-            except Exception as e:
-                logger.error(f"Failed to decode RGB image for {self._name}: {e}")
-                return None
-
-    def get_latest_image(self) -> np.ndarray | None:
-        """Get the latest RGB image.
-
-        Alias for get_latest_data() for clarity.
-
-        Returns:
-            Latest RGB image as numpy array (HxWxC) if available, None otherwise.
-        """
-        return self.get_latest_data()
-
-
-class DepthCameraSubscriber(BaseZenohSubscriber):
-    """Zenoh subscriber for depth camera data.
-
-    This subscriber handles depth image data encoded using the dexsensor
-    camera serialization format with compression.
-    Uses lazy decoding - data is only decoded when requested.
-    """
-
-    def __init__(
-        self,
-        topic: str,
-        zenoh_session: zenoh.Session,
-        name: str = "depth_camera_subscriber",
-        enable_fps_tracking: bool = True,
-        fps_log_interval: int = 30,
-        custom_data_handler: CustomDataHandler | None = None,
-    ) -> None:
-        """Initialize the depth camera subscriber.
-
-        Args:
-            topic: Zenoh topic to subscribe to for depth data.
-            zenoh_session: Active Zenoh session for communication.
-            name: Name for logging purposes.
-            enable_fps_tracking: Whether to track and log FPS metrics.
-            fps_log_interval: Number of frames between FPS calculations.
-            custom_data_handler: Optional custom function to handle incoming data.
-                                If provided, this will replace the default data
-                                handling logic entirely.
-        """
-        super().__init__(
-            topic,
-            zenoh_session,
-            name,
-            enable_fps_tracking,
-            fps_log_interval,
-            custom_data_handler,
-        )
-        self._latest_raw_data: bytes | None = None
-
-    def _data_handler(self, sample: zenoh.Sample) -> None:
-        """Handle incoming depth image data.
-
-        Args:
-            sample: Zenoh sample containing encoded depth image data.
-        """
-        with self._data_lock:
-            self._latest_raw_data = sample.payload.to_bytes()
-            self._active = True
-
-        self._update_fps_metrics()
-
-    def get_latest_data(self) -> np.ndarray | None:
-        """Get the latest depth data.
-
-        Returns:
-            depth_image if available, None otherwise. Depth values in meters as numpy array (HxW)
-        """
-        with self._data_lock:
-            if self._latest_raw_data is None:
-                return None
-
-            if decode_depth is None:
-                logger.error(
-                    f"Cannot decode depth image for {self._name}: dexsensor not available"
-                )
-                return None
-
-            try:
-                # Decode the depth image
-                depth = decode_depth(self._latest_raw_data)
-                return depth
-            except Exception as e:
-                logger.error(f"Failed to decode depth image for {self._name}: {e}")
-                return None
-
-
-class RGBDCameraSubscriber(BaseZenohSubscriber):
-    """Zenoh subscriber for RGBD camera data.
-
-    This subscriber handles both RGB and depth data from an RGBD camera,
-    subscribing to separate topics for RGB and depth streams.
-    """
-
-    def __init__(
-        self,
-        rgb_topic: str,
-        depth_topic: str,
-        zenoh_session: zenoh.Session,
-        name: str = "rgbd_camera_subscriber",
-        enable_fps_tracking: bool = True,
-        fps_log_interval: int = 30,
-        custom_data_handler: CustomDataHandler | None = None,
-    ) -> None:
-        """Initialize the RGBD camera subscriber.
-
-        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 logging purposes.
-            enable_fps_tracking: Whether to track and log FPS metrics.
-            fps_log_interval: Number of frames between FPS calculations.
-            custom_data_handler: Optional custom function to handle incoming data.
-                                If provided, this will replace the default data
-                                handling logic entirely.
-        """
-        # Initialize with RGB topic as primary
-        super().__init__(
-            rgb_topic,
-            zenoh_session,
-            name,
-            enable_fps_tracking,
-            fps_log_interval,
-            custom_data_handler,
-        )
-
-        # Create separate subscribers for RGB and depth
-        self._rgb_subscriber = RGBCameraSubscriber(
-            rgb_topic,
-            zenoh_session,
-            f"{name}_rgb",
-            enable_fps_tracking,
-            fps_log_interval,
-            custom_data_handler,
-        )
-        self._depth_subscriber = DepthCameraSubscriber(
-            depth_topic,
-            zenoh_session,
-            f"{name}_depth",
-            enable_fps_tracking,
-            fps_log_interval,
-            custom_data_handler,
-        )
-
-    def _data_handler(self, sample: zenoh.Sample) -> None:
-        """Handle incoming data - not used as we use separate subscribers."""
-        pass
-
-    def get_latest_data(self) -> tuple[np.ndarray, np.ndarray] | None:
-        """Get the latest RGBD data.
-
-        Returns:
-            Tuple of (rgb_image, depth_image, depth_min, depth_max) if both available, None otherwise.
-            rgb_image: RGB image as numpy array (HxWxC)
-            depth_image: Depth values in meters as numpy array (HxW)
-        """
-        rgb_image = self._rgb_subscriber.get_latest_data()
-        depth_image = self._depth_subscriber.get_latest_data()
-
-        if rgb_image is not None and depth_image is not None:
-            return rgb_image, depth_image
-        return None
-
-    def get_latest_rgb(self) -> np.ndarray | None:
-        """Get the latest RGB image.
-
-        Returns:
-            Latest RGB image as numpy array (HxWxC) if available, None otherwise.
-        """
-        return self._rgb_subscriber.get_latest_image()
-
-    def get_latest_depth(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._depth_subscriber.get_latest_data()
-
-    def wait_for_active(self, timeout: float = 5.0) -> bool:
-        """Wait for both RGB and depth subscribers to start receiving data.
-
-        Args:
-            timeout: Maximum time to wait in seconds.
-
-        Returns:
-            True if both subscribers become active, False if timeout is reached.
-        """
-        rgb_active = self._rgb_subscriber.wait_for_active(timeout)
-        depth_active = self._depth_subscriber.wait_for_active(timeout)
-        return rgb_active and depth_active
-
-    def is_active(self) -> bool:
-        """Check if both RGB and depth subscribers are actively receiving data.
-
-        Returns:
-            True if both subscribers are active, False otherwise.
-        """
-        return self._rgb_subscriber.is_active() and self._depth_subscriber.is_active()
-
-    def shutdown(self) -> None:
-        """Stop both subscribers and release resources."""
-        self._rgb_subscriber.shutdown()
-        self._depth_subscriber.shutdown()
-        super().shutdown()
-
-    @property
-    def rgb_fps(self) -> float:
-        """Get the RGB stream FPS measurement.
-
-        Returns:
-            Current RGB frames per second measurement.
-        """
-        return self._rgb_subscriber.fps
-
-    @property
-    def depth_fps(self) -> float:
-        """Get the depth stream FPS measurement.
-
-        Returns:
-            Current depth frames per second measurement.
-        """
-        return self._depth_subscriber.fps

dexcontrol/utils/subscribers/decoders.py

@@ -1,88 +0,0 @@
-# Copyright (C) 2025 Dexmate Inc.
-#
-# 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
-
-"""Decoder functions for Zenoh subscribers.
-
-This module provides common decoder functions that can be used with the
-GenericZenohSubscriber to transform raw Zenoh bytes into specific data formats.
-"""
-
-import json
-from collections.abc import Callable
-from typing import Any, Type, TypeVar
-
-import zenoh
-from google.protobuf.message import Message
-
-M = TypeVar("M", bound=Message)
-
-# Type alias for decoder functions
-DecoderFunction = Callable[[zenoh.ZBytes], Any]
-
-
-def protobuf_decoder(message_type: Type[M]) -> DecoderFunction:
-    """Create a decoder function for a specific protobuf message type.
-
-    Args:
-        message_type: Protobuf message class to decode to.
-
-    Returns:
-        Decoder function that parses bytes into the specified protobuf message.
-    """
-
-    def decode(data: zenoh.ZBytes) -> M:
-        message = message_type()
-        message.ParseFromString(data.to_bytes())
-        return message
-
-    return decode
-
-
-def raw_bytes_decoder(data: zenoh.ZBytes) -> bytes:
-    """Decoder that returns raw bytes.
-
-    Args:
-        data: Zenoh bytes data.
-
-    Returns:
-        Raw bytes data.
-    """
-    return data.to_bytes()
-
-
-def json_decoder(data: zenoh.ZBytes) -> Any:
-    """Decoder that parses JSON from bytes.
-
-    Args:
-        data: Zenoh bytes data containing JSON.
-
-    Returns:
-        Parsed JSON data.
-
-    Raises:
-        json.JSONDecodeError: If the data is not valid JSON.
-    """
-    return json.loads(data.to_bytes().decode("utf-8"))
-
-
-def string_decoder(data: zenoh.ZBytes, encoding: str = "utf-8") -> str:
-    """Decoder that converts bytes to string.
-
-    Args:
-        data: Zenoh bytes data.
-        encoding: Text encoding to use for decoding.
-
-    Returns:
-        Decoded string.
-
-    Raises:
-        UnicodeDecodeError: If the data cannot be decoded with the specified encoding.
-    """
-    return data.to_bytes().decode(encoding)

dexcontrol/utils/subscribers/generic.py

@@ -1,110 +0,0 @@
-# Copyright (C) 2025 Dexmate Inc.
-#
-# 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
-
-"""Generic Zenoh subscriber with configurable decoder functions.
-
-This module provides a flexible subscriber that can handle any type of data
-by accepting decoder functions that transform raw Zenoh bytes into the desired format.
-"""
-
-from typing import Any
-
-import zenoh
-from loguru import logger
-
-from .base import BaseZenohSubscriber, CustomDataHandler
-from .decoders import DecoderFunction
-
-
-class GenericZenohSubscriber(BaseZenohSubscriber):
-    """Generic Zenoh subscriber with configurable decoder function.
-
-    This subscriber can handle any type of data by accepting a decoder function
-    that transforms the raw Zenoh bytes into the desired data format.
-    Uses lazy decoding - data is only decoded when requested.
-    """
-
-    def __init__(
-        self,
-        topic: str,
-        zenoh_session: zenoh.Session,
-        decoder: DecoderFunction | None = None,
-        name: str = "generic_subscriber",
-        enable_fps_tracking: bool = False,
-        fps_log_interval: int = 100,
-        custom_data_handler: CustomDataHandler | None = None,
-    ) -> None:
-        """Initialize the generic Zenoh subscriber.
-
-        Args:
-            topic: Zenoh topic to subscribe to for data.
-            zenoh_session: Active Zenoh session for communication.
-            decoder: Optional function to decode raw bytes into desired format.
-                     If None, raw bytes are returned.
-            name: Name for logging purposes.
-            enable_fps_tracking: Whether to track and log FPS metrics.
-            fps_log_interval: Number of frames between FPS calculations.
-            custom_data_handler: Optional custom function to handle incoming data.
-                                If provided, this will replace the default data
-                                handling logic entirely.
-        """
-        super().__init__(
-            topic,
-            zenoh_session,
-            name,
-            enable_fps_tracking,
-            fps_log_interval,
-            custom_data_handler,
-        )
-        self._decoder = decoder
-        self._latest_raw_data: zenoh.ZBytes = zenoh.ZBytes("")
-
-    def _data_handler(self, sample: zenoh.Sample) -> None:
-        """Handle incoming data.
-
-        Args:
-            sample: Zenoh sample containing data.
-        """
-        with self._data_lock:
-            self._latest_raw_data = sample.payload
-            self._active = True
-
-        self._update_fps_metrics()
-
-    def get_latest_data(self) -> Any | None:
-        """Get the latest data.
-
-        Returns:
-            Latest decoded data if decoder is provided and decoding succeeded,
-            otherwise raw bytes. None if no data received.
-        """
-        with self._data_lock:
-            if not self._active:
-                return None
-
-            if self._decoder is not None:
-                try:
-                    return self._decoder(self._latest_raw_data)
-                except Exception as e:
-                    logger.error(f"Failed to decode data for {self._name}: {e}")
-                    return None
-            else:
-                return self._latest_raw_data.to_bytes()
-
-    def get_latest_raw_data(self) -> bytes | None:
-        """Get the latest raw data bytes.
-
-        Returns:
-            Latest raw data bytes if available, None otherwise.
-        """
-        with self._data_lock:
-            if not self._active:
-                return None
-            return self._latest_raw_data.to_bytes()

dexcontrol/utils/subscribers/imu.py

@@ -1,175 +0,0 @@
-# Copyright (C) 2025 Dexmate Inc.
-#
-# 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
-
-"""IMU Zenoh subscriber for inertial measurement data.
-
-This module provides a specialized subscriber for IMU data,
-using the serialization format from dexsensor.
-"""
-
-from typing import Any
-
-import numpy as np
-import zenoh
-from loguru import logger
-
-from .base import BaseZenohSubscriber, CustomDataHandler
-
-# Import IMU serialization functions from dexsensor
-try:
-    from dexsensor.serialization.imu import decode_imu_data
-except ImportError:
-    logger.error(
-        "Failed to import dexsensor IMU serialization functions. Please install dexsensor."
-    )
-    decode_imu_data = None
-
-
-class IMUSubscriber(BaseZenohSubscriber):
-    """Zenoh subscriber for IMU data.
-
-    This subscriber handles IMU data encoded using the dexsensor
-    IMU serialization format with compression.
-    Uses lazy decoding - data is only decoded when requested.
-    """
-
-    def __init__(
-        self,
-        topic: str,
-        zenoh_session: zenoh.Session,
-        name: str = "imu_subscriber",
-        enable_fps_tracking: bool = True,
-        fps_log_interval: int = 50,
-        custom_data_handler: CustomDataHandler | None = None,
-    ) -> None:
-        """Initialize the IMU subscriber.
-
-        Args:
-            topic: Zenoh topic to subscribe to for IMU data.
-            zenoh_session: Active Zenoh session for communication.
-            name: Name for logging purposes.
-            enable_fps_tracking: Whether to track and log FPS metrics.
-            fps_log_interval: Number of frames between FPS calculations.
-            custom_data_handler: Optional custom function to handle incoming data.
-                                If provided, this will replace the default data
-                                handling logic entirely.
-        """
-        super().__init__(
-            topic,
-            zenoh_session,
-            name,
-            enable_fps_tracking,
-            fps_log_interval,
-            custom_data_handler,
-        )
-        self._latest_raw_data: bytes | None = None
-
-    def _data_handler(self, sample: zenoh.Sample) -> None:
-        """Handle incoming IMU data.
-
-        Args:
-            sample: Zenoh sample containing encoded IMU data.
-        """
-        with self._data_lock:
-            self._latest_raw_data = sample.payload.to_bytes()
-            self._active = True
-
-        self._update_fps_metrics()
-
-    def get_latest_data(self) -> dict[str, Any] | None:
-        """Get the latest IMU data.
-
-        Returns:
-            Latest IMU data dictionary if available, None otherwise.
-            Dictionary contains:
-                - acceleration: Linear acceleration [x, y, z] in m/s²
-                - angular_velocity: Angular velocity [x, y, z] in rad/s
-                - orientation: Orientation quaternion [x, y, z, w]
-                - magnetometer: Magnetic field [x, y, z] in µT (if available)
-                - timestamp: Timestamp of the measurement
-        """
-        with self._data_lock:
-            if self._latest_raw_data is None:
-                return None
-
-            if decode_imu_data is None:
-                logger.error(
-                    f"Cannot decode IMU data for {self._name}: dexsensor not available"
-                )
-                return None
-
-            try:
-                # Decode the IMU data
-                imu_data = decode_imu_data(self._latest_raw_data)
-                # Return a copy to avoid external modifications
-                return {
-                    key: value.copy() if isinstance(value, np.ndarray) else value
-                    for key, value in imu_data.items()
-                }
-            except Exception as e:
-                logger.error(f"Failed to decode IMU data for {self._name}: {e}")
-                return None
-
-    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.
-        """
-        imu_data = self.get_latest_data()
-        if imu_data is not None:
-            return imu_data["acceleration"]
-        return None
-
-    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.
-        """
-        imu_data = self.get_latest_data()
-        if imu_data is not None:
-            return imu_data["angular_velocity"]
-        return None
-
-    def get_orientation(self) -> np.ndarray | None:
-        """Get the latest orientation quaternion.
-
-        Returns:
-            Orientation quaternion [x, y, z, w] if available, None otherwise.
-        """
-        imu_data = self.get_latest_data()
-        if imu_data is not None:
-            return imu_data["orientation"]
-        return None
-
-    def get_magnetometer(self) -> np.ndarray | None:
-        """Get the latest magnetometer reading.
-
-        Returns:
-            Magnetic field [x, y, z] in µT if available, None otherwise.
-        """
-        imu_data = self.get_latest_data()
-        if imu_data is not None and "magnetometer" in imu_data:
-            magnetometer = imu_data["magnetometer"]
-            return magnetometer if magnetometer is not None else None
-        return None
-
-    def has_magnetometer(self) -> bool:
-        """Check if the latest IMU data includes magnetometer information.
-
-        Returns:
-            True if magnetometer data is available, False otherwise.
-        """
-        imu_data = self.get_latest_data()
-        if imu_data is not None:
-            magnetometer = imu_data.get("magnetometer")
-            return magnetometer is not None and len(magnetometer) > 0
-        return False