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

dexcontrol/utils/rate_limiter.py

@@ -1,172 +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
-
-"""Rate limiter utility for maintaining consistent execution rates."""
-
-import sys
-import time
-from typing import Final
-
-from loguru import logger
-
-
-class RateLimiter:
-    """Class for limiting execution rate to a target frequency.
-
-    This class provides rate limiting functionality by sleeping between iterations to
-    maintain a desired execution frequency. It also tracks statistics about the achieved
-    rate and missed deadlines.
-
-    Attributes:
-        period_sec: Time period between iterations in seconds.
-        target_rate_hz: Desired execution rate in Hz.
-        window_size: Size of moving average window for rate calculations.
-        last_time_sec: Timestamp of the last iteration.
-        next_time_sec: Scheduled timestamp for the next iteration.
-        start_time_sec: Timestamp when the rate limiter was initialized or reset.
-        duration_buffer: List of recent iteration durations for rate calculation.
-        missed_deadlines: Counter for iterations that missed their scheduled time.
-        iterations: Total number of iterations since initialization or reset.
-    """
-
-    def __init__(self, rate_hz: float, window_size: int = 50) -> None:
-        """Initializes rate limiter.
-
-        Args:
-            rate_hz: Desired rate in Hz.
-            window_size: Size of moving average window for rate calculations.
-
-        Raises:
-            ValueError: If rate_hz is not positive.
-        """
-        if rate_hz <= 0:
-            raise ValueError("Rate must be positive")
-
-        self.period_sec: Final[float] = 1.0 / rate_hz
-        self.target_rate_hz: Final[float] = rate_hz
-        self.window_size: Final[int] = max(1, window_size)
-
-        # Initialize timing variables
-        now_sec = time.monotonic()
-        self.last_time_sec: float = now_sec
-        self.next_time_sec: float = now_sec + self.period_sec
-        self.start_time_sec: float = now_sec
-
-        # Initialize statistics
-        self.duration_buffer: list[float] = []
-        self.missed_deadlines: int = 0
-        self.iterations: int = 0
-        self._MAX_ITERATIONS: Final[int] = sys.maxsize - 1000  # Leave some buffer
-
-    def sleep(self) -> None:
-        """Sleeps to maintain desired rate.
-
-        Sleeps for the appropriate duration to maintain the target rate. If the next
-        scheduled time has already passed, increments the missed deadlines counter.
-        Uses monotonic time for reliable timing regardless of system clock changes.
-        """
-        current_time_sec = time.monotonic()
-        sleep_time_sec = self.next_time_sec - current_time_sec
-
-        if sleep_time_sec > 0:
-            time.sleep(sleep_time_sec)
-        else:
-            self.missed_deadlines += 1
-
-        # Update timing and statistics
-        now_sec = time.monotonic()
-
-        # Reset iterations if approaching max value
-        if self.iterations >= self._MAX_ITERATIONS:
-            logger.warning(
-                "Iteration counter approaching max value, resetting statistics"
-            )
-            self.reset()
-            return
-
-        self.iterations += 1
-
-        if self.iterations > 1:  # Skip first iteration
-            duration_sec = now_sec - self.last_time_sec
-            if len(self.duration_buffer) >= self.window_size:
-                self.duration_buffer.pop(0)
-            self.duration_buffer.append(duration_sec)
-
-        self.last_time_sec = now_sec
-
-        # More efficient way to advance next_time_sec when multiple periods behind
-        periods_behind = max(
-            0, int((now_sec - self.next_time_sec) / self.period_sec) + 1
-        )
-        self.next_time_sec += periods_behind * self.period_sec
-
-    def get_actual_rate(self) -> float:
-        """Calculates actual achieved rate in Hz using moving average.
-
-        Returns:
-            Current execution rate based on recent iterations.
-        """
-        if not self.duration_buffer:
-            return 0.0
-        avg_duration_sec = sum(self.duration_buffer) / len(self.duration_buffer)
-        return 0.0 if avg_duration_sec <= 0 else 1.0 / avg_duration_sec
-
-    def get_average_rate(self) -> float:
-        """Calculates average rate over entire run.
-
-        Returns:
-            Average execution rate since start or last reset.
-        """
-        if self.iterations < 2:
-            return 0.0
-        total_time_sec = time.monotonic() - self.start_time_sec
-        return 0.0 if total_time_sec <= 0 else self.iterations / total_time_sec
-
-    def reset(self) -> None:
-        """Resets the rate limiter state and statistics."""
-        now_sec = time.monotonic()
-        self.last_time_sec = now_sec
-        self.next_time_sec = now_sec + self.period_sec
-        self.start_time_sec = now_sec
-        self.duration_buffer.clear()
-        self.missed_deadlines = 0
-        self.iterations = 0
-
-    def get_stats(self) -> dict[str, float | int]:
-        """Gets runtime statistics.
-
-        Returns:
-            Dictionary containing execution statistics including actual rate,
-            average rate, target rate, missed deadlines and iteration count.
-        """
-        return {
-            "actual_rate": self.get_actual_rate(),
-            "average_rate": self.get_average_rate(),
-            "target_rate": self.target_rate_hz,
-            "missed_deadlines": self.missed_deadlines,
-            "iterations": self.iterations,
-        }
-
-
-if __name__ == "__main__":
-    rate_limiter = RateLimiter(100.0)  # 100Hz
-
-    try:
-        while True:
-            rate_limiter.sleep()
-            actual_rate = rate_limiter.get_actual_rate()
-            logger.info(f"Rate: {actual_rate:.2f} Hz")
-
-    except KeyboardInterrupt:
-        stats = rate_limiter.get_stats()
-        logger.info("\nFinal stats:")
-        logger.info(f"Average rate: {stats['average_rate']:.2f} Hz")
-        logger.info(f"Final rate: {stats['actual_rate']:.2f} Hz")
-        logger.info(f"Missed deadlines: {stats['missed_deadlines']}")

