dexcontrol 0.2.1__py3-none-any.whl

dexcontrol/sensors/lidar/rplidar.py

@@ -0,0 +1,164 @@
+# 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 sensor implementations using Zenoh subscribers.
+
+This module provides LIDAR sensor classes that use the specialized LIDAR
+subscriber for scan data.
+"""
+
+from typing import Any
+
+import numpy as np
+import zenoh
+
+from dexcontrol.utils.subscribers.lidar import LidarSubscriber
+
+
+class RPLidarSensor:
+    """LIDAR sensor using Zenoh subscriber.
+
+    This sensor provides LIDAR scan data using the LidarSubscriber
+    for efficient data handling with lazy decoding.
+    """
+
+    def __init__(
+        self,
+        configs,
+        zenoh_session: zenoh.Session,
+    ) -> None:
+        """Initialize the LIDAR sensor.
+
+        Args:
+            topic: Zenoh topic to subscribe to for LIDAR 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 LIDAR subscriber
+        self._subscriber = LidarSubscriber(
+            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 LIDAR sensor."""
+        self._subscriber.shutdown()
+
+    def is_active(self) -> bool:
+        """Check if the LIDAR 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 LIDAR 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) -> 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
+        """
+        return self._subscriber.get_latest_data()
+
+    def get_ranges(self) -> np.ndarray | None:
+        """Get the latest range measurements.
+
+        Returns:
+            Array of range measurements if available, None otherwise.
+        """
+        return self._subscriber.get_ranges()
+
+    def get_angles(self) -> np.ndarray | None:
+        """Get the latest angle measurements.
+
+        Returns:
+            Array of angle measurements if available, None otherwise.
+        """
+        return self._subscriber.get_angles()
+
+    def get_intensities(self) -> np.ndarray | None:
+        """Get the latest intensity measurements.
+
+        Returns:
+            Array of intensity measurements if available, None otherwise.
+        """
+        return self._subscriber.get_intensities()
+
+    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
+        """
+        return self._subscriber.get_scan_info()
+
+    def get_point_count(self) -> int:
+        """Get the number of points in the latest scan.
+
+        Returns:
+            Number of points in the scan, 0 if no data available.
+        """
+        ranges = self.get_ranges()
+        if ranges is not None:
+            return len(ranges)
+        return 0
+
+    def has_intensities(self) -> bool:
+        """Check if the latest scan data includes intensity information.
+
+        Returns:
+            True if intensity data is available, False otherwise.
+        """
+        return self._subscriber.has_intensities()
+
+    @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 LIDAR name.
+
+        Returns:
+            LIDAR name string.
+        """
+        return self._name

dexcontrol/sensors/manager.py

@@ -0,0 +1,185 @@
+# 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 manager for managing robot sensors with Zenoh communication.
+
+This module provides the Sensors class that manages multiple sensor instances
+based on configuration and provides unified access to them.
+"""
+
+import time
+import traceback
+from typing import TYPE_CHECKING, Any
+
+import hydra.utils as hydra_utils
+import zenoh
+from loguru import logger
+from omegaconf import DictConfig, OmegaConf
+
+from dexcontrol.config.sensors.vega_sensors import VegaSensorsConfig
+
+if TYPE_CHECKING:
+    from dexcontrol.sensors.camera.gemini_camera import GeminiCameraSensor
+    from dexcontrol.sensors.camera.rgb_camera import RGBCameraSensor
+    from dexcontrol.sensors.imu.gemini_imu import GeminiIMUSensor
+    from dexcontrol.sensors.imu.nine_axis_imu import NineAxisIMUSensor
+    from dexcontrol.sensors.lidar.rplidar import RPLidarSensor
+    from dexcontrol.sensors.ultrasonic import UltrasonicSensor
+
+
+class Sensors:
+    """Manages robot sensors including cameras, IMUs, LiDAR, and ultrasonic sensors.
+
+    Provides a unified interface for creating, managing, and shutting down all types of
+    sensors used by the robot. Instantiates sensors based on configuration and provides
+    access to them as attributes.
+
+    Attributes:
+        _sensors: List of instantiated sensor objects.
+        _zenoh_session: Active Zenoh session for communication.
+    """
+
+    if TYPE_CHECKING:
+        # Type annotations for dynamically created sensor attributes
+        head_camera: GeminiCameraSensor
+        base_left_camera: RGBCameraSensor
+        base_right_camera: RGBCameraSensor
+        base_front_camera: RGBCameraSensor
+        base_back_camera: RGBCameraSensor
+        base_imu: NineAxisIMUSensor
+        head_imu: GeminiIMUSensor
+        lidar: RPLidarSensor
+        ultrasonic: UltrasonicSensor
+
+    def __init__(self, configs: DictConfig | VegaSensorsConfig,
+                 zenoh_session: zenoh.Session) -> None:
+        """Initialize sensors from configuration.
+
+        Args:
+            configs: Configuration for all sensors (VegaSensorsConfig or DictConfig).
+            zenoh_session: Active Zenoh session for communication.
+        """
+        self._sensors: list[Any] = []
+        self._zenoh_session = zenoh_session
+
+        dict_configs = (OmegaConf.structured(configs) if isinstance(configs, VegaSensorsConfig)
+                       else configs)
+
+        for sensor_name, sensor_config in dict_configs.items():
+            sensor = self._create_sensor(sensor_config, str(sensor_name))
+            if sensor is not None:
+                setattr(self, str(sensor_name), sensor)
+                self._sensors.append(sensor)
+
+    def _create_sensor(self, config: DictConfig | None, name: str) -> Any | None:
+        """Creates and initializes a sensor from config.
+
+        Args:
+            config: Sensor configuration.
+            name: Name of the sensor for logging.
+
+        Returns:
+            Initialized sensor object or None if creation fails.
+        """
+        if config is None:
+            logger.debug(f"Sensor {name} config is None")
+            return None
+
+        if not (hasattr(config, '_target_') and config._target_):
+            return None
+
+        try:
+            temp_config = OmegaConf.create({
+                '_target_': config._target_,
+                'configs': {k: v for k, v in config.items() if k != '_target_'}
+            })
+
+            sensor = hydra_utils.instantiate(
+                temp_config,
+                zenoh_session=self._zenoh_session,
+            )
+
+            if hasattr(sensor, "start"):
+                sensor.start()
+
+            return sensor
+
+        except Exception as e:
+            logger.error(f"Failed to instantiate sensor {name}: {e}")
+            logger.debug(f"Full traceback:\n{traceback.format_exc()}")
+            return None
+
+    def shutdown(self) -> None:
+        """Shuts down all active sensors."""
+        logger.info("Shutting down all sensors...")
+        for sensor in self._sensors:
+            try:
+                if hasattr(sensor, "shutdown"):
+                    sensor.shutdown()
+            except Exception as e:
+                logger.error(f"Error shutting down sensor: {e}")
+
+        logger.info("All sensors shut down")
+
+    def get_active_sensors(self) -> list[str]:
+        """Gets list of active sensor names.
+
+        Returns:
+            List of sensor names that are currently active.
+        """
+        excluded_attrs = {'shutdown', 'get_active_sensors', 'wait_for_sensors'}
+        active_sensors = []
+
+        for attr_name in dir(self):
+            if (not attr_name.startswith('_') and
+                attr_name not in excluded_attrs):
+                sensor = getattr(self, attr_name)
+                if (sensor is not None and
+                    hasattr(sensor, 'is_active') and
+                    sensor.is_active()):
+                    active_sensors.append(attr_name)
+        return active_sensors
+
+    def wait_for_sensors(self, timeout: float = 5.0) -> bool:
+        """Waits for all enabled sensors to become active.
+
+        Args:
+            timeout: Maximum time to wait for sensors to become active.
+
+        Returns:
+            True if all sensors became active, False otherwise.
+        """
+        start_time = time.time()
+
+        while time.time() - start_time < timeout:
+            if all(sensor.is_active() for sensor in self._sensors
+                  if hasattr(sensor, 'is_active')):
+                logger.info("All sensors are active")
+                return True
+            time.sleep(0.1)
+
+        logger.warning(f"Not all sensors became active within {timeout}s")
+        return False
+
+    def wait_for_all_active(self, timeout: float = 5.0) -> bool:
+        """Waits for all enabled sensors to become active.
+
+        Alias for wait_for_sensors() for backward compatibility.
+
+        Args:
+            timeout: Maximum time to wait for sensors to become active.
+
+        Returns:
+            True if all sensors became active, False otherwise.
+        """
+        return self.wait_for_sensors(timeout)
+
+    def get_sensor_count(self) -> int:
+        """Gets the total number of instantiated sensors.
+
+        Returns:
+            Number of sensors that were successfully created.
+        """
+        return len(self._sensors)

