hex-zmq-servers 0.3.3__tar.gz → 0.3.4__tar.gz

{hex_zmq_servers-0.3.3/hex_zmq_servers.egg-info → hex_zmq_servers-0.3.4}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: hex_zmq_servers
-Version: 0.3.3
+Version: 0.3.4
 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>

{hex_zmq_servers-0.3.3 → hex_zmq_servers-0.3.4}/hex_zmq_servers/__init__.py

@@ -9,8 +9,8 @@
 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 HexRate, hex_zmq_ts_now, hex_zmq_ts_delta_ms
-from .zmq_base import HexSafeValue, HexZMQClientBase, HexZMQServerBase, hex_server_helper
+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
@@ -62,9 +62,11 @@ __all__ = [
     # 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",
-    "HexSafeValue",
     "HexZMQClientBase",
     "HexZMQServerBase",
     "hex_server_helper",

{hex_zmq_servers-0.3.3 → hex_zmq_servers-0.3.4}/hex_zmq_servers/cam/berxel/cam_berxel.py

@@ -9,12 +9,10 @@
 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,
-    HexSafeValue,
-)
+from ...zmq_base import hex_zmq_ts_now
 from ...hex_launch import hex_log, HEX_LOG_LEVEL
 from berxel_py_wrapper import *
 
@@ -74,10 +72,10 @@ class HexCamBerxel(HexCamBase):
         self._wait_for_working()
         return self.__serial_number
 
-    def work_loop(self, hex_values: list[HexSafeValue | threading.Event]):
-        rgb_value = hex_values[0]
-        depth_value = hex_values[1]
-        stop_event = hex_values[2]
+    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
@@ -89,13 +87,13 @@ class HexCamBerxel(HexCamBase):
             # collect rgb frame
             if hawk_rgb_frame is not None:
                 ts, frame = self.__unpack_frame(hawk_rgb_frame, False)
-                rgb_value.set((ts, rgb_count, frame))
+                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)
-                depth_value.set((ts, depth_count, frame))
+                depth_queue.append((ts, depth_count, frame))
                 depth_count = (depth_count + 1) % self._max_seq_num
 
             self.__device.releaseFrame(hawk_rgb_frame)

{hex_zmq_servers-0.3.3 → hex_zmq_servers-0.3.4}/hex_zmq_servers/cam/berxel/cam_berxel_cli.py

@@ -24,6 +24,7 @@ class HexCamBerxelClient(HexCamClientBase):
         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"})

{hex_zmq_servers-0.3.3 → hex_zmq_servers-0.3.4}/hex_zmq_servers/cam/cam_base.py

@@ -8,10 +8,11 @@
 
 import threading
 import numpy as np
+from collections import deque
 from abc import abstractmethod
 
 from ..device_base import HexDeviceBase
-from ..zmq_base import HexSafeValue, HexZMQClientBase, HexZMQServerBase
+from ..zmq_base import HexZMQClientBase, HexZMQServerBase, HexRate
 
 NET_CONFIG = {
     "ip": "127.0.0.1",
@@ -31,7 +32,7 @@ class HexCamBase(HexDeviceBase):
         HexDeviceBase.__del__(self)
 
     @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")
 
@@ -47,14 +48,30 @@ class HexCamClientBase(HexZMQClientBase):
         HexZMQClientBase.__init__(self, net_config)
         self._rgb_seq = 0
         self._depth_seq = 0
+        self._rgb_queue = deque(maxlen=10)
+        self._depth_queue = deque(maxlen=10)
 
     def __del__(self):
         HexZMQClientBase.__del__(self)
 
-    def get_rgb(self):
+    def get_rgb(self, newest: bool = False):
+        try:
+            return self._rgb_queue.popleft(
+            ) if not newest else self._rgb_queue[-1]
+        except IndexError:
+            return None, None
+
+    def get_depth(self, newest: bool = False):
+        try:
+            return self._depth_queue.popleft(
+            ) if not newest else self._depth_queue[-1]
+        except IndexError:
+            return None, None
+
+    def _get_rgb_inner(self):
         return self._process_frame(False)
 
-    def get_depth(self):
+    def _get_depth_inner(self):
         return self._process_frame(True)
 
     def _process_frame(self, depth_flag: bool):
@@ -83,14 +100,25 @@ class HexCamClientBase(HexZMQClientBase):
             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_value = HexSafeValue()
-        self._depth_value = HexSafeValue()
+        self._rgb_queue = deque(maxlen=10)
+        self._depth_queue = deque(maxlen=10)
 
     def __del__(self):
         HexZMQServerBase.__del__(self)
@@ -99,8 +127,8 @@ class HexCamServerBase(HexZMQServerBase):
     def work_loop(self):
         try:
             self._device.work_loop([
-                self._rgb_value,
-                self._depth_value,
+                self._rgb_queue,
+                self._depth_queue,
                 self._stop_event,
             ])
         finally:
@@ -115,9 +143,11 @@ class HexCamServerBase(HexZMQServerBase):
 
         try:
             if depth_flag:
-                ts, count, img = self._depth_value.get()
+                ts, count, img = self._depth_queue.popleft()
             else:
-                ts, count, img = self._rgb_value.get()
+                ts, count, img = 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

{hex_zmq_servers-0.3.3 → hex_zmq_servers-0.3.4}/hex_zmq_servers/cam/dummy/cam_dummy.py

@@ -8,12 +8,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,
-    HexSafeValue,
 )
 from ...hex_launch import hex_log, HEX_LOG_LEVEL
 
