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

dexcontrol/sensors/imu/chassis_imu.py

@@ -8,23 +8,19 @@
 # 2. Commercial License
 #    For commercial licensing terms, contact: contact@dexmate.ai
 
-"""Ultrasonic sensor implementations using Zenoh subscribers.
+"""Chassis IMU sensor implementation using DexComm subscribers.
 
-This module provides ultrasonic sensor classes that use the generic
-subscriber for distance measurements.
+This module provides IMU sensor class for chassis IMU data
+using DexComm's Raw API.
 """
 
-from typing import Literal, cast
-
 import numpy as np
-import zenoh
 
-from dexcontrol.proto import dexcontrol_msg_pb2
-from dexcontrol.utils.subscribers import ProtobufZenohSubscriber
+from dexcontrol.comm import create_imu_subscriber
 
 
 class ChassisIMUSensor:
-    """Chassis IMU sensor using Zenoh subscriber.
+    """Chassis IMU sensor using DexComm subscriber.
 
     This sensor provides IMU data from the chassis
     """
@@ -32,41 +28,34 @@ class ChassisIMUSensor:
     def __init__(
         self,
         configs,
-        zenoh_session: zenoh.Session,
     ) -> None:
-        """Initialize the ultrasonic sensor.
+        """Initialize the chassis IMU sensor.
 
         Args:
-            configs: Configuration for the ultrasonic sensor.
-            zenoh_session: Active Zenoh session for communication.
+            configs: Configuration for the chassis IMU sensor.
         """
         self._name = configs.name
 
-        # Create the generic subscriber with JSON decoder
-        self._subscriber = ProtobufZenohSubscriber(
+        # Create IMU subscriber using DexComm integration
+        self._subscriber = create_imu_subscriber(
             topic=configs.topic,
-            zenoh_session=zenoh_session,
-            message_type=dexcontrol_msg_pb2.IMUState,
-            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."""
+        """Shutdown the chassis IMU sensor."""
         self._subscriber.shutdown()
 
     def is_active(self) -> bool:
-        """Check if the ultrasonic sensor is actively receiving data.
+        """Check if the chassis IMU sensor is actively receiving data.
 
         Returns:
             True if receiving data, False otherwise.
         """
-        return self._subscriber.is_active()
+        data = self._subscriber.get_latest()
+        return data is not None
 
     def wait_for_active(self, timeout: float = 5.0) -> bool:
-        """Wait for the ultrasonic sensor to start receiving data.
+        """Wait for the chassis IMU sensor to start receiving data.
 
         Args:
             timeout: Maximum time to wait in seconds.
@@ -74,80 +63,110 @@ class ChassisIMUSensor:
         Returns:
             True if sensor becomes active, False if timeout is reached.
         """
-        return self._subscriber.wait_for_active(timeout)
+        msg = self._subscriber.wait_for_message(timeout)
+        return msg is not None
 
-    def get_obs(self, obs_keys: list[Literal['ang_vel', 'acc', 'quat']] | None = None) -> dict[Literal['ang_vel', 'acc', 'quat', 'timestamp_ns'], np.ndarray]:
-        """Get observation data for the ZED IMU sensor.
+    def get_obs(
+        self, obs_keys: list[str] | None = None
+    ) -> dict[str, np.ndarray] | None:
+        """Get observation data for the chassis IMU sensor.
 
         Args:
             obs_keys: List of observation keys to retrieve. If None, returns all available data.
-                     Valid keys: ['ang_vel', 'acc', 'quat']
+                     Valid keys: ['ang_vel', 'acc', 'quat', 'mag', 'timestamp']
 
         Returns:
             Dictionary with observation data including all IMU measurements.
             Keys are mapped as follows:
