dexcontrol 0.2.1__py3-none-any.whl

dexcontrol/utils/subscribers/generic.py

@@ -0,0 +1,105 @@
+# 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].
+
+"""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

@@ -0,0 +1,170 @@
+# 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 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

dexcontrol/utils/subscribers/lidar.py

@@ -0,0 +1,195 @@
+# 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].
+
+"""LIDAR Zenoh subscriber for scan data.
+
+This module provides a specialized subscriber for LIDAR scan 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 lidar serialization functions from dexsensor
+try:
+    from dexsensor.serialization.lidar import decode_scan_data
+except ImportError:
+    logger.error(
+        "Failed to import dexsensor lidar serialization functions. Please install dexsensor."
+    )
+    decode_scan_data = None
+
+
+class LidarSubscriber(BaseZenohSubscriber):
+    """Zenoh subscriber for LIDAR scan data.
+
+    This subscriber handles LIDAR scan data encoded using the dexsensor
+    lidar serialization format with compression.
+    Uses lazy decoding - data is only decoded when requested.
+    """
+
+    def __init__(
+        self,
+        topic: str,
+        zenoh_session: zenoh.Session,
+        name: str = "lidar_subscriber",
+        enable_fps_tracking: bool = True,
+        fps_log_interval: int = 50,
+        custom_data_handler: CustomDataHandler | None = None,
+    ) -> None:
+        """Initialize the LIDAR subscriber.
+
+        Args:
+            topic: Zenoh topic to subscribe to for LIDAR 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 LIDAR scan data.
+
+        Args:
+            sample: Zenoh sample containing encoded LIDAR scan 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 LIDAR scan data.
+
+        Returns:
+            Latest scan data dictionary if available, None otherwise.
+            Dictionary contains:
+                - ranges: Array of range measurements
+                - angles: Array of corresponding angles
+                - intensities: Array of intensity values (if available)
+                - angle_min: Minimum angle of the scan
+                - angle_max: Maximum angle of the scan
+                - angle_increment: Angular distance between measurements
+                - scan_time: Time for a complete scan
+                - time_increment: Time between measurements
+                - range_min: Minimum range value
+                - range_max: Maximum range value
+        """
+        with self._data_lock:
+            if self._latest_raw_data is None:
+                return None
+
+            if decode_scan_data is None:
+                logger.error(
+                    f"Cannot decode LIDAR scan for {self._name}: dexsensor not available"
+                )
+                return None
+
+            try:
+                # Decode the LIDAR scan data
+                scan_data = decode_scan_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 scan_data.items()
+                }
+            except Exception as e:
+                logger.error(f"Failed to decode LIDAR scan for {self._name}: {e}")
+                return None
+
+    def get_latest_scan(self) -> dict[str, Any] | None:
+        """Get the latest LIDAR scan data.
+
+        Alias for get_latest_data() for clarity.
+
+        Returns:
+            Latest scan data dictionary if available, None otherwise.
+        """
+        return self.get_latest_data()
+
+    def get_ranges(self) -> np.ndarray | None:
+        """Get the latest range measurements.
+
+        Returns:
+            Array of range measurements if available, None otherwise.
+        """
+        scan_data = self.get_latest_data()
+        if scan_data is not None:
+            return scan_data["ranges"]
+        return None
+
+    def get_angles(self) -> np.ndarray | None:
+        """Get the latest angle measurements.
+
+        Returns:
+            Array of angle measurements if available, None otherwise.
+        """
+        scan_data = self.get_latest_data()
+        if scan_data is not None:
+            return scan_data["angles"]
+        return None
+
+    def get_intensities(self) -> np.ndarray | None:
+        """Get the latest intensity measurements.
+
+        Returns:
+            Array of intensity measurements if available, None otherwise.
+        """
+        scan_data = self.get_latest_data()
+        if scan_data is not None and "intensities" in scan_data:
+            intensities = scan_data["intensities"]
+            return intensities if intensities is not None else None
+        return None
+
+    def get_scan_info(self) -> dict[str, float] | None:
+        """Get scan metadata information.
+
+        Returns:
+            Dictionary with scan metadata if available, None otherwise.
+            Contains: angle_min, angle_max, angle_increment, scan_time,
+                     time_increment, range_min, range_max
+        """
+        scan_data = self.get_latest_data()
+        if scan_data is not None:
+            return {
+                "angle_min": scan_data["angle_min"],
+                "angle_max": scan_data["angle_max"],
+                "angle_increment": scan_data["angle_increment"],
+                "scan_time": scan_data["scan_time"],
+                "time_increment": scan_data["time_increment"],
+                "range_min": scan_data["range_min"],
+                "range_max": scan_data["range_max"],
+            }
+        return None
+
+    def has_intensities(self) -> bool:
+        """Check if the latest scan data includes intensity information.
+
+        Returns:
+            True if intensity data is available, False otherwise.
+        """
+        scan_data = self.get_latest_data()
+        if scan_data is not None:
+            intensities = scan_data.get("intensities")
+            return intensities is not None and len(intensities) > 0
+        return False