dexcontrol/utils/rtc_utils.py

@@ -1,144 +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
-
-"""RTC utilities for dexcontrol.
-
-This module provides utility functions for creating RTC subscribers
-that first query Zenoh for connection information.
-"""
-
-import zenoh
-from loguru import logger
-
-from dexcontrol.utils.subscribers.rtc import RTCSubscriber
-from dexcontrol.utils.zenoh_utils import query_zenoh_json
-
-
-def query_rtc_info(
-    zenoh_session: zenoh.Session,
-    info_topic: str,
-    timeout: float = 2.0,
-    max_retries: int = 1,
-    retry_delay: float = 0.5,
-) -> dict | None:
-    """Query Zenoh for RTC connection information.
-
-    Args:
-        zenoh_session: Active Zenoh session for communication.
-        info_topic: Zenoh topic to query for RTC info.
-        timeout: Maximum time to wait for a response in seconds.
-        max_retries: Maximum number of retry attempts.
-        retry_delay: Initial delay between retries (doubles each retry).
-
-    Returns:
-        Dictionary containing host and port information if successful, None otherwise.
-    """
-
-    # Use the general Zenoh query function
-    info = query_zenoh_json(
-        zenoh_session=zenoh_session,
-        topic=info_topic,
-        timeout=timeout,
-        max_retries=max_retries,
-        retry_delay=retry_delay,
-    )
-
-    return info
-
-
-def create_rtc_subscriber_from_zenoh(
-    zenoh_session: zenoh.Session,
-    info_topic: str,
-    name: str = "rtc_subscriber",
-    enable_fps_tracking: bool = True,
-    fps_log_interval: int = 100,
-    query_timeout: float = 2.0,
-    max_retries: int = 1,
-) -> RTCSubscriber | None:
-    """Create a RTC subscriber by first querying Zenoh for connection info.
-
-    Args:
-        zenoh_session: Active Zenoh session for communication.
-        info_topic: Zenoh topic to query for RTC connection info.
-        name: Name for logging purposes.
-        enable_fps_tracking: Whether to track and log FPS metrics.
-        fps_log_interval: Number of frames between FPS calculations.
-        query_timeout: Maximum time to wait for Zenoh query response.
-        max_retries: Maximum number of retry attempts for Zenoh query.
-
-    Returns:
-        RTCSubscriber instance if successful, None otherwise.
-    """
-    # Query Zenoh for RTC connection information
-    rtc_info = query_rtc_info(zenoh_session, info_topic, query_timeout, max_retries)
-
-    if rtc_info is None:
-        logger.error("Failed to get RTC connection info from Zenoh")
-        return None
-
-    url = rtc_info.get("signaling_url")
-
-    if not url:
-        logger.error(f"Invalid RTC info: url={url}")
-        return None
-
-    # Construct WebSocket URL
-    ws_url = url
-    logger.info(f"Creating RTC subscriber with URL: {ws_url}")
-
-    try:
-        # Create and return the RTC subscriber
-        subscriber = RTCSubscriber(
-            url=ws_url,
-            name=name,
-            enable_fps_tracking=enable_fps_tracking,
-            fps_log_interval=fps_log_interval,
-        )
-        return subscriber
-    except Exception as e:
-        logger.error(f"Failed to create RTC subscriber: {e}")
-        return None
-
-
-def create_rtc_subscriber_with_config(
-    zenoh_session: zenoh.Session,
-    config,
-    name: str = "rtc_subscriber",
-    enable_fps_tracking: bool = True,
-    fps_log_interval: int = 100,
-) -> RTCSubscriber | None:
-    """Create a RTC subscriber using configuration object.
-
-    Args:
-        zenoh_session: Active Zenoh session for communication.
-        config: Configuration object containing info_key.
-        name: Name for logging purposes.
-        enable_fps_tracking: Whether to track and log FPS metrics.
-        fps_log_interval: Number of frames between FPS calculations.
-    Returns:
-        RTCSubscriber instance if successful, None otherwise.
-    """
-    if "info_key" not in config:
-        logger.error("Config subscriber_config missing info_key")
-        return None
-
-    if not config["enable"]:
-        logger.info(f"Skipping {name} because it is disabled")
-        return None
-
-    info_topic = config["info_key"]
-
-    return create_rtc_subscriber_from_zenoh(
-        zenoh_session=zenoh_session,
-        info_topic=info_topic,
-        name=name,
-        enable_fps_tracking=enable_fps_tracking,
-        fps_log_interval=fps_log_interval,
-    )