-            - 'ang_vel': Angular velocity from 'angular_velocity'
-            - 'acc': Linear acceleration from 'acceleration'
-            - 'quat': Orientation quaternion from 'orientation', in wxyz convention
+            - 'ang_vel': Angular velocity from 'gyro'
+            - 'acc': Linear acceleration from 'acc'
+            - 'quat': Orientation quaternion from 'quat' [w, x, y, z]
+            - 'mag': Magnetometer from 'mag' (if available)
             - 'timestamp_ns': Timestamp in nanoseconds
         """
         if obs_keys is None:
-            obs_keys = ['ang_vel', 'acc', 'quat']
+            obs_keys = ["ang_vel", "acc", "quat"]
 
-        data = self._subscriber.get_latest_data()
-        data = cast(dexcontrol_msg_pb2.IMUState, data)
+        data = self._subscriber.get_latest()
         if data is None:
-            raise RuntimeError("No IMU data available")
+            return None
 
         obs_out = {}
 
+        # Add timestamp if available
+        if "timestamp" in data:
+            obs_out["timestamp_ns"] = data["timestamp"]
+
         for key in obs_keys:
-            if key == 'ang_vel':
-                obs_out[key] = np.array([data.gyro_x, data.gyro_y, data.gyro_z])
-            elif key == 'acc':
-                obs_out[key] = np.array([data.acc_x, data.acc_y, data.acc_z])
-            elif key == 'quat':
-                obs_out[key] = np.array([data.quat_w, data.quat_x, data.quat_y, data.quat_z])
-            else:
-                raise ValueError(f"Invalid observation key: {key}")
-
-        if hasattr(data, 'timestamp_ns'):
-            obs_out['timestamp_ns'] = data.timestamp_ns
+            if key == "ang_vel":
+                obs_out[key] = data.get("gyro", np.zeros(3))
+            elif key == "acc":
+                obs_out[key] = data.get("acc", np.zeros(3))
+            elif key == "quat":
+                obs_out[key] = data.get("quat", np.array([1.0, 0.0, 0.0, 0.0]))
+            elif key == "mag" and "mag" in data:
+                obs_out[key] = data["mag"]
+            elif key == "timestamp" and "timestamp" in data:
+                obs_out["timestamp_ns"] = data["timestamp"]
 
         return obs_out
 
-    def get_acceleration(self) -> np.ndarray:
-        """Get the latest linear acceleration from ZED IMU.
+    def get_acc(self) -> np.ndarray | None:
+        """Get the latest linear acceleration from chassis IMU.
 
         Returns:
             Linear acceleration [x, y, z] in m/s² if available, None otherwise.
         """
-        return self.get_obs(obs_keys=['acc'])['acc']
+        data = self._subscriber.get_latest()
+        return data.get("acc") if data else None
 
-    def get_angular_velocity(self) -> np.ndarray:
-        """Get the latest angular velocity from ZED IMU.
+    def get_gyro(self) -> np.ndarray | None:
+        """Get the latest angular velocity from chassis IMU.
 
         Returns:
             Angular velocity [x, y, z] in rad/s if available, None otherwise.
         """
-        return self.get_obs(obs_keys=['ang_vel'])['ang_vel']
+        data = self._subscriber.get_latest()
+        return data.get("gyro") if data else None
 
-    def get_orientation(self) -> np.ndarray:
-        """Get the latest orientation quaternion from ZED IMU.
+    def get_quat(self) -> np.ndarray | None:
+        """Get the latest orientation quaternion from chassis IMU.
 
         Returns:
-            Orientation quaternion [x, y, z, w] if available, None otherwise.
+            Orientation quaternion [w, x, y, z] if available, None otherwise.
+            Note: dexcomm uses [w, x, y, z] quaternion format.
         """
-        return self.get_obs(obs_keys=['quat'])['quat']
+        data = self._subscriber.get_latest()
+        return data.get("quat") if data else None
 
-    @property
-    def fps(self) -> float:
-        """Get the current FPS measurement.
+    def get_mag(self) -> np.ndarray | None:
+        """Get the latest magnetometer reading from chassis IMU.
+
+        Returns:
+            Magnetic field [x, y, z] in µT if available, None otherwise.
+        """
+        data = self._subscriber.get_latest()
+        if not data or not isinstance(data, dict):
+            return None
+        return data.get("mag", None)
+
+    def has_mag(self) -> bool:
+        """Check if the chassis IMU has magnetometer data available.
 
         Returns:
