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

hex_zmq_servers/__init__.py

@@ -0,0 +1,173 @@
+#!/usr/bin/env python3
+# -*- coding:utf-8 -*-
+################################################################
+# Copyright 2025 Dong Zhaorui. All rights reserved.
+# Author: Dong Zhaorui 847235539@qq.com
+# Date  : 2025-09-15
+################################################################
+
+from .hex_launch import HexLaunch, HexNodeConfig, HEX_LOG_LEVEL, hex_dict_str, hex_log, hex_err
+
+from .device_base import HexDeviceBase
+from .zmq_base import hex_zmq_ts_to_ns, ns_to_hex_zmq_ts, hex_ns_now, hex_zmq_ts_now, hex_zmq_ts_delta_ms
+from .zmq_base import HexRate, HexZMQClientBase, HexZMQServerBase, hex_server_helper
+from .zmq_base import HexZMQDummyClient, HexZMQDummyServer
+
+from .robot import HexRobotBase, HexRobotClientBase, HexRobotServerBase
+from .robot import HexRobotDummy, HexRobotDummyClient, HexRobotDummyServer
+from .robot import HexRobotGello, HexRobotGelloClient, HexRobotGelloServer
+from .robot import HexRobotHexarm, HexRobotHexarmClient, HexRobotHexarmServer, HEXARM_URDF_PATH_DICT
+
+from .cam import HexCamBase, HexCamClientBase, HexCamServerBase
+from .cam import HexCamDummy, HexCamDummyClient, HexCamDummyServer
+from .cam import HexCamRGB, HexCamRGBClient, HexCamRGBServer
+
+import os
+
+file_dir = os.path.dirname(os.path.abspath(__file__))
+HEX_ZMQ_SERVERS_PATH_DICT = {
+    "zmq_dummy": f"{file_dir}/zmq_base.py",
+    "robot_dummy": f"{file_dir}/robot/dummy/robot_dummy_srv.py",
+    "robot_gello": f"{file_dir}/robot/gello/robot_gello_srv.py",
+    "robot_hexarm": f"{file_dir}/robot/hexarm/robot_hexarm_srv.py",
+    "cam_dummy": f"{file_dir}/cam/dummy/cam_dummy_srv.py",
+    "cam_rgb": f"{file_dir}/cam/rgb/cam_rgb_srv.py",
+}
+HEX_ZMQ_CONFIGS_PATH_DICT = {
+    "zmq_dummy": f"{file_dir}/config/zmq_dummy.json",
+    "robot_dummy": f"{file_dir}/config/robot_dummy.json",
+    "robot_gello": f"{file_dir}/config/robot_gello.json",
+    "robot_hexarm": f"{file_dir}/config/robot_hexarm.json",
+    "cam_dummy": f"{file_dir}/config/cam_dummy.json",
+    "cam_rgb": f"{file_dir}/config/cam_rgb.json",
+}
+
+__all__ = [
+    # version
+    "__version__",
+
+    # path
+    "HEX_ZMQ_SERVERS_PATH_DICT",
+    "HEX_ZMQ_CONFIGS_PATH_DICT",
+    "HEXARM_URDF_PATH_DICT",
+
+    # launch
+    "HexLaunch",
+    "HexNodeConfig",
+    "HEX_LOG_LEVEL",
+    "hex_dict_str",
+    "hex_log",
+    "hex_err",
+
+    # base
+    "HexDeviceBase",
+    "HexRate",
+    "hex_zmq_ts_to_ns",
+    "ns_to_hex_zmq_ts",
+    "hex_ns_now",
+    "hex_zmq_ts_now",
+    "hex_zmq_ts_delta_ms",
+    "HexZMQClientBase",
+    "HexZMQServerBase",
+    "hex_server_helper",
+    "HexZMQDummyClient",
+    "HexZMQDummyServer",
+
+    # robot
+    "HexRobotBase",
+    "HexRobotClientBase",
+    "HexRobotServerBase",
+    "HexRobotDummy",
+    "HexRobotDummyClient",
+    "HexRobotDummyServer",
+    "HexRobotGello",
+    "HexRobotGelloClient",
+    "HexRobotGelloServer",
+    "HexRobotHexarm",
+    "HexRobotHexarmClient",
+    "HexRobotHexarmServer",
+
+    # camera
+    "HexCamBase",
+    "HexCamClientBase",
+    "HexCamServerBase",
+    "HexCamDummy",
+    "HexCamDummyClient",
+    "HexCamDummyServer",
+    "HexCamRGB",
+    "HexCamRGBClient",
+    "HexCamRGBServer",
+]
+
+# Check optional dependencies availability
+from importlib.util import find_spec
+
+_HAS_BERXEL = find_spec("berxel_py_wrapper") is not None
+_HAS_REALSENSE = find_spec("pyrealsense2") is not None
+_HAS_MUJOCO = find_spec("mujoco") is not None
+
+# Optional: berxel
+if _HAS_BERXEL:
+    from .cam import HexCamBerxel, HexCamBerxelClient, HexCamBerxelServer
+    HEX_ZMQ_SERVERS_PATH_DICT.update({
+        "cam_berxel":
+        f"{file_dir}/cam/berxel/cam_berxel_srv.py",
+    })
+    HEX_ZMQ_CONFIGS_PATH_DICT.update({
+        "cam_berxel":
+        f"{file_dir}/config/cam_berxel.json",
+    })
+    __all__.extend([
+        "HexCamBerxel",
+        "HexCamBerxelClient",
+        "HexCamBerxelServer",
+    ])
+
+# Optional: realsense
+if _HAS_REALSENSE:
+    from .cam import HexCamRealsense, HexCamRealsenseClient, HexCamRealsenseServer
+    HEX_ZMQ_SERVERS_PATH_DICT.update({
+        "cam_realsense":
+        f"{file_dir}/cam/realsense/cam_realsense_srv.py",
+    })
+    HEX_ZMQ_CONFIGS_PATH_DICT.update({
+        "cam_realsense":
+        f"{file_dir}/config/cam_realsense.json",
+    })
+    __all__.extend([
+        "HexCamRealsense",
+        "HexCamRealsenseClient",
+        "HexCamRealsenseServer",
+    ])
+
+# Optional: mujoco
+if _HAS_MUJOCO:
+    from .mujoco import HexMujocoBase, HexMujocoClientBase, HexMujocoServerBase
+    from .mujoco import HexMujocoArcherY6, HexMujocoArcherY6Client, HexMujocoArcherY6Server
+    from .mujoco import HexMujocoE3Desktop, HexMujocoE3DesktopClient, HexMujocoE3DesktopServer
+    HEX_ZMQ_SERVERS_PATH_DICT.update({
+        "mujoco_archer_y6":
+        f"{file_dir}/mujoco/archer_y6/mujoco_archer_y6_srv.py",
+        "mujoco_e3_desktop":
+        f"{file_dir}/mujoco/e3_desktop/mujoco_e3_desktop_srv.py",
+    })
+    HEX_ZMQ_CONFIGS_PATH_DICT.update({
+        "mujoco_archer_y6":
+        f"{file_dir}/config/mujoco_archer_y6.json",
+        "mujoco_e3_desktop":
+        f"{file_dir}/config/mujoco_e3_desktop.json",
+    })
+    __all__.extend([
+        # mujoco
+        "HexMujocoBase",
+        "HexMujocoClientBase",
+        "HexMujocoServerBase",
+        "HexMujocoArcherY6",
+        "HexMujocoArcherY6Client",
+        "HexMujocoArcherY6Server",
+        "HexMujocoE3Desktop",
+        "HexMujocoE3DesktopClient",
+        "HexMujocoE3DesktopServer",
+    ])
+
+# print("#### Thanks for using hex_zmq_servers :D ####")

