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

dexcontrol/comm/subscribers.py

@@ -0,0 +1,329 @@
+# 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
+
+"""Core subscriber utilities using DexComm's Raw API.
+
+Simple factory functions for creating DexComm subscribers with
+DexControl's default configurations.
+"""
+
+from pathlib import Path
+from typing import Any, Callable, Optional
+
+from dexcomm import BufferedSubscriber, Subscriber, wait_for_message
+from dexcomm.serialization import deserialize_auto
+from loguru import logger
+
+from dexcontrol.utils.comm_helper import get_zenoh_config_path
+from dexcontrol.utils.os_utils import resolve_key_name
+
+# Import specialized deserializers
+try:
+    from dexcomm.serialization import deserialize_depth, deserialize_image
+
+    IMAGE_DESERIALIZERS_AVAILABLE = True
+except ImportError:
+    logger.debug("DexComm image deserializers not available")
+    deserialize_image = None
+    deserialize_depth = None
+    IMAGE_DESERIALIZERS_AVAILABLE = False
+
+try:
+    from dexcomm.serialization import deserialize_imu
+
+    IMU_DESERIALIZER_AVAILABLE = True
+except ImportError:
+    logger.debug("DexComm IMU deserializer not available")
+    deserialize_imu = None
+    IMU_DESERIALIZER_AVAILABLE = False
+
+try:
+    from dexcomm.serialization.lidar import deserialize_lidar_2d
+
+    LIDAR_DESERIALIZER_AVAILABLE = True
+except ImportError:
+    logger.debug("DexComm LiDAR deserializer not available")
+    deserialize_lidar_2d = None
+    LIDAR_DESERIALIZER_AVAILABLE = False
+
+
+def create_subscriber(
+    topic: str,
+    callback: Optional[Callable[[Any], None]] = None,
+    buffer_size: int = 1,
+    deserializer: Optional[Callable[[bytes], Any]] = deserialize_auto,
+    config: Optional[Path] = None,
+) -> Subscriber:
+    """Create a DexComm Subscriber with DexControl defaults.
+
+    Simple wrapper that handles topic resolution and default configuration.
+
+    Args:
+        topic: Topic to subscribe to (will be resolved with robot namespace)
+        callback: Optional callback function for incoming messages
+        buffer_size: Number of messages to buffer (default 1 for latest only)
+        deserializer: Deserialization function (default: auto-detect)
+        config: Optional config path (default: use robot's config)
+
+    Returns:
+        DexComm Subscriber instance
+
+    Example:
+        ```python
+        # Simple subscription
+        sub = create_subscriber("sensors/imu")
+        data = sub.get_latest()
+
+        # With callback
+        sub = create_subscriber(
+            "robot/state",
+            callback=lambda msg: print(f"State: {msg}")
+        )
+        ```
+    """
+    full_topic = resolve_key_name(topic)
+    config_path = config or get_zenoh_config_path()
+
+    logger.debug(f"Creating subscriber for topic: {full_topic}")
+
+    return Subscriber(
+        topic=full_topic,
+        callback=callback,
+        deserializer=deserializer,
+        buffer_size=buffer_size,
+        config=config_path,
+    )
+
+
+def create_buffered_subscriber(
+    topic: str,
+    callback: Optional[Callable[[Any], None]] = None,
+    buffer_size: int = 100,
+    deserializer: Optional[Callable[[bytes], Any]] = deserialize_auto,
+    config: Optional[Path] = None,
+) -> BufferedSubscriber:
+    """Create a DexComm BufferedSubscriber for data collection.
+
+    Args:
+        topic: Topic to subscribe to
+        callback: Optional callback function
+        buffer_size: Maximum messages to buffer (default 100)
+        deserializer: Deserialization function
+        config: Optional config path
+
+    Returns:
+        DexComm BufferedSubscriber instance
+
+    Example:
+        ```python
+        # Collect sensor data
+        sub = create_buffered_subscriber("sensors/lidar", buffer_size=500)
+        # ... wait for data ...
+        all_scans = sub.get_buffer()
+        ```
+    """
+    full_topic = resolve_key_name(topic)
+    config_path = config or get_zenoh_config_path()
+
+    logger.debug(f"Creating buffered subscriber for topic: {full_topic}")
+
+    return BufferedSubscriber(
+        topic=full_topic,
+        callback=callback,
+        deserializer=deserializer,
+        buffer_size=buffer_size,
+        config=config_path,
+    )
+
+
+def create_camera_subscriber(
+    topic: str,
+    callback: Optional[Callable[[Any], None]] = None,
+    buffer_size: int = 1,
+) -> Subscriber:
+    """Create a subscriber for RGB camera data.
+
+    Uses DexComm's image deserializer for efficient image decompression.
+
+    Args:
+        topic: Camera topic
+        callback: Optional callback for images
+        buffer_size: Number of images to buffer
+
+    Returns:
+        Subscriber configured for RGB images
+    """
+    deserializer = (
+        deserialize_image if IMAGE_DESERIALIZERS_AVAILABLE else deserialize_auto
+    )
+
+    return create_subscriber(
+        topic=topic,
+        callback=callback,
+        buffer_size=buffer_size,
+        deserializer=deserializer,
+    )
+
+
+def create_depth_subscriber(
+    topic: str,
+    callback: Optional[Callable[[Any], None]] = None,
+    buffer_size: int = 1,
+) -> Subscriber:
+    """Create a subscriber for depth camera data.
+
+    Uses DexComm's depth deserializer for 16-bit depth images.
+
+    Args:
+        topic: Depth camera topic
+        callback: Optional callback for depth images
+        buffer_size: Number of images to buffer
+
+    Returns:
+        Subscriber configured for depth images
+    """
+    deserializer = (
+        deserialize_depth if IMAGE_DESERIALIZERS_AVAILABLE else deserialize_auto
+    )
+
+    return create_subscriber(
+        topic=topic,
+        callback=callback,
+        buffer_size=buffer_size,
+        deserializer=deserializer,
+    )
+
+
+def create_imu_subscriber(
+    topic: str,
+    callback: Optional[Callable[[Any], None]] = None,
+    buffer_size: int = 1,
+) -> Subscriber:
+    """Create a subscriber for IMU data.
+
+    Uses DexComm's IMU deserializer for 6-DOF/9-DOF IMU data.
+
+    Args:
+        topic: IMU topic
+        callback: Optional callback for IMU data
+        buffer_size: Number of samples to buffer
+
+    Returns:
+        Subscriber configured for IMU data
+    """
+    deserializer = deserialize_imu if IMU_DESERIALIZER_AVAILABLE else deserialize_auto
+
+    return create_subscriber(
+        topic=topic,
+        callback=callback,
+        buffer_size=buffer_size,
+        deserializer=deserializer,
+    )
+
+
+def create_lidar_subscriber(
+    topic: str,
+    callback: Optional[Callable[[Any], None]] = None,
+    buffer_size: int = 1,
+) -> Subscriber:
+    """Create a subscriber for 2D LiDAR data.
+
+    Uses DexComm's LiDAR deserializer for laser scan data.
+
+    Args:
+        topic: LiDAR topic
+        callback: Optional callback for scan data
+        buffer_size: Number of scans to buffer
+
+    Returns:
+        Subscriber configured for LiDAR data
+    """
+    deserializer = (
+        deserialize_lidar_2d if LIDAR_DESERIALIZER_AVAILABLE else deserialize_auto
+    )
+
+    return create_subscriber(
+        topic=topic,
+        callback=callback,
+        buffer_size=buffer_size,
+        deserializer=deserializer,
+    )
+
+
+def create_generic_subscriber(
+    topic: str,
+    callback: Optional[Callable[[Any], None]] = None,
+    buffer_size: int = 1,
+    raw_bytes: bool = False,
+) -> Subscriber:
+    """Create a generic subscriber with auto-detection or raw bytes.
+
+    Args:
+        topic: Topic to subscribe to
+        callback: Optional callback
+        buffer_size: Number of messages to buffer
+        raw_bytes: If True, return raw bytes without deserialization
+
+    Returns:
+        Generic subscriber instance
+    """
+    deserializer = None if raw_bytes else deserialize_auto
+
+    return create_subscriber(
+        topic=topic,
+        callback=callback,
+        buffer_size=buffer_size,
+        deserializer=deserializer,
+    )
+
+
+def quick_subscribe(topic: str, callback: Callable[[Any], None]) -> Subscriber:
+    """Quick one-liner to subscribe with a callback.
+
+    Args:
+        topic: Topic to subscribe to
+        callback: Callback function
+
+    Returns:
+        Subscriber instance
+
+    Example:
+        ```python
+        sub = quick_subscribe("robot/state", lambda msg: print(msg))
+        ```
+    """
+    return create_subscriber(topic, callback=callback)
+
+
+def wait_for_any_message(topic: str, timeout: float = 5.0) -> Optional[Any]:
+    """Wait for a message on a topic.
+
+    Convenience wrapper around DexComm's wait_for_message.
+
+    Args:
+        topic: Topic to wait on
+        timeout: Maximum wait time in seconds
+
+    Returns:
+        Received message or None if timeout
+
+    Example:
+        ```python
+        state = wait_for_any_message("robot/state", timeout=2.0)
+        if state:
+            print(f"Robot is in state: {state}")
+        ```
+    """
+    full_topic = resolve_key_name(topic)
+    return wait_for_message(
+        topic=full_topic,
+        timeout=timeout,
+        config=get_zenoh_config_path(),
+    )