dexcontrol/utils/subscribers/protobuf.py

@@ -0,0 +1,106 @@
+# 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].
+
+"""Protobuf-specific Zenoh subscriber.
+
+This module provides a subscriber specifically designed for handling protobuf messages
+with automatic parsing and type safety. Uses lazy decoding for efficiency.
+"""
+
+from typing import TypeVar, cast
+
+import zenoh
+from google.protobuf.message import Message
+from loguru import logger
+
+from .base import BaseZenohSubscriber, CustomDataHandler
+
+M = TypeVar("M", bound=Message)
+
+
+class ProtobufZenohSubscriber(BaseZenohSubscriber):
+    """Zenoh subscriber specifically for protobuf messages.
+
+    This subscriber automatically handles protobuf message parsing and provides
+    type-safe access to the latest message data. Uses lazy decoding - protobuf
+    messages are only parsed when requested.
+    """
+
+    def __init__(
+        self,
+        topic: str,
+        zenoh_session: zenoh.Session,
+        message_type: type[M],
+        name: str = "protobuf_subscriber",
+        enable_fps_tracking: bool = False,
+        fps_log_interval: int = 100,
+        custom_data_handler: CustomDataHandler | None = None,
+    ) -> None:
+        """Initialize the protobuf Zenoh subscriber.
+
+        Args:
+            topic: Zenoh topic to subscribe to for protobuf messages.
+            zenoh_session: Active Zenoh session for communication.
+            message_type: Protobuf message class to parse incoming data.
+            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._message_type = message_type
+        self._latest_raw_data: zenoh.ZBytes = zenoh.ZBytes("")
+
+    def _data_handler(self, sample: zenoh.Sample) -> None:
+        """Handle incoming protobuf data.
+
+        Args:
+            sample: Zenoh sample containing protobuf data.
+        """
+        with self._data_lock:
+            self._latest_raw_data = sample.payload
+            self._active = True
+
+        self._update_fps_metrics()
+
+    def get_latest_data(self) -> M | None:
+        """Get the latest protobuf message.
+
+        Returns:
+            Latest parsed protobuf message if available and parsing succeeded,
+            None otherwise.
+        """
+        with self._data_lock:
+            if not self._active:
+                return None
+
+            # Parse protobuf message on demand
+            try:
+                message = self._message_type()
+                message.ParseFromString(self._latest_raw_data.to_bytes())
+                return cast(M, message)
+            except Exception as e:
+                logger.error(f"Failed to parse protobuf message for {self._name}: {e}")
+                return None
+
+    def get_latest_raw_data(self) -> bytes | None:
+        """Get the latest raw protobuf data bytes.
+
+        Returns:
+            Latest raw protobuf data bytes if available, None otherwise.
+        """
+        with self._data_lock:
+            if not self._active:
+                return None
+            return self._latest_raw_data.to_bytes()