Hartmut/gdvrpn
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

read whole blocks of data and marshalling to messages

Browse source
This commit is contained in:
Hartmut Seichter 2025-06-27 23:36:10 +02:00
parent 0e6197b5fa
commit 0c43e22d90
2 changed files with 85 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

4
SocketClient.gd
View file

@ -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)

87
VRPN.gd
View file

@ -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)