-            Current frames per second measurement.
+            True if magnetometer data is available, False otherwise.
         """
-        return self._subscriber.fps
+        data = self._subscriber.get_latest()
+        if not data or not isinstance(data, dict):
+            return False
+        return "mag" in data and data["mag"] is not None
+
+    # Backward compatibility aliases
+    get_acceleration = get_acc
+    get_angular_velocity = get_gyro
+    get_orientation = get_quat
+    get_magnetometer = get_mag
+    has_magnetometer = has_mag
 
     @property
     def name(self) -> str:

dexcontrol/sensors/imu/zed_imu.py

@@ -11,9 +11,8 @@
 """ZED IMU sensor implementation using Zenoh subscriber."""
 
 import numpy as np
-import zenoh
 
-from dexcontrol.utils.subscribers.imu import IMUSubscriber
+from dexcontrol.comm import create_imu_subscriber
 
 
 class ZedIMUSensor:
@@ -28,23 +27,17 @@ class ZedIMUSensor:
     def __init__(
         self,
         configs,
-        zenoh_session: zenoh.Session,
     ) -> None:
         """Initialize the ZED IMU sensor.
 
         Args:
             configs: Configuration object containing topic, name, and other settings.
-            zenoh_session: Active Zenoh session for communication.
         """
         self._name = configs.name
 
-        # Create the IMU subscriber
-        self._subscriber = IMUSubscriber(
+
+        self._subscriber = create_imu_subscriber(
             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:
@@ -57,7 +50,8 @@ class ZedIMUSensor:
         Returns:
             True if receiving data, False otherwise.
         """
-        return self._subscriber.is_active()
+        data = self._subscriber.get_latest()
+        return data is not None
 
     def wait_for_active(self, timeout: float = 5.0) -> bool:
         """Wait for the ZED IMU sensor to start receiving data.
@@ -68,92 +62,109 @@ class ZedIMUSensor:
         Returns:
             True if sensor becomes active, False if timeout is reached.
         """
-        return self._subscriber.wait_for_active(timeout)
+        msg = self._subscriber.wait_for_message(timeout)
+        return msg is not None
 
     def get_obs(self, obs_keys: list[str] | None = None) -> dict[str, np.ndarray] | None:
         """Get observation data for the ZED IMU sensor.
 
         Args:
             obs_keys: List of observation keys to retrieve. If None, returns all available data.
-                     Valid keys: ['ang_vel', 'acc', 'quat', 'timestamp']
+                     Valid keys: ['ang_vel', 'acc', 'quat', 'mag', 'timestamp']
 
         Returns:
             Dictionary with observation data including all IMU measurements.
             Keys are mapped as follows:
-            - 'ang_vel': Angular velocity from 'angular_velocity'
-            - 'acc': Linear acceleration from 'acceleration'
-            - 'quat': Orientation quaternion from 'orientation'
+            - 'ang_vel': Angular velocity from 'gyro'
+            - 'acc': Linear acceleration from 'acc'
+            - 'quat': Orientation quaternion from 'quat'
+            - 'mag': Magnetometer from 'mag' (if available)
             - 'timestamp_ns': Timestamp from 'timestamp'
         """
         if obs_keys is None:
             obs_keys = ['ang_vel', 'acc', 'quat']
 
-        data = self._subscriber.get_latest_data()
+        data = self._subscriber.get_latest()
         if data is None:
             return None
 
-        obs_out = {'timestamp_ns': data['timestamp']}
+        obs_out = {}
+
+        # Add timestamp if available
+        if 'timestamp' in data:
+            obs_out['timestamp_ns'] = data['timestamp']
 
         for key in obs_keys:
             if key == 'ang_vel':
-                obs_out[key] = data['angular_velocity']
+                obs_out[key] = data.get('gyro', np.zeros(3))
             elif key == 'acc':
