diff --git a/SocketClient.gd b/SocketClient.gd index a42a1d2..40996bf 100644 --- a/SocketClient.gd +++ b/SocketClient.gd @@ -63,33 +63,61 @@ func _on_data(data : Array): # kaboom we just send back the same cookie :) self.send(bytes) else: - # non-cookie messages - var peer := StreamPeerBuffer.new() - peer.data_array = bytes.slice(0,24) - peer.big_endian = true + # non-cookie + var header := StreamPeerBuffer.new() + header.data_array = bytes.slice(0,aligned_size(20)) + header.big_endian = true # Header - var length = peer.get_32() # length of message - var time_sec = peer.get_32() # time sec - var time_msec = peer.get_32() # time micro sec - var sender_id = peer.get_32() # sender id - var message_type = peer.get_32() # type of message (payload) - var sequence_num = peer.get_32() # inofficial sequence number (padding) + var length = header.get_32() # length of message + var time_sec = header.get_32() # time sec + var time_msec = header.get_32() # time micro sec + var sender_id = header.get_32() # sender id + var message_type = header.get_32() # type of message (payload) + var sequence_num = header.get_32() # inofficial sequence number (padding) - 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) + if false: + 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(bytes) + + # sender description + if message_type == -1: + var body := StreamPeerBuffer.new() + body.data_array = bytes.slice(24,length-24) + body.big_endian = true + var body_length = body.get_32() + print(body_length) + + #var sender_name = peer.get_string(body_length) + #print("sender id {0} is {1}".format([sender_id,sender_name])) # quat pos if message_type == 4: - peer.data_array = bytes.slice(24) - peer.big_endian = true + var body := StreamPeerBuffer.new() + + body.data_array = bytes.slice(24,length-24) + body.big_endian = true + var sensor_id = body.get_32() + var padding = body.get_32() + var pos = Vector3(body.get_float(),body.get_float(),body.get_float()) + var quat_w = body.get_float() + var quat_x = body.get_float() + var quat_y = body.get_float() + var quat_z = body.get_float() + var quat = Quaternion(quat_x,quat_y,quat_z,quat_w) - - + #print("Pos_Quat ",sensor_id,pos,quat) + + +static func aligned_size(actual_size : int,alignment : int = 8) -> int: + return (actual_size + alignment - 1) & ~(alignment - 1) + func _on_connected(s : StreamPeerTCP): print("Connected to",s.get_connected_host()) # Replace with function body. diff --git a/vrpn.test.cfg b/vrpn.test.cfg index 9d520be..51f8da3 100644 --- a/vrpn.test.cfg +++ b/vrpn.test.cfg @@ -25,7 +25,7 @@ # int number_of_sensors # float rate_at_which_to_report_updates -#vrpn_Tracker_NULL Tracker0 2 2.0 +# vrpn_Tracker_NULL Tracker0 2 2.0 ################################################################################ # Spin Tracker. This is a "device" that reports a spinning rotation at the