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

hex_zmq_servers/cam/berxel/cam_berxel.py

@@ -12,7 +12,7 @@ import numpy as np
 from collections import deque
 
 from ..cam_base import HexCamBase
-from ...zmq_base import hex_zmq_ts_now
+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 *
 
@@ -77,8 +77,21 @@ class HexCamBerxel(HexCamBase):
         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)
@@ -86,13 +99,14 @@ class HexCamBerxel(HexCamBase):
 
             # collect rgb frame
             if hawk_rgb_frame is not None:
-                ts, frame = self.__unpack_frame(hawk_rgb_frame, False)
+                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)
+                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
 
@@ -102,12 +116,17 @@ class HexCamBerxel(HexCamBase):
         # close
         self.close()
 
-    def __unpack_frame(self, hawk_frame: BerxelHawkFrame, depth: bool = False):
+    def __unpack_frame(
+        self,
+        hawk_frame: BerxelHawkFrame,
+        depth: bool = False,
+        bias_ns: int = 0,
+    ):
         # common variables
-        berxel_ts = hawk_frame.getTimeStamp()
+        berxel_ts_ns = bias_ns + int(hawk_frame.getTimeStamp() * 1_000)
         ts = {
-            "s": berxel_ts // 1_000_000,
-            "ns": berxel_ts % 1_000_000 * 1_000,
+            "s": berxel_ts_ns // 1_000_000_000,
+            "ns": berxel_ts_ns % 1_000_000_000,
         }
         width = hawk_frame.getWidth()
         height = hawk_frame.getHeight()
@@ -176,7 +195,8 @@ class HexCamBerxel(HexCamBase):
                     device_idx = idx
                     break
             if device_idx == -1:
-                print(f"can not find device with serial number: {serial_number}")
+                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}")
@@ -196,7 +216,9 @@ class HexCamBerxel(HexCamBase):
         self.__device.setColorExposureGain(self.__exposure, self.__gain)
         self.__device.setRegistrationEnable(True)
         self.__device.setFrameSync(True)
-        self.__device.setSystemClock()
+        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

hex_zmq_servers/hex_launch.py

@@ -124,7 +124,7 @@ class HexNodeConfig():
         launch_path_dict: dict,
     ) -> HexNodeConfig:
         cfg_list = []
-        for launch_name, launch_path in launch_path_dict.items():
+        for launch_name, (launch_path, launch_arg) in launch_path_dict.items():
             node_default_params_dict = launch_default_params_dict.get(
                 launch_name, {})
 
@@ -146,6 +146,7 @@ class HexNodeConfig():
                 HexNodeConfig.get_node_cfgs_from_launch(
                     launch_path,
                     launch_update_cfg,
+                    launch_arg,
                 ))
 
         final_cfg = HexNodeConfig()
@@ -159,6 +160,7 @@ class HexNodeConfig():
     def get_node_cfgs_from_launch(
         launch_path: str,
         params: dict | HexNodeConfig = {},
+        launch_arg: dict | None = None,
     ) -> HexNodeConfig:
         # normalize the path
         launch_path = os.path.abspath(launch_path)
@@ -185,10 +187,13 @@ class HexNodeConfig():
         get_node_cfgs_func = getattr(launch_module, "get_node_cfgs")
         if isinstance(params, dict):
             print("params is dict")
-            node_cfgs = get_node_cfgs_func(params)
+            node_cfgs = get_node_cfgs_func(params, launch_arg)
         elif isinstance(params, HexNodeConfig):
             print("params is HexNodeConfig")
-            node_cfgs = get_node_cfgs_func(params.get_cfgs(use_list=False))
+            node_cfgs = get_node_cfgs_func(
+                params.get_cfgs(use_list=False),
+                launch_arg,
+            )
         else:
             raise ValueError(f"Invalid params: {params}")
         return node_cfgs

hex_zmq_servers/robot/hexarm/robot_hexarm.py

@@ -287,6 +287,5 @@ class HexRobotHexarm(HexRobotBase):
             return
         self._working.clear()
         self.__arm_archer.stop()
-        time.sleep(0.5)
         self.__hex_api.close()
         hex_log(HEX_LOG_LEVEL["info"], "HexRobotHexarm closed")

{hex_zmq_servers-0.3.4.dist-info → hex_zmq_servers-0.3.6.dist-info}/METADATA

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: hex_zmq_servers
-Version: 0.3.4
+Version: 0.3.6
 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.4.dist-info → hex_zmq_servers-0.3.6.dist-info}/RECORD

@@ -1,11 +1,11 @@
 hex_zmq_servers/__init__.py,sha256=1dnB9YTRq5uAFHo_3dMp1t1UxS4ZrcuZ_7CzBwqAfEM,5426
 hex_zmq_servers/device_base.py,sha256=dABpeUg84Au-vtbVzvnv1Tmn_898CNOkIvH2UDUnV3I,1191
-hex_zmq_servers/hex_launch.py,sha256=opS_WwntAzV3UjOCVBo7FaLVW_dP3jlO40q-gFSBy68,17543
+hex_zmq_servers/hex_launch.py,sha256=x-xCBhpjgK6yWHGk5o5kYsb5ef2GsdeLr8rX59EJR-0,17700
 hex_zmq_servers/zmq_base.py,sha256=RFRvl95oci6d-urP5n-qGDh5090pZ5q7Oy8_vNPqJxI,16380
 hex_zmq_servers/cam/__init__.py,sha256=hZ4NTEaO9uxcBua-nyJTpXxmk_GiIAcK2uWz3QQbwvE,1408
 hex_zmq_servers/cam/cam_base.py,sha256=3INQUYzzue4B0EiLEuo8IZDfLTifjmqXpXIkPaNA7dY,5134
 hex_zmq_servers/cam/berxel/__init__.py,sha256=3fK06RWAS_q8a_3z-RxFTv32e99og-4dMZTxH08TOcY,508
