35 lines
1.3 KiB
Python
35 lines
1.3 KiB
Python
'''
|
|
Author: liu xiaoyan
|
|
Date: 2023-12-01 14:32:04
|
|
LastEditors: WZX 17839623189@168.com
|
|
LastEditTime: 2023-12-05 14:02:25
|
|
FilePath: /wzx/zkmetaapi/ZKMetaUnit/utils/mqtt_msg_decode.py
|
|
'''
|
|
import binascii
|
|
import struct
|
|
import math
|
|
|
|
def calcu_distance(point_a, point_b):
|
|
return math.sqrt(math.pow(point_a[0]-point_b[0], 2)+math.pow(point_a[1]-point_b[1], 2))
|
|
|
|
def calcu_vecloty(a, b):
|
|
return math.sqrt(math.pow(a, 2) + math.pow(b, 2))
|
|
|
|
def parse_robot_state(payload):
|
|
msg_payload = binascii.hexlify(payload)
|
|
data_bytes = bytes.fromhex(msg_payload.decode())
|
|
rpy = [struct.unpack_from('<h', data_bytes, offset=2*t)[0] for t in range(3)]
|
|
motorTotations = [struct.unpack_from('<h', data_bytes, offset=2*t+6)[0] for t in range(12)]
|
|
position = [round(struct.unpack_from('<f', data_bytes, offset=4*t+52)[0], 3) for t in range(3)]
|
|
bodyHeight = round(struct.unpack_from('<f', data_bytes, offset=64)[0], 4)
|
|
velocity = [round(struct.unpack_from('<f', data_bytes, offset=4*t+68)[0], 3) for t in range(3)]
|
|
return{
|
|
"rpy": rpy,
|
|
"forward_left": motorTotations[3:6],
|
|
"forward_right": motorTotations[0:3],
|
|
"back_left": motorTotations[9:],
|
|
"back_right": motorTotations[6:9],
|
|
"position": position,
|
|
"bodyHeight": bodyHeight,
|
|
"velocity": velocity
|
|
} |