-                obs_out[key] = data['acceleration']
+                obs_out[key] = data.get('acc', np.zeros(3))
             elif key == 'quat':
-                obs_out[key] = data['orientation']
-            else:
-                raise ValueError(f"Invalid observation key: {key}")
+                obs_out[key] = data.get('quat', np.array([1.0, 0.0, 0.0, 0.0]))
+            elif key == 'mag' and 'mag' in data:
+                obs_out[key] = data['mag']
+            elif key == 'timestamp' and 'timestamp' in data:
+                obs_out['timestamp_ns'] = data['timestamp']
 
         return obs_out
 
-    def get_acceleration(self) -> np.ndarray | None:
+    def get_acc(self) -> np.ndarray | None:
         """Get the latest linear acceleration from ZED IMU.
 
         Returns:
             Linear acceleration [x, y, z] in m/s² if available, None otherwise.
         """
-        return self._subscriber.get_acceleration()
+        data = self._subscriber.get_latest()
+        return data.get('acc') if data else None
 
-    def get_angular_velocity(self) -> np.ndarray | None:
+    def get_gyro(self) -> np.ndarray | None:
         """Get the latest angular velocity from ZED IMU.
 
         Returns:
             Angular velocity [x, y, z] in rad/s if available, None otherwise.
         """
-        return self._subscriber.get_angular_velocity()
+        data = self._subscriber.get_latest()
+        return data.get('gyro') if data else None
 
-    def get_orientation(self) -> np.ndarray | None:
+    def get_quat(self) -> np.ndarray | None:
         """Get the latest orientation quaternion from ZED IMU.
 
         Returns:
-            Orientation quaternion [x, y, z, w] if available, None otherwise.
+            Orientation quaternion [w, x, y, z] if available, None otherwise.
+            Note: dexcomm uses [w, x, y, z] quaternion format.
         """
-        return self._subscriber.get_orientation()
+        data = self._subscriber.get_latest()
+        return data.get('quat') if data else None
 
-    def get_magnetometer(self) -> np.ndarray | None:
+    def get_mag(self) -> np.ndarray | None:
         """Get the latest magnetometer reading from ZED IMU.
 
         Returns:
             Magnetic field [x, y, z] in µT if available, None otherwise.
         """
-        return self._subscriber.get_magnetometer()
+        data = self._subscriber.get_latest()
+        if not data or not isinstance(data, dict):
+            return None
+        return data.get('mag', None)
 
-    def has_magnetometer(self) -> bool:
+    def has_mag(self) -> bool:
         """Check if the ZED IMU has magnetometer data available.
 
         Returns:
             True if magnetometer data is available, False otherwise.
         """
-        return self._subscriber.has_magnetometer()
-
-    @property
-    def fps(self) -> float:
-        """Get the current FPS measurement.
-
-        Returns:
-            Current frames per second measurement.
-        """
-        return self._subscriber.fps
+        data = self._subscriber.get_latest()
+        if not data or not isinstance(data, dict):
+            return False
+        return 'mag' in data and data['mag'] is not None
+
+
+    # Backward compatibility aliases
+    get_acceleration = get_acc
+    get_angular_velocity = get_gyro
+    get_orientation = get_quat
+    get_magnetometer = get_mag
+    has_magnetometer = has_mag
 
     @property
     def name(self) -> str:

dexcontrol/sensors/lidar/rplidar.py

@@ -17,9 +17,8 @@ subscriber for scan data.
 from typing import Any
 
 import numpy as np
-import zenoh
 
-from dexcontrol.utils.subscribers.lidar import LidarSubscriber
+from dexcontrol.comm import create_lidar_subscriber
 
 
 class RPLidarSensor:
@@ -32,26 +31,17 @@ class RPLidarSensor:
     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.
+            configs: Configuration for the LIDAR sensor.
         """
         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,
+        self._subscriber = create_lidar_subscriber(
+            topic=configs.topic
         )
 
 
@@ -65,7 +55,8 @@ class RPLidarSensor:
         Returns:
             True if receiving data, False otherwise.
         """
