dexcontrol 0.2.12__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/core/chassis.py

@@ -14,14 +14,19 @@ from dataclasses import dataclass, field
 @dataclass
 class ChassisConfig:
     _target_: str = "dexcontrol.core.chassis.Chassis"
-    control_pub_topic: str = "control/chassis"
-    state_sub_topic: str = "state/chassis"
+    steer_control_pub_topic: str = "control/chassis/steer"
+    steer_state_sub_topic: str = "state/chassis/steer"
+    drive_control_pub_topic: str = "control/chassis/drive"
+    drive_state_sub_topic: str = "state/chassis/drive"
     dof: int = 2
     center_to_wheel_axis_dist: float = (
         0.219  # the distance between base center and wheel axis in m
     )
     wheels_dist: float = 0.45  # the distance between two wheels in m (0.41 for vega-rc2, 0.45 for vega-1)
-    joint_name: list[str] = field(
-        default_factory=lambda: ["L_wheel_j1", "L_wheel_j2", "R_wheel_j1", "R_wheel_j2"]
+    steer_joint_name: list[str] = field(
+        default_factory=lambda: ["L_wheel_j1", "R_wheel_j1"]
+    )
+    drive_joint_name: list[str] = field(
+        default_factory=lambda: ["L_wheel_j2", "R_wheel_j2"]
     )
     max_vel: float = 0.8

dexcontrol/config/core/hand.py

@@ -16,6 +16,7 @@ class HandConfig:
     _target_: str = "dexcontrol.core.hand.HandF5D6"
     state_sub_topic: str = "state/hand/right"
     control_pub_topic: str = "control/hand/right"
+    touch_sensor_sub_topic: str = "state/hand/right/touch"  # Only for V2 hand
     dof: int = 6
     joint_name: list[str] = field(
         default_factory=lambda: [

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/config/vega.py

@@ -77,6 +77,7 @@ class VegaConfig:
         default_factory=lambda: HandConfig(
             state_sub_topic="state/hand/left",
             control_pub_topic="control/hand/left",
+            touch_sensor_sub_topic="state/hand/left/touch",
             joint_name=[
                 "L_th_j1",
                 "L_ff_j1",
@@ -91,6 +92,7 @@ class VegaConfig:
         default_factory=lambda: HandConfig(
             state_sub_topic="state/hand/right",
             control_pub_topic="control/hand/right",
+            touch_sensor_sub_topic="state/hand/right/touch",
             joint_name=[
                 "R_th_j1",
                 "R_ff_j1",
@@ -111,8 +113,9 @@ class VegaConfig:
     heartbeat: HeartbeatConfig = field(default_factory=HeartbeatConfig)
 
     # Queries
-    version_info_name: str = "info/version"
+    version_info_name: str = "info/versions"  # Updated to use JSON interface
     status_info_name: str = "info/status"
+    hand_info_query_name: str = "info/hand_type"
     reboot_query_name: str = "system/reboot"
     clear_error_query_name: str = "system/clear_error"
     led_query_name: str = "system/led"

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.ArmState,
-            zenoh_session=zenoh_session,
+            state_message_type=dexcontrol_msg_pb2.MotorStateWithCurrent,
             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
@@ -97,6 +94,9 @@ class Arm(RobotJointComponent):
     def set_mode(self, mode: Literal["position", "disable"]) -> None:
         """Sets the operating mode of the arm.
 
+        .. deprecated::
+            Use set_modes() instead for setting arm modes.
+
         Args:
             mode: Operating mode for the arm. Must be either "position" or "disable".
                 "position": Enable position control
@@ -105,18 +105,46 @@ class Arm(RobotJointComponent):
         Raises:
             ValueError: If an invalid mode is specified.
         """
+        logger.warning("arm.set_mode() is deprecated, use set_modes() instead")
+        self.set_modes([mode] * 7)
+
+    def set_modes(self, modes: list[Literal["position", "disable", "release"]]) -> None:
+        """Sets the operating modes of the arm.
+
+        Args:
+            modes: List of operating modes for the arm. Each mode must be either "position", "disable", or "current".
+
+        Raises:
+            ValueError: If any mode in the list is invalid.
+        """
         mode_map = {
             "position": dexcontrol_query_pb2.SetArmMode.Mode.POSITION,
             "disable": dexcontrol_query_pb2.SetArmMode.Mode.DISABLE,
+            "release": dexcontrol_query_pb2.SetArmMode.Mode.CURRENT,
         }
 
-        if mode not in mode_map:
-            raise ValueError(
-                f"Invalid mode: {mode}. Must be one of {list(mode_map.keys())}"
-            )
+        for mode in modes:
+            if mode not in mode_map:
+                raise ValueError(
+                    f"Invalid mode: {mode}. Must be one of {list(mode_map.keys())}"
+                )
 
-        query_msg = dexcontrol_query_pb2.SetArmMode(mode=mode_map[mode])
-        self.mode_querier.get(payload=query_msg.SerializeToString())
+        if len(modes) != 7:
+            raise ValueError("Arm modes length must match arm DoF (7).")
+
+        converted_modes = [mode_map[mode] for mode in modes]
+        query_msg = dexcontrol_query_pb2.SetArmMode(modes=converted_modes)
+        # 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.
@@ -124,8 +152,8 @@ class Arm(RobotJointComponent):
         Args:
             joint_pos: Joint positions as numpy array.
         """
-        control_msg = dexcontrol_msg_pb2.ArmCommand()
-        control_msg.joint_pos.extend(joint_pos.tolist())
+        control_msg = dexcontrol_msg_pb2.MotorPosVelCurrentCommand()
+        control_msg.pos.extend(joint_pos.tolist())
         self._publish_control(control_msg)
 
     def set_joint_pos(
@@ -287,10 +315,9 @@ class Arm(RobotJointComponent):
             joint_vel, clip_value=self._joint_vel_limit
         )
 
-        control_msg = dexcontrol_msg_pb2.ArmCommand(
-            command_type=dexcontrol_msg_pb2.ArmCommand.CommandType.VELOCITY_FEEDFORWARD,
-            joint_pos=list(target_pos),
-            joint_vel=list(target_vel),
+        control_msg = dexcontrol_msg_pb2.MotorPosVelCurrentCommand(
+            pos=list(target_pos),
+            vel=list(target_vel),
         )
         self._publish_control(control_msg)
 
@@ -298,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()
@@ -318,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):
@@ -328,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,
         )