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

MVP with all VRPN and network code in one implementation

Browse source
This commit is contained in:
Hartmut Seichter 2025-06-30 23:25:47 +02:00
parent 1320473e9d
commit d665378c6c
6 changed files with 112 additions and 161 deletions
Download patch file Download diff file Expand all files Collapse all files

125
VRPN.gd
View file

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