hex-zmq-servers 0.3.9__py3-none-any.whl

hex_zmq_servers/zmq_base.py

@@ -0,0 +1,547 @@
+#!/usr/bin/env python3
+# -*- coding:utf-8 -*-
+################################################################
+# Copyright 2025 Dong Zhaorui. All rights reserved.
+# Author: Dong Zhaorui 847235539@qq.com
+# Date  : 2025-09-12
+################################################################
+
+import os, signal, json
+import time, ctypes, ctypes.util
+import threading
+import zmq
+import numpy as np
+from abc import ABC, abstractmethod
+
+MAX_SEQ_NUM = int(1e12)
+MAX_DEQUE_LEN = 10
+
+################################################################
+# Time Related
+################################################################
+
+
+class SingletonMeta(type):
+    _instances = {}
+
+    def __call__(cls, *args, **kwargs):
+        if cls not in cls._instances:
+            cls._instances[cls] = super().__call__(*args, **kwargs)
+        return cls._instances[cls]
+
+
+class HexTimeManager(metaclass=SingletonMeta):
+
+    class timespec(ctypes.Structure):
+        _fields_ = [("tv_sec", ctypes.c_long), ("tv_nsec", ctypes.c_long)]
+
+    def __init__(self):
+        self.__use_ptp = False
+        ptp_path = os.getenv("HEX_PTP_CLOCK", None)
+        if ptp_path is not None:
+            self.__fd = os.open(ptp_path, os.O_RDONLY | os.O_CLOEXEC)
+            self.__clock_id = ((~self.__fd) << 3) | 3
+            self.__libc = ctypes.CDLL(
+                ctypes.util.find_library("c"),
+                use_errno=True,
+            )
+            self.__use_ptp = True
+            print(f"Using PTP clock from {ptp_path}")
+        else:
+            print("Using system clock")
+
+    def __del__(self):
+        if self.__use_ptp:
+            os.close(self.__fd)
+
+    def get_now_ns(self) -> int:
+        if self.__use_ptp:
+            ts = self.timespec()
+            if self.__libc.clock_gettime(self.__clock_id,
+                                         ctypes.byref(ts)) != 0:
+                err = ctypes.get_errno()
+                raise OSError(err, os.strerror(err))
+            return ts.tv_sec * 1_000_000_000 + ts.tv_nsec
+        else:
+            return time.perf_counter_ns()
+
+
+_HEX_TIME_MANAGER = HexTimeManager()
+
+
+def hex_zmq_ts_to_ns(ts: dict) -> int:
+    return ts['s'] * 1_000_000_000 + ts['ns']
+
+
+def ns_to_hex_zmq_ts(ns: int) -> dict:
+    return {
+        "s": ns // 1_000_000_000,
+        "ns": ns % 1_000_000_000,
+    }
+
+
+def hex_ns_now() -> int:
+    return _HEX_TIME_MANAGER.get_now_ns()
+
+
+def hex_zmq_ts_now() -> dict:
+    return ns_to_hex_zmq_ts(hex_ns_now())
+
+
+def hex_zmq_ts_delta_ms(curr_ts, hdr_ts) -> float:
+    try:
+        return (curr_ts['s'] - hdr_ts['s']) * 1_000 + (
+            curr_ts['ns'] - hdr_ts['ns']) / 1_000_000
+
+    except Exception as e:
+        print(f"hex_zmq_ts_delta_ms failed: {e}")
+        return np.inf
+
+
+class HexRate:
+
+    def __init__(self, hz: float, spin_threshold_ns: int = 10_000):
+        if hz <= 0:
+            raise ValueError("hz must be greater than 0")
+        if spin_threshold_ns < 0:
+            raise ValueError("spin_threshold_ns must be non-negative")
+        self.__period_ns = int(1_000_000_000 / hz)
+        self.__next_ns = self.__now_ns() + self.__period_ns
+        self.__spin_threshold_ns = spin_threshold_ns
+
+    @staticmethod
+    def __now_ns() -> int:
+        return hex_ns_now()
+
+    def reset(self):
+        self.__next_ns = self.__now_ns() + self.__period_ns
+
+    def sleep(self):
+        target_ns = self.__next_ns
+        now_ns = self.__now_ns()
+        remain_ns = target_ns - now_ns
+        if remain_ns <= 0:
+            needed_period = (now_ns - target_ns) // self.__period_ns + 1
+            self.__next_ns += needed_period * self.__period_ns
+            return
+
+        spin_threshold = min(self.__spin_threshold_ns, self.__period_ns)
+        coarse_sleep_ns = remain_ns - spin_threshold
+        if coarse_sleep_ns > 0:
+            time.sleep(coarse_sleep_ns / 1_000_000_000.0)
+
+        while True:
+            now_ns = self.__now_ns()
+            if now_ns >= target_ns:
+                break
+            if target_ns - now_ns > 50_000:
+                time.sleep(0)
+
+        self.__next_ns += self.__period_ns
+
+
+################################################################
+# ZMQ Related
+################################################################
+
+NET_CONFIG = {
+    "ip": "127.0.0.1",
+    "port": 12345,
+    "realtime_mode": False,
+    "deque_maxlen": 10,
+    "client_timeout_ms": 200,
+    "server_timeout_ms": 1_000,
+    "server_num_workers": 4,
+}
+
+
+class HexZMQClientBase(ABC):
+
+    def __init__(self, net_config: dict = NET_CONFIG):
+        self._max_seq_num = MAX_SEQ_NUM
+        self._realtime_mode = net_config.get("realtime_mode", False)
+        self._deque_maxlen = max(
+            1,
+            net_config.get("deque_maxlen", MAX_DEQUE_LEN),
+        )
+        try:
+            port = net_config["port"]
+            ip = net_config["ip"]
+            client_timeout_ms = net_config["client_timeout_ms"]
+        except KeyError as ke:
+            missing_key = ke.args[0]
+            raise ValueError(
+                f"net_config is not valid, missing key: {missing_key}")
+
+        self._context = zmq.Context().instance()
+        self._ip = ip
+        self._port = port
+        self._timeout_ms = client_timeout_ms
+        self._socket = None
+        self._lock = threading.Lock()
+        self.__make_socket()
+
+        # receive thread
+        self._recv_thread = threading.Thread(
+            target=self._recv_loop,
+            daemon=True,
+        )
+        self._recv_flag = False
+
+    def __del__(self):
+        self.close()
+
+    def __make_socket(self):
+        if self._socket is not None:
+            try:
+                self._socket.close(0)
+            except Exception:
+                pass
+
+        new_socket = self._context.socket(zmq.REQ)
+        new_socket.setsockopt(zmq.LINGER, 0)
+        new_socket.setsockopt(zmq.RCVTIMEO, self._timeout_ms)
+        new_socket.setsockopt(zmq.SNDTIMEO, self._timeout_ms)
+        new_socket.setsockopt(zmq.IMMEDIATE, 1)
+        new_socket.setsockopt(zmq.TCP_KEEPALIVE, 1)
+        new_socket.connect(f"tcp://{self._ip}:{self._port}")
+        self._socket = new_socket
+
+    def request(self, req_dict: dict, req_buf: np.ndarray | None = None):
+        with self._lock:
+            try:
+                self.__send_req(req_dict, req_buf)
+            except zmq.Again:
+                print("client send failed; recreate socket")
+                self.__make_socket()
+                return None, None
+
+            resp_hdr, resp_buf = self.__recv_resp()
+            if resp_hdr is None:
+                print("client recv failed; recreate socket")
+                self.__make_socket()
+            return resp_hdr, resp_buf
+
+    def is_working(self) -> bool:
+        working_hdr, _ = self.request({"cmd": "is_working"})
+        if working_hdr is None:
+            return False
+        else:
+            return working_hdr["cmd"] == "is_working_ok"
+
+    def __send_req(self, req_dict: dict, req_buf: np.ndarray | None = None):
+        # construct send header
+        if not "cmd" in req_dict:
+            raise ValueError("`cmd` is required")
+        if req_buf is None:
+            req_buf = np.zeros(0, dtype=np.uint8)
+        if not req_buf.flags.c_contiguous:
+            req_buf = np.ascontiguousarray(req_buf)
+        send_hdr = {
+            "cmd": req_dict["cmd"],
+            "ts": req_dict.get("ts", hex_zmq_ts_now()),
+            "args": req_dict.get("args", None),
+            "dtype": str(req_buf.dtype),
+            "shape": tuple(req_buf.shape),
+        }
+
+        try:
+            self._socket.send_multipart(
+                [json.dumps(send_hdr).encode("utf-8"),
+                 memoryview(req_buf)],
+                copy=(req_buf.nbytes < 65536),
+            )
+        except zmq.Again:
+            print("client send failed")
+            raise
+
+    def __recv_resp(self):
+        try:
+            frames = self._socket.recv_multipart()
+            if len(frames) != 2:
+                raise ValueError("invalid response")
+            send_hdr_bytes, raw_buf = frames
+            resp_hdr = json.loads(send_hdr_bytes)
+            resp_buf = np.frombuffer(
+                raw_buf,
+                dtype=np.dtype(resp_hdr["dtype"]),
+            ).reshape(
+                tuple(resp_hdr["shape"]),
+                order="C",
+            )
+            return resp_hdr, resp_buf
+        except zmq.Again:
+            return None, None
+
+    def close(self):
+        self._recv_flag = False
+        self._recv_thread.join()
+        if self._socket is not None:
+            try:
+                self._socket.close(0)
+            except Exception:
+                pass
+
+    def _wait_for_working(self, timeout: float = 5.0):
+        for _ in range(int(timeout * 10)):
+            if self.is_working():
+                if hasattr(self, "seq_clear"):
+                    self.seq_clear()
+                break
+            else:
+                time.sleep(0.1)
+        self._recv_flag = True
+        self._recv_thread.start()
+
+    @abstractmethod
+    def _recv_loop(self):
+        raise NotImplementedError(
+            "`_receive_thread` should be implemented by the child class")
+
+
+class HexZMQServerBase(ABC):
+
+    def __init__(
+        self,
+        net_config: dict = NET_CONFIG,
+    ):
+        self._max_seq_num = MAX_SEQ_NUM
+        self._realtime_mode = net_config.get("realtime_mode", False)
+        self._deque_maxlen = max(
+            1,
+            net_config.get("deque_maxlen", MAX_DEQUE_LEN),
+        )
+        try:
+            port = net_config["port"]
+            ip = net_config["ip"]
+            num_workers = net_config["server_num_workers"]
+            timeout_ms = net_config["server_timeout_ms"]
+        except KeyError as ke:
+            missing_key = ke.args[0]
+            raise ValueError(
+                f"net_config is not valid, missing key: {missing_key}")
+
+        self._stop_event = threading.Event()
+        self._num_workers = max(1, min(num_workers, os.cpu_count()))
+        self._timeout_ms = timeout_ms
+
+        self._context = zmq.Context().instance()
+        self._frontend = self._context.socket(zmq.ROUTER)
+        self._frontend.setsockopt(zmq.LINGER, 0)
+        self._frontend.setsockopt(zmq.TCP_KEEPALIVE, 1)
+        self._frontend.bind(f"tcp://{ip}:{port}")
+
+        self._backend = self._context.socket(zmq.DEALER)
+        self._backend.setsockopt(zmq.LINGER, 0)
+        self._backend.bind(f"inproc://hex_workers")
+
+        self._workers: list[threading.Thread] = []
+        self._proxy_thread: threading.Thread | None = None
+
+    def __del__(self):
+        self.close()
+
+    def _single_thread(self, worker_id: int):
+        socket = self._context.socket(zmq.REP)
+        socket.setsockopt(zmq.LINGER, 0)
+        socket.setsockopt(zmq.RCVTIMEO, self._timeout_ms)
+        socket.connect(f"inproc://hex_workers")
+
+        while not self._stop_event.is_set():
+            try:
+                frames = socket.recv_multipart()
+            except zmq.Again:
+                continue
+
+            try:
+                if len(frames) != 2:
+                    raise ValueError("invalid request")
+                send_hdr_bytes, raw_buf = frames
+                req_hdr = json.loads(send_hdr_bytes)
+                req_buf = np.frombuffer(
+                    raw_buf,
+                    dtype=np.dtype(req_hdr["dtype"])).reshape(req_hdr["shape"],
+                                                              order="C")
+
+                resp_hdr, resp_buf = self._process_request(req_hdr, req_buf)
+
+                if resp_buf is None:
+                    resp_buf = np.zeros(0, dtype=np.uint8)
+                if not resp_buf.flags.c_contiguous:
+                    resp_buf = np.ascontiguousarray(resp_buf)
+                send_hdr = {
+                    "cmd": resp_hdr["cmd"],
+                    "ts": resp_hdr.get("ts", hex_zmq_ts_now()),
+                    "args": resp_hdr.get("args", None),
+                    "dtype": str(resp_buf.dtype),
+                    "shape": tuple(resp_buf.shape),
+                }
+
+                socket.send_multipart([
+                    json.dumps(send_hdr).encode("utf-8"),
+                    memoryview(resp_buf)
+                ],
+                                      copy=(resp_buf.nbytes < 65536))
+
+            except Exception as e:
+                err_hdr = {
+                    "cmd": (f"{req_hdr.get('cmd')}_error"
+                            if isinstance(req_hdr, dict) and "cmd" in req_hdr
+                            else "error"),
+                    "args": {
+                        "err": str(e)
+                    },
+                    "ts":
+                    hex_zmq_ts_now(),
+                    "dtype":
+                    "uint8",
+                    "shape": (0, ),
+                }
+                socket.send_multipart(
+                    [json.dumps(err_hdr).encode("utf-8"),
+                     memoryview(b"")],
+                    copy=True)
+
+        socket.close(0)
+
+    def start(self):
+        for i in range(self._num_workers):
+            th = threading.Thread(
+                target=self._single_thread,
+                args=(i, ),
+                daemon=True,
+            )
+            th.start()
+            self._workers.append(th)
+
+        def _proxy():
+            try:
+                zmq.proxy(self._frontend, self._backend)
+            except Exception:
+                pass
+
+        self._proxy_thread = threading.Thread(target=_proxy, daemon=True)
+        self._proxy_thread.start()
+
+    def close(self):
+        self._stop_event.set()
+        try:
+            if self._frontend:
+                self._frontend.close(0)
+        except Exception:
+            pass
+        try:
+            if self._backend:
+                self._backend.close(0)
+        except Exception:
+            pass
+
+    def no_ts_hdr(self, hdr: dict, ok_flag: bool) -> dict:
+        return {
+            "cmd": f"{hdr['cmd']}_ok"
+        } if ok_flag else {
+            "cmd": f"{hdr['cmd']}_failed"
+        }
+
+    @abstractmethod
+    def work_loop(self):
+        raise NotImplementedError(
+            "`work_loop` should be implemented by the child class")
+
+    @abstractmethod
+    def _process_request(self, recv_hdr: dict, recv_buf: np.ndarray):
+        raise NotImplementedError(
+            "`_process_request` should be implemented by the child class")
+
+
+################################################################
+# Server Helper
+################################################################
+def hex_server_helper(cfg: dict, server_cls: type):
+    try:
+        net = cfg["net"]
+        params = cfg["params"]
+    except KeyError as ke:
+        missing_key = ke.args[0]
+        raise ValueError(f"cfg is not valid, missing key: {missing_key}")
+
+    server = server_cls(net, params)
+
+    shutdown_flag = False
+
+    def signal_handler(signum, frame):
+        nonlocal shutdown_flag
+        if not shutdown_flag:
+            shutdown_flag = True
+            print(
+                f"[server] Received signal {signal.Signals(signum).name}, shutting down..."
+            )
+            server._stop_event.set()
+
+    signal.signal(signal.SIGTERM, signal_handler)
+    signal.signal(signal.SIGINT, signal_handler)
+
+    try:
+        server.start()
+        server.work_loop()
+    finally:
+        server.close()
+
+
+################################################################
+# Dummy Sample
+################################################################
+
+
+class HexZMQDummyClient(HexZMQClientBase):
+
+    def __init__(
+        self,
+        net_config: dict = NET_CONFIG,
+    ):
+        HexZMQClientBase.__init__(self, net_config)
+
+    def single_test(self):
+        resp_hdr, resp_buf = self.request({"cmd": "test"})
+        print(f"resp_hdr: {resp_hdr}")
+        print(f"resp_buf: {resp_buf}")
+
+
+class HexZMQDummyServer(HexZMQServerBase):
+
+    def __init__(
+        self,
+        net_config: dict = NET_CONFIG,
+        params_config: dict = {},
+    ):
+        HexZMQServerBase.__init__(self, net_config)
+
+    def work_loop(self):
+        try:
+            while not self._stop_event.is_set():
+                time.sleep(1)
+        finally:
+            self.close()
+
+    def _process_request(self, recv_hdr: dict, recv_buf: np.ndarray):
+        if recv_hdr["cmd"] == "test":
+            print("test received")
+            print(f"recv_hdr: {recv_hdr}")
+            print(f"recv_buf: {recv_buf}")
+            resp_hdr = {
+                "cmd": "test_ok",
+            }
+            return resp_hdr, None
+        else:
+            raise ValueError(f"unknown command: {recv_hdr['cmd']}")
+
+
+if __name__ == "__main__":
+    import argparse, json
+
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--cfg", type=str, required=True)
+    args = parser.parse_args()
+    cfg = json.loads(args.cfg)
+
+    hex_server_helper(cfg, HexZMQDummyServer)

