hex-zmq-servers 0.3.2__py3-none-any.whl → 0.3.4__py3-none-any.whl

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",
+]

hex_zmq_servers/cam/rgb/cam_rgb.py

@@ -0,0 +1,133 @@
+#!/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 platform
+import cv2
+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
+
+CAMERA_CONFIG = {
+    "cam_path": "/dev/video0",
+    "resolution": [640, 480],
+    "crop": [0, 640, 0, 480],
+    "exposure": 100,
+    "temperature": 4000,
+    "frame_rate": 30,
+    "sens_ts": True,
+}
+
+
+class HexCamRGB(HexCamBase):
+
+    def __init__(
+        self,
+        camera_config: dict = CAMERA_CONFIG,
+    ):
+        HexCamBase.__init__(self)
+
+        try:
+            self.__cam_path = camera_config["cam_path"]
+            self.__resolution = camera_config["resolution"]
+            self.__crop = camera_config["crop"]
+            self.__exposure = camera_config["exposure"]
+            self.__temperature = camera_config["temperature"]
+            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
+        # camera variables
+        self.__cap = cv2.VideoCapture(self.__cam_path)
+        # camera variables
+        self.__intri = np.zeros(4)
+
+        # open device
+        self.__cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"MJPG"))
+        self.__cap.set(cv2.CAP_PROP_FRAME_WIDTH, self.__resolution[0])
+        self.__cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self.__resolution[1])
+        self.__cap.set(cv2.CAP_PROP_FPS, self.__frame_rate)
+        ae_open = 1.0 if platform.system() == "Windows" else 3.0
+        ae_close = 0.0 if platform.system() == "Windows" else 1.0
+        ae_value = 10000 * (2**self.__exposure) if platform.system(
+        ) == "Windows" else self.__exposure
+        self.__cap.set(cv2.CAP_PROP_AUTO_EXPOSURE, ae_open)
+        if self.__exposure != 0:
+            self.__cap.set(cv2.CAP_PROP_AUTO_EXPOSURE, ae_close)
+            self.__cap.set(cv2.CAP_PROP_EXPOSURE, ae_value)
+        self.__cap.set(cv2.CAP_PROP_AUTO_WB, 1)
+        if self.__temperature != 0:
+            self.__cap.set(cv2.CAP_PROP_AUTO_WB, 0)
+            self.__cap.set(cv2.CAP_PROP_WB_TEMPERATURE, self.__temperature)
+
+        print("#############################")
+        print(
+            f"# Resolution: ({self.__cap.get(cv2.CAP_PROP_FRAME_WIDTH)}, {self.__cap.get(cv2.CAP_PROP_FRAME_HEIGHT)})"
+        )
+        print(f"# FPS: {self.__cap.get(cv2.CAP_PROP_FPS)}")
+        four_cc_int = int(self.__cap.get(cv2.CAP_PROP_FOURCC))
+        four_cc_str = chr(four_cc_int & 0xff) + chr(
+            (four_cc_int >> 8) & 0xff) + chr((four_cc_int >> 16) & 0xff) + chr(
+                (four_cc_int >> 24) & 0xff)
+        print(f"# FourCC: {four_cc_str}")
+        print(f"# Auto Exposure: {self.__cap.get(cv2.CAP_PROP_AUTO_EXPOSURE)}")
+        print(f"# Exposure: {self.__cap.get(cv2.CAP_PROP_EXPOSURE)}")
+        print(f"# Auto WB: {self.__cap.get(cv2.CAP_PROP_AUTO_WB)}")
+        print(
+            f"# WB Temperature: {self.__cap.get(cv2.CAP_PROP_WB_TEMPERATURE)}")
+        print("#############################")
+
+        # start work loop
+        self._working.set()
+
+    def get_intri(self) -> np.ndarray:
+        self._wait_for_working()
+        return self.__intri
+
+    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_queue.append((hex_zmq_ts_now(), 0,
+                         np.zeros((self.__resolution[1], self.__resolution[0]),
+                                  dtype=np.uint16)))
+        rate = HexRate(self.__frame_rate * 5)
+        while self._working.is_set() and not stop_event.is_set():
+            # read frame
+            ret, frame = self.__cap.read()
+
+            # collect rgb frame
+            if ret:
+                frame = frame[self.__crop[2]:self.__crop[3],
+                              self.__crop[0]:self.__crop[1]]
+                rgb_queue.append((hex_zmq_ts_now(), rgb_count, frame))
+                rgb_count = (rgb_count + 1) % self._max_seq_num
+
+            rate.sleep()
+
+        # close
+        self.close()
+
+    def close(self):
+        if not self._working.is_set():
+            return
+        self._working.clear()
+        self.__cap.release()
+        hex_log(HEX_LOG_LEVEL["info"], "HexCamRGB closed")

