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

hex_zmq_servers/cam/cam_base.py

@@ -0,0 +1,189 @@
+#!/usr/bin/env python3
+# -*- coding:utf-8 -*-
+################################################################
+# Copyright 2025 Dong Zhaorui. All rights reserved.
+# Author: Dong Zhaorui 847235539@qq.com
+# Date  : 2025-09-16
+################################################################
+
+import threading
+import numpy as np
+from collections import deque
+from abc import abstractmethod
+
+from ..device_base import HexDeviceBase
+from ..zmq_base import HexZMQClientBase, HexZMQServerBase, HexRate
+
+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 HexCamBase(HexDeviceBase):
+
+    def __init__(self, realtime_mode: bool = False):
+        HexDeviceBase.__init__(self, realtime_mode)
+
+    def __del__(self):
+        HexDeviceBase.__del__(self)
+
+    @abstractmethod
+    def work_loop(self, hex_queues: list[deque | threading.Event]):
+        raise NotImplementedError(
+            "`work_loop` should be implemented by the child class")
+
+    @abstractmethod
+    def close(self):
+        raise NotImplementedError(
+            "`close` should be implemented by the child class")
+
+
+class HexCamClientBase(HexZMQClientBase):
+
+    def __init__(self, net_config: dict = NET_CONFIG):
+        HexZMQClientBase.__init__(self, net_config)
+        self._rgb_seq = 0
+        self._used_rgb_seq = 0
+        self._depth_seq = 0
+        self._used_depth_seq = 0
+        self._rgb_queue = deque(maxlen=self._deque_maxlen)
+        self._depth_queue = deque(maxlen=self._deque_maxlen)
+
+    def __del__(self):
+        HexZMQClientBase.__del__(self)
+
+    def get_rgb(self, newest: bool = False):
+        try:
+            if self._realtime_mode or newest:
+                hdr, img = self._rgb_queue[-1]
+                if self._used_rgb_seq != hdr["args"]:
+                    self._used_rgb_seq = hdr["args"]
+                    return hdr, img
+                else:
+                    return None, None
+            else:
+                return self._rgb_queue.popleft()
+        except IndexError:
+            return None, None
+
+    def get_depth(self, newest: bool = False):
+        try:
+            if self._realtime_mode or newest:
+                hdr, img = self._depth_queue[-1]
+                if self._used_depth_seq != hdr["args"]:
+                    self._used_depth_seq = hdr["args"]
+                    return hdr, img
+                else:
+                    return None, None
+            else:
+                return self._depth_queue.popleft()
+        except IndexError:
+            return None, None
+
+    def _get_rgb_inner(self):
+        return self._process_frame(False)
+
+    def _get_depth_inner(self):
+        return self._process_frame(True)
+
+    def _process_frame(self, depth_flag: bool):
+        req_cmd = f"get_{'depth' if depth_flag else 'rgb'}"
+        hdr, img = self.request({
+            "cmd":
+            req_cmd,
+            "args": (1 + (self._depth_seq if depth_flag else self._rgb_seq)) %
+            self._max_seq_num,
+        })
+
+        try:
+            cmd = hdr["cmd"]
+            if cmd == f"{req_cmd}_ok":
+                if depth_flag:
+                    self._depth_seq = hdr["args"]
+                else:
+                    self._rgb_seq = hdr["args"]
+                return hdr, img
+            else:
+                return None, None
+        except KeyError:
+            print(f"\033[91m{hdr['cmd']} requires `cmd`\033[0m")
+            return None, None
+        except Exception as e:
+            print(f"\033[91m__process_frame failed: {e}\033[0m")
+            return None, None
+
+    def _recv_loop(self):
+        rate = HexRate(200)
+        while self._recv_flag:
+            hdr, img = self._get_rgb_inner()
+            if hdr is not None:
+                self._rgb_queue.append((hdr, img))
+            hdr, img = self._get_depth_inner()
+            if hdr is not None:
+                self._depth_queue.append((hdr, img))
+            rate.sleep()
+
+
+class HexCamServerBase(HexZMQServerBase):
+
+    def __init__(self, net_config: dict = NET_CONFIG):
+        HexZMQServerBase.__init__(self, net_config)
+        self._device: HexDeviceBase = None
+        self._rgb_queue = deque(maxlen=self._deque_maxlen)
+        self._depth_queue = deque(maxlen=self._deque_maxlen)
+
+    def __del__(self):
+        HexZMQServerBase.__del__(self)
+        self._device.close()
+
+    def work_loop(self):
+        try:
+            self._device.work_loop([
+                self._rgb_queue,
+                self._depth_queue,
+                self._stop_event,
+            ])
+        finally:
+            self._device.close()
+
+    def _get_frame(self, recv_hdr: dict, depth_flag: bool):
+        try:
+            seq = recv_hdr["args"]
+        except KeyError:
+            print(f"\033[91m{recv_hdr['cmd']} requires `args`\033[0m")
+            return {"cmd": f"{recv_hdr['cmd']}_failed"}, None
+
+        try:
+            if depth_flag:
+                ts, count, img = self._depth_queue[
+                    -1] if self._realtime_mode else self._depth_queue.popleft(
+                    )
+            else:
+                ts, count, img = self._rgb_queue[
+                    -1] if self._realtime_mode else self._rgb_queue.popleft()
+        except IndexError:
+            return {"cmd": f"{recv_hdr['cmd']}_failed"}, None
+        except Exception as e:
+            print(f"\033[91m{recv_hdr['cmd']} failed: {e}\033[0m")
+            return {"cmd": f"{recv_hdr['cmd']}_failed"}, None
+
+        delta = (count - seq) % self._max_seq_num
+        if delta >= 0 and delta < 1e6:
+            return {
+                "cmd": f"{recv_hdr['cmd']}_ok",
+                "ts": ts,
+                "args": count
+            }, img
+        else:
+            return {"cmd": f"{recv_hdr['cmd']}_failed"}, None
+
+    @abstractmethod
+    def _process_request(self, recv_hdr: dict, recv_buf: np.ndarray):
+        raise NotImplementedError(
+            "`_process_request` should be implemented by the child class")