-hex_zmq_servers/cam/berxel/cam_berxel.py,sha256=l7SGnMUsM4D8HqHrJM9_QSyvLeNSRSDb37-_tSNHF-c,9035
+hex_zmq_servers/cam/berxel/cam_berxel.py,sha256=yC1-eBZHrwVjyinZyg3AWKmpE7WqNSAbeP10sTWZP24,9820
 hex_zmq_servers/cam/berxel/cam_berxel_cli.py,sha256=ZbtZ4fjO0MCjqstfCxJwH1sMAtHKFKFIg9QI1zxplMQ,811
 hex_zmq_servers/cam/berxel/cam_berxel_srv.py,sha256=8QPhPpRsYnbbIFTQAT3sZA1nRTq1XpKg-GmD6ERUu9w,2311
 hex_zmq_servers/cam/dummy/__init__.py,sha256=EziQ8H8d9Vm1AYw4YryIumTXpx-lwEnlfzTl7VAnKHM,499
@@ -90,7 +90,7 @@ hex_zmq_servers/robot/gello/robot_gello.py,sha256=DyMELg490HykRYdbs4QVluR8oC7KAY
 hex_zmq_servers/robot/gello/robot_gello_cli.py,sha256=wnbRVrpIEE8G4ZzfjuSJA5TMPig-2I1oLJE-6b0kFSY,699
 hex_zmq_servers/robot/gello/robot_gello_srv.py,sha256=vCWgKpsO4MhZLspdaG45uHj8EB0gnptuj2SuxCaoEVI,2841
 hex_zmq_servers/robot/hexarm/__init__.py,sha256=07EUc2fCKqxYZbUUcDeD_AXvCAWI6r1qBjpWJ58_AcY,1414
-hex_zmq_servers/robot/hexarm/robot_hexarm.py,sha256=dUd_6JSHvlEIxkYLF1xCBZTOOcxt_VcfS2qcb-vEkgU,10364
+hex_zmq_servers/robot/hexarm/robot_hexarm.py,sha256=-i8cB-FuW0X3k9MmGujz3MwmZ-usHjq6uiox6ox6bGk,10340
 hex_zmq_servers/robot/hexarm/robot_hexarm_cli.py,sha256=KO-I_CCjWwdckRKlxX7gIxUgsUKW3rcOycBo1nAxFEY,852
 hex_zmq_servers/robot/hexarm/robot_hexarm_srv.py,sha256=PE_WyLRXV_DbYVVQ0Wd60KlZZ37vfoEJVEhf2_4xADQ,2759
 hex_zmq_servers/robot/hexarm/urdf/archer_l6y/empty.urdf,sha256=g7Yn_uIM_ddb_K32FnDgHWSsvxH08oaPDiMb0yUb0Lg,7808
@@ -103,8 +103,8 @@ hex_zmq_servers/robot/hexarm/urdf/archer_y6/gp100.urdf,sha256=i8vpARxlQpYskLmTZ7
 hex_zmq_servers/robot/hexarm/urdf/archer_y6/gp100_handle.urdf,sha256=Tpii1Wk26jRtMqLWI2knrqBTIh7hCjpgGySlrBgdvpA,7458
 hex_zmq_servers/robot/hexarm/urdf/archer_y6/gp100_p050.urdf,sha256=6Zzc_PXDwtxw9xQEXQOH0hu_moaiQzeMC22hUqAQS9s,7459
 hex_zmq_servers/robot/hexarm/urdf/archer_y6/gp100_p050_handle.urdf,sha256=HNa7wweNYUj8YtSUfx87ASeNEIPIjB-gZ4O7RyHng6g,7456
-hex_zmq_servers-0.3.4.dist-info/licenses/LICENSE,sha256=QwcOLU5TJoTeUhuIXzhdCEEDDvorGiC6-3YTOl4TecE,11356
-hex_zmq_servers-0.3.4.dist-info/METADATA,sha256=vlE6l40uTHq955ndiXzA52hwcjhW7CvLfbPxwVx7Y6c,4750
-hex_zmq_servers-0.3.4.dist-info/WHEEL,sha256=_zCd3N1l69ArxyTb8rzEoP9TpbYXkqRFSNOD5OuxnTs,91
-hex_zmq_servers-0.3.4.dist-info/top_level.txt,sha256=lPH1DfgMrQOe0Grh8zSZopf6LmnLvb_aStVmZ41PyAg,16
-hex_zmq_servers-0.3.4.dist-info/RECORD,,
+hex_zmq_servers-0.3.6.dist-info/licenses/LICENSE,sha256=QwcOLU5TJoTeUhuIXzhdCEEDDvorGiC6-3YTOl4TecE,11356
+hex_zmq_servers-0.3.6.dist-info/METADATA,sha256=tAFV1LQtSeqWfvAc2Dr-WsqR7lmHi7Yf8jWF1zreDWk,4750
+hex_zmq_servers-0.3.6.dist-info/WHEEL,sha256=_zCd3N1l69ArxyTb8rzEoP9TpbYXkqRFSNOD5OuxnTs,91
+hex_zmq_servers-0.3.6.dist-info/top_level.txt,sha256=lPH1DfgMrQOe0Grh8zSZopf6LmnLvb_aStVmZ41PyAg,16
+hex_zmq_servers-0.3.6.dist-info/RECORD,,