dexcontrol/config/sensors/cameras/__init__.py

@@ -8,8 +8,7 @@
 # 2. Commercial License
 #    For commercial licensing terms, contact: contact@dexmate.ai
 
-from .luxonis_camera import LuxonisCameraConfig
 from .rgb_camera import RGBCameraConfig
 from .zed_camera import ZedCameraConfig
 
-__all__ = ["RGBCameraConfig", "ZedCameraConfig", "LuxonisCameraConfig"]
+__all__ = ["RGBCameraConfig", "ZedCameraConfig"]

dexcontrol/config/sensors/cameras/zed_camera.py

@@ -39,12 +39,12 @@ class ZedCameraConfig:
         default_factory=lambda: {
             "left_rgb": {
                 "enable": True,
-                "info_key": "camera/head/left_rgb/info",
+                "info_key": "camera/head/info",  # Query the main camera info endpoint
                 "topic": "camera/head/left_rgb",
             },
             "right_rgb": {
                 "enable": True,
-                "info_key": "camera/head/right_rgb/info",
+                "info_key": "camera/head/info",  # Query the main camera info endpoint
                 "topic": "camera/head/right_rgb",
             },
             "depth": {

dexcontrol/config/sensors/vega_sensors.py

@@ -18,7 +18,7 @@ LiDAR and ultrasonic sensors.
 from collections.abc import Callable
 from dataclasses import dataclass, field
 
-from .cameras import LuxonisCameraConfig, RGBCameraConfig, ZedCameraConfig
+from .cameras import RGBCameraConfig, ZedCameraConfig
 from .imu import ChassisIMUConfig, ZedIMUConfig
 from .lidar import RPLidarConfig
 from .ultrasonic import UltraSonicConfig
@@ -33,15 +33,17 @@ def _make_rgb_camera(name: str) -> Callable[[], RGBCameraConfig]:
     Returns:
         Factory function that creates an RGBCameraConfig
     """
-    return lambda: RGBCameraConfig(
-        subscriber_config=dict(
-            rgb=dict(
-                enable=True,
-                info_key=f"camera/base/{name}/rgb/info",
-                topic=f"camera/base/{name}/rgb",
-            )
-        ),
-        name=f"base_{name}_camera",
+    return (
+        lambda: RGBCameraConfig(
+            subscriber_config=dict(
+                rgb=dict(
+                    enable=True,
+                    info_key=f"camera/base_{name}/info",  # Matches dexsensor: camera/base_{name}/info
+                    topic=f"camera/base_{name}/rgb",
+                )
+            ),
+            name=f"base_{name}_camera",
+        )
     )
 
 
@@ -58,12 +60,6 @@ class VegaSensorsConfig:
     """
 
     head_camera: ZedCameraConfig = field(default_factory=ZedCameraConfig)
-    left_wrist_camera: LuxonisCameraConfig = field(
-        default_factory=lambda: LuxonisCameraConfig(name="left_wrist_camera")
-    )
-    right_wrist_camera: LuxonisCameraConfig = field(
-        default_factory=lambda: LuxonisCameraConfig(name="right_wrist_camera")
-    )
     base_left_camera: RGBCameraConfig = field(default_factory=_make_rgb_camera("left"))
     base_right_camera: RGBCameraConfig = field(
         default_factory=_make_rgb_camera("right")
@@ -79,8 +75,6 @@ class VegaSensorsConfig:
 
     def __post_init__(self) -> None:
         self.head_camera.enable = False
-        self.left_wrist_camera.enable = False
-        self.right_wrist_camera.enable = False
         self.base_left_camera.enable = False
         self.base_right_camera.enable = False
         self.base_front_camera.enable = False

dexcontrol/core/arm.py

@@ -15,18 +15,20 @@ communication and the ArmWrenchSensor class for reading wrench sensor data.
 """
 
 import time
-from typing import Any, Final, Literal
+from typing import Any, Literal
 
 import numpy as np
-import zenoh
+from dexcomm import Publisher, call_service
+from dexcomm.serialization import serialize_protobuf
+from dexcomm.utils import RateLimiter
 from jaxtyping import Float
 from loguru import logger
 
 from dexcontrol.config.core.arm import ArmConfig
 from dexcontrol.core.component import RobotComponent, RobotJointComponent
 from dexcontrol.proto import dexcontrol_msg_pb2, dexcontrol_query_pb2
+from dexcontrol.utils.comm_helper import get_zenoh_config_path
 from dexcontrol.utils.os_utils import resolve_key_name
-from dexcontrol.utils.rate_limiter import RateLimiter
 from dexcontrol.utils.trajectory_utils import generate_linear_trajectory
 
 
@@ -44,19 +46,16 @@ class Arm(RobotJointComponent):
     def __init__(
         self,
         configs: ArmConfig,
-        zenoh_session: zenoh.Session,
     ) -> None:
         """Initialize the arm controller.
 
         Args:
             configs: Configuration parameters for the arm including communication topics.
-            zenoh_session: Active Zenoh communication session for message passing.
         """
         super().__init__(
             state_sub_topic=configs.state_sub_topic,
             control_pub_topic=configs.control_pub_topic,
             state_message_type=dexcontrol_msg_pb2.MotorStateWithCurrent,
-            zenoh_session=zenoh_session,
             joint_name=configs.joint_name,
             joint_limit=configs.joint_limit
             if hasattr(configs, "joint_limit")
@@ -67,22 +66,20 @@ class Arm(RobotJointComponent):
             pose_pool=configs.pose_pool,
         )
 
-        self.mode_querier: Final[zenoh.Querier] = zenoh_session.declare_querier(
-            resolve_key_name(configs.set_mode_query), timeout=2.0
-        )
+        # Note: Mode querier functionality will need to be updated to use DexComm
+        # For now, we'll store the query topic for later use
+        self._mode_query_topic = resolve_key_name(configs.set_mode_query)
 
         # Initialize wrench sensor if configured
         self.wrench_sensor: ArmWrenchSensor | None = None
         if configs.wrench_sub_topic:
-            self.wrench_sensor = ArmWrenchSensor(
-                configs.wrench_sub_topic, zenoh_session
-            )
+            self.wrench_sensor = ArmWrenchSensor(configs.wrench_sub_topic)
 
-        # Initialize end effector pass through publisher
-        self._ee_pass_through_publisher: Final[zenoh.Publisher] = (
-            zenoh_session.declare_publisher(
-                resolve_key_name(configs.ee_pass_through_pub_topic)
-            )
+        # Initialize end effector pass through publisher using DexComm
+        self._ee_pass_through_publisher = Publisher(
+            topic=resolve_key_name(configs.ee_pass_through_pub_topic),
+            serializer=serialize_protobuf,
+            config=get_zenoh_config_path(),
         )
 
         self._default_control_hz = configs.default_control_hz
@@ -137,7 +134,17 @@ class Arm(RobotJointComponent):
 
         converted_modes = [mode_map[mode] for mode in modes]
         query_msg = dexcontrol_query_pb2.SetArmMode(modes=converted_modes)
-        self.mode_querier.get(payload=query_msg.SerializeToString())
+        # Use DexComm's call_service for mode setting
+        from dexcontrol.utils.comm_helper import get_zenoh_config_path
+
+        call_service(
+            self._mode_query_topic,
+            request=query_msg,
+            timeout=0.05,
+            config=get_zenoh_config_path(),
+            request_serializer=lambda x: x.SerializeToString(),
+            response_deserializer=None,
+        )
 
     def _send_position_command(self, joint_pos: np.ndarray) -> None:
         """Send joint position command.
@@ -318,8 +325,8 @@ class Arm(RobotJointComponent):
         """Cleans up all Zenoh resources."""
         super().shutdown()
         try:
-            if hasattr(self, "mode_querier") and self.mode_querier:
-                self.mode_querier.undeclare()
+            # Mode querier cleanup no longer needed with DexComm
+            pass
         except Exception as e:
             # Don't log "Undeclared querier" errors as warnings - they're expected during shutdown
             error_msg = str(e).lower()
@@ -338,8 +345,7 @@ class Arm(RobotJointComponent):
             message: The message to send to the robot arm.
         """
         control_msg = dexcontrol_msg_pb2.EndEffectorPassThroughCommand(data=message)
-        control_data = control_msg.SerializeToString()
-        self._ee_pass_through_publisher.put(control_data)
+        self._ee_pass_through_publisher.publish(control_msg)
 
 
 class ArmWrenchSensor(RobotComponent):
@@ -348,16 +354,14 @@ class ArmWrenchSensor(RobotComponent):
     This class provides methods to read wrench sensor data through Zenoh communication.
     """
 
-    def __init__(self, state_sub_topic: str, zenoh_session: zenoh.Session) -> None:
+    def __init__(self, state_sub_topic: str) -> None:
         """Initialize the wrench sensor reader.
 
         Args:
             state_sub_topic: Topic to subscribe to for wrench sensor data.
-            zenoh_session: Active Zenoh communication session for message passing.
         """
         super().__init__(
             state_sub_topic=state_sub_topic,
-            zenoh_session=zenoh_session,
             state_message_type=dexcontrol_msg_pb2.WrenchState,
         )
 

dexcontrol/core/chassis.py

@@ -17,13 +17,12 @@ Zenoh communication. It handles steering and wheel velocity control.
 import time
 
 import numpy as np
-import zenoh
+from dexcomm.utils import RateLimiter
 from jaxtyping import Float
 
 from dexcontrol.config.core import ChassisConfig
 from dexcontrol.core.component import RobotJointComponent
 from dexcontrol.proto import dexcontrol_msg_pb2
-from dexcontrol.utils.rate_limiter import RateLimiter
 
 
 class ChassisSteer(RobotJointComponent):
@@ -36,20 +35,17 @@ class ChassisSteer(RobotJointComponent):
     def __init__(
         self,
         configs: ChassisConfig,
-        zenoh_session: zenoh.Session,
     ) -> None:
         """Initialize the hand controller.
 
         Args:
             configs: Hand configuration parameters containing communication topics
                 and predefined hand positions.
-            zenoh_session: Active Zenoh communication session for message passing.
         """
         super().__init__(
             state_sub_topic=configs.steer_state_sub_topic,
             control_pub_topic=configs.steer_control_pub_topic,
             state_message_type=dexcontrol_msg_pb2.MotorStateWithCurrent,
-            zenoh_session=zenoh_session,
             joint_name=configs.steer_joint_name,
         )
 
@@ -77,19 +73,16 @@ class ChassisDrive(RobotJointComponent):
     def __init__(
         self,
         configs: ChassisConfig,
-        zenoh_session: zenoh.Session,
     ) -> None:
         """Initialize the base controller.
 
         Args:
             configs: Base configuration parameters containing communication topics.
-            zenoh_session: Active Zenoh communication session for message passing.
         """
         super().__init__(
             state_sub_topic=configs.drive_state_sub_topic,
             control_pub_topic=configs.drive_control_pub_topic,
             state_message_type=dexcontrol_msg_pb2.MotorStateWithCurrent,
-            zenoh_session=zenoh_session,
             joint_name=configs.drive_joint_name,
         )
 
@@ -126,16 +119,14 @@ class Chassis:
     def __init__(
         self,
         configs: ChassisConfig,
-        zenoh_session: zenoh.Session,
     ) -> None:
         """Initialize the base controller.
 
         Args:
             configs: Base configuration parameters containing communication topics.
-            zenoh_session: Active Zenoh communication session for message passing.
         """
-        self.chassis_steer = ChassisSteer(configs, zenoh_session)
-        self.chassis_drive = ChassisDrive(configs, zenoh_session)
+        self.chassis_steer = ChassisSteer(configs)
+        self.chassis_drive = ChassisDrive(configs)
 
         self.max_vel = configs.max_vel
         self._center_to_wheel_axis_dist = configs.center_to_wheel_axis_dist