hex_zmq_servers/cam/rgb/cam_rgb_cli.py

@@ -0,0 +1,41 @@
+#!/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
+from ...zmq_base import HexRate
+
+NET_CONFIG = {
+    "ip": "127.0.0.1",
+    "port": 12345,
+    "client_timeout_ms": 200,
+    "server_timeout_ms": 1_000,
+    "server_num_workers": 4,
+}
+
+
+class HexCamRGBClient(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
+    
+    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))
+            rate.sleep()
+

hex_zmq_servers/cam/rgb/cam_rgb_srv.py

@@ -0,0 +1,75 @@
+#!/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_rgb import HexCamRGB
+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.rgb.cam_rgb import HexCamRGB
+
+NET_CONFIG = {
+    "ip": "127.0.0.1",
+    "port": 12345,
+    "client_timeout_ms": 200,
+    "server_timeout_ms": 1_000,
+    "server_num_workers": 4,
+}
+
+CAMERA_CONFIG = {
+    "cam_path": "/dev/video0",
+    "resolution": [640, 480],
+    "frame_rate": 30,
+    "sens_ts": True,
+}
+
+
+class HexCamRGBServer(HexCamServerBase):
+
+    def __init__(
+        self,
+        net_config: dict = NET_CONFIG,
+        params_config: dict = CAMERA_CONFIG,
+    ):
+        HexCamServerBase.__init__(self, net_config)
+
+        # camera
+        self._device = HexCamRGB(params_config)
+
+    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, HexCamRGBServer)

hex_zmq_servers/config/cam_realsense.json

@@ -0,0 +1,15 @@
+{
+    "net": {
+        "ip": "127.0.0.1",
+        "port": 12345,
+        "client_timeout_ms": 200,
+        "server_timeout_ms": 1000,
+        "server_num_workers": 4
+    },
+    "params": {
+        "serial_number": "243422073194",
+        "resolution": [640, 480],
+        "frame_rate": 30,
+        "sens_ts": true
+    }
+}

hex_zmq_servers/config/cam_rgb.json

@@ -0,0 +1,26 @@
+{
+    "net": {
+        "ip": "127.0.0.1",
+        "port": 12345,
+        "client_timeout_ms": 200,
+        "server_timeout_ms": 1000,
+        "server_num_workers": 4
+    },
+    "params": {
+        "cam_path": "/dev/video0",
+        "resolution": [
+            640,
+            480
+        ],
+        "crop": [
+            0,
+            640,
+            0,
+            480
+        ],
+        "exposure": 70,
+        "temperature": 0,
+        "frame_rate": 30,
+        "sens_ts": true
+    }
+}

hex_zmq_servers/config/mujoco_archer_y6.json

@@ -7,7 +7,7 @@
         "server_num_workers": 4
     },
     "params": {
-        "states_rate": 500,
+        "states_rate": 1000,
         "img_rate": 30,
         "tau_ctrl": false,
         "mit_kp": [
@@ -28,6 +28,7 @@
             0.31,
             1.0
         ],
+        "cam_type": "empty",
         "headless": false,
         "sens_ts": true
     }

hex_zmq_servers/config/mujoco_e3_desktop.json

@@ -7,7 +7,7 @@
         "server_num_workers": 4
     },
     "params": {
-        "states_rate": 500,
+        "states_rate": 1000,
         "img_rate": 30,
         "tau_ctrl": false,
         "mit_kp": [
@@ -28,6 +28,11 @@
             0.31,
             1.0
         ],
+        "cam_type": [
+            "empty",
+            "empty",
+            "empty"
+        ],
         "headless": false,
         "sens_ts": true
     }