dexcontrol/sensors/ultrasonic.py

@@ -0,0 +1,110 @@
+# 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].
+
+"""Ultrasonic sensor implementations using Zenoh subscribers.
+
+This module provides ultrasonic sensor classes that use the generic
+subscriber for distance measurements.
+"""
+
+import numpy as np
+import zenoh
+
+from dexcontrol.proto import dexcontrol_msg_pb2
+from dexcontrol.utils.subscribers import ProtobufZenohSubscriber
+
+
+class UltrasonicSensor:
+    """Ultrasonic sensor using Zenoh subscriber.
+
+    This sensor provides distance measurements from ultrasonic sensors
+    """
+
+    def __init__(
+        self,
+        configs,
+        zenoh_session: zenoh.Session,
+    ) -> None:
+        """Initialize the ultrasonic sensor.
+
+        Args:
+            configs: Configuration for the ultrasonic sensor.
+            zenoh_session: Active Zenoh session for communication.
+        """
+        self._name = configs.name
+
+        # Create the generic subscriber with JSON decoder
+        self._subscriber = ProtobufZenohSubscriber(
+            topic=configs.topic,
+            zenoh_session=zenoh_session,
+            message_type=dexcontrol_msg_pb2.UltrasonicState,
+            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 ultrasonic sensor."""
+        self._subscriber.shutdown()
+
+    def is_active(self) -> bool:
+        """Check if the ultrasonic 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 ultrasonic 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 observation data for the ultrasonic sensor.
+
+        This method provides a standardized observation format
+        that can be used in robotics applications.
+
+        Returns:
+            Numpy array of distances in meters with shape (4,) in the order:
+            [front_left, front_right, back_left, back_right].
+        """
+        data = self._subscriber.get_latest_data()
+        if data is not None:
+            obs = [
+                data.front_left,
+                data.front_right,
+                data.back_left,
+                data.back_right,
+            ]
+            return np.array(obs, dtype=np.float32)
+
+        return None
+
+    @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/utils/__init__.py

@@ -0,0 +1,15 @@
+# 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].
+
+from .subscribers import (
+    BaseZenohSubscriber,
+    DecoderFunction,
+    GenericZenohSubscriber,
+    ProtobufZenohSubscriber,
+    json_decoder,
+    protobuf_decoder,
+    raw_bytes_decoder,
+    string_decoder,
+)

dexcontrol/utils/constants.py

@@ -0,0 +1,12 @@
+"""Constants used throughout the dexcontrol package."""
+
+from typing import Final
+
+# Environment variable for robot name
+ROBOT_NAME_ENV_VAR: Final[str] = "ROBOT_NAME"
+
+# Environment variable for communication config path
+COMM_CFG_PATH_ENV_VAR: Final[str] = "DEXMATE_COMM_CFG_PATH"
+
+# Environment variable to disable heartbeat monitoring
+DISABLE_HEARTBEAT_ENV_VAR: Final[str] = "DEXCONTROL_DISABLE_HEARTBEAT"

dexcontrol/utils/io_utils.py

@@ -0,0 +1,26 @@
+# 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].
+
+from typing import Any
+
+from loguru import logger
+
+
+def print_dict(d: dict[str, Any], indent: int = 0) -> None:
+    """Print a nested dictionary with nice formatting.
+
+    Recursively prints dictionary contents with proper indentation.
+
+    Args:
+        d: Dictionary to print.
+        indent: Current indentation level in spaces.
+    """
+    indent_str = " " * indent
+    for key, value in d.items():
+        if isinstance(value, dict):
+            logger.info(f"{indent_str}{key}:")
+            print_dict(value, indent + 4)
+        else:
+            logger.info(f"{indent_str}{key}: {value}")