hex_zmq_servers/cam/__init__.py

@@ -0,0 +1,52 @@
+#!/usr/bin/env python3
+# -*- coding:utf-8 -*-
+################################################################
+# Copyright 2025 Dong Zhaorui. All rights reserved.
+# Author: Dong Zhaorui 847235539@qq.com
+# Date  : 2025-09-12
+################################################################
+
+from .cam_base import HexCamBase, HexCamClientBase, HexCamServerBase
+from .dummy import HexCamDummy, HexCamDummyClient, HexCamDummyServer
+from .rgb import HexCamRGB, HexCamRGBClient, HexCamRGBServer
+
+__all__ = [
+    # base
+    "HexCamBase",
+    "HexCamClientBase",
+    "HexCamServerBase",
+
+    # dummy
+    "HexCamDummy",
+    "HexCamDummyClient",
+    "HexCamDummyServer",
+
+    # rgb
+    "HexCamRGB",
+    "HexCamRGBClient",
+    "HexCamRGBServer",
+]
+
+# Check optional dependencies availability
+from importlib.util import find_spec
+
+_HAS_BERXEL = find_spec("berxel_py_wrapper") is not None
+_HAS_REALSENSE = find_spec("pyrealsense2") is not None
+
+# Optional: berxel
+if _HAS_BERXEL:
+    from .berxel import HexCamBerxel, HexCamBerxelClient, HexCamBerxelServer
+    __all__.extend([
+        "HexCamBerxel",
+        "HexCamBerxelClient",
+        "HexCamBerxelServer",
+    ])
+
+# Optional: realsense
+if _HAS_REALSENSE:
+    from .realsense import HexCamRealsense, HexCamRealsenseClient, HexCamRealsenseServer
+    __all__.extend([
+        "HexCamRealsense",
+        "HexCamRealsenseClient",
+        "HexCamRealsenseServer",
+    ])