hex_zmq_servers/config/robot_hexarm.json

@@ -9,7 +9,7 @@
     "params": {
         "device_ip": "172.18.8.161",
         "device_port": 8439,
-        "control_hz": 500,
+        "control_hz": 1000,
         "arm_type": "archer_l6y",
         "use_gripper": true,
         "mit_kp": [

hex_zmq_servers/device_base.py

@@ -7,9 +7,10 @@
 ################################################################
 
 import threading
+from collections import deque
 from abc import ABC, abstractmethod
 
-from .zmq_base import MAX_SEQ_NUM, HexSafeValue
+from .zmq_base import MAX_SEQ_NUM
 
 
 class HexDeviceBase(ABC):
@@ -32,7 +33,7 @@ class HexDeviceBase(ABC):
             self._working.wait(0.1)
 
     @abstractmethod
-    def work_loop(self, hex_values: list[HexSafeValue | threading.Event]):
+    def work_loop(self, hex_queues: list[deque | threading.Event]):
         raise NotImplementedError(
             "`work_loop` should be implemented by the child class")
 

hex_zmq_servers/mujoco/archer_y6/model/robot.xml

@@ -6,37 +6,37 @@
         # link 1
         <body name="link_1" pos="0 0 0.0665">
             <inertial pos="0.008 -0.0023 0.0213" quat="0.707107 0.707107 0 0" mass="0.2154" diaginertia="0.0003 0.0003 0.0002"/>
-            <joint name="joint_1" pos="0 0 0" axis="0 0 1" range="-2.7 2.7" limited="true" armature="0.1" damping="3.0" frictionloss="1.0"/>
+            <joint name="joint_1" pos="0 0 0" axis="0 0 1" range="-2.7 2.7" limited="true" armature="0.01" damping="0.1" frictionloss="0.5"/>
             <geom type="mesh" rgba="0.92549 0.92549 0.92549 1" mesh="arm_link_1"/>
 
             # link 2
             <body name="link_2" pos="0.02 0 0.045">
                 <inertial pos="0.0001 -0.0028 0.1317" quat="0.999789 0.0205404 0 0" mass="1.3123" diaginertia="0.018 0.0177288 0.000671225"/>
-                <joint name="joint_2" pos="0 0 0" axis="0 1 0" range="-1.57 2.09" limited="true" armature="0.1" damping="3.0" frictionloss="1.0"/>
+                <joint name="joint_2" pos="0 0 0" axis="0 1 0" range="-1.57 2.09" limited="true" armature="0.01" damping="0.1" frictionloss="0.5"/>
                 <geom type="mesh" rgba="1 1 1 1" mesh="arm_link_2"/>
 
                 # link 3
                 <body name="link_3" pos="0 0 0.264">
                     <inertial pos="-0.051 0.0008 0.1447" quat="0.996909 0.0323182 -0.0688403 -0.019747" mass="1.1083" diaginertia="0.0115968 0.0112369 0.00136632"/>
-                    <joint name="joint_3" pos="0 0 0" axis="0 1 0" range="0 3.14" limited="true" armature="0.1" damping="3.0" frictionloss="1.0"/>
+                    <joint name="joint_3" pos="0 0 0" axis="0 1 0" range="0 3.14" limited="true" armature="0.01" damping="0.1" frictionloss="0.5"/>
                     <geom type="mesh" rgba="1 1 1 1" mesh="arm_link_3"/>
 
                     # link 4
                     <body name="link_4" pos="-0.06 0 0.245">
                         <inertial pos="-0.0597 -0.001 0.0572" quat="0 0.92388 0 0.382683" mass="0.5398" diaginertia="0.0011 0.001 0.0005"/>
-                        <joint name="joint_4" pos="0 0 0" axis="0 1 0" range="-1.57 1.57" limited="true" armature="0.1" damping="3.0" frictionloss="1.0"/>
+                        <joint name="joint_4" pos="0 0 0" axis="0 1 0" range="-1.57 1.57" limited="true" armature="0.01" damping="0.02" frictionloss="0.1"/>
                         <geom type="mesh" rgba="0.792157 0.792157 0.792157 1" mesh="arm_link_4"/>
 
                         # link 5
                         <body name="link_5" pos="-0.0553 0 0.070">
                             <inertial pos="0.0292 0.0001 -0.0107" quat="0.707107 0 0 0.707107" mass="0.3984" diaginertia="0.0003 0.0002 0.0002"/>
-                            <joint name="joint_5" pos="0 0 0" axis="-1 0 0" range="-1.57 1.57" limited="true" armature="0.1" damping="3.0" frictionloss="1.0"/>
+                            <joint name="joint_5" pos="0 0 0" axis="-1 0 0" range="-1.57 1.57" limited="true" armature="0.01" damping="0.02" frictionloss="0.1"/>
                             <geom type="mesh" rgba="0.898039 0.917647 0.929412 1" mesh="arm_link_5"/>
 
                             # gripper base link
                             <body name="gripper_base_link" pos="0.0553 0 0.029" quat="0.707107 0.0 -0.707107 0.0">
                                 <inertial pos="-0.0139 0.0001 0.0553" quat="0.567843 0.421372 0.421372 0.567843" mass="0.7651" diaginertia="0.0021 0.00143028 0.00106972"/>
-                                <joint name="joint_6" pos="0 0 0" axis="1 0 0" range="-1.57 1.57" limited="true" armature="0.1" damping="3.0" frictionloss="1.0"/>
+                                <joint name="joint_6" pos="0 0 0" axis="1 0 0" range="-1.57 1.57" limited="true" armature="0.01" damping="0.02" frictionloss="0.1"/>
                                 <geom type="mesh" rgba="1 1 1 1" mesh="gripper_base_link"/>
                                 <geom pos="0.04 0 0.12" quat="0.6830127 0.6830127 0.1830127 -0.1830127" type="mesh" rgba="0.1 0.1 0.1 1" mesh="camera_link" />
                                 <camera name="end_camera" pos="0.066 0 0.105" quat="0.8660254 0.0 -0.5 0.0" fovy="50"/>

hex_zmq_servers/mujoco/archer_y6/model/scene.xml

@@ -1,7 +1,7 @@
 <mujoco model="archer_y6 scene">
     <compiler angle="radian"/>
     <option integrator="implicitfast"/>
-    <option timestep="2e-3"/>
+    <option timestep="1e-3"/>
 
     <statistic center="0.3 0 0.45" extent="0.8" meansize="0.05"/>
 

hex_zmq_servers/mujoco/archer_y6/mujoco_archer_y6.py

@@ -11,22 +11,24 @@ import copy
 import threading
 import cv2
 import numpy as np
+from collections import deque
 
 import mujoco
 from mujoco import viewer
 
 from ..mujoco_base import HexMujocoBase
 from ...zmq_base import (
+    hex_ns_now,
     hex_zmq_ts_now,
+    ns_to_hex_zmq_ts,
     hex_zmq_ts_delta_ms,
     HexRate,
-    HexSafeValue,
 )
 from ...hex_launch import hex_log, HEX_LOG_LEVEL
 from hex_robo_utils import HexCtrlUtilMitJoint as CtrlUtil
 
 MUJOCO_CONFIG = {
-    "states_rate": 500,
+    "states_rate": 1000,
     "img_rate": 30,
     "tau_ctrl": False,
     "mit_kp": [200.0, 200.0, 200.0, 75.0, 15.0, 15.0, 20.0],
@@ -34,6 +36,12 @@ MUJOCO_CONFIG = {
     "headless": False,
     "sens_ts": True,
 }
+CAMERA_CONFIG = {
+    "empty": (False, False),
+    "rgb": (True, False),
+    "berxel": (True, True),
+    "realsense": (True, True),
+}
 
 
 class HexMujocoArcherY6(HexMujocoBase):
@@ -50,6 +58,7 @@ class HexMujocoArcherY6(HexMujocoBase):
             self.__tau_ctrl = mujoco_config["tau_ctrl"]
             self.__mit_kp = mujoco_config["mit_kp"]
             self.__mit_kd = mujoco_config["mit_kd"]
+            self.__cam_type = mujoco_config["cam_type"]
             self.__headless = mujoco_config["headless"]
             self.__sens_ts = mujoco_config["sens_ts"]
         except KeyError as ke:
@@ -95,20 +104,31 @@ class HexMujocoArcherY6(HexMujocoBase):
         self.__states_trig_thresh = int(self.__sim_rate / states_rate)
 
         # camera init
-        width, height = 640, 400
-        fovy_rad = self.__model.cam_fovy[0] * np.pi / 180.0
-        focal = 0.5 * height / np.tan(fovy_rad / 2.0)
-        self._intri = np.array([focal, focal, height / 2, height / 2])
-        self.__rgb_cam = mujoco.Renderer(self.__model, height, width)
-        self.__depth_cam = mujoco.Renderer(self.__model, height, width)
-        self.__depth_cam.enable_depth_rendering()
         self.__img_trig_thresh = int(self.__sim_rate / img_rate)
+        self.__width, self.__height = 640, 400
+        fovy_rad = self.__model.cam_fovy[0] * np.pi / 180.0
+        focal = 0.5 * self.__height / np.tan(fovy_rad / 2.0)
+        self._intri = np.array(
+            [focal, focal, self.__height / 2, self.__height / 2])
+        self.__use_rgb, self.__use_depth = CAMERA_CONFIG.get(
+            self.__cam_type, (False, False))
+        self.__rgb_cam, self.__depth_cam = None, None
+        if self.__use_rgb:
+            self.__rgb_cam = mujoco.Renderer(self.__model, self.__height,
+                                             self.__width)
+        if self.__use_depth:
+            self.__depth_cam = mujoco.Renderer(self.__model, self.__height,
+                                               self.__width)
+            self.__depth_cam.enable_depth_rendering()
 
         # viewer init
         mujoco.mj_forward(self.__model, self.__data)
         if not self.__headless:
             self.__viewer = viewer.launch_passive(self.__model, self.__data)
 
+        # time init
+        self.__bias_ns = hex_ns_now() - self.__data.time * 1_000_000_000
+
         # start work loop
         self._working.set()
 
@@ -124,13 +144,13 @@ class HexMujocoArcherY6(HexMujocoBase):
             self.__viewer.sync()
         return True
 
-    def work_loop(self, hex_values: list[HexSafeValue | threading.Event]):
-        states_robot_value = hex_values[0]
-        states_obj_value = hex_values[1]
-        cmds_robot_value = hex_values[2]
-        rgb_value = hex_values[3]
-        depth_value = hex_values[4]
-        stop_event = hex_values[5]
+    def work_loop(self, hex_queues: list[deque | threading.Event]):
+        states_robot_queue = hex_queues[0]
+        states_obj_queue = hex_queues[1]
+        cmds_robot_queue = hex_queues[2]
+        rgb_queue = hex_queues[3]
+        depth_queue = hex_queues[4]
+        stop_event = hex_queues[5]
 
         last_states_ts = {"s": 0, "ns": 0}
         states_robot_count = 0
@@ -138,11 +158,18 @@ class HexMujocoArcherY6(HexMujocoBase):
         last_cmds_robot_seq = -1
         rgb_count = 0
         depth_count = 0
+        cmds_robot = None
 
         rate = HexRate(self.__sim_rate)
         states_trig_count = 0
         img_trig_count = 0
-
+        init_ts = self.__mujoco_ts() if self.__sens_ts else hex_zmq_ts_now()
+        rgb_queue.append((init_ts, 0,
+                       np.zeros((self.__height, self.__width, 3),
+                                dtype=np.uint8)))
+        depth_queue.append((init_ts, 0,
+                         np.zeros((self.__height, self.__width),
+                                  dtype=np.uint16)))
         while self._working.is_set() and not stop_event.is_set():
             states_trig_count += 1
             if states_trig_count >= self.__states_trig_thresh:
@@ -151,45 +178,53 @@ class HexMujocoArcherY6(HexMujocoBase):
                 # states
                 ts, states_robot, states_obj = self.__get_states()
                 if states_robot is not None:
-                    if hex_zmq_ts_delta_ms(ts, last_states_ts) > 1.0:
+                    if hex_zmq_ts_delta_ms(ts, last_states_ts) > 1e-6:
                         last_states_ts = ts
 
                         # states robot
-                        states_robot_value.set(
+                        states_robot_queue.append(
                             (ts, states_robot_count, states_robot))
                         states_robot_count = (states_robot_count +
                                               1) % self._max_seq_num
 
                         # states obj
-                        states_obj_value.set(
+                        states_obj_queue.append(
                             (ts, states_obj_count, states_obj))
                         states_obj_count = (states_obj_count +
                                             1) % self._max_seq_num
 
                 # cmds
-                cmds_robot_pack = cmds_robot_value.get(timeout_s=-1.0)
+                cmds_robot_pack = None
+                try:
+                    cmds_robot_pack = cmds_robot_queue.popleft()
+                except IndexError:
+                    pass
                 if cmds_robot_pack is not None:
-                    ts, seq, cmds = cmds_robot_pack
+                    ts, seq, cmds_robot_get = cmds_robot_pack
                     if seq != last_cmds_robot_seq:
                         last_cmds_robot_seq = seq
                         if hex_zmq_ts_delta_ms(hex_zmq_ts_now(), ts) < 200.0:
-                            self.__set_cmds(cmds)
+                            cmds_robot = cmds_robot_get.copy()
+                if cmds_robot is not None:
+                    self.__set_cmds(cmds_robot)
 
             img_trig_count += 1
             if img_trig_count >= self.__img_trig_thresh:
                 img_trig_count = 0
 
                 # rgb
-                ts, rgb_img = self.__get_rgb()
-                if rgb_img is not None:
-                    rgb_value.set((ts, rgb_count, rgb_img))
-                    rgb_count = (rgb_count + 1) % self._max_seq_num
+                if self.__use_rgb:
+                    ts, rgb_img = self.__get_rgb()
+                    if rgb_img is not None:
+                        rgb_queue.append((ts, rgb_count, rgb_img))
+                        rgb_count = (rgb_count + 1) % self._max_seq_num
 
                 # depth
-                ts, depth_img = self.__get_depth()
-                if depth_img is not None:
-                    depth_value.set((ts, depth_count, depth_img))
-                    depth_count = (depth_count + 1) % self._max_seq_num
+                if self.__use_depth:
+                    ts, depth_img = self.__get_depth()
+                    if depth_img is not None:
+                        depth_queue.append((ts, depth_count, depth_img))
+                        depth_count = (depth_count + 1) % self._max_seq_num
 
             # mujoco step
             mujoco.mj_step(self.__model, self.__data)
@@ -236,7 +271,8 @@ class HexMujocoArcherY6(HexMujocoBase):
                     cmd_kp = cmds[:, 3].copy()
                     cmd_kd = cmds[:, 4].copy()
                 else:
-                    raise ValueError(f"The shape of cmds is invalid: {cmds.shape}")
+                    raise ValueError(
+                        f"The shape of cmds is invalid: {cmds.shape}")
             else:
                 raise ValueError(f"The shape of cmds is invalid: {cmds.shape}")
             tar_pos = self._apply_pos_limits(
@@ -272,18 +308,17 @@ class HexMujocoArcherY6(HexMujocoBase):
         ), depth_img
 
     def __mujoco_ts(self):
-        mujoco_ts = self.__data.time
-        return {
-            "s": int(mujoco_ts // 1),
-            "ns": int((mujoco_ts % 1) * 1_000_000_000),
-        }
+        mujoco_ts = self.__data.time * 1_000_000_000 + self.__bias_ns
+        return ns_to_hex_zmq_ts(mujoco_ts)
 
     def close(self):
         if not self._working.is_set():
             return
         self._working.clear()
-        self.__rgb_cam.close()
-        self.__depth_cam.close()
+        if self.__rgb_cam is not None:
+            self.__rgb_cam.close()
+        if self.__depth_cam is not None:
+            self.__depth_cam.close()
         if not self.__headless:
             self.__viewer.close()
         hex_log(HEX_LOG_LEVEL["info"], "HexMujocoArcherY6 closed")