hex_zmq_servers-0.3.9.dist-info/METADATA

@@ -0,0 +1,147 @@
+Metadata-Version: 2.4
+Name: hex_zmq_servers
+Version: 0.3.9
+Summary: HEXFELLOW ZMQ Servers
+Author-email: Dong Zhaorui <joray.dong@hexfellow.com>
+Maintainer-email: jecjune <zejun.chen@hexfellow.com>, Dong Zhaorui <joray.dong@hexfellow.com>
+License-Expression: Apache-2.0
+Project-URL: Homepage, https://github.com/hexfellow/hex_zmq_servers
+Project-URL: Repository, https://github.com/hexfellow/hex_zmq_servers.git
+Project-URL: Bug Tracker, https://github.com/hexfellow/hex_zmq_servers/issues
+Project-URL: Documentation, https://github.com/hexfellow/hex_zmq_servers/wiki
+Keywords: hex_zmq_servers
+Classifier: Development Status :: 4 - Beta
+Classifier: Intended Audience :: Developers
+Classifier: Intended Audience :: Science/Research
+Classifier: Topic :: Software Development :: Libraries :: Python Modules
+Classifier: Programming Language :: Python :: 3
+Classifier: Programming Language :: Python :: 3.10
+Classifier: Programming Language :: Python :: 3.11
+Classifier: Programming Language :: Python :: 3.12
+Classifier: Operating System :: POSIX :: Linux
+Requires-Python: >=3.10
+Description-Content-Type: text/markdown
+License-File: LICENSE
+Requires-Dist: pyzmq>=27.0.1
+Requires-Dist: hex_device<1.4.0,>=1.3.1
+Requires-Dist: hex_robo_utils<0.3.0,>=0.2.0
+Requires-Dist: dynamixel-sdk==3.8.4
+Requires-Dist: opencv-python>=4.2
+Provides-Extra: berxel
+Requires-Dist: berxel_py_wrapper>=2.0.182; extra == "berxel"
+Provides-Extra: realsense
+Requires-Dist: pyrealsense2>=2.56.5.9235; extra == "realsense"
+Provides-Extra: mujoco
+Requires-Dist: mujoco>=3.3.3; extra == "mujoco"
+Provides-Extra: all
+Requires-Dist: berxel_py_wrapper>=2.0.182; extra == "all"
+Requires-Dist: mujoco>=3.3.3; extra == "all"
+Requires-Dist: pyrealsense2>=2.56.5.9235; extra == "all"
+Dynamic: license-file
+
+# hex_zmq_servers
+
+## Introduction
+
+**`hex_zmq_servers`** is a comprehensive distributed device control framework based on ZeroMQ, providing efficient client-server communication for HEXFELLOW devices.
+
+## Project Structure
+
+```bash
+hex_zmq_servers/
+├── hex_zmq_servers/         # Core library
+│   ├── robot/               # Robot devices
+│   ├── cam/                 # Camera devices
+│   ├── mujoco/              # Mujoco simulation devices
+│   └── config/              # Default configuration files
+├── examples/                # Example code
+│   ├── basic/               # Basic examples (single device)
+│   └── adv/                 # Advanced examples (multi-device coordination)
+└── venv.sh                  # Virtual environment script
+```
+
+## Devices
+
+### Robot
+
+- **dummy**: Dummy robot, for testing and development
+- **gello**: GELLO robot, based on Dynamixel servo
+- **hexarm**: HexArm robot of HEXFELLOW
+
+### Camera
+
+- **dummy**: Dummy camera, for testing and development
+- **berxel**: Berxel depth camera, providing RGB and depth images
+
+### Mujoco
+
+- **archer_y6**: Physical simulation of Archer Y6 robot
+- **e3_desktop**: Physical simulation of E3 Desktop robot
+
+## Installation
+
+### Install from PyPI
+
+1. For those who only want to use the library in their projects, it is recommended to install it from PyPI.
+
+    ```bash
+    pip install hex_zmq_servers[all]
+    ```
+
+2. If you don't want to install the extra dependencies for extra devices, you can run:
+
+    ```bash
+    pip install hex_zmq_servers
+    ```
+
+### Install from Source Code
+
+1. For those who want to test the examples or contribute to the project, you can install it from source code.
+
+    ```bash
+    git clone https://github.com/hexfellow/hex_zmq_servers.git
+    cd hex_zmq_servers
+    ./venv.sh
+    ```
+
+2. If you don't want to install the extra dependencies for extra devices, you can run:
+
+    ```bash
+    git clone https://github.com/hexfellow/hex_zmq_servers.git
+    cd hex_zmq_servers
+    ./venv.sh --min
+    ```
+
+    (**Important**) Some examples would not work without the extra dependencies.
+
+3. If you don't want to install the examples, you can run:
+
+    ```bash
+    git clone https://github.com/hexfellow/hex_zmq_servers.git
+    cd hex_zmq_servers
+    ./venv.sh --pkg-only
+    ```
+
+## Examples
+
+There are two types of examples in the project:
+
+- **basic/**: Basic examples, showing the usage of a single device
+- **adv/**: Advanced examples, showing multi-device coordination
+
+More details please refer to [examples/README.md](examples/README.md)
+
+## Contributions
+
+Welcome to submit issues and pull requests!
+
+## License
+
+Apache License 2.0
+
+## Contact
+
+- Author: [Dong Zhaorui](https://github.com/IBNBlank)
+- Maintainer: [jecjune](https://github.com/Jecjune)
+- GitHub: [hex_zmq_servers](https://github.com/hexfellow/hex_zmq_servers)
+- Issue Tracker: [hex_zmq_servers](https://github.com/hexfellow/hex_zmq_servers/issues)