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

dexcontrol/core/component.py

@@ -8,24 +8,27 @@
 # 2. Commercial License
 #    For commercial licensing terms, contact: contact@dexmate.ai
 
-"""Base module for robot components with Zenoh communication.
+"""Base module for robot components using DexComm communication.
 
-This module provides base classes for robot components that communicate via Zenoh.
-It includes RobotComponent for state-only components and RobotJointComponent for
-components that also support control commands.
+This module provides base classes for robot components that use DexComm's
+Raw API for communication. It includes RobotComponent for state-only components
+and RobotJointComponent for components that also support control commands.
 """
 
 import time
-from typing import Any, Final, Mapping, TypeVar
+from typing import Any, Final, Mapping, Optional, TypeVar
 
 import numpy as np
-import zenoh
+
+# Use DexComm for all communication
+from dexcomm import Publisher, Subscriber
+from dexcomm.serialization import deserialize_protobuf, serialize_protobuf
 from google.protobuf.message import Message
 from jaxtyping import Float
 from loguru import logger
 
+from dexcontrol.utils.comm_helper import get_zenoh_config_path
 from dexcontrol.utils.os_utils import resolve_key_name
-from dexcontrol.utils.subscribers import ProtobufZenohSubscriber
 
 # Type variable for Message subclasses
 M = TypeVar("M", bound=Message)
@@ -47,41 +50,55 @@ class RobotComponent:
     def __init__(
         self,
         state_sub_topic: str,
-        zenoh_session: zenoh.Session,
         state_message_type: type[M],
     ) -> None:
         """Initializes RobotComponent.
 
         Args:
             state_sub_topic: Topic to subscribe to for state updates.