dexcontrol/utils/subscribers/__init__.py

@@ -1,52 +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
-
-"""Zenoh subscriber utilities for dexcontrol.
-
-This module provides a collection of subscriber classes and utilities for handling
-Zenoh communication in a flexible and reusable way.
-"""
-
-from .base import BaseZenohSubscriber, CustomDataHandler
-from .camera import DepthCameraSubscriber, RGBCameraSubscriber, RGBDCameraSubscriber
-from .decoders import (
-    DecoderFunction,
-    json_decoder,
-    protobuf_decoder,
-    raw_bytes_decoder,
-    string_decoder,
-)
-from .generic import GenericZenohSubscriber
-from .imu import IMUSubscriber
-from .lidar import LidarSubscriber
-from .protobuf import ProtobufZenohSubscriber
-from .rtc import RTCSubscriber
-
-__all__ = [
-    "BaseZenohSubscriber",
-    "CustomDataHandler",
-    "GenericZenohSubscriber",
-    "ProtobufZenohSubscriber",
-    "DecoderFunction",
-    "protobuf_decoder",
-    "raw_bytes_decoder",
-    "json_decoder",
-    "string_decoder",
-    # Camera subscribers
-    "RGBCameraSubscriber",
-    "DepthCameraSubscriber",
-    "RGBDCameraSubscriber",
-    # Lidar subscriber
-    "LidarSubscriber",
-    # IMU subscriber
-    "IMUSubscriber",
-    # RTC subscriber
-    "RTCSubscriber",
-]

dexcontrol/utils/subscribers/base.py

