diff --git a/SocketClient.gd b/SocketClient.gd index c377dfb..ae12224 100644 --- a/SocketClient.gd +++ b/SocketClient.gd @@ -59,11 +59,11 @@ func _on_data(data : Array): var as_cookie = bytes.get_string_from_ascii() # Cookie Hack! - if as_cookie.begins_with("vrpn: ver."): + if as_cookie.begins_with(VRPN.magic_cookie_start): # # kaboom we just send back the same cookie :) self.send(bytes) else: - VRPN.marshal(bytes) + VRPN.marshall_block(bytes) diff --git a/VRPN.gd b/VRPN.gd index 1f22af4..250c096 100644 --- a/VRPN.gd +++ b/VRPN.gd @@ -2,7 +2,81 @@ extends Node class_name VRPN -static func marshal(data : PackedByteArray): +const magic_cookie_start : String = "vrpn: ver." + + +static func marshall_block(data : PackedByteArray) -> void: + + # need to fix that + var block_offset : int = 0 + var header_size = aligned_size(20) # kinda redundant as we take the seq number as well + + while data.size() > block_offset: + # reader for stream + var header := StreamPeerBuffer.new() + # get block addresses + header.data_array = data.slice(block_offset,block_offset+header_size) + # make sure we read as big endian + header.big_endian = true + + # read header + var length := header.get_32() as int # length of message + var time_sec := header.get_32() as int # datetime sec + var time_msec := header.get_32() as int # datetime micro sec + var sender_id := header.get_32() as int # sender id + var message_type := header.get_32() as int # type of message (payload) + var sequence_num := header.get_32() as int # inofficial sequence number (padding) + + if true: + print("length '%d'" % length) + print("time_sec '%d'" % time_sec) + print("time_msec '%d'" % time_msec) + print("sender_id '%d'" % sender_id) + print("message_type '%d'" % message_type) + print("sequence_num '%d'" % sequence_num) + + # print + print("block_offset:{0} header_size:{1} length:{2}".format([block_offset,header_size,length])) + + marshall_body(data.slice(block_offset+header_size,block_offset+length),message_type) + + # next datablock + block_offset += aligned_size(length) + + +static func marshall_body(data : PackedByteArray,message_type : int): + var body := StreamPeerBuffer.new() + body.data_array = data + body.big_endian = true + # sender description + match message_type: + -1,-2: + # get length of string + var body_length = body.get_32() + # get string + var sender_name = body.get_string(body_length) + print("sender name is %s:" % sender_name) + #print(body.data_array) + 4: # quat pos + # get id + var sensor_id = body.get_32() + var padding = body.get_32() + var pos = Vector3(body.get_double(),body.get_double(),body.get_double()) + # VRPN quaternions are w,xyz + var quat_w = body.get_double() + var quat_x = body.get_double() + var quat_y = body.get_double() + var quat_z = body.get_double() + var quat = Quaternion(quat_x,quat_y,quat_z,quat_w) + print("sensor id {0} {1} {2}".format([sensor_id,pos,quat])) + _: + print("unhandled message type {0}".format([message_type])) + pass + +static func marshal(data : PackedByteArray) -> void: + + # actual size + print("Data size %d " % data.size()) # need to fix that var block_offset : int = 0 @@ -31,8 +105,12 @@ static func marshal(data : PackedByteArray): print("message_type '%d'" % message_type) print("sequence_num '%d'" % sequence_num) + + # print - #print("block_offset:{0} header_size:{1} length:{2}".format([block_offset,header_size,length])) + print("block_offset:{0} header_size:{1} length:{2}".format([block_offset,header_size,length])) + + # sender description match message_type: @@ -44,7 +122,8 @@ static func marshal(data : PackedByteArray): var body_length = body.get_32() # get string var sender_name = body.get_string(body_length) - #print("sender id {0} is '{1}'".format([sender_id,sender_name])) + print("sender id {0} is '{1}'".format([sender_id,sender_name])) + #print(body.data_array) 4: # quat pos # set body var body := StreamPeerBuffer.new() @@ -66,5 +145,5 @@ static func marshal(data : PackedByteArray): print("message id {0}".format([message_type])) -static func aligned_size(actual_size : int,alignment : int = 8) -> int: +static func aligned_size(actual_size : int, alignment : int = 8) -> int: return (actual_size + alignment - 1) & ~(alignment - 1)