diff --git a/example/test_crc.py b/example/test_crc.py new file mode 100644 index 0000000..e4cf992 --- /dev/null +++ b/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) diff --git a/unitree_sdk2py/utils/crc.py b/unitree_sdk2py/utils/crc.py index 3ab5650..93e4651 100644 --- a/unitree_sdk2py/utils/crc.py +++ b/unitree_sdk2py/utils/crc.py @@ -8,6 +8,9 @@ from ..idl.unitree_go.msg.dds_ import LowState_ 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 @@ class CRC(Singleton): #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 @@ class CRC(Singleton): return calcData - def __Crc32(self, data): + def _crc_py(self, data): bit = 0 crc = 0xFFFFFFFF polynomial = 0x04c11db7 @@ -199,3 +214,15 @@ class CRC(Singleton): 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) diff --git a/unitree_sdk2py/utils/lib/crc_aarch64.so b/unitree_sdk2py/utils/lib/crc_aarch64.so new file mode 100644 index 0000000..878df74 Binary files /dev/null and b/unitree_sdk2py/utils/lib/crc_aarch64.so differ diff --git a/unitree_sdk2py/utils/lib/crc_amd64.so b/unitree_sdk2py/utils/lib/crc_amd64.so new file mode 100644 index 0000000..258055d Binary files /dev/null and b/unitree_sdk2py/utils/lib/crc_amd64.so differ