-            zenoh_session: Active Zenoh session for communication.
             state_message_type: Protobuf message class for component state.
         """
         self._state_message_type = state_message_type
-        self._zenoh_session: Final[zenoh.Session] = zenoh_session
-        self._init_subscriber(state_sub_topic, state_message_type, zenoh_session)
+        self._latest_state: Optional[M] = None
+        self._is_active = False
+        self._init_subscriber(state_sub_topic, state_message_type)
 
     def _init_subscriber(
         self,
         state_sub_topic: str,
         state_message_type: type[M],
-        zenoh_session: zenoh.Session,
     ) -> None:
-        """Initialize the Zenoh subscriber for state updates.
+        """Initialize the subscriber for state updates using DexComm.
 
         Args:
             state_sub_topic: Topic to subscribe to for state updates.
             state_message_type: Protobuf message class for component state.
-            zenoh_session: Active Zenoh session for communication.
-        """
-        self._subscriber = ProtobufZenohSubscriber(
-            topic=state_sub_topic,
-            zenoh_session=zenoh_session,
-            message_type=state_message_type,
-            name=f"{self.__class__.__name__}",
-            enable_fps_tracking=False,
+        """
+        # Resolve topic with robot namespace
+        full_topic = resolve_key_name(state_sub_topic)
+
+        # Create DexComm subscriber with protobuf deserialization
+        self._subscriber = Subscriber(
+            topic=full_topic,
+            callback=self._on_state_update,
+            deserializer=lambda data: deserialize_protobuf(data, state_message_type),
+            config=get_zenoh_config_path(),
         )
 
+        logger.debug(
+            f"Created subscriber for {self.__class__.__name__} on {full_topic}"
+        )
+
+    def _on_state_update(self, state: M) -> None:
+        """Handle incoming state updates.
+
+        Args:
+            state: Deserialized protobuf state message.
+        """
+        self._latest_state = state
+        if state:
+            self._is_active = True
+
     def _get_state(self) -> M:
         """Gets the current state of the component.
 
@@ -91,10 +108,9 @@ class RobotComponent:
         Raises:
             None: If no state data is available.
         """
-        state = self._subscriber.get_latest_data()
-        if state is None:
+        if self._latest_state is None:
             logger.error(f"No state data available for {self.__class__.__name__}")
-        return state
+        return self._latest_state
 
     def wait_for_active(self, timeout: float = 5.0) -> bool:
         """Waits for the component to start receiving state updates.
@@ -105,7 +121,12 @@ class RobotComponent:
         Returns:
             True if component becomes active, False if timeout is reached.
         """
-        return self._subscriber.wait_for_active(timeout)
+        start_time = time.time()
+        while time.time() - start_time < timeout:
+            if self._is_active:
+                return True
+            time.sleep(0.1)
+        return False
 
     def is_active(self) -> bool:
         """Check if component is receiving state updates.
@@ -113,7 +134,7 @@ class RobotComponent:
         Returns:
             True if component is active, False otherwise.
         """
-        return self._subscriber.is_active()
+        return self._is_active
 
     def shutdown(self) -> None:
         """Cleans up Zenoh resources."""
@@ -149,7 +170,7 @@ class RobotJointComponent(RobotComponent):
     Extends RobotComponent to add APIs for interacting with joints.
 
     Attributes:
-        _publisher: Zenoh publisher for control commands.
+        _publisher: Publisher for control commands (Zenoh or dexcomm).
         _joint_name: List of joint names for this component.
         _pose_pool: Dictionary of predefined poses for this component.
     """
@@ -178,7 +199,6 @@ class RobotJointComponent(RobotComponent):
         state_sub_topic: str,
         control_pub_topic: str,
         state_message_type: type[M],
-        zenoh_session: zenoh.Session,
         joint_name: list[str] | None = None,
         joint_limit: list[list[float]] | None = None,
         joint_vel_limit: list[float] | None = None,
@@ -190,16 +210,24 @@ class RobotJointComponent(RobotComponent):
             state_sub_topic: Topic to subscribe to for state updates.
             control_pub_topic: Topic to publish control commands.
             state_message_type: Protobuf message class for component state.
-            zenoh_session: Active Zenoh session for communication.
             joint_name: List of joint names for this component.
+            joint_limit: Joint position limits.
+            joint_vel_limit: Joint velocity limits.
             pose_pool: Dictionary of predefined poses for this component.
         """
-        super().__init__(state_sub_topic, zenoh_session, state_message_type)
+        super().__init__(state_sub_topic, state_message_type)
+
+        # Resolve topic with robot namespace
+        full_topic = resolve_key_name(control_pub_topic)
 
-        resolved_topic: Final[str] = resolve_key_name(control_pub_topic)
-        self._publisher: Final[zenoh.Publisher] = zenoh_session.declare_publisher(
-            resolved_topic
+        # Create DexComm publisher with protobuf serialization
+        self._publisher: Final[Publisher] = Publisher(
+            topic=full_topic,
+            serializer=serialize_protobuf,
+            config=get_zenoh_config_path(),
         )
+
+        logger.debug(f"Created publisher for {self.__class__.__name__} on {full_topic}")
         self._joint_name: list[str] | None = joint_name
         self._joint_limit: np.ndarray | None = (
             np.array(joint_limit) if joint_limit else None
@@ -218,22 +246,19 @@ class RobotJointComponent(RobotComponent):
         Args:
             control_msg: Protobuf control message to publish.
         """
-        msg_bytes = control_msg.SerializeToString()
-        self._publisher.put(msg_bytes)
+        # DexComm publisher with protobuf serializer handles this
+        self._publisher.publish(control_msg)
 
     def shutdown(self) -> None:
-        """Cleans up all Zenoh resources."""
+        """Cleans up all communication resources."""
         super().shutdown()
         try:
             if hasattr(self, "_publisher") and self._publisher:
-                self._publisher.undeclare()
+                self._publisher.shutdown()
         except Exception as e:
-            # Don't log "Undeclared publisher" 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 publisher for {self.__class__.__name__}: {e}"
-                )
+            logger.warning(
+                f"Error shutting down publisher for {self.__class__.__name__}: {e}"
+            )
 
     @property
     def joint_name(self) -> list[str]:

dexcontrol/core/hand.py

@@ -18,8 +18,8 @@ from enum import Enum
 from typing import Any, cast
 
 import numpy as np
-import zenoh
 from jaxtyping import Float
+from loguru import logger
 
 from dexcontrol.config.core.hand import HandConfig
 from dexcontrol.core.component import RobotComponent, RobotJointComponent
@@ -42,7 +42,6 @@ class Hand(RobotJointComponent):
     def __init__(
         self,
         configs: HandConfig,
-        zenoh_session: zenoh.Session,
         hand_type: HandType = HandType.HandF5D6_V1,
     ) -> None:
         """Initialize the hand controller.
@@ -50,13 +49,11 @@ class Hand(RobotJointComponent):
         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.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,
         )
 
@@ -129,17 +126,14 @@ class HandF5D6(Hand):
     def __init__(
         self,
         configs: HandConfig,
-        zenoh_session: zenoh.Session,
         hand_type: HandType = HandType.HandF5D6_V1,
     ) -> None:
-        super().__init__(configs, zenoh_session)
+        super().__init__(configs)
 
         # Initialize touch sensor for F5D6_V2 hands
         self._hand_type = hand_type
         if self._hand_type == HandType.HandF5D6_V2:
-            self._touch_sensor = HandF5D6TouchSensor(
-                configs.touch_sensor_sub_topic, zenoh_session
-            )
+            self._touch_sensor = HandF5D6TouchSensor(configs.touch_sensor_sub_topic)
         elif self._hand_type == HandType.HandF5D6_V1:
             self._touch_sensor = None
         else:
@@ -164,27 +158,30 @@ class HandF5D6(Hand):
         exit_on_reach_kwargs: dict[str, Any] | None = None,
     ) -> None:
         """Close the hand fully using a two-step approach for better control."""
-        if self.is_joint_pos_reached(self._joint_pos_close, tolerance=0.1):
-            return
-
-        # First step: Move to intermediate position
-        intermediate_pos = self._get_intermediate_close_position()
-        first_step_wait_time = 0.8
-        self.set_joint_pos(
-            intermediate_pos,
-            wait_time=first_step_wait_time,
-            exit_on_reach=exit_on_reach,
-            exit_on_reach_kwargs=exit_on_reach_kwargs,
-        )
+        try:
+            if self.is_joint_pos_reached(self._joint_pos_close, tolerance=0.1):
+                return
+
+            # First step: Move to intermediate position
+            intermediate_pos = self._get_intermediate_close_position()
+            first_step_wait_time = 0.8
+            self.set_joint_pos(
+                intermediate_pos,
+                wait_time=first_step_wait_time,
+                exit_on_reach=exit_on_reach,
+                exit_on_reach_kwargs=exit_on_reach_kwargs,
+            )
 
-        # Second step: Move to final closed position
-        remaining_wait_time = max(0.0, wait_time - first_step_wait_time)
-        self.set_joint_pos(
-            self._joint_pos_close,
-            wait_time=remaining_wait_time,
-            exit_on_reach=exit_on_reach,
-            exit_on_reach_kwargs=exit_on_reach_kwargs,
-        )
+            # Second step: Move to final closed position
+            remaining_wait_time = max(0.0, wait_time - first_step_wait_time)
+            self.set_joint_pos(
+                self._joint_pos_close,
+                wait_time=remaining_wait_time,
+                exit_on_reach=exit_on_reach,
+                exit_on_reach_kwargs=exit_on_reach_kwargs,
+            )
+        except Exception as e:
+            logger.warning(f"Failed to close hand: {e}")
 
     def open_hand(
         self,
@@ -193,27 +190,30 @@ class HandF5D6(Hand):
         exit_on_reach_kwargs: dict[str, Any] | None = None,
     ) -> None:
         """Open the hand fully using a two-step approach for better control."""
-        if self.is_joint_pos_reached(self._joint_pos_open, tolerance=0.1):
-            return
-
-        # First step: Move to intermediate position
-        intermediate_pos = self._get_intermediate_open_position()
-        first_step_wait_time = 0.3
-        self.set_joint_pos(
-            intermediate_pos,
-            wait_time=first_step_wait_time,
-            exit_on_reach=exit_on_reach,
-            exit_on_reach_kwargs=exit_on_reach_kwargs,
-        )
+        try:
+            if self.is_joint_pos_reached(self._joint_pos_open, tolerance=0.1):
+                return
+
+            # First step: Move to intermediate position
+            intermediate_pos = self._get_intermediate_open_position()
+            first_step_wait_time = 0.3
+            self.set_joint_pos(
+                intermediate_pos,
+                wait_time=first_step_wait_time,
+                exit_on_reach=exit_on_reach,
+                exit_on_reach_kwargs=exit_on_reach_kwargs,
+            )
 
-        # Second step: Move to final open position
-        remaining_wait_time = max(0.0, wait_time - first_step_wait_time)
-        self.set_joint_pos(
-            self._joint_pos_open,
-            wait_time=remaining_wait_time,
-            exit_on_reach=exit_on_reach,
-            exit_on_reach_kwargs=exit_on_reach_kwargs,
-        )
+            # Second step: Move to final open position
+            remaining_wait_time = max(0.0, wait_time - first_step_wait_time)
+            self.set_joint_pos(
+                self._joint_pos_open,
+                wait_time=remaining_wait_time,
+                exit_on_reach=exit_on_reach,
+                exit_on_reach_kwargs=exit_on_reach_kwargs,
+            )
+        except Exception as e:
+            logger.warning(f"Failed to open hand: {e}")
 
     def _get_intermediate_close_position(self) -> np.ndarray:
         """Get intermediate position for closing hand.
@@ -254,16 +254,14 @@ class HandF5D6TouchSensor(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.HandTouchSensorState,
         )
 

dexcontrol/core/head.py

@@ -15,17 +15,16 @@ communication. It handles joint position and velocity control, mode setting, and
 state monitoring.
 """
 
-import time
-from typing import Final, Literal
+from typing import Literal
 
 import numpy as np
-import zenoh
+from dexcomm import call_service
 from jaxtyping import Float
-from loguru import logger
 
 from dexcontrol.config.core import HeadConfig
 from dexcontrol.core.component import 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
 
 
@@ -44,19 +43,16 @@ class Head(RobotJointComponent):
     def __init__(
         self,
         configs: HeadConfig,
-        zenoh_session: zenoh.Session,
     ) -> None:
         """Initialize the head controller.
 
         Args:
             configs: Configuration parameters for the head 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.MotorStateWithTorque,
-            zenoh_session=zenoh_session,
             joint_name=configs.joint_name,
             joint_limit=configs.joint_limit
             if hasattr(configs, "joint_limit")
@@ -67,9 +63,8 @@ class Head(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
-        )
+        # Store the query topic for later use with DexComm
+        self._mode_query_topic = resolve_key_name(configs.set_mode_query)
         assert self._joint_vel_limit is not None, "joint_vel_limit is not set"
 
     def set_joint_pos(
@@ -198,13 +193,15 @@ class Head(RobotJointComponent):
 
         query_msg = dexcontrol_query_pb2.SetHeadMode()
         query_msg.mode = mode_map[mode]
-        replies = self.mode_querier.get(payload=query_msg.SerializeToString())
 
-        for reply in replies:
-            if reply.ok is not None and reply.ok.payload is not None:
-                # TODO: handle the reply message of head mode
-                pass
-        time.sleep(0.5)
+        call_service(
+            self._mode_query_topic,
+            request=query_msg,
+            timeout=1.0,
+            config=get_zenoh_config_path(),
+            request_serializer=lambda x: x.SerializeToString(),
+            response_deserializer=None,
+        )
 
     def get_joint_limit(self) -> Float[np.ndarray, "3 2"] | None:
         """Get the joint limits of the head.
@@ -225,16 +222,7 @@ class Head(RobotJointComponent):
         """Clean up Zenoh resources for the head component."""
         self.stop()
         super().shutdown()
-        try:
-            if hasattr(self, "mode_querier") and self.mode_querier:
-                self.mode_querier.undeclare()
-        except Exception as e:
-            # Don't log "Undeclared querier" 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 mode querier for {self.__class__.__name__}: {e}"
-                )
+        # No need to undeclare queriers when using DexComm
 
     def _process_joint_velocities(
         self,