-        return self._subscriber.is_active()
+        data = self._subscriber.get_latest()
+        return data is not None
 
     def wait_for_active(self, timeout: float = 5.0) -> bool:
         """Wait for the LIDAR sensor to start receiving data.
@@ -76,7 +67,8 @@ class RPLidarSensor:
         Returns:
             True if sensor becomes active, False if timeout is reached.
         """
-        return self._subscriber.wait_for_active(timeout)
+        msg = self._subscriber.wait_for_message(timeout)
+        return msg is not None
 
     def get_obs(self) -> dict[str, Any] | None:
         """Get the latest LIDAR scan data.
@@ -89,7 +81,8 @@ class RPLidarSensor:
                 - qualities: Array of quality values (0-255) if available, None otherwise
                 - timestamp: Timestamp in nanoseconds (int)
         """
-        return self._subscriber.get_latest_data()
+        data = self._subscriber.get_latest()
+        return data
 
     def get_ranges(self) -> np.ndarray | None:
         """Get the latest range measurements.
@@ -97,7 +90,8 @@ class RPLidarSensor:
         Returns:
             Array of range measurements in meters if available, None otherwise.
         """
-        return self._subscriber.get_ranges()
+        data = self._subscriber.get_latest()
+        return data.ranges if data else None
 
     def get_angles(self) -> np.ndarray | None:
         """Get the latest angle measurements.
@@ -105,7 +99,8 @@ class RPLidarSensor:
         Returns:
             Array of angle measurements in radians if available, None otherwise.
         """
-        return self._subscriber.get_angles()
+        data = self._subscriber.get_latest()
+        return data.angles if data else None
 
     def get_qualities(self) -> np.ndarray | None:
         """Get the latest quality measurements.
@@ -113,7 +108,8 @@ class RPLidarSensor:
         Returns:
             Array of quality values (0-255) if available, None otherwise.
         """
-        return self._subscriber.get_qualities()
+        data = self._subscriber.get_latest()
+        return data.qualities if data else None
 
     def get_point_count(self) -> int:
         """Get the number of points in the latest scan.

dexcontrol/sensors/manager.py

@@ -8,7 +8,7 @@
 # 2. Commercial License
 #    For commercial licensing terms, contact: contact@dexmate.ai
 
-"""Sensor manager for managing robot sensors with Zenoh communication.
+"""Sensor manager for managing robot sensors.
 
 This module provides the Sensors class that manages multiple sensor instances
 based on configuration and provides unified access to them.
@@ -19,14 +19,12 @@ 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.luxonis_camera import LuxonisCameraSensor
     from dexcontrol.sensors.camera.rgb_camera import RGBCameraSensor
     from dexcontrol.sensors.camera.zed_camera import ZedCameraSensor
     from dexcontrol.sensors.imu.chassis_imu import ChassisIMUSensor
@@ -44,14 +42,11 @@ class Sensors:
 
     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: ZedCameraSensor
-        left_wrist_camera: LuxonisCameraSensor
-        right_wrist_camera: LuxonisCameraSensor
         base_left_camera: RGBCameraSensor
         base_right_camera: RGBCameraSensor
         base_front_camera: RGBCameraSensor
@@ -61,16 +56,13 @@ class Sensors:
         lidar: RPLidarSensor
         ultrasonic: UltrasonicSensor
 
-    def __init__(self, configs: DictConfig | VegaSensorsConfig,
-                 zenoh_session: zenoh.Session) -> None:
+    def __init__(self, configs: DictConfig | VegaSensorsConfig) -> 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)
@@ -108,10 +100,8 @@ class Sensors:
                 'configs': {k: v for k, v in config.items() if k != '_target_'}
             })
 
-            sensor = hydra_utils.instantiate(
-                temp_config,
-                zenoh_session=self._zenoh_session,
-            )
+            # Note: zenoh_session no longer needed as DexComm handles sessions
+            sensor = hydra_utils.instantiate(temp_config)
 
             if hasattr(sensor, "start"):
                 sensor.start()