hex_zmq_servers/cam/berxel/__init__.py

@@ -0,0 +1,17 @@
+#!/usr/bin/env python3
+# -*- coding:utf-8 -*-
+################################################################
+# Copyright 2025 Dong Zhaorui. All rights reserved.
+# Author: Dong Zhaorui 847235539@qq.com
+# Date  : 2025-09-12
+################################################################
+
+from .cam_berxel import HexCamBerxel
+from .cam_berxel_cli import HexCamBerxelClient
+from .cam_berxel_srv import HexCamBerxelServer
+
+__all__ = [
+    "HexCamBerxel",
+    "HexCamBerxelClient",
+    "HexCamBerxelServer",
+]

hex_zmq_servers/cam/berxel/cam_berxel.py

@@ -0,0 +1,282 @@
+#!/usr/bin/env python3
+# -*- coding:utf-8 -*-
+################################################################
+# Copyright 2025 Dong Zhaorui. All rights reserved.
+# Author: Dong Zhaorui 847235539@qq.com
+# Date  : 2025-09-14
+################################################################
+
+import cv2
+import threading
+import numpy as np
+from collections import deque
+
+from ..cam_base import HexCamBase
+from ...zmq_base import hex_ns_now, hex_zmq_ts_now
+from ...hex_launch import hex_log, HEX_LOG_LEVEL
+from berxel_py_wrapper import *
+
+CAMERA_CONFIG = {
+    "serial_number": 'P100RYB4C03M2B322',
+    "exposure": 10000,
+    "gain": 100,
+    "frame_rate": 30,
+    "sens_ts": True,
+}
+
+
+class HexCamBerxel(HexCamBase):
+
+    def __init__(
+        self,
+        camera_config: dict = CAMERA_CONFIG,
+        realtime_mode: bool = False,
+    ):
+        HexCamBase.__init__(self, realtime_mode)
+
+        try:
+            self.__serial_number = camera_config["serial_number"]
+            self.__exposure = camera_config["exposure"]
+            self.__gain = camera_config["gain"]
+            self.__frame_rate = camera_config["frame_rate"]
+            self.__sens_ts = camera_config["sens_ts"]
+        except KeyError as ke:
+            missing_key = ke.args[0]
+            raise ValueError(
+                f"camera_config is not valid, missing key: {missing_key}")
+
+        # variables
+        # berxel variables
+        self.__context = None
+        self.__device = None
+        # camera variables
+        self.__intri = np.zeros(4)
+
+        # open device
+        if not self.__open_device(self.__serial_number):
+            print("open device failed")
+            return
+
+        # start stream
+        if not self.__start_stream():
+            print("start stream failed")
+            return
+
+        # start work loop
+        self._working.set()
+
+    def get_intri(self) -> np.ndarray:
+        self._wait_for_working()
+        return self.__intri
+
+    def get_serial_number(self) -> np.ndarray:
+        self._wait_for_working()
+        return self.__serial_number
+
+    def work_loop(self, hex_queues: list[deque | threading.Event]):
+        rgb_queue = hex_queues[0]
+        depth_queue = hex_queues[1]
+        stop_event = hex_queues[2]
+
+        # clean cache
+        clean_cnt = 0
+        while clean_cnt < 5:
+            hawk_rgb_frame = self.__device.readColorFrame(40)
+            hawk_depth_frame = self.__device.readDepthFrame(40)
+            if hawk_rgb_frame is not None:
+                self.__device.releaseFrame(hawk_rgb_frame)
+            if hawk_depth_frame is not None:
+                self.__device.releaseFrame(hawk_depth_frame)
+            clean_cnt += 1
+            time.sleep(0.01)
+
+        rgb_count = 0
+        depth_count = 0
+        bias_ns = hex_ns_now() - time.time_ns()
+        while self._working.is_set() and not stop_event.is_set():
+            # read frame
+            hawk_rgb_frame = self.__device.readColorFrame(40)
+            hawk_depth_frame = self.__device.readDepthFrame(40)
+
+            # collect rgb frame
+            if hawk_rgb_frame is not None:
+                ts, frame = self.__unpack_frame(hawk_rgb_frame, False, bias_ns)
+                rgb_queue.append((ts, rgb_count, frame))
+                rgb_count = (rgb_count + 1) % self._max_seq_num
+
+            # collect depth frame
+            if hawk_depth_frame is not None:
+                ts, frame = self.__unpack_frame(hawk_depth_frame, True,
+                                                bias_ns)
+                depth_queue.append((ts, depth_count, frame))
+                depth_count = (depth_count + 1) % self._max_seq_num
+
+            self.__device.releaseFrame(hawk_rgb_frame)
+            self.__device.releaseFrame(hawk_depth_frame)
+
+        # close
+        self.close()
+
+    def __unpack_frame(
+        self,
+        hawk_frame: BerxelHawkFrame,
+        depth: bool = False,
+        bias_ns: int = 0,
+    ):
+        # common variables
+        berxel_ts_ns = bias_ns + int(hawk_frame.getTimeStamp() * 1_000)
+        ts = {
+            "s": berxel_ts_ns // 1_000_000_000,
+            "ns": berxel_ts_ns % 1_000_000_000,
+        }
+        width = hawk_frame.getWidth()
+        height = hawk_frame.getHeight()
+
+        if depth:
+            # depth frame
+            frame_buffer = hawk_frame.getDataAsUint16()
+            frame = np.ndarray(
+                shape=(height, width),
+                dtype=np.uint16,
+                buffer=frame_buffer,
+            )
+            pixel_type = hawk_frame.getPixelType()
+            if pixel_type == BerxelHawkPixelType.forward_dict[
+                    'BERXEL_HAWK_PIXEL_TYPE_DEP_16BIT_12I_4D']:
+                frame = frame // 16
+            elif pixel_type == BerxelHawkPixelType.forward_dict[
+                    'BERXEL_HAWK_PIXEL_TYPE_DEP_16BIT_13I_3D']:
+                frame = frame // 8
+            else:
+                raise ValueError(f"pixel_type: {pixel_type} not supported")
+        else:
+            # rgb frame
+            frame_buffer = hawk_frame.getDataAsUint8()
+            frame = np.ndarray(
+                shape=(height, width, 3),
+                dtype=np.uint8,
+                buffer=frame_buffer,
+            )
+            frame = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
+
+        return ts if self.__sens_ts else hex_zmq_ts_now(), frame
+
+    def __open_device(self, serial_number: str | None = None) -> bool:
+        # init context
+        self.__context = BerxelHawkContext()
+        if self.__context is None:
+            print("init failed")
+            return False
+        self.__context.initCamera()
+
+        # open device
+        device_list = self.__context.getDeviceList()
+        if len(device_list) < 1:
+            print("can not find device")
+            return False
+        if serial_number is not None:
+            device_idx = -1
+
+            # check serial number
+            def same_serial(tar_serial, device_serial):
+
+                def norm_serial(x):
+                    if x is None:
+                        return None
+                    if isinstance(x, (bytes, bytearray)):
+                        x = x.decode('utf-8', 'ignore')
+                    x = x.replace('\x00', '').strip()
+                    return x.upper()
+
+                return norm_serial(tar_serial) == norm_serial(device_serial)
+
+            for idx, device in enumerate(device_list):
+                if same_serial(serial_number, device.serialNumber):
+                    print(f"find device with serial number: {serial_number}")
+                    device_idx = idx
+                    break
+            if device_idx == -1:
+                print(
+                    f"can not find device with serial number: {serial_number}")
+                print("available device serial numbers:")
+                for device in device_list:
+                    print(f"{device.serialNumber}")
+                return False
+            self.__device = self.__context.openDevice(device_list[device_idx])
+        else:
+            print("No serial number, use first device")
+            self.__device = self.__context.openDevice(device_list[0])
+
+        if self.__device is None:
+            print("open device failed")
+            return False
+
+        return True
+
+    def __start_stream(self):
+        self.__device.setColorExposureGain(self.__exposure, self.__gain)
+        self.__device.setRegistrationEnable(True)
+        self.__device.setFrameSync(True)
+        while self.__device.setSystemClock() != 0:
+            print("set system clock failed")
+            time.sleep(0.1)
+
+        intrinsic_params = self.__device.getDeviceIntriscParams()
+        self.__intri[0] = intrinsic_params.colorIntrinsicParams.fx / 2
+        self.__intri[1] = intrinsic_params.colorIntrinsicParams.fy / 2
+        self.__intri[2] = intrinsic_params.colorIntrinsicParams.cx / 2
+        self.__intri[3] = intrinsic_params.colorIntrinsicParams.cy / 2
+
+        color_frame_mode = self.__device.getCurrentFrameMode(
+            BerxelHawkStreamType.forward_dict['BERXEL_HAWK_COLOR_STREAM'])
+        depth_frame_mode = self.__device.getCurrentFrameMode(
+            BerxelHawkStreamType.forward_dict['BERXEL_HAWK_DEPTH_STREAM'])
+        color_frame_mode.framerate = self.__frame_rate
+        depth_frame_mode.framerate = self.__frame_rate
+        self.__device.setFrameMode(
+            BerxelHawkStreamType.forward_dict['BERXEL_HAWK_COLOR_STREAM'],
+            color_frame_mode)
+        self.__device.setFrameMode(
+            BerxelHawkStreamType.forward_dict['BERXEL_HAWK_DEPTH_STREAM'],
+            depth_frame_mode)
+        ret = self.__device.startStreams(
+            BerxelHawkStreamType.forward_dict['BERXEL_HAWK_DEPTH_STREAM']
+            | BerxelHawkStreamType.forward_dict['BERXEL_HAWK_COLOR_STREAM'])
+        if ret == 0:
+            print("start stream succeed")
+            return True
+        else:
+            print("start stream failed")
+            return False
+
+    def close(self):
+        if not self._working.is_set():
+            return
+        self._working.clear()
+        self.__stop_stream()
+        self.__close_device()
+        hex_log(HEX_LOG_LEVEL["info"], "HexCamBerxel closed")
+
+    def __stop_stream(self):
+        if self.__device is None:
+            return False
+        ret = self.__device.stopStream(
+            BerxelHawkStreamType.forward_dict['BERXEL_HAWK_DEPTH_STREAM']
+            | BerxelHawkStreamType.forward_dict['BERXEL_HAWK_COLOR_STREAM'])
+        if ret == 0:
+            return True
+        else:
+            return False
+
+    def __close_device(self):
+        if self.__context is None:
+            return
+        if self.__device is None:
+            return
+
+        ret = self.__context.closeDevice(self.__device)
+        if ret == 0:
+            print("clsoe device succeed")
+        else:
+            print("close device Failed")
+        self.__context.destroyCamera()

