gdvrpn/VRPN.gd

extends Node
class_name VRPN

# 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)


var sensors : Dictionary[int,String] = {}
var messages : Dictionary[int,String] = {}

signal connected(s:StreamPeerTCP)
signal data(data:Array)
signal disconnected
signal error

@export var tracker_receivers : Array[VRPN_Receiver] = []

@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
	
	while data.size() > block_offset:
		# reader for stream
		var header := StreamPeerBuffer.new()
		# get block addresses
		header.data_array = data.slice(block_offset,block_offset+header_size)
		# make sure we read as big endian
		header.big_endian = true 
		
		# read header
		var length := header.get_32() as int # length of message
		var time_sec := header.get_32() as int  # datetime sec
		var time_msec := header.get_32() as int # datetime micro sec
		var sender_id := header.get_32() as int  # sender id 
		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:
			print("length       '%d'" % length)
			print("time_sec     '%d'" % time_sec)
			print("time_msec    '%d'" % time_msec)
			print("sender_id    '%d'" % sender_id)
			print("message_type '%d'" % message_type)
			print("sequence_num '%d'" % sequence_num)
		
		marshall_body(data.slice(block_offset+header_size,block_offset+length),message_type,sender_id,session)
		
		# 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 : VRPN):
	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: # sensor names
				var name = decode_string(body)
				print("sensor name is '%s' with '%d" % [name,sender_id])
				session.sensors[sender_id] = name
			-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
		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
			var sensor_id = body.get_32()
			var padding = body.get_32() # padding 
			var pos = Vector3(body.get_double(),body.get_double(),body.get_double())
			# VRPN quaternions are w,xyz
			var quat_w = body.get_double()
			var quat_x = body.get_double()
			var quat_y = body.get_double()
			var quat_z = body.get_double()
			#var quat = Quaternion(quat_x,quat_y,quat_z,quat_w).normalized()
			# bug? Documentation of Godot Quaternion c'tor is x,y,x,w
			# but implementation follow classic w,x,y,z order
			var quat = Quaternion(quat_w,quat_x,quat_y,quat_z).normalized()
			
			for r in session.tracker_receivers:
				r._on_pos_quat({
					"tracker" : session.sensors[sender_id],
					"sensor" : sensor_id,
					"position" : pos,
					"rotation" : quat
				})
		_:
			pass
			#print("unhandled message type {0}".format([message_type]))


static func aligned_size(actual_size : int, alignment : int = 8) -> int:
	return (actual_size + alignment - 1) & ~(alignment - 1)