MVP with all VRPN and network code in one implementation
This commit is contained in:
parent
1320473e9d
commit
d665378c6c
6 changed files with 112 additions and 161 deletions
11
Root.tscn
11
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"]
|
[ext_resource type="Script" uid="uid://dmq3i7qmo1qe0" path="res://VRPN.gd" id="2_24d08"]
|
||||||
|
|
||||||
[node name="Node3D" type="Node3D"]
|
[node name="Node3D" type="Node3D"]
|
||||||
script = ExtResource("1_gxo8o")
|
|
||||||
|
|
||||||
[node name="VRPN" type="Node3D" parent="."]
|
[node name="VRPN" type="Node3D" parent="."]
|
||||||
script = ExtResource("2_24d08")
|
script = ExtResource("2_24d08")
|
||||||
|
|
||||||
[connection signal="connected" from="." to="." method="_on_connected"]
|
[node name="RB1" type="Node3D" parent="VRPN"]
|
||||||
[connection signal="data" from="." to="." method="_on_data"]
|
|
||||||
[connection signal="disconnected" from="." to="." method="_on_disconnected"]
|
[node name="RB2" type="Node3D" parent="VRPN"]
|
||||||
[connection signal="error" from="." to="." method="_on_error"]
|
|
||||||
|
|
|
@ -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.
|
|
||||||
|
|
|
@ -1 +0,0 @@
|
||||||
uid://vnywsf0rn1ax
|
|
119
VRPN.gd
119
VRPN.gd
|
@ -1,23 +1,116 @@
|
||||||
extends Node
|
extends Node
|
||||||
class_name VRPN
|
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."
|
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 sensors : Dictionary[int,String] = {}
|
||||||
var messages : Dictionary[int,String] = {}
|
var messages : Dictionary[int,String] = {}
|
||||||
func _init():
|
|
||||||
pass
|
|
||||||
|
|
||||||
@export
|
signal connected(s:StreamPeerTCP)
|
||||||
|
signal data(data:Array)
|
||||||
|
signal disconnected
|
||||||
|
signal error
|
||||||
|
|
||||||
static func marshall_block(data : PackedByteArray,session : Session) -> void:
|
@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
|
# need to fix that
|
||||||
var block_offset : int = 0
|
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:
|
while data.size() > block_offset:
|
||||||
# reader for stream
|
# reader for stream
|
||||||
|
@ -43,9 +136,6 @@ static func marshall_block(data : PackedByteArray,session : Session) -> void:
|
||||||
print("message_type '%d'" % message_type)
|
print("message_type '%d'" % message_type)
|
||||||
print("sequence_num '%d'" % sequence_num)
|
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)
|
marshall_body(data.slice(block_offset+header_size,block_offset+length),message_type,sender_id,session)
|
||||||
|
|
||||||
# next datablock
|
# next datablock
|
||||||
|
@ -56,7 +146,7 @@ static func decode_string(stream : StreamPeerBuffer) -> String:
|
||||||
return stream.get_string(len)
|
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()
|
var body := StreamPeerBuffer.new()
|
||||||
body.data_array = data
|
body.data_array = data
|
||||||
body.big_endian = true
|
body.big_endian = true
|
||||||
|
@ -66,15 +156,14 @@ static func marshall_body(data : PackedByteArray,message_type : int, sender_id:
|
||||||
if message_type < 0:
|
if message_type < 0:
|
||||||
# message and sender descriptions
|
# message and sender descriptions
|
||||||
match message_type:
|
match message_type:
|
||||||
-1:
|
-1: # sensor names
|
||||||
var name = decode_string(body)
|
var name = decode_string(body)
|
||||||
print("sensor name is '%s' with '%d" % [name,sender_id])
|
print("sensor name is '%s' with '%d" % [name,sender_id])
|
||||||
session.sensors[sender_id] = name
|
session.sensors[sender_id] = name
|
||||||
-2:
|
-2: # message names
|
||||||
var name = decode_string(body)
|
var name = decode_string(body)
|
||||||
print("message name is '%s' for message_type '%d'" % [name,sender_id])
|
print("message name is '%s' for message_type '%d'" % [name,sender_id])
|
||||||
session.messages[sender_id] = name
|
session.messages[sender_id] = name
|
||||||
# nothing to decode
|
|
||||||
return
|
return
|
||||||
|
|
||||||
# now we use the string identifiers
|
# 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_y = body.get_double()
|
||||||
var quat_z = body.get_double()
|
var quat_z = body.get_double()
|
||||||
var quat = Quaternion(quat_x,quat_y,quat_z,quat_w)
|
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("Sender:{0} Sensor:{1} Pos:{2} Quat:{3}".format([session.sensors[sender_id],sensor_id,pos,quat]))
|
||||||
_:
|
_:
|
||||||
pass
|
pass
|
||||||
|
|
50
uvrpn.gd
50
uvrpn.gd
|
@ -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
|
|
|
@ -1 +0,0 @@
|
||||||
uid://ca5psnjx63ua8
|
|
Loading…
Add table
Add a link
Reference in a new issue