@@ -1,281 +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
-
-"""Base Zenoh subscriber utilities.
-
-This module provides the abstract base class for all Zenoh subscribers
-and common utilities used across different subscriber implementations.
-"""
-
-import threading
-import time
-from abc import ABC, abstractmethod
-from collections.abc import Callable
-from typing import Any, Final, TypeVar
-
-import zenoh
-from google.protobuf.message import Message
-from loguru import logger
-
-from dexcontrol.utils.os_utils import resolve_key_name
-
-# Type variable for Message subclasses
-M = TypeVar("M", bound=Message)
-
-# Type alias for custom data handler functions
-CustomDataHandler = Callable[[zenoh.Sample], None]
-
-
-class BaseZenohSubscriber(ABC):
-    """Base class for Zenoh subscribers with configurable data handling.
-
-    This class provides a common interface for subscribing to Zenoh topics
-    and processing incoming data through configurable decoder functions.
-    It handles the Zenoh communication setup and provides thread-safe access
-    to the latest data.
-
-    Attributes:
-        _active: Whether subscriber is receiving data updates.
-        _data_lock: Lock for thread-safe data access.
-        _zenoh_session: Active Zenoh session for communication.
-        _subscriber: Zenoh subscriber for data.
-        _topic: The resolved Zenoh topic name.
-        _enable_fps_tracking: Whether to track and log FPS metrics.
-        _frame_count: Counter for frames processed since last FPS calculation.
-        _fps: Most recently calculated frames per second value.
-        _last_fps_time: Timestamp of last FPS calculation.
-        _fps_log_interval: Number of frames between FPS calculations.
-        _name: Name for logging purposes.
-        _custom_data_handler: Optional custom data handler function.
-        _last_data_time: Timestamp of last data received.
-    """
-
-    def __init__(
-        self,
-        topic: str,
-        zenoh_session: zenoh.Session,
-        name: str = "subscriber",
-        enable_fps_tracking: bool = False,
-        fps_log_interval: int = 100,
-        custom_data_handler: CustomDataHandler | None = None,
-    ) -> None:
-        """Initialize the base Zenoh subscriber.
-
-        Args:
-            topic: Zenoh topic to subscribe to for 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.
-        """
-        self._active: bool = False
-        self._data_lock: Final[threading.RLock] = threading.RLock()
-        self._zenoh_session: Final[zenoh.Session] = zenoh_session
-        self._name = name
-        self._custom_data_handler = custom_data_handler
-
-        # Data freshness tracking
-        self._last_data_time: float | None = None
-
-        # FPS tracking
-        self._enable_fps_tracking = enable_fps_tracking
-        self._fps_log_interval = fps_log_interval
-        self._frame_count = 0
-        self._fps = 0.0
-        self._last_fps_time = time.time()
-
-        # Setup Zenoh subscriber
-        self._topic: Final[str] = resolve_key_name(topic)
-        self._subscriber: Final[zenoh.Subscriber] = zenoh_session.declare_subscriber(
-            self._topic, self._data_handler_wrapper
-        )
-
-        logger.info(f"Initialized {self._name} subscriber for topic: {self._topic}")
-
-    def _data_handler_wrapper(self, sample: zenoh.Sample) -> None:
-        """Wrapper for data handling that calls either custom or default handler.
-
-        Args:
-            sample: Zenoh sample containing data.
-        """
-        # Update data freshness timestamp
-        with self._data_lock:
-            self._last_data_time = time.monotonic()
-
-        # Call custom data handler if provided, otherwise call default handler
-        if self._custom_data_handler is not None:
-            try:
-                self._custom_data_handler(sample)
-            except Exception as e:
-                logger.error(f"Custom data handler failed for {self._name}: {e}")
-        else:
-            # Call the default data handler
-            self._data_handler(sample)
-
-    @abstractmethod
-    def _data_handler(self, sample: zenoh.Sample) -> None:
-        """Handle incoming data.
-
-        This method must be implemented by subclasses to process the specific
-        type of data they handle.
-
-        Args:
-            sample: Zenoh sample containing data.
-        """
-        pass
-
-    @abstractmethod
-    def get_latest_data(self) -> Any | None:
-        """Get the latest data.
-
-        Returns:
-            Latest data if available, None otherwise.
-        """
-        pass
-
-    def _update_fps_metrics(self) -> None:
-        """Update FPS tracking metrics.
-
-        Increments frame counter and recalculates FPS at specified intervals.
-        Only has an effect if fps_tracking was enabled during initialization.
-        """
-        if not self._enable_fps_tracking:
-            return
-
-        self._frame_count += 1
-        if self._frame_count >= self._fps_log_interval:
-            current_time = time.time()
-            elapsed = current_time - self._last_fps_time
-            self._fps = self._frame_count / elapsed
-            logger.info(f"{self._name} {self._topic} frequency: {self._fps:.2f} Hz")
-            self._frame_count = 0
-            self._last_fps_time = current_time
-
-    def wait_for_active(self, timeout: float = 5.0) -> bool:
-        """Wait for the subscriber to start receiving data.
-
-        Args:
-            timeout: Maximum time to wait in seconds.
-
-        Returns:
-            True if subscriber becomes active, False if timeout is reached.
-        """
-        start_time = time.monotonic()
-        check_interval = min(0.05, timeout / 10)  # Check every 10ms or less
-
-        while True:
-            with self._data_lock:
-                if self._active:
-                    return True
-
-            elapsed = time.monotonic() - start_time
-            if elapsed >= timeout:
-                logger.error(f"No data received from {self._topic} after {timeout}s")
-                return False
-
-            # Sleep for the shorter of: remaining time or check interval
-            sleep_time = min(check_interval, timeout - elapsed)
-            time.sleep(sleep_time)
-
-    def is_active(self) -> bool:
-        """Check if the subscriber is actively receiving data.
-
-        Returns:
-            True if subscriber is active, False otherwise.
-        """
-        with self._data_lock:
-            return self._active
-
-    def shutdown(self) -> None:
-        """Stop the subscriber and release resources."""
-        # Mark as inactive first to prevent further data processing
-        with self._data_lock:
-            self._active = False
-
-        # Small delay to allow any ongoing data processing to complete
-        time.sleep(0.05)
-
-        try:
-            if hasattr(self, "_subscriber") and self._subscriber:
-                self._subscriber.undeclare()
-        except Exception as e:
-            # Don't log "Undeclared subscriber" errors as warnings - they're expected during shutdown
-            error_msg = str(e).lower()
-            if not ("undeclared" in error_msg or "closed" in error_msg):
-                logger.warning(f"Error undeclaring subscriber for {self._topic}: {e}")
-
-        # Additional delay to allow Zenoh to process the undeclare operation
-        time.sleep(0.02)
-
-    @property
-    def topic(self) -> str:
-        """Get the Zenoh topic name.
-
-        Returns:
-            The resolved Zenoh topic name.
-        """
-        return self._topic
-
-    @property
-    def fps(self) -> float:
-        """Get the current FPS measurement.
-
-        Returns:
-            Current frames per second measurement.
-        """
-        return self._fps
-
-    @property
-    def name(self) -> str:
-        """Get the subscriber name.
-
-        Returns:
-            The subscriber name.
-        """
-        return self._name
-
-    def is_data_fresh(self, max_age_seconds: float) -> bool:
-        """Check if the most recent data is fresh (received within the specified time).
-
-        This method checks if data has been received within the specified time window,
-        regardless of whether the data content has changed. This is useful for
-        detecting communication failures or stale data streams.
-
-        Args:
-            max_age_seconds: Maximum age in seconds for data to be considered fresh.
-
-        Returns:
-            True if fresh data was received within the time window, False otherwise.
-            Returns False if no data has ever been received.
-        """
-        with self._data_lock:
-            if self._last_data_time is None:
-                return False
-
-            current_time = time.monotonic()
-            age = current_time - self._last_data_time
-            return age <= max_age_seconds
-
-    def get_time_since_last_data(self) -> float | None:
-        """Get the time elapsed since the last data was received.
-
-        Returns:
-            Time in seconds since last data was received, or None if no data
-            has ever been received.
-        """
-        with self._data_lock:
-            if self._last_data_time is None:
-                return None
-
-            current_time = time.monotonic()
-            return current_time - self._last_data_time