diff --git a/Root.tscn b/Root.tscn index 2b2de5b..ddc3977 100644 --- a/Root.tscn +++ b/Root.tscn @@ -1,13 +1,10 @@ -[gd_scene load_steps=3 format=3 uid="uid://bj5ykdjle10tt"] +[gd_scene load_steps=2 format=3 uid="uid://bj5ykdjle10tt"] [ext_resource type="Script" uid="uid://vnywsf0rn1ax" path="res://SocketClient.gd" id="1_gxo8o"] -[ext_resource type="Script" uid="uid://dmq3i7qmo1qe0" path="res://VRPN.gd" id="2_24d08"] [node name="Node3D" type="Node3D"] script = ExtResource("1_gxo8o") - -[node name="VRPN" type="Node3D" parent="."] -script = ExtResource("2_24d08") +vrpn_server = "212.201.64.122" [connection signal="connected" from="." to="." method="_on_connected"] [connection signal="data" from="." to="." method="_on_data"] diff --git a/SocketClient.gd b/SocketClient.gd index 92f3ea3..d75c83d 100644 --- a/SocketClient.gd +++ b/SocketClient.gd @@ -5,15 +5,12 @@ signal data(data:Array) signal disconnected signal error -# Graphics Interaction Lab OptiTrack system @212.201.64.122 @onready var _stream: StreamPeerTCP = StreamPeerTCP.new() -@export var vrpn_server : String = "127.0.0.1" +@export var vrpn_server : String = "localhost" @export var vrpn_port : int = 3883 -var session : VRPN.Session = VRPN.Session.new() - func _ready() -> void: self.connect_to_host(vrpn_server,vrpn_port) @@ -70,7 +67,7 @@ func _on_data(data : Array): # kaboom we just send back the same cookie :) self.send(bytes) else: - VRPN.marshall_block(bytes,session) + VRPN.marshall_block(bytes) diff --git a/VRPN.gd b/VRPN.gd index fcc0de2..4426d80 100644 --- a/VRPN.gd +++ b/VRPN.gd @@ -1,19 +1,11 @@ extends Node -class_name VRPN -enum TrackingData { POS_QUAT, VELOCITY } +class_name VRPN const magic_cookie_start : String = "vrpn: ver." -class Session: - var sensors : Dictionary[int,String] = {} - var messages : Dictionary[int,String] = {} - func _init(): - pass -@export - -static func marshall_block(data : PackedByteArray,session : Session) -> void: +static func marshall_block(data : PackedByteArray) -> void: # need to fix that var block_offset : int = 0 @@ -35,7 +27,7 @@ static func marshall_block(data : PackedByteArray,session : Session) -> void: 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 false: + if true: print("length '%d'" % length) print("time_sec '%d'" % time_sec) print("time_msec '%d'" % time_msec) @@ -44,46 +36,31 @@ static func marshall_block(data : PackedByteArray,session : Session) -> void: 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])) - marshall_body(data.slice(block_offset+header_size,block_offset+length),message_type,sender_id,session) + marshall_body(data.slice(block_offset+header_size,block_offset+length),message_type) # next datablock block_offset += aligned_size(length) -static func decode_string(stream : StreamPeerBuffer) -> String: - var len = stream.get_32() - return stream.get_string(len) - -static func marshall_body(data : PackedByteArray,message_type : int, sender_id: int, session : Session): +static func marshall_body(data : PackedByteArray,message_type : int): var body := StreamPeerBuffer.new() body.data_array = data body.big_endian = true - - # only take message_type directly for negative (-1,-2) - # messages that provide dynamic descriptors - if message_type < 0: - # message and sender descriptions - match message_type: - -1: - var name = decode_string(body) - print("sensor name is '%s' with '%d" % [name,sender_id]) - session.sensors[sender_id] = name - -2: - var name = decode_string(body) - print("message name is '%s' for message_type '%d'" % [name,sender_id]) - session.messages[sender_id] = name - # nothing to decode - return - - # now we use the string identifiers - # because they are supposedly dynamically assigned - match session.messages[message_type]: - 'vrpn_Tracker Pos_Quat': # quat pos + # 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() # padding + 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() @@ -91,10 +68,9 @@ static func marshall_body(data : PackedByteArray,message_type : int, sender_id: var quat_y = body.get_double() var quat_z = body.get_double() var quat = Quaternion(quat_x,quat_y,quat_z,quat_w) - print("Sender:{0} Sensor:{1} Pos:{2} Quat:{3}".format([session.sensors[sender_id],sensor_id,pos,quat])) + print("sensor id {0} {1} {2}".format([sensor_id,pos,quat])) _: - pass - #print("unhandled message type {0}".format([message_type])) + print("unhandled message type {0}".format([message_type])) static func aligned_size(actual_size : int, alignment : int = 8) -> int: