Improve crc efficiency use ctypes

Author: yangning wu

example/test_crc.py

@@ -0,0 +1,46 @@
+import time
+import sys
+
+from unitree_sdk2py.core.channel import ChannelFactoryInitialize
+from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_
+from unitree_sdk2py.utils.crc import CRC
+
+crc = CRC()
+
+PosStopF = 2.146e9
+VelStopF = 16000.0
+
+if __name__ == "__main__":
+    ChannelFactoryInitialize(0)
+
+    cmd = unitree_go_msg_dds__LowCmd_()
+    cmd.head[0] = 0xFE
+    cmd.head[1] = 0xEF
+    cmd.level_flag = 0xFF
+    cmd.gpio = 0
+    for i in range(20):
+        cmd.motor_cmd[i].mode = 0x01
+        cmd.motor_cmd[i].q = PosStopF
+        cmd.motor_cmd[i].kp = 0
+        cmd.motor_cmd[i].dq = VelStopF
+        cmd.motor_cmd[i].kd = 0
+        cmd.motor_cmd[i].tau = 0
+
+    cmd.motor_cmd[0].q = 0.0
+    cmd.motor_cmd[0].kp = 0.0
+    cmd.motor_cmd[0].dq = 0.0
+    cmd.motor_cmd[0].kd = 0.0
+    cmd.motor_cmd[0].tau = 1.0
+
+    cmd.motor_cmd[1].q = 0.0
+    cmd.motor_cmd[1].kp = 10.0
+    cmd.motor_cmd[1].dq = 0.0
+    cmd.motor_cmd[1].kd = 1.0
+    cmd.motor_cmd[1].tau = 0.0
+
+    now = time.perf_counter()
+    
+
+    cmd.crc = crc.Crc(cmd)
+
+    print("CRC:", cmd.crc, "Time cost:", (time.perf_counter() - now)*1000)

unitree_sdk2py/utils/crc.py

@@ -8,6 +8,9 @@
 
 from ..idl.unitree_hg.msg.dds_ import LowCmd_ as HGLowCmd_
 from ..idl.unitree_hg.msg.dds_ import LowState_ as HGLowState_
+import ctypes
+import os
+import platform
 
 class CRC(Singleton):
     def __init__(self):
@@ -21,6 +24,18 @@ def __init__(self):
         #size 2092
         self.__packFmtHGLowState = '<2I2B2xI' + '13fh2x' + 'B3x4f2hf7I' * 35 + '40B5I'
 
+        
+        script_dir = os.path.dirname(os.path.abspath(__file__))
+        self.platform = platform.system()
+        if self.platform == "Linux":
+            if platform.machine()=="x86_64":
+                self.crc_lib = ctypes.CDLL(script_dir + '/lib/crc_amd64.so')
+            elif platform.machine()=="aarch64":
+                self.crc_lib = ctypes.CDLL(script_dir + '/lib/crc_arm64.so')
+
+            self.crc_lib.crc32_core.argtypes = (ctypes.POINTER(ctypes.c_uint32), ctypes.c_uint32)
+            self.crc_lib.crc32_core.restype = ctypes.c_uint32
+    
     def Crc(self, msg: idl.IdlStruct):
         if msg.__idl_typename__ == 'unitree_go.msg.dds_.LowCmd_':
             return self.__Crc32(self.__PackLowCmd(msg))
@@ -177,7 +192,7 @@ def __Trans(self, packData):
 
         return calcData
 
-    def __Crc32(self, data):
+    def _crc_py(self, data):
         bit = 0
         crc = 0xFFFFFFFF
         polynomial = 0x04c11db7
@@ -199,3 +214,15 @@ def __Crc32(self, data):
                 bit >>= 1
 
         return crc
+
+    def _crc_ctypes(self, data):
+        uint32_array = (ctypes.c_uint32 * len(data))(*data)
+        length = len(data)
+        crc=self.crc_lib.crc32_core(uint32_array, length)
+        return crc
+
+    def __Crc32(self, data):
+        if self.platform == "Linux":
+            return self._crc_ctypes(data)
+        else:
+            return self._crc_py(data)