hex_zmq_servers/cam/berxel/cam_berxel_cli.py

@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+# -*- coding:utf-8 -*-
+################################################################
+# Copyright 2025 Dong Zhaorui. All rights reserved.
+# Author: Dong Zhaorui 847235539@qq.com
+# Date  : 2025-09-12
+################################################################
+
+from ..cam_base import HexCamClientBase
+
+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 HexCamBerxelClient(HexCamClientBase):
+
+    def __init__(
+        self,
+        net_config: dict = NET_CONFIG,
+    ):
+        HexCamClientBase.__init__(self, net_config)
+        self._wait_for_working()
+
+    def get_intri(self):
+        intri_hdr, intri = self.request({"cmd": "get_intri"})
+        return intri_hdr, intri

hex_zmq_servers/cam/berxel/cam_berxel_srv.py

@@ -0,0 +1,79 @@
+#!/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 numpy as np
+
+try:
+    from ..cam_base import HexCamServerBase
+    from .cam_berxel import HexCamBerxel
+except (ImportError, ValueError):
+    import sys
+    from pathlib import Path
+    this_file = Path(__file__).resolve()
+    project_root = this_file.parents[3]
+    if str(project_root) not in sys.path:
+        sys.path.insert(0, str(project_root))
+    from hex_zmq_servers.cam.cam_base import HexCamServerBase
+    from hex_zmq_servers.cam.berxel.cam_berxel import HexCamBerxel
+
+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,
+}
+
+CAMERA_CONFIG = {
+    "serial_number": 'P100RYB4C03M2B322',
+    "exposure": 10000,
+    "gain": 100,
+    "frame_rate": 30,
+    "sens_ts": True,
+}
+
+
+class HexCamBerxelServer(HexCamServerBase):
+
+    def __init__(
+        self,
+        net_config: dict = NET_CONFIG,
+        params_config: dict = CAMERA_CONFIG,
+    ):
+        HexCamServerBase.__init__(self, net_config)
+
+        # camera
+        self._device = HexCamBerxel(params_config,
+                                    net_config.get("realtime_mode", False))
+
+    def _process_request(self, recv_hdr: dict, recv_buf: np.ndarray):
+        if recv_hdr["cmd"] == "is_working":
+            return self.no_ts_hdr(recv_hdr, self._device.is_working()), None
+        elif recv_hdr["cmd"] == "get_intri":
+            intri = self._device.get_intri()
+            return self.no_ts_hdr(recv_hdr, intri is not None), intri
+        elif recv_hdr["cmd"] == "get_rgb":
+            return self._get_frame(recv_hdr, False)
+        elif recv_hdr["cmd"] == "get_depth":
+            return self._get_frame(recv_hdr, True)
+        else:
+            raise ValueError(f"unknown command: {recv_hdr['cmd']}")
+
+
+if __name__ == "__main__":
+    import argparse, json
+    from hex_zmq_servers.zmq_base import hex_server_helper
+
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--cfg", type=str, required=True)
+    args = parser.parse_args()
+    cfg = json.loads(args.cfg)
+
+    hex_server_helper(cfg, HexCamBerxelServer)