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

all pieces are there - now refactoring into something usable

Browse source
This commit is contained in:
Hartmut Seichter 2025-06-30 19:56:15 +02:00
parent de9a4990e5
commit d3b5dbe322
3 changed files with 47 additions and 21 deletions
Download patch file Download diff file Expand all files Collapse all files

1
Root.tscn
View file

@ -4,7 +4,6 @@
[node name="Node3D" type="Node3D"] [node name="Node3D" type="Node3D"]
script = ExtResource("1_gxo8o") script = ExtResource("1_gxo8o")
vrpn_server = "212.201.64.122"
[connection signal="connected" from="." to="." method="_on_connected"] [connection signal="connected" from="." to="." method="_on_connected"]
[connection signal="data" from="." to="." method="_on_data"] [connection signal="data" from="." to="." method="_on_data"]

7
SocketClient.gd
View file

@ -5,12 +5,15 @@ signal data(data:Array)
signal disconnected signal disconnected
signal error signal error
# Graphics Interaction Lab OptiTrack system @212.201.64.122
@onready var _stream: StreamPeerTCP = StreamPeerTCP.new() @onready var _stream: StreamPeerTCP = StreamPeerTCP.new()
@export var vrpn_server : String = "localhost" @export var vrpn_server : String = "127.0.0.1"
@export var vrpn_port : int = 3883 @export var vrpn_port : int = 3883
var session : VRPN.Session = VRPN.Session.new()
func _ready() -> void: func _ready() -> void:
self.connect_to_host(vrpn_server,vrpn_port) self.connect_to_host(vrpn_server,vrpn_port)
@ -67,7 +70,7 @@ func _on_data(data : Array):
# kaboom we just send back the same cookie :) # kaboom we just send back the same cookie :)
self.send(bytes) self.send(bytes)
else: else:
VRPN.marshall_block(bytes) VRPN.marshall_block(bytes,session)

60
VRPN.gd
View file

@ -2,10 +2,18 @@ extends Node
class_name VRPN class_name VRPN
enum { POS_QUAT }
const magic_cookie_start : String = "vrpn: ver." const magic_cookie_start : String = "vrpn: ver."
class Session:
var sensors : Dictionary[int,String] = {}
var messages : Dictionary[int,String] = {}
func _init():
pass
static func marshall_block(data : PackedByteArray) -> void:
static func marshall_block(data : PackedByteArray,session : Session) -> void:
# need to fix that # need to fix that
var block_offset : int = 0 var block_offset : int = 0
@ -27,7 +35,7 @@ static func marshall_block(data : PackedByteArray) -> void:
var message_type := header.get_32() as int # type of message (payload) var message_type := header.get_32() as int # type of message (payload)
var sequence_num := header.get_32() as int # inofficial sequence number (padding) var sequence_num := header.get_32() as int # inofficial sequence number (padding)
if true: if false:
print("length '%d'" % length) print("length '%d'" % length)
print("time_sec '%d'" % time_sec) print("time_sec '%d'" % time_sec)
print("time_msec '%d'" % time_msec) print("time_msec '%d'" % time_msec)
@ -36,31 +44,46 @@ static func marshall_block(data : PackedByteArray) -> void:
print("sequence_num '%d'" % sequence_num) print("sequence_num '%d'" % sequence_num)
# print # 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) marshall_body(data.slice(block_offset+header_size,block_offset+length),message_type,sender_id,session)
# next datablock # next datablock
block_offset += aligned_size(length) 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): static func marshall_body(data : PackedByteArray,message_type : int, sender_id: int, session : Session):
var body := StreamPeerBuffer.new() var body := StreamPeerBuffer.new()
body.data_array = data body.data_array = data
body.big_endian = true body.big_endian = true
# sender description
match message_type: # only take message_type directly for negative (-1,-2)
-1,-2: # messages that provide dynamic descriptors
# get length of string if message_type < 0:
var body_length = body.get_32() # message and sender descriptions
# get string match message_type:
var sender_name = body.get_string(body_length) -1:
print("sender name is %s:" % sender_name) var name = decode_string(body)
#print(body.data_array) print("sensor name is '%s' with '%d" % [name,sender_id])
4: # quat pos 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
# get id # get id
var sensor_id = body.get_32() var sensor_id = body.get_32()
var padding = body.get_32() var padding = body.get_32() # padding
var pos = Vector3(body.get_double(),body.get_double(),body.get_double()) var pos = Vector3(body.get_double(),body.get_double(),body.get_double())
# VRPN quaternions are w,xyz # VRPN quaternions are w,xyz
var quat_w = body.get_double() var quat_w = body.get_double()
@ -68,9 +91,10 @@ static func marshall_body(data : PackedByteArray,message_type : int):
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("sensor id {0} {1} {2}".format([sensor_id,pos,quat])) print("Tracker:{0} Sensor:{1} Pos:{2} Quat:{3}".format([session.sensors[sender_id],sensor_id,pos,quat]))
_: _:
print("unhandled message type {0}".format([message_type])) pass
#print("unhandled message type {0}".format([message_type]))
static func aligned_size(actual_size : int, alignment : int = 8) -> int: static func aligned_size(actual_size : int, alignment : int = 8) -> int: