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

dexcontrol/core/robot_query_interface.py

@@ -8,30 +8,32 @@
 # 2. Commercial License
 #    For commercial licensing terms, contact: contact@dexmate.ai
 
-"""Zenoh query utilities for robot communication.
+"""Query utilities for robot communication using DexComm.
 
-This module provides the ZenohQueryable class that encapsulates all zenoh-based
-queries and communication with the robot server. It handles various query types
-including hand type detection, version information, status queries, and control operations.
+This module provides the RobotQueryInterface class that encapsulates all communication
+queries with the robot server using DexComm's service pattern. It handles various query
+types including hand type detection, version information, status queries, and control
+operations.
 """
 
 import json
 import time
 from typing import TYPE_CHECKING, Any, Literal, cast
 
-import zenoh
+# Use DexComm for all communication
+from dexcomm import call_service
 from loguru import logger
 
 from dexcontrol.config.vega import VegaConfig, get_vega_config
 from dexcontrol.core.hand import HandType
 from dexcontrol.proto import 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.pb_utils import (
     ComponentStatus,
     status_to_dict,
 )
 from dexcontrol.utils.viz_utils import show_component_status
-from dexcontrol.utils.zenoh_utils import compute_ntp_stats, create_zenoh_session
 
 if TYPE_CHECKING:
     from dexcontrol.config.vega import VegaConfig
@@ -49,63 +51,47 @@ class RobotQueryInterface:
         ...     version_info = interface.get_version_info()
     """
 
-    def __init__(self, zenoh_session: zenoh.Session, configs: "VegaConfig"):
-        """Initialize the RobotQueryInterface with session and config.
+    def __init__(self, configs: "VegaConfig"):
+        """Initialize the RobotQueryInterface.
 
         Args:
-            zenoh_session: Active zenoh session for communication.
             configs: Robot configuration containing query names.
         """
-        self._zenoh_session: zenoh.Session = zenoh_session
+        # Session parameter kept for compatibility but not used
         self._configs = configs
         self._owns_session = False
 
     @classmethod
-    def create(
-        cls,
-        zenoh_config_file: str | None = None,
-    ) -> "RobotQueryInterface":
-        """Create a standalone RobotQueryInterface with its own zenoh session.
+    def create(cls) -> "RobotQueryInterface":
+        """Create a standalone RobotQueryInterface.
 
         This class method provides a convenient way to create a RobotQueryInterface
-        without requiring the full Robot class. The created interface will manage
-        its own zenoh session and configuration.
-
-        Args:
-            zenoh_config_file: Path to zenoh configuration file. If None,
-                              uses the default configuration path.
-            robot_config_path: Path to robot configuration file. If None,
-                              uses default configuration for detected robot model.
+        without requiring the full Robot class. DexComm handles all session
+        management internally.
 
         Returns:
             RobotQueryInterface instance ready for use.
 
-        Raises:
-            RuntimeError: If zenoh session initialization fails.
-            ValueError: If robot configuration cannot be loaded.
-
         Example:
-            >>> query_interface = RobotQueryInterface.create_standalone()
+            >>> query_interface = RobotQueryInterface.create()
             >>> version_info = query_interface.get_version_info()
             >>> query_interface.close()
         """
-
-        session = create_zenoh_session(zenoh_config_file)
+        # DexComm handles session internally, we just need config
         config: VegaConfig = get_vega_config()
-
-        instance = cls(session, config)
+        instance = cls(config)
         instance._owns_session = True
         return instance
 
     def close(self) -> None:
-        """Close the zenoh session if owned by this instance.
+        """Close the communication session if owned by this instance.
 
         This method should be called when done using a standalone
         RobotQueryInterface to properly clean up resources.
         """
-        if self._owns_session and self._zenoh_session:
-            logger.debug("Closing zenoh session")
-            self._zenoh_session.close()
+        if self._owns_session:
+            # DexComm cleanup is handled automatically
+            logger.debug("DexComm session cleanup handled automatically")
 
     def __enter__(self) -> "RobotQueryInterface":
         """Enter context manager."""
@@ -128,37 +114,42 @@ class RobotQueryInterface:
             RuntimeError: If hand type information cannot be retrieved.
         """
         try:
-            # Sleep more time to avoice zenoh query timeout
-            time.sleep(2)
-            replies = self._zenoh_session.get(
-                resolve_key_name(self._configs.hand_info_query_name), timeout=5.0
+            # Query hand type using DexComm service
+            full_topic = resolve_key_name(self._configs.hand_info_query_name)
+
+            response = call_service(
+                full_topic,
+                timeout=5.0,
+                config=get_zenoh_config_path(),
+                request_serializer=None,
+                response_deserializer=None,
             )
 
-            for reply in replies:
-                if reply.ok and reply.ok.payload:
-                    # Parse JSON response (not protobuf)
-                    payload_bytes = reply.ok.payload.to_bytes()
-                    payload_str = payload_bytes.decode("utf-8")
-                    hand_info = json.loads(payload_str)
-
-                    # Validate the expected format
-                    if isinstance(hand_info, dict):
-                        logger.info(f"End effector hand types: {hand_info}")
-                        return {
-                            "left": HandType(hand_info["left"]),
-                            "right": HandType(hand_info["right"]),
-                        }
-                    else:
-                        logger.warning(
-                            f"Invalid hand type format received: {hand_info}"
-                        )
+            if response:
+                # Parse JSON response directly
+                if isinstance(response, bytes):
+                    payload_str = response.decode("utf-8")
+                else:
+                    payload_str = response
+
+                hand_info = json.loads(payload_str)
+
+                # Validate the expected format
+                if isinstance(hand_info, dict):
+                    logger.info(f"End effector hand types: {hand_info}")
+                    return {
+                        "left": HandType(hand_info["left"]),
+                        "right": HandType(hand_info["right"]),
+                    }
+                else:
+                    logger.warning(f"Invalid hand type format received: {hand_info}")
 
             # If no valid response received, assume v1 for backward compatibility
-            return {"left": HandType.HandF5D6_V1, "right": HandType.HandF5D6_V1}
+            return {"left": HandType.UNKNOWN, "right": HandType.UNKNOWN}
 
         except Exception as e:
-            logger.warning(f"Failed to query hand type: {e}. Assuming v1 hand types.")
-            return {"left": HandType.HandF5D6_V1, "right": HandType.HandF5D6_V1}
+            logger.warning(f"Failed to query hand type: {e}. V1 hand type unknown.")
+            return {"left": HandType.UNKNOWN, "right": HandType.UNKNOWN}
 
     def query_ntp(
         self,
@@ -189,23 +180,29 @@ class RobotQueryInterface:
         time_offset = []
         time_rtt = []
 
-        querier = self._zenoh_session.declare_querier(ntp_key, timeout=timeout)
-        time.sleep(0.1)
-
         reply_count = 0
         for i in range(sample_count):
             request = dexcontrol_query_pb2.NTPRequest()
             request.client_send_time_ns = time.time_ns()
             request.sample_count = sample_count
             request.sample_index = i
-            replies = querier.get(payload=request.SerializeToString())
 
-            for reply in replies:
-                reply_count += 1
-                if reply.ok and reply.ok.payload:
+            # Use call_service for NTP query
+            try:
+                response_data = call_service(
+                    ntp_key,
+                    request=request,
+                    timeout=timeout,
+                    config=get_zenoh_config_path(),
+                    request_serializer=lambda x: x.SerializeToString(),
+                    response_deserializer=None,
+                )
+
+                if response_data:
+                    reply_count += 1
                     client_receive_time_ns = time.time_ns()
                     response = dexcontrol_query_pb2.NTPResponse()
-                    response.ParseFromString(reply.ok.payload.to_bytes())
+                    response.ParseFromString(response_data)
                     t0 = request.client_send_time_ns
                     t1 = response.server_receive_time_ns
                     t2 = response.server_send_time_ns
@@ -214,16 +211,25 @@ class RobotQueryInterface:
                     rtt = (t3 - t0) / 1e9
                     time_offset.append(offset)
                     time_rtt.append(rtt)
+            except Exception as e:
+                logger.debug(f"NTP query {i} failed: {e}")
+
             if i < sample_count - 1:
                 time.sleep(0.01)
-
-        querier.undeclare()
         if reply_count == 0:
-            return {"success": False, "offset": 0, "rtt": 0}
+            return {"success": False, "offset": 0.0, "rtt": 0.0}
+
+        # Compute simple NTP statistics
+        import numpy as np
 
-        stats = compute_ntp_stats(time_offset, time_rtt)
-        offset = stats["offset (mean)"]
-        rtt = stats["round_trip_time (mean)"]
+        stats = {
+            "offset (mean)": float(np.mean(time_offset)) if time_offset else 0.0,
+            "round_trip_time (mean)": float(np.mean(time_rtt)) if time_rtt else 0.0,
+            "offset (std)": float(np.std(time_offset)) if time_offset else 0.0,
+            "round_trip_time (std)": float(np.std(time_rtt)) if time_rtt else 0.0,
+        }
+        offset = float(stats["offset (mean)"])
+        rtt = float(stats["round_trip_time (mean)"])
         if show:
             from dexcontrol.utils.viz_utils import show_ntp_stats
 
@@ -263,34 +269,38 @@ class RobotQueryInterface:
             RuntimeError: If version information cannot be retrieved.
         """
         try:
-            replies = self._zenoh_session.get(
-                resolve_key_name(self._configs.version_info_name), timeout=5.0
+            response = call_service(
+                resolve_key_name(self._configs.version_info_name),
+                timeout=5.0,
+                config=get_zenoh_config_path(),
+                request_serializer=None,
+                response_deserializer=None,
             )
 
-            for reply in replies:
-                if reply.ok and reply.ok.payload:
-                    try:
-                        # Parse JSON response
-                        payload_bytes = reply.ok.payload.to_bytes()
-                        payload_str = payload_bytes.decode("utf-8")
-                        version_info = json.loads(payload_str)
-
-                        # Validate expected structure
-                        if (
-                            isinstance(version_info, dict)
-                            and "server" in version_info
-                            and "client" in version_info
-                        ):
-                            if show:
-                                self._show_version_info(version_info)
-                            return version_info
-                        else:
-                            logger.warning(
-                                f"Invalid version info format received: {version_info}"
-                            )
-                    except (json.JSONDecodeError, UnicodeDecodeError) as e:
-                        logger.warning(f"Failed to parse version info response: {e}")
-                        continue
+            if response:
+                try:
+                    # Parse JSON response directly
+                    if isinstance(response, bytes):
+                        payload_str = response.decode("utf-8")
+                    else:
+                        payload_str = response
+                    version_info = json.loads(payload_str)
+
+                    # Validate expected structure
+                    if (
+                        isinstance(version_info, dict)
+                        and "server" in version_info
+                        and "client" in version_info
+                    ):
+                        if show:
+                            self._show_version_info(version_info)
+                        return version_info
+                    else:
+                        logger.warning(
+                            f"Invalid version info format received: {version_info}"
+                        )
+                except (json.JSONDecodeError, UnicodeDecodeError) as e:
+                    logger.warning(f"Failed to parse version info response: {e}")
 
             raise RuntimeError("No valid version information received from server")
 
@@ -312,19 +322,22 @@ class RobotQueryInterface:
             RuntimeError: If status information cannot be retrieved.
         """
         try:
-            replies = self._zenoh_session.get(
-                resolve_key_name(self._configs.status_info_name)
+            response = call_service(
+                resolve_key_name(self._configs.status_info_name),
+                timeout=2.0,
+                config=get_zenoh_config_path(),
+                request_serializer=None,
+                response_deserializer=None,
             )
+
             status_dict = {}
-            for reply in replies:
-                if reply.ok and reply.ok.payload:
-                    status_bytes = reply.ok.payload.to_bytes()
-                    status_msg = cast(
-                        dexcontrol_query_pb2.ComponentStates,
-                        dexcontrol_query_pb2.ComponentStates.FromString(status_bytes),
-                    )
-                    status_dict = status_to_dict(status_msg)
-                    break
+            if response:
+                # Parse protobuf response directly
+                status_msg = cast(
+                    dexcontrol_query_pb2.ComponentStates,
+                    dexcontrol_query_pb2.ComponentStates.FromString(response),
+                )
+                status_dict = status_to_dict(status_msg)
 
             if show:
                 show_component_status(status_dict)
@@ -358,9 +371,14 @@ class RobotQueryInterface:
             query_msg = dexcontrol_query_pb2.RebootComponent(
                 component=component_map[part]
             )
-            self._zenoh_session.get(
+
+            call_service(
                 resolve_key_name(self._configs.reboot_query_name),
-                payload=query_msg.SerializeToString(),
+                request=query_msg,
+                timeout=2.0,
+                config=get_zenoh_config_path(),
+                request_serializer=lambda x: x.SerializeToString(),
+                response_deserializer=None,
             )
             logger.info(f"Rebooting component: {part}")
         except Exception as e:
@@ -391,11 +409,16 @@ class RobotQueryInterface:
 
         try:
             query_msg = dexcontrol_query_pb2.ClearError(component=component_map[part])
-            self._zenoh_session.get(
+
+            call_service(
                 resolve_key_name(self._configs.clear_error_query_name),
-                handler=lambda reply: logger.info(f"Cleared error of {part}"),
-                payload=query_msg.SerializeToString(),
+                request=query_msg,
+                timeout=2.0,
+                config=get_zenoh_config_path(),
+                request_serializer=lambda x: x.SerializeToString(),
+                response_deserializer=None,
             )
+            logger.info(f"Cleared error of {part}")
         except Exception as e:
             raise RuntimeError(
                 f"Failed to clear error for component {part}: {e}"

dexcontrol/core/torso.py

@@ -15,7 +15,6 @@ communication. It handles joint position and velocity control and state monitori
 """
 
 import numpy as np
-import zenoh
 from jaxtyping import Float
 
 from dexcontrol.config.core import TorsoConfig
@@ -37,20 +36,17 @@ class Torso(RobotJointComponent):
     def __init__(
         self,
         configs: TorsoConfig,
-        zenoh_session: zenoh.Session,
     ) -> None:
         """Initialize the torso controller.
 
         Args:
             configs: Torso configuration parameters containing communication topics
                 and default velocity settings.
-            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")

dexcontrol/robot.py

@@ -25,7 +25,6 @@ from __future__ import annotations
 import os
 import signal
 import sys
-import threading
 import time
 import weakref
 from typing import TYPE_CHECKING, Any, Final, Literal, cast
@@ -33,7 +32,8 @@ from typing import TYPE_CHECKING, Any, Final, Literal, cast
 import hydra.utils
 import numpy as np
 import omegaconf
-import zenoh
+from dexcomm import cleanup_session
+from dexcomm.utils import RateLimiter
 from loguru import logger
 from rich.console import Console
 from rich.table import Table
@@ -47,13 +47,7 @@ from dexcontrol.core.robot_query_interface import RobotQueryInterface
 from dexcontrol.sensors import Sensors
 from dexcontrol.utils.constants import ROBOT_NAME_ENV_VAR
 from dexcontrol.utils.os_utils import check_version_compatibility, get_robot_model
-from dexcontrol.utils.rate_limiter import RateLimiter
 from dexcontrol.utils.trajectory_utils import generate_linear_trajectory
-from dexcontrol.utils.zenoh_utils import (
-    close_zenoh_session_with_timeout,
-    create_zenoh_session,
-    wait_for_zenoh_cleanup,
-)
 
 if TYPE_CHECKING:
     from dexcontrol.core.arm import Arm
@@ -142,7 +136,6 @@ class Robot(RobotQueryInterface):
         self,
         robot_model: str | None = None,
         configs: VegaConfig | None = None,
-        zenoh_config_file: str | None = None,
         auto_shutdown: bool = True,
     ) -> None:
         """Initializes the Robot with the given configuration.
@@ -153,8 +146,6 @@ class Robot(RobotQueryInterface):
                 Ignored if configs is provided.
             configs: Configuration parameters for all robot components.
                 If None, will use the configuration specified by robot_model.
-            zenoh_config_file: Optional path to the zenoh config file.
-                Defaults to None to use system defaults.
             auto_shutdown: Whether to automatically register signal handlers for
                 graceful shutdown on program interruption. Default is True.
 
@@ -168,19 +159,18 @@ class Robot(RobotQueryInterface):
             robot_model = get_robot_model()
         self._robot_model: Final[str] = robot_model
 
-        # Load configuration and initialize zenoh session
+        # Load configuration
         self._configs: Final[VegaConfig] = configs or get_vega_config(robot_model)
-        self._zenoh_session: zenoh.Session = create_zenoh_session(zenoh_config_file)
 
-        # Initialize ZenohQueryable parent class
-        super().__init__(self._zenoh_session, self._configs)
+        super().__init__(self._configs)
 
         self._robot_name: Final[str] = os.getenv(ROBOT_NAME_ENV_VAR, "robot")
         self._pv_components: list[str] = [
             "head",
             "torso",
         ]
-        self._log_subscriber = ServerLogSubscriber(self._zenoh_session)
+        # Note: zenoh_session no longer needed as DexComm handles sessions
+        self._log_subscriber = ServerLogSubscriber()
         self._hand_types: dict[str, HandType] = {}
 
         # Register for automatic shutdown on signals if enabled
@@ -280,7 +270,8 @@ class Robot(RobotQueryInterface):
 
     def _initialize_sensors(self) -> None:
         """Initialize sensors and wait for activation."""
-        self.sensors = Sensors(self._configs.sensors, self._zenoh_session)
+        # Note: zenoh_session no longer needed as DexComm handles sessions
+        self.sensors = Sensors(self._configs.sensors)
         self.sensors.wait_for_all_active()
 
     def _initialize_robot_components(self) -> None:
@@ -290,7 +281,6 @@ class Robot(RobotQueryInterface):
 
         initialized_components = []
         failed_components = []
-
         for component_name, component_config in config_dict.items():
             if component_name == "sensors":
                 continue
@@ -335,9 +325,8 @@ class Robot(RobotQueryInterface):
                     temp_config["hand_type"] = hand_type
 
                 # Instantiate component with error handling
-                component_instance = hydra.utils.instantiate(
-                    temp_config, zenoh_session=self._zenoh_session
-                )
+                # Note: zenoh_session no longer needed as DexComm handles sessions
+                component_instance = hydra.utils.instantiate(temp_config)
 
                 # Store component instance both as attribute and in tracking dictionaries
                 setattr(self, str(component_name), component_instance)
@@ -347,7 +336,6 @@ class Robot(RobotQueryInterface):
                 logger.error(f"Failed to initialize component {component_name}: {e}")
                 failed_components.append(component_name)
                 # Continue with other components rather than failing completely
-
         # Report initialization summary
         if failed_components:
             logger.warning(
@@ -614,23 +602,13 @@ class Robot(RobotQueryInterface):
         except Exception as e:  # pylint: disable=broad-except
             logger.debug(f"Error shutting down log subscriber: {e}")
 
-        # Close Zenoh session
-        close_zenoh_session_with_timeout(self._zenoh_session)
-        lingering_threads = wait_for_zenoh_cleanup()
-        if lingering_threads:
-            self._assist_clean_exit_if_needed()
+        # Cleanup DexComm shared session
+        try:
+            cleanup_session()
+        except Exception as e:
+            logger.debug(f"Session cleanup note: {e}")
         logger.info("Robot shutdown complete")
 
-    def _assist_clean_exit_if_needed(self) -> None:
-        """Assist with clean exit if this is the last robot and we're exiting."""
-        if (
-            not _active_robots  # No more active robots
-            and threading.current_thread() is threading.main_thread()  # In main thread
-            and not hasattr(sys, "ps1")  # Not in interactive mode
-        ):
-            logger.debug("Assisting clean exit due to lingering Zenoh threads")
-            sys.exit(0)
-
     def is_shutdown(self) -> bool:
         """Check if the robot has been shutdown.
 

dexcontrol/sensors/__init__.py

@@ -15,7 +15,7 @@ using Zenoh subscribers for data communication.
 """
 
 # Import camera sensors
-from .camera import LuxonisCameraSensor, RGBCameraSensor, ZedCameraSensor
+from .camera import RGBCameraSensor, ZedCameraSensor
 
 # Import IMU sensors
 from .imu import ChassisIMUSensor, ZedIMUSensor
@@ -31,7 +31,6 @@ __all__ = [
     # Camera sensors
     "RGBCameraSensor",
     "ZedCameraSensor",
-    "LuxonisCameraSensor",
 
     # IMU sensors
     "ChassisIMUSensor",

dexcontrol/sensors/camera/__init__.py

@@ -14,12 +14,10 @@ This module provides camera sensor classes that use the specialized camera
 subscribers for RGB and RGBD camera data, matching the dexsensor structure.
 """
 
-from .luxonis_camera import LuxonisCameraSensor
 from .rgb_camera import RGBCameraSensor
 from .zed_camera import ZedCameraSensor
 
 __all__ = [
     "RGBCameraSensor",
     "ZedCameraSensor",
-    "LuxonisCameraSensor",
 ]