| import struct |
| import cyclonedds |
| import cyclonedds.idl as idl |
|
|
| from .singleton import Singleton |
| from ..idl.unitree_go.msg.dds_ import LowCmd_ |
| 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): |
| |
| |
| self.__packFmtLowCmd = '<4B4IH2x' + 'B3x5f3I' * 20 + '4B' + '55Bx2I' |
| |
| self.__packFmtLowState = '<4B4IH2x' + '13fb3x' + 'B3x7fb3x3I' * 20 + '4BiH4b15H' + '8hI41B3xf2b2x2f4h2I' |
| |
| self.__packFmtHGLowCmd = '<2B2x' + 'B3x5fI' * 35 + '5I' |
| |
| 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_aarch64.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)) |
| elif msg.__idl_typename__ == 'unitree_go.msg.dds_.LowState_': |
| return self.__Crc32(self.__PackLowState(msg)) |
| if msg.__idl_typename__ == 'unitree_hg.msg.dds_.LowCmd_': |
| return self.__Crc32(self.__PackHGLowCmd(msg)) |
| elif msg.__idl_typename__ == 'unitree_hg.msg.dds_.LowState_': |
| return self.__Crc32(self.__PackHGLowState(msg)) |
| else: |
| raise TypeError('unknown IDL message type to crc') |
|
|
| def __PackLowCmd(self, cmd: LowCmd_): |
| origData = [] |
| origData.extend(cmd.head) |
| origData.append(cmd.level_flag) |
| origData.append(cmd.frame_reserve) |
| origData.extend(cmd.sn) |
| origData.extend(cmd.version) |
| origData.append(cmd.bandwidth) |
|
|
| for i in range(20): |
| origData.append(cmd.motor_cmd[i].mode) |
| origData.append(cmd.motor_cmd[i].q) |
| origData.append(cmd.motor_cmd[i].dq) |
| origData.append(cmd.motor_cmd[i].tau) |
| origData.append(cmd.motor_cmd[i].kp) |
| origData.append(cmd.motor_cmd[i].kd) |
| origData.extend(cmd.motor_cmd[i].reserve) |
|
|
| origData.append(cmd.bms_cmd.off) |
| origData.extend(cmd.bms_cmd.reserve) |
|
|
| origData.extend(cmd.wireless_remote) |
| origData.extend(cmd.led) |
| origData.extend(cmd.fan) |
| origData.append(cmd.gpio) |
| origData.append(cmd.reserve) |
| origData.append(cmd.crc) |
|
|
| return self.__Trans(struct.pack(self.__packFmtLowCmd, *origData)) |
|
|
| def __PackLowState(self, state: LowState_): |
| origData = [] |
| origData.extend(state.head) |
| origData.append(state.level_flag) |
| origData.append(state.frame_reserve) |
| origData.extend(state.sn) |
| origData.extend(state.version) |
| origData.append(state.bandwidth) |
| |
| origData.extend(state.imu_state.quaternion) |
| origData.extend(state.imu_state.gyroscope) |
| origData.extend(state.imu_state.accelerometer) |
| origData.extend(state.imu_state.rpy) |
| origData.append(state.imu_state.temperature) |
| |
| for i in range(20): |
| origData.append(state.motor_state[i].mode) |
| origData.append(state.motor_state[i].q) |
| origData.append(state.motor_state[i].dq) |
| origData.append(state.motor_state[i].ddq) |
| origData.append(state.motor_state[i].tau_est) |
| origData.append(state.motor_state[i].q_raw) |
| origData.append(state.motor_state[i].dq_raw) |
| origData.append(state.motor_state[i].ddq_raw) |
| origData.append(state.motor_state[i].temperature) |
| origData.append(state.motor_state[i].lost) |
| origData.extend(state.motor_state[i].reserve) |
|
|
| origData.append(state.bms_state.version_high) |
| origData.append(state.bms_state.version_low) |
| origData.append(state.bms_state.status) |
| origData.append(state.bms_state.soc) |
| origData.append(state.bms_state.current) |
| origData.append(state.bms_state.cycle) |
| origData.extend(state.bms_state.bq_ntc) |
| origData.extend(state.bms_state.mcu_ntc) |
| origData.extend(state.bms_state.cell_vol) |
| |
| origData.extend(state.foot_force) |
| origData.extend(state.foot_force_est) |
| origData.append(state.tick) |
| origData.extend(state.wireless_remote) |
| origData.append(state.bit_flag) |
| origData.append(state.adc_reel) |
| origData.append(state.temperature_ntc1) |
| origData.append(state.temperature_ntc2) |
| origData.append(state.power_v) |
| origData.append(state.power_a) |
| origData.extend(state.fan_frequency) |
| origData.append(state.reserve) |
| origData.append(state.crc) |
|
|
| return self.__Trans(struct.pack(self.__packFmtLowState, *origData)) |
|
|
| def __PackHGLowCmd(self, cmd: HGLowCmd_): |
| origData = [] |
| origData.append(cmd.mode_pr) |
| origData.append(cmd.mode_machine) |
|
|
| for i in range(35): |
| origData.append(cmd.motor_cmd[i].mode) |
| origData.append(cmd.motor_cmd[i].q) |
| origData.append(cmd.motor_cmd[i].dq) |
| origData.append(cmd.motor_cmd[i].tau) |
| origData.append(cmd.motor_cmd[i].kp) |
| origData.append(cmd.motor_cmd[i].kd) |
| origData.append(cmd.motor_cmd[i].reserve) |
|
|
| origData.extend(cmd.reserve) |
| origData.append(cmd.crc) |
|
|
| return self.__Trans(struct.pack(self.__packFmtHGLowCmd, *origData)) |
|
|
| def __PackHGLowState(self, state: HGLowState_): |
| origData = [] |
| origData.extend(state.version) |
| origData.append(state.mode_pr) |
| origData.append(state.mode_machine) |
| origData.append(state.tick) |
| |
| origData.extend(state.imu_state.quaternion) |
| origData.extend(state.imu_state.gyroscope) |
| origData.extend(state.imu_state.accelerometer) |
| origData.extend(state.imu_state.rpy) |
| origData.append(state.imu_state.temperature) |
| |
| for i in range(35): |
| origData.append(state.motor_state[i].mode) |
| origData.append(state.motor_state[i].q) |
| origData.append(state.motor_state[i].dq) |
| origData.append(state.motor_state[i].ddq) |
| origData.append(state.motor_state[i].tau_est) |
| origData.extend(state.motor_state[i].temperature) |
| origData.append(state.motor_state[i].vol) |
| origData.extend(state.motor_state[i].sensor) |
| origData.append(state.motor_state[i].motorstate) |
| origData.extend(state.motor_state[i].reserve) |
|
|
| origData.extend(state.wireless_remote) |
| origData.extend(state.reserve) |
| origData.append(state.crc) |
|
|
| return self.__Trans(struct.pack(self.__packFmtHGLowState, *origData)) |
|
|
| def __Trans(self, packData): |
| calcData = [] |
| calcLen = ((len(packData)>>2)-1) |
|
|
| for i in range(calcLen): |
| d = ((packData[i*4+3] << 24) | (packData[i*4+2] << 16) | (packData[i*4+1] << 8) | (packData[i*4])) |
| calcData.append(d) |
|
|
| return calcData |
|
|
| def _crc_py(self, data): |
| bit = 0 |
| crc = 0xFFFFFFFF |
| polynomial = 0x04c11db7 |
|
|
| for i in range(len(data)): |
| bit = 1 << 31 |
| current = data[i] |
|
|
| for b in range(32): |
| if crc & 0x80000000: |
| crc = (crc << 1) & 0xFFFFFFFF |
| crc ^= polynomial |
| else: |
| crc = (crc << 1) & 0xFFFFFFFF |
|
|
| if current & bit: |
| crc ^= polynomial |
|
|
| 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) |
|
|