hex_zmq_servers/cam/dummy/__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_dummy import HexCamDummy
+from .cam_dummy_cli import HexCamDummyClient
+from .cam_dummy_srv import HexCamDummyServer
+
+__all__ = [
+    "HexCamDummy",
+    "HexCamDummyClient",
+    "HexCamDummyServer",
+]

hex_zmq_servers/cam/dummy/cam_dummy.py

@@ -0,0 +1,69 @@
+#!/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 threading
+import numpy as np
+from collections import deque
+
+from ..cam_base import HexCamBase
+from ...zmq_base import (
+    hex_zmq_ts_now,
+    HexRate,
+)
+from ...hex_launch import hex_log, HEX_LOG_LEVEL
+
+
+class HexCamDummy(HexCamBase):
+
+    def __init__(self, params_config: dict = {}, realtime_mode: bool = False):
+        HexCamBase.__init__(self, realtime_mode)
+        self._working.set()
+
+    def __del__(self):
+        HexCamBase.__del__(self)
+
+    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]
+
+        rgb_count = 0
+        depth_count = 0
+        rate = HexRate(60)
+        while self._working.is_set() and not stop_event.is_set():
+            # rgb
+            rgb_img = np.random.randint(
+                0,
+                255,
+                (480, 640, 3),
+                dtype=np.uint8,
+            )
+            rgb_queue.append((hex_zmq_ts_now(), rgb_count, rgb_img))
+            rgb_count = (rgb_count + 1) % self._max_seq_num
+
+            # depth
+            depth_img = np.random.randint(
+                0,
+                65535,
+                (480, 640),
+                dtype=np.uint16,
+            )
+            depth_queue.append((hex_zmq_ts_now(), depth_count, depth_img))
+            depth_count = (depth_count + 1) % self._max_seq_num
+
+            # sleep
+            rate.sleep()
+
+        # close
+        self.close()
+
+    def close(self):
+        if not self._working.is_set():
+            return
+        self._working.clear()
+        hex_log(HEX_LOG_LEVEL["info"], "HexCamDummy closed")

hex_zmq_servers/cam/dummy/cam_dummy_cli.py

@@ -0,0 +1,29 @@
+#!/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 HexCamDummyClient(HexCamClientBase):
+
+    def __init__(
+        self,
+        net_config: dict = NET_CONFIG,
+    ):
+        HexCamClientBase.__init__(self, net_config)
+        self._wait_for_working()

hex_zmq_servers/cam/dummy/cam_dummy_srv.py

@@ -0,0 +1,68 @@
+#!/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_dummy import HexCamDummy
+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.dummy.cam_dummy import HexCamDummy
+
+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 HexCamDummyServer(HexCamServerBase):
+
+    def __init__(
+        self,
+        net_config: dict = NET_CONFIG,
+        params_config: dict = {},
+    ):
+        HexCamServerBase.__init__(self, net_config)
+
+        # camera
+        self._device = HexCamDummy(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_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, HexCamDummyServer)

hex_zmq_servers/cam/realsense/__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_realsense import HexCamRealsense
+from .cam_realsense_cli import HexCamRealsenseClient
+from .cam_realsense_srv import HexCamRealsenseServer
+
+__all__ = [
+    "HexCamRealsense",
+    "HexCamRealsenseClient",
+    "HexCamRealsenseServer",
+]

hex_zmq_servers/cam/realsense/cam_realsense.py

