Test to decode rokoko UDP data and send it through OSC

rokoko-decode.py

# author: A.J. Loonstra <arnaud@sphaero.org>
# Test to decode rokoko UDP data
#
import struct
import socket
from collections import namedtuple
import liblo
import time

UDP_IP = "0.0.0.0"
UDP_PORT = 7777

def rokoko_decode(data):
    needle = 0
    rokodata = {} #{'productname' : struct.unpack("4s", data[needle:4]) }
    needle = 4
    for x in range(19): # 19 sensors
        d = {}
        sensorid = struct.unpack("8c", data[needle:needle+8])
        needle += 8
        rokodata[sensorid] = rokokosensor._make(struct.unpack('4f9f', bytes(data)[needle:needle+52]))
        needle += 52
        print(sensorid, rokodata[sensorid], needle)
    
    return rokodata

try:
    target = liblo.Address("10.2.4.47", 8888)
except liblo.AddressError as err:
    print(err)
    sys.exit()

sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind((UDP_IP, UDP_PORT))

rokokosensor = namedtuple('rokokosensor', "w x y z gyroX gyroY gyroZ accelX accelY accelZ magX magY magZ") 

idconv = {
"40":"Head",
"a3":"LBack",
"a2":"RBack",
"21":"RShoulder",
"22":"RUArm",
"23":"RFArm",
"24":"RHand",
"61":"LShoulder",
"62":"LUArm",
"63":"LFArm",
"64":"LHand",
"a0":"RHip",
"a1":"LHip",
"01":"RThigh",
"02":"RShin",
"03":"RFoot",
"81":"LThigh",
"82":"LShin",
"83":"LFoot"
}

#data = (67, 53, 48, 0, 160, 0, 0, 0, 84, 28, 92, 191, 94, 138, 2, 191, 192, 40, 218, 188, 167, 77, 122, 63, 253, 47, 48, 62, 157, 74, 179, 57, 187, 207, 245, 61, 249, 105, 118, 186, 1, 214, 73, 57, 193, 1, 172, 184, 254, 173, 229, 63, 50, 129, 138, 191, 200, 239, 37, 191, 0, 0, 0, 0, 1, 0, 0, 0, 65, 61, 40, 63, 214, 228, 180, 61, 20, 160, 63, 63, 220, 193, 8, 63, 136, 191, 198, 58, 130, 41, 109, 62, 46, 33, 80, 191, 148, 200, 35, 186, 180, 144, 50, 185, 35, 26, 212, 57, 32, 126, 218, 63, 197, 55, 141, 191, 196, 211, 38, 191, 0, 0, 0, 0, 129, 0, 0, 0, 7, 198, 183, 62, 4, 137, 177, 190, 32, 215, 93, 63, 66, 217, 109, 60, 20, 3, 104, 63, 8, 117, 214, 62, 187, 57, 95, 61, 92, 82, 85, 186, 92, 158, 10, 58, 193, 86, 198, 56, 108, 222, 240, 63, 179, 206, 135, 191, 107, 244, 36, 191, 0, 0, 0, 0, 2, 0, 0, 0, 4, 167, 166, 190, 186, 79, 94, 191, 47, 134, 191, 62, 53, 128, 11, 63, 65, 204, 234, 190, 12, 135, 96, 62, 0, 182, 42, 191, 142, 170, 99, 185, 107, 255, 86, 185, 122, 247, 101, 185, 122, 251, 167, 63, 132, 5, 117, 191, 13, 130, 246, 190, 0, 0, 0, 0, 130, 0, 0, 0, 251, 202, 47, 191, 132, 94, 42, 63, 47, 195, 149, 62, 63, 113, 53, 60, 155, 112, 155, 189, 35, 182, 51, 191, 227, 65, 53, 63, 200, 202, 192, 57, 159, 105, 250, 185, 184, 133, 242, 57, 18, 217, 196, 63, 215, 59, 68, 191, 135, 81, 45, 191, 0, 0, 0, 0, 3, 0, 0, 0, 238, 104, 12, 191, 220, 3, 49, 191, 191, 184, 240, 62, 202, 141, 184, 190, 221, 178, 208, 62, 53, 208, 139, 190, 218, 22, 75, 63, 55, 46, 160, 185, 240, 138, 37, 58, 167, 68, 245, 56, 252, 114, 139, 63, 49, 95, 83, 191, 238, 253, 20, 190, 0, 0, 0, 0, 131, 0, 0, 0, 105, 185, 6, 63, 155, 210, 16, 63, 88, 132, 34, 191, 65, 122, 211, 189, 53, 115, 63, 62, 152, 235, 29, 191, 62, 237, 65, 63, 8, 27, 153, 56, 218, 248, 19, 186, 187, 134, 170, 57, 63, 234, 191, 63, 161, 178, 66, 191, 103, 110, 130, 190, 0, 0, 0, 0, 161, 0, 0, 0, 9, 40, 42, 62, 152, 88, 116, 63, 152, 168, 125, 190, 193, 158, 125, 63, 166, 44, 8, 62, 90, 97, 147, 188, 8, 217, 182, 60, 155, 103, 29, 186, 17, 240, 20, 185, 106, 225, 131, 57, 113, 123, 232, 63, 74, 102, 149, 191, 43, 250, 45, 191, 0, 0, 0, 0, 162, 0, 0, 0, 113, 29, 82, 191, 203, 196, 17, 191, 224, 192, 60, 189, 51, 0, 126, 63, 127, 43, 189, 61, 134, 237, 19, 189, 177, 234, 154, 189, 55, 211, 18, 58, 16, 238, 149, 185, 132, 205, 22, 186, 168, 35, 234, 63, 141, 250, 181, 191, 99, 189, 63, 191, 0, 0, 0, 0, 163, 0, 0, 0, 108, 67, 155, 190, 206, 243, 104, 191, 119, 207, 144, 62, 151, 180, 50, 63, 99, 171, 226, 62, 204, 84, 15, 63, 96, 213, 104, 189, 169, 95, 204, 184, 112, 196, 55, 58, 244, 36, 21, 58, 88, 128, 229, 63, 182, 98, 213, 191, 67, 232, 75, 191, 0, 0, 0, 0, 64, 0, 0, 0, 4, 38, 200, 62, 168, 242, 82, 63, 7, 249, 209, 62, 231, 71, 30, 60, 87, 2, 11, 63, 132, 243, 86, 63, 254, 152, 194, 187, 169, 9, 139, 185, 137, 255, 244, 57, 251, 48, 58, 58, 53, 169, 222, 63, 54, 141, 220, 191, 138, 27, 89, 191, 0, 0, 0, 0, 33, 0, 0, 0, 189, 121, 85, 191, 82, 215, 189, 62, 79, 83, 209, 62, 208, 64, 75, 63, 143, 174, 133, 189, 43, 130, 3, 60, 82, 186, 26, 191, 255, 108, 235, 185, 128, 29, 72, 186, 254, 192, 107, 57, 248, 53, 225, 63, 109, 180, 212, 191, 15, 101, 76, 191, 0, 0, 0, 0, 97, 0, 0, 0, 255, 183, 71, 191, 62, 252, 145, 190, 204, 140, 14, 191, 195, 43, 5, 63, 174, 93, 121, 61, 175, 254, 76, 61, 225, 179, 89, 63, 145, 35, 37, 58, 119, 170, 114, 57, 253, 27, 7, 58, 89, 202, 233, 63, 169, 17, 214, 191, 2, 100, 75, 191, 0, 0, 0, 0, 34, 0, 0, 0, 34, 170, 56, 63, 200, 233, 16, 190, 240, 142, 45, 191, 165, 46, 111, 63, 223, 163, 111, 62, 193, 94, 137, 62, 191, 189, 146, 60, 239, 247, 21, 185, 229, 204, 43, 183, 205, 61, 144, 57, 210, 187, 204, 63, 152, 103, 218, 191, 47, 68, 75, 191, 0, 0, 0, 0, 98, 0, 0, 0, 20, 150, 172, 190, 163, 23, 109, 191, 232, 72, 45, 190, 61, 200, 17, 62, 13, 232, 7, 63, 194, 225, 82, 63, 217, 82, 14, 62, 176, 160, 15, 58, 44, 38, 2, 58, 68, 107, 40, 58, 36, 29, 252, 63, 89, 127, 204, 191, 249, 86, 85, 191, 0, 0, 0, 0, 35, 0, 0, 0, 35, 208, 145, 190, 80, 105, 98, 63, 135, 76, 189, 62, 181, 59, 62, 63, 54, 18, 156, 62, 182, 14, 44, 62, 42, 80, 18, 191, 231, 19, 221, 183, 73, 0, 183, 185, 61, 130, 9, 57, 7, 66, 201, 63, 180, 251, 251, 191, 228, 254, 108, 191, 0, 0, 0, 0, 99, 0, 0, 0, 122, 188, 181, 190, 177, 120, 65, 191, 212, 225, 12, 63, 34, 60, 25, 189, 239, 42, 106, 63, 120, 180, 176, 62, 213, 203, 83, 190, 38, 254, 25, 186, 52, 82, 76, 185, 228, 237, 93, 185, 10, 154, 220, 63, 52, 129, 219, 191, 201, 7, 114, 191, 0, 0, 0, 0, 36, 0, 0, 0, 140, 2, 52, 63, 12, 218, 240, 190, 236, 125, 8, 191, 198, 135, 90, 63, 77, 114, 224, 190, 242, 77, 48, 62, 152, 208, 99, 190, 125, 5, 77, 185, 80, 152, 41, 186, 232, 114, 15, 58, 228, 235, 168, 63, 81, 185, 0, 192, 33, 186, 126, 191, 0, 0, 0, 0, 100, 0, 0, 0, 79, 109, 120, 191, 173, 78, 98, 190, 80, 13, 199, 189, 208, 66, 85, 61, 98, 36, 122, 63, 146, 169, 66, 62, 224, 189, 163, 189, 168, 52, 172, 55, 236, 247, 57, 186, 142, 233, 237, 185, 130, 173, 199, 63, 123, 184, 193, 191, 21, 174, 99, 191, 0, 0, 0, 0)
#name, sensorID, x, y, z, w, gyroX, gyroY, gyroZ, accelX, accelY, accelZ, magX, magY, magZ = \
#    struct.unpack('4sc4f9f', bytes(data)[:60])
#print( name.decode('utf-8'), sensorID, x, y, z, w, gyroX, gyroY, gyroZ, accelX, accelY, accelZ, magX, magY, magZ )
#rd = rokoko_decode(bytes(data))

while True:
    data, addr = sock.recvfrom(1200) # buffer size is 1024 bytes
    rd = rokoko_decode(data)
    #print(len(data))

    for k,v in rd.items():
        msg = liblo.Message("/{0}".format(idconv[k[0].hex()]))
        msg.add(v.w, v.x, v.y, v.z)
        liblo.send(target, msg)