@@ -27,10 +27,10 @@ class HexCamDummy(HexCamBase):
     def __del__(self):
         HexCamBase.__del__(self)
 
-    def work_loop(self, hex_values: list[HexSafeValue | threading.Event]):
-        rgb_value = hex_values[0]
-        depth_value = hex_values[1]
-        stop_event = hex_values[2]
+    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
@@ -43,7 +43,7 @@ class HexCamDummy(HexCamBase):
                 (480, 640, 3),
                 dtype=np.uint8,
             )
-            rgb_value.set((hex_zmq_ts_now(), rgb_count, rgb_img))
+            rgb_queue.append((hex_zmq_ts_now(), rgb_count, rgb_img))
             rgb_count = (rgb_count + 1) % self._max_seq_num
 
             # depth
@@ -53,7 +53,7 @@ class HexCamDummy(HexCamBase):
                 (480, 640),
                 dtype=np.uint16,
             )
-            depth_value.set((hex_zmq_ts_now(), depth_count, depth_img))
+            depth_queue.append((hex_zmq_ts_now(), depth_count, depth_img))
             depth_count = (depth_count + 1) % self._max_seq_num
 
             # sleep

{hex_zmq_servers-0.3.3 → hex_zmq_servers-0.3.4}/hex_zmq_servers/cam/dummy/cam_dummy_cli.py

@@ -24,3 +24,4 @@ class HexCamDummyClient(HexCamClientBase):
         net_config: dict = NET_CONFIG,
     ):
         HexCamClientBase.__init__(self, net_config)
+        self._wait_for_working()

{hex_zmq_servers-0.3.3 → hex_zmq_servers-0.3.4}/hex_zmq_servers/cam/realsense/cam_realsense.py

@@ -6,15 +6,15 @@
 # Date  : 2025-09-14
 ################################################################
 
-import time
 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,
-    HexSafeValue,
 )
 from ...hex_launch import hex_log, HEX_LOG_LEVEL
 import pyrealsense2 as rs
@@ -103,13 +103,13 @@ class HexCamRealsense(HexCamBase):
         self._wait_for_working()
         return self.__serial_number
 
-    def work_loop(self, hex_values: list[HexSafeValue | threading.Event]):
-        rgb_value = hex_values[0]
-        depth_value = hex_values[1]
-        stop_event = hex_values[2]
+    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(time.time_ns()) - np.int64(
+        bias_ns = np.int64(hex_ns_now()) - np.int64(
             frames.get_frame_metadata(rs.frame_metadata_value.sensor_timestamp)
             * 1_000)
 
@@ -134,14 +134,14 @@ class HexCamRealsense(HexCamBase):
             color_frame = aligned_frames.get_color_frame()
             if color_frame:
 
-                rgb_value.set((sen_ts if self.__sens_ts else cur_ns, rgb_count,
+                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_value.set(
+                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

{hex_zmq_servers-0.3.3 → hex_zmq_servers-0.3.4}/hex_zmq_servers/cam/realsense/cam_realsense_cli.py

@@ -24,6 +24,7 @@ class HexCamRealsenseClient(HexCamClientBase):
         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"})

{hex_zmq_servers-0.3.3 → hex_zmq_servers-0.3.4}/hex_zmq_servers/cam/rgb/cam_rgb.py

@@ -6,21 +6,25 @@
 # 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,
-    HexSafeValue,
 )
 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,
 }
@@ -37,6 +41,9 @@ class HexCamRGB(HexCamBase):
         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:
@@ -55,6 +62,18 @@ class HexCamRGB(HexCamBase):
         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(
@@ -66,6 +85,11 @@ class HexCamRGB(HexCamBase):
             (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
@@ -75,13 +99,13 @@ class HexCamRGB(HexCamBase):
         self._wait_for_working()
         return self.__intri
 
-    def work_loop(self, hex_values: list[HexSafeValue | threading.Event]):
-        rgb_value = hex_values[0]
-        depth_value = hex_values[1]
-        stop_event = hex_values[2]
+    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_value.set((hex_zmq_ts_now(), 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)
@@ -91,7 +115,9 @@ class HexCamRGB(HexCamBase):
 
             # collect rgb frame
             if ret:
-                rgb_value.set((hex_zmq_ts_now(), rgb_count, frame))
+                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()

{hex_zmq_servers-0.3.3 → hex_zmq_servers-0.3.4}/hex_zmq_servers/cam/rgb/cam_rgb_cli.py

@@ -7,6 +7,7 @@
 ################################################################
 
 from ..cam_base import HexCamClientBase
+from ...zmq_base import HexRate
 
 NET_CONFIG = {
     "ip": "127.0.0.1",
@@ -24,7 +25,17 @@ class HexCamRGBClient(HexCamClientBase):
         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-0.3.3 → hex_zmq_servers-0.3.4}/hex_zmq_servers/config/cam_rgb.json

@@ -12,6 +12,14 @@
             640,
             480
         ],
+        "crop": [
+            0,
+            640,
+            0,
+            480
+        ],
+        "exposure": 70,
+        "temperature": 0,
         "frame_rate": 30,
         "sens_ts": true
     }

{hex_zmq_servers-0.3.3 → hex_zmq_servers-0.3.4}/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": [

{hex_zmq_servers-0.3.3 → hex_zmq_servers-0.3.4}/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": [

{hex_zmq_servers-0.3.3 → hex_zmq_servers-0.3.4}/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-0.3.3 → hex_zmq_servers-0.3.4}/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-0.3.3 → hex_zmq_servers-0.3.4}/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-0.3.3 → hex_zmq_servers-0.3.4}/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],
@@ -124,6 +126,9 @@ class HexMujocoArcherY6(HexMujocoBase):
         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()
 
@@ -139,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
@@ -159,10 +164,10 @@ class HexMujocoArcherY6(HexMujocoBase):
         states_trig_count = 0
         img_trig_count = 0
         init_ts = self.__mujoco_ts() if self.__sens_ts else hex_zmq_ts_now()
-        rgb_value.set((init_ts, 0,
+        rgb_queue.append((init_ts, 0,
                        np.zeros((self.__height, self.__width, 3),
                                 dtype=np.uint8)))
-        depth_value.set((init_ts, 0,
+        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():
@@ -173,23 +178,27 @@ 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_robot_get = cmds_robot_pack
                     if seq != last_cmds_robot_seq:
@@ -207,14 +216,14 @@ class HexMujocoArcherY6(HexMujocoBase):
                 if self.__use_rgb:
                     ts, rgb_img = self.__get_rgb()
                     if rgb_img is not None:
-                        rgb_value.set((ts, rgb_count, rgb_img))
+                        rgb_queue.append((ts, rgb_count, rgb_img))
                         rgb_count = (rgb_count + 1) % self._max_seq_num
 
                 # depth
                 if self.__use_depth:
                     ts, depth_img = self.__get_depth()
                     if depth_img is not None:
-                        depth_value.set((ts, depth_count, depth_img))
+                        depth_queue.append((ts, depth_count, depth_img))
                         depth_count = (depth_count + 1) % self._max_seq_num
 
             # mujoco step
@@ -299,11 +308,8 @@ 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():