@@ -0,0 +1,159 @@
+#!/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 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,
+    hex_zmq_ts_delta_ms,
+)
+from ...hex_launch import hex_log, HEX_LOG_LEVEL
+import pyrealsense2 as rs
+
+CAMERA_CONFIG = {
+    "serial_number": '243422073194',
+    "resolution": [640, 480],
+    "frame_rate": 30,
+    "sens_ts": True,
+}
+
+
+class HexCamRealsense(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.__resolution = camera_config["resolution"]
+            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
+        # realsense variables
+        ctx = rs.context()
+        serial_numbers = []
+        for dev in ctx.query_devices():
+            serial = dev.get_info(rs.camera_info.serial_number)
+            name = dev.get_info(rs.camera_info.name)
+            print(f"  - Device: {name}, Serial: {serial}")
+            serial_numbers.append(serial)
+        if self.__serial_number not in serial_numbers:
+            print(
+                f"can not find device with serial number: {self.__serial_number}"
+            )
+            return
+
+        # camera variables
+        self.__intri = np.zeros(4)
+
+        # open device
+        self.__pipeline = rs.pipeline()
+        config = rs.config()
+        config.enable_device(self.__serial_number)
+        config.enable_stream(
+            rs.stream.color,
+            self.__resolution[0],
+            self.__resolution[1],
+            rs.format.bgr8,
+            self.__frame_rate,
+        )
+        config.enable_stream(
+            rs.stream.depth,
+            self.__resolution[0],
+            self.__resolution[1],
+            rs.format.z16,
+            self.__frame_rate,
+        )
+        profile = self.__pipeline.start(config)
+        color_profile = profile.get_stream(rs.stream.color)
+        color_intrinsics = color_profile.as_video_stream_profile(
+        ).get_intrinsics()
+        self.__intri[0] = color_intrinsics.fx
+        self.__intri[1] = color_intrinsics.fy
+        self.__intri[2] = color_intrinsics.ppx
+        self.__intri[3] = color_intrinsics.ppy
+        self.__align = rs.align(rs.stream.color)
+        self.__bias_ns = None
+
+        # 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]
+
+        frames = self.__pipeline.wait_for_frames()
+        bias_ns = np.int64(hex_ns_now()) - np.int64(
+            frames.get_frame_metadata(rs.frame_metadata_value.sensor_timestamp)
+            * 1_000)
+
+        rgb_count = 0
+        depth_count = 0
+        while self._working.is_set() and not stop_event.is_set():
+            # read frame
+            aligned_frames = self.__align.process(
+                self.__pipeline.wait_for_frames())
+            cur_ns = hex_zmq_ts_now()
+            sen_ts_ns = bias_ns + np.int64(
+                aligned_frames.get_frame_metadata(
+                    rs.frame_metadata_value.sensor_timestamp) * 1_000)
+            sen_ts = {
+                "s": sen_ts_ns // 1_000_000_000,
+                "ns": sen_ts_ns % 1_000_000_000,
+            }
+            if hex_zmq_ts_delta_ms(cur_ns, sen_ts) < 0:
+                sen_ts = cur_ns
+
+            # collect rgb frame
+            color_frame = aligned_frames.get_color_frame()
+            if color_frame:
+
+                rgb_queue.append(
+                    (sen_ts if self.__sens_ts else cur_ns, rgb_count,
+                     np.asanyarray(color_frame.get_data())))
+                rgb_count = (rgb_count + 1) % self._max_seq_num
+
+            # collect depth frame
+            depth_frame = aligned_frames.get_depth_frame()
+            if depth_frame:
+                depth_queue.append(
+                    (sen_ts if self.__sens_ts else cur_ns, depth_count,
+                     np.asanyarray(depth_frame.get_data())))
+                depth_count = (depth_count + 1) % self._max_seq_num
+
+        # close
+        self.close()
+
+    def close(self):
+        if not self._working.is_set():
+            return
+        self._working.clear()
+        self.__pipeline.stop()
+        hex_log(HEX_LOG_LEVEL["info"], "HexCamRealsense closed")

hex_zmq_servers/cam/realsense/cam_realsense_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 HexCamRealsenseClient(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/realsense/cam_realsense_srv.py

@@ -0,0 +1,78 @@
+#!/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_realsense import HexCamRealsense
+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.realsense.cam_realsense import HexCamRealsense
+
+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": '243422073194',
+    "resolution": [640, 480],
+    "frame_rate": 30,
+    "sens_ts": True,
+}
+
+
+class HexCamRealsenseServer(HexCamServerBase):
+
+    def __init__(
+        self,
+        net_config: dict = NET_CONFIG,
+        params_config: dict = CAMERA_CONFIG,
+    ):
+        HexCamServerBase.__init__(self, net_config)
+
+        # camera
+        self._device = HexCamRealsense(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, HexCamRealsenseServer)

hex_zmq_servers/cam/rgb/__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_rgb import HexCamRGB
+from .cam_rgb_cli import HexCamRGBClient
+from .cam_rgb_srv import HexCamRGBServer
+
+__all__ = [
+    "HexCamRGB",
+    "HexCamRGBClient",
+    "HexCamRGBServer",
+]