MVP with all VRPN and network code in one implementation
This commit is contained in:
Hartmut Seichter
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"]
|
||||
|
||||
[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"]
|
||||
|
|
|
|||
|
|
@ -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
|
||||
125
VRPN.gd
125
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
|
||||
|
|
|
|||
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