diff --git a/Root.tscn b/Root.tscn index 2b2de5b..7b125c8 100644 --- a/Root.tscn +++ b/Root.tscn @@ -1,15 +1,12 @@ -[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") -[connection signal="connected" from="." to="." method="_on_connected"] -[connection signal="data" from="." to="." method="_on_data"] -[connection signal="disconnected" from="." to="." method="_on_disconnected"] -[connection signal="error" from="." to="." method="_on_error"] +[node name="RB1" type="Node3D" parent="VRPN"] + +[node name="RB2" type="Node3D" parent="VRPN"] diff --git a/SocketClient.gd b/SocketClient.gd deleted file mode 100644 index 92f3ea3..0000000 --- a/SocketClient.gd +++ /dev/null @@ -1,85 +0,0 @@ -extends Node - -signal connected(s:StreamPeerTCP) -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_port : int = 3883 - -var session : VRPN.Session = VRPN.Session.new() - -func _ready() -> void: - self.connect_to_host(vrpn_server,vrpn_port) - -func _process(delta: float) -> void: - var old_status = _stream.get_status() - _stream.poll() - var new_status = _stream.get_status() - if old_status != new_status: - match new_status: - _stream.STATUS_NONE: - emit_signal("disconnected") - _stream.STATUS_CONNECTING: - print("Connecting.") - _stream.STATUS_CONNECTED: - print("Connected.") - emit_signal("connected",_stream.poll() -) - _stream.STATUS_ERROR: - print("Error with socket stream.") - emit_signal("error") - - if new_status == _stream.STATUS_CONNECTED: - var available_bytes: int = _stream.get_available_bytes() - if available_bytes > 0: - var res = _stream.get_partial_data(available_bytes) - if res[0] != OK: - emit_signal("error") - else: - emit_signal("data", res[1]) - -func connect_to_host(host: String, port: int) -> void: - print("Connecting to %s:%d" % [host, port]) - if _stream.connect_to_host(host, port) != OK: - print("Error connecting to host.") - emit_signal("error") - -func send(data: PackedByteArray) -> bool: - if _stream.get_status() != _stream.STATUS_CONNECTED: - print("Error: Stream is not currently connected.") - return false - var error: int = _stream.put_data(data) - if error != OK: - print("Error writing to stream: ", error) - return false - return true - - -func _on_data(data : Array): - var bytes = PackedByteArray(data) - var as_cookie = bytes.get_string_from_ascii() - - # Cookie Hack! - if as_cookie.begins_with(VRPN.magic_cookie_start): # - # kaboom we just send back the same cookie :) - self.send(bytes) - else: - VRPN.marshall_block(bytes,session) - - - -func _on_connected(s : StreamPeerTCP): - print("Connected to",s.get_connected_host()) # Replace with function body. - -func _on_disconnected(): - print("Disconnected") # Replace with function body. - -func _on_error(): - print("Error") # Replace with function body. - diff --git a/SocketClient.gd.uid b/SocketClient.gd.uid deleted file mode 100644 index 2e69597..0000000 --- a/SocketClient.gd.uid +++ /dev/null @@ -1 +0,0 @@ -uid://vnywsf0rn1ax diff --git a/VRPN.gd b/VRPN.gd index fcc0de2..5262e89 100644 --- a/VRPN.gd +++ b/VRPN.gd @@ -1,24 +1,117 @@ extends Node class_name VRPN -enum TrackingData { POS_QUAT, VELOCITY } +# tracking associated data +enum TrackingData { POS_QUAT, VELOCITY, ACCELERATION } +# magic cookie const magic_cookie_start : String = "vrpn: ver." +# kinda redundant as we take the seq number as well +static var header_size : int = aligned_size(20) -class Session: - var sensors : Dictionary[int,String] = {} - var messages : Dictionary[int,String] = {} - func _init(): - pass -@export +var sensors : Dictionary[int,String] = {} +var messages : Dictionary[int,String] = {} -static func marshall_block(data : PackedByteArray,session : Session) -> void: +signal connected(s:StreamPeerTCP) +signal data(data:Array) +signal disconnected +signal error + +@export var tracker_targets : Dictionary[String,Node3D] = {} + + +# 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_port : int = 3883 + +func _ready() -> void: + + self.connect_to_host(vrpn_server,vrpn_port) + + if not connected.has_connections(): + connected.connect(self._on_connected) + if not data.has_connections(): + data.connect(self._on_data) + if not disconnected.has_connections(): + disconnected.connect(self._on_disconnected) + if not error.has_connections(): + error.connect(self._on_error) + +func _process(delta: float) -> void: + var old_status = _stream.get_status() + _stream.poll() + var new_status = _stream.get_status() + if old_status != new_status: + match new_status: + _stream.STATUS_NONE: + emit_signal("disconnected") + _stream.STATUS_CONNECTING: + print("Connecting.") + _stream.STATUS_CONNECTED: + print("Connected.") + emit_signal("connected",_stream.poll() +) + _stream.STATUS_ERROR: + print("Error with socket stream.") + emit_signal("error") + + if new_status == _stream.STATUS_CONNECTED: + var available_bytes: int = _stream.get_available_bytes() + if available_bytes > 0: + var res = _stream.get_partial_data(available_bytes) + if res[0] != OK: + emit_signal("error") + else: + emit_signal("data", res[1]) + +func connect_to_host(host: String, port: int) -> void: + print("Connecting to %s:%d" % [host, port]) + if _stream.connect_to_host(host, port) != OK: + print("Error connecting to host.") + emit_signal("error") + +func send(data: PackedByteArray) -> bool: + if _stream.get_status() != _stream.STATUS_CONNECTED: + print("Error: Stream is not currently connected.") + return false + var error: int = _stream.put_data(data) + if error != OK: + print("Error writing to stream: ", error) + return false + return true + + +func _on_data(data : Array): + var bytes = PackedByteArray(data) + var as_cookie = bytes.get_string_from_ascii() + + # Cookie Hack! + if as_cookie.begins_with(VRPN.magic_cookie_start): # + # kaboom we just send back the same cookie :) + self.send(bytes) + else: + VRPN.marshall_block(bytes,self) + + +func _on_connected(s : StreamPeerTCP): + print("Connected to",s.get_connected_host()) # Replace with function body. + +func _on_disconnected(): + print("Disconnected") # Replace with function body. + +func _on_error(): + print("Error") # Replace with function body. + + +static func marshall_block(data : PackedByteArray,session : VRPN) -> 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() @@ -43,9 +136,6 @@ static func marshall_block(data : PackedByteArray,session : Session) -> void: 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,sender_id,session) # next datablock @@ -56,7 +146,7 @@ static func decode_string(stream : StreamPeerBuffer) -> String: 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, sender_id: int, session : VRPN): var body := StreamPeerBuffer.new() body.data_array = data body.big_endian = true @@ -66,15 +156,14 @@ static func marshall_body(data : PackedByteArray,message_type : int, sender_id: if message_type < 0: # message and sender descriptions match message_type: - -1: + -1: # sensor names var name = decode_string(body) print("sensor name is '%s' with '%d" % [name,sender_id]) session.sensors[sender_id] = name - -2: + -2: # message names 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 @@ -91,6 +180,8 @@ 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])) _: pass diff --git a/uvrpn.gd b/uvrpn.gd deleted file mode 100644 index 4267cde..0000000 --- a/uvrpn.gd +++ /dev/null @@ -1,50 +0,0 @@ -extends Node - -@export var url : String = "localhost" -@export var port : int = 3883 - -var vrpn_cookie : String = "vrpn: ver. 07.35" - -#var server : UDPServer = null -@onready var socket : StreamPeerTCP = StreamPeerTCP.new() - -func _ready() -> void: - #if StreamPeerTCP.STATUS_NONE == socket.get_status(): - if socket.connect_to_host(url,port) == OK: - print("Socket connected ...") - socket.set_no_delay(true) - send_data(socket,vrpn_cookie.to_utf8_buffer()) - #server = UDPServer.new() - #server.listen(3883) - else: - print("Error connecting to server") - #else: - #print("Stream not ready") - - -func _process(delta: float) -> void: - if socket: - socket.poll() - ##return - ##if server: - ##server.poll() - ##if server.is_connection_available(): - ##var peer = server.take_connection() - ##var packet = peer.get_packet() - ##print("Accepted peer: %s:%s" % [peer.get_packet_ip(), peer.get_packet_port()]) - ##print("Received data: %s" % [packet.get_string_from_utf8()]) - ### Reply so it knows we received the message. - ##peer.put_packet(packet) - - -static func send_data(socket : StreamPeerTCP, data: PackedByteArray) -> bool: - print(socket.get_status()) - if socket.get_status() == StreamPeerTCP.STATUS_CONNECTED: - if socket.put_data(data) == OK: - return true - else: - print("Error writing data ...") - return false - else: - print("Error connecting") - return false diff --git a/uvrpn.gd.uid b/uvrpn.gd.uid deleted file mode 100644 index 9ba71af..0000000 --- a/uvrpn.gd.uid +++ /dev/null @@ -1 +0,0 @@ -uid://ca5psnjx63ua8