car-os/networking/scripts/udp_server.gd

# server_node.gd
class_name ServerNode
extends Node

signal camera_position_updated(pos_x, pos_y, pos_z)
signal camera_rotation_updated(rot_x, rot_y, rot_z)
signal sensor_point_generated(pos_x, pos_y, pos_z)

var server = UDPServer.new()
var port = 5001
var awaiting_sensors = {}
var invalid_sensors = {}
var valid_sensors = {}

var use_thread: bool = false
var thread: Thread
var mutex: Mutex
var exit_thread = true

var signal_queue = []

func _ready():
	ServerSignals.sensor_approved.connect(_on_sensor_approved)
	ServerSignals.sensor_declined.connect(_on_sensor_declined)
	server.listen(port, "0.0.0.0")
	if not server.is_listening():
		print('Unable to listen on port: ', port)
	print('Listening on port: ', port)
	
	mutex = Mutex.new()
	if use_thread:
		thread = Thread.new()
		print('Starting UDP Server thread')
		thread.start(_thread_function)
	else:
		print('UDP server running in single threaded mode')

func _process(delta: float) -> void:
	if not use_thread:
		_process_connections()
	while signal_queue.size() > 0:
		mutex.lock()
		var signal_data = signal_queue.pop_front()
		mutex.unlock()
		if signal_data[0] == "new_sensor_connected":
			SensorSignals.new_sensor_connected.emit(signal_data[1], signal_data[2])
		elif signal_data[0] == "sensor_request_received":
			ServerSignals.sensor_request_received.emit(signal_data[1], signal_data[2])
		elif signal_data[0] == "on_time_data_received":
			TimeHelpers.on_time_data_received(signal_data[1])
		elif signal_data[0] == "gps_data_received":
			DataSignals.gps_data_received.emit(signal_data[1], signal_data[2], signal_data[3], signal_data[4], signal_data[5])
		elif signal_data[0] == "sat_data_received":
			DataSignals.sat_data_received.emit(signal_data[1], signal_data[2], signal_data[3], signal_data[4])
		elif signal_data[0] == "accelerometer_data_received":
			DataSignals.accelerometer_data_received.emit(signal_data[1], signal_data[2], signal_data[3], signal_data[4], signal_data[5])
		elif signal_data[0] == "magnetometer_data_received":
			DataSignals.magnetometer_data_received.emit(signal_data[1], signal_data[2], signal_data[3])
		elif signal_data[0] == "lidar_2d_data_received":
			DataSignals.lidar_2d_data_received.emit(signal_data[1], signal_data[2], signal_data[3], signal_data[4])

func _exit_tree() -> void:
	if use_thread:
		mutex.lock()
		exit_thread = true
		mutex.unlock()
		thread.wait_to_finish()

func _on_sensor_approved(sensor_ip: String):
	mutex.lock()
	valid_sensors[sensor_ip] = awaiting_sensors[sensor_ip]
	if sensor_ip in awaiting_sensors:
		awaiting_sensors.erase(sensor_ip)
	signal_queue.append(["new_sensor_connected", sensor_ip, valid_sensors[sensor_ip]])
	mutex.unlock()

func _on_sensor_declined(sensor_ip: String):
	mutex.lock()
	invalid_sensors[sensor_ip] = 0
	if sensor_ip in awaiting_sensors:
		awaiting_sensors.erase(sensor_ip)
	mutex.unlock()

func _thread_function():
	print('UDP Server thread started')
	while true:
		mutex.lock()
		var should_exit = exit_thread
		mutex.unlock()
		if should_exit:
			break
		_process_connections()

func _process_connections():
	server.poll()
	if server.is_connection_available():
		var peer = server.take_connection()
		var packet = peer.get_packet()
		if not _verify_checksum(packet):
			print('Received packet with incorrect checksum: %s' % peer.get_packet_ip())
		else:
			
			_handle_new_connection(peer, packet)

func _handle_new_connection(peer: PacketPeerUDP, packet: PackedByteArray):
	if packet.get(0) == 0xD4 and packet.get(1) == 0x53:
		"""
		| 2 bytes      | 2 bytes       | 2 bytes        | variable length | 2 bytes  |
		| packet start | message class | payload length | payload         | checksum |
		First two bytes are the frame start - always 0xD4 and 0x53
		The two bytes for message class and id identify the packet contents
		The two bytes for payload length should be read as an u16 integer
		and indicate the payload size.
		The 2-byte checksuim is calculated over the packet's contents - the message
		class and id bytes, payload length byts, and the payload itself.
		The formula is:
			var ck_a = 0
			var ck_b = 0
			for i in range(2, packet.size()-2):
				ck_a += packet.get(i)
				ck_b += ck_a
			packet[packet.size()-2] = ck_a
			packet[packet.size()-1] = ck_b
		"""
		_handle_six_fps_message(peer, packet)
	else:
		print("Received: '%s' %s:%s" % [packet.get_string_from_utf8(), peer.get_packet_ip(), peer.get_packet_port()])

func _calculate_checksum(packet) -> Array:
	var ck_a = 0
	var ck_b = 0
	for i in range(2, packet.size()-2):
		ck_a += packet.get(i)
		ck_b += ck_a
	return [ck_a % 0x100, ck_b % 0x100]

func _verify_checksum(packet) -> bool:
	var calculated_checksum = _calculate_checksum(packet)
	return packet.get(packet.size()-1) == calculated_checksum[1] and packet.get(packet.size()-2) == calculated_checksum[0]
		
func _handle_six_fps_message(peer: PacketPeerUDP, packet: PackedByteArray):
	var peer_ip = peer.get_packet_ip()
	if peer_ip in invalid_sensors:
		return
	if peer_ip in awaiting_sensors:
		return
	mutex.lock()
	var valid_sensor: bool = peer.get_packet_ip() in valid_sensors
	mutex.unlock()
	var message_class = packet.get(2)
	var message_id = packet.get(3)
	if (not valid_sensor) and (message_class == 0x6e and message_id == 0x77):
		_handle_new_peer_request(peer_ip, packet)
	if not valid_sensor:
		return
	elif message_class == 0x6e and message_id == 0x4e:
		_handle_heartbeat_packet(peer_ip, packet)
	elif message_class == 0x2a and message_id == 0xac:
		_handle_time_packet(peer_ip, packet)
	elif message_class == 0x2a and message_id == 0x7c:
		#print('GPS packet received')
		_handle_position_packet(peer_ip, packet)
	elif message_class == 0x2a and message_id == 0x7b:
		#print('Sats packet received')
		_handle_gps_sats_packet(peer_ip, packet)
	elif message_class == 0x2a and message_id == 0xfa:
		#print('Accel packet received')
		_handle_accel_packet(peer_ip, packet)
	elif message_class == 0x2a and message_id == 0x39:
		#print('Mag packet received')
		_handle_magnetic_packet(peer_ip, packet)
	elif message_class == 0x2a and message_id == 0x6f:
		_handle_lidar_2d_packet(peer_ip, packet)
	elif message_class == 0x2a and message_id == 0xa7:
		_handle_public_key_packet(peer_ip, packet)
	elif message_class == 0x2a and message_id == 0x5e: # not in use
		_handle_euler_packet(peer_ip, packet)
	elif message_class == 0x2a and message_id == 0xba: # not in use
		_handle_quaternion_packet(peer_ip, packet)
	
func _handle_new_peer_request(peer_ip: String, packet: PackedByteArray):
	"""
	offset | size   | type | description
	4      | 1 byte | bool | peer provides datetime
	5      | 1 byte | bool | peer provides gps
	6      | 1 byte | bool | peer provides accelerometer
	7     | 1 byte | bool | peer provides gyroscope
	8     | 1 byte | bool | peer provides magnetometer
	9     | 1 byte | bool | peer provides euler orientation
	10     | 1 byte | bool | peer provides quaternion orientation
	"""
	mutex.lock()
	if packet.get(4) == 0xf1:
		awaiting_sensors[peer_ip] = "VEHICLE"
	elif packet.get(4) == 0x23:
		awaiting_sensors[peer_ip] = "LIDAR_2D"
	signal_queue.append(["sensor_request_received", peer_ip, awaiting_sensors[peer_ip]])
	mutex.unlock()
	
func _parse_timestamp_slice(packet: PackedByteArray):
	var hour = float(packet.decode_u8(0))
	var minute = float(packet.decode_u8(1))
	var second = float(packet.decode_u8(2))
	var nanosecond = float(packet.decode_u32(3)) * pow(10,-9)
	return [hour, minute, second, nanosecond]
	
func _handle_heartbeat_packet(peer_ip: String, packet: PackedByteArray):
	#print('heartbeat')
	var timestamp_list = _parse_timestamp_slice(packet.slice(4, 11))
	mutex.lock()
	signal_queue.append(["on_time_data_received", timestamp_list])
	mutex.unlock()
	#print(hour, ' ', minute, ' ', second)
	
func _handle_time_packet(peer_ip: String, packet: PackedByteArray):
	#print('time')
	var timestamp_list = _parse_timestamp_slice(packet.slice(4, 11))
	mutex.lock()
	signal_queue.append(["on_time_data_received", timestamp_list])
	mutex.unlock()
	#print(timestamp_list)
	
func _handle_position_packet(peer_ip: String, packet: PackedByteArray):
	#print('gps')
	var timestamp_list = _parse_timestamp_slice(packet.slice(4, 11))
	mutex.lock()
	signal_queue.append(["on_time_data_received", timestamp_list])
	mutex.unlock()
	var lat: float = (float(packet.decode_u64(11)) / 10.**9) - 90
	var lon: float = (float(packet.decode_u64(19)) / 10.**9) - 180
	var alt_neg = packet.decode_u8(27)
	var alt_m: float = packet.decode_u32(28) / 1000.0 * (-1. ** alt_neg)
	var geoid_neg = packet.decode_u8(32)
	var goid_m: float = packet.decode_u16(33) / 100 * (-1. ** geoid_neg)
	var speed_neg = packet.decode_u8(35)
	var speed_kmh: float = packet.decode_u32(36) / 1000000.0 * (-1 ** speed_neg)
	var pdop: float = packet.decode_u32(40) / 1000.0
	var hdop: float = packet.decode_u32(44) / 1000.0
	var vdop: float = packet.decode_u32(49) / 1000.0
	mutex.lock()
	signal_queue.append(["gps_data_received", peer_ip, timestamp_list, [lat, lon, alt_m, goid_m], speed_kmh, [pdop, hdop, vdop]])
	signal_queue.append(["raw_data_received", peer_ip, "GPS", timestamp_list, {"lat": lat, "lon": lon, "alt_m": alt_m, "speed_kmh": speed_kmh, "pdop": pdop, "hdop": hdop, "vdop": vdop}])
	mutex.unlock()
	
func _handle_gps_sats_packet(peer_ip: String, packet: PackedByteArray):
	var timestamp_list = _parse_timestamp_slice(packet.slice(4, 11))
	mutex.lock()
	signal_queue.append(["on_time_data_received", timestamp_list])
	mutex.unlock()
	var ga = packet.decode_u8(11)
	var gb = packet.decode_u8(12)
	var gi = packet.decode_u8(13)
	var gl = packet.decode_u8(14)
	var gp = packet.decode_u8(15)
	var gq = packet.decode_u8(16)
	var gn = packet.decode_u8(17)
	var num_entries = packet.decode_u8(18)
	var sats_offset = 19
	var el_offset = 4
	var az_offset = 5
	var sat_data = []
	for i in range(num_entries):
		var start_offset = sats_offset + (i * 7)
		var sat_name  = packet.slice(start_offset, start_offset + el_offset).get_string_from_ascii()
		var elevation = packet.decode_u8(start_offset+el_offset)
		var azimuth = packet.decode_u16(start_offset+az_offset)
		sat_data.append([sat_name, elevation, azimuth])
	mutex.lock()
	signal_queue.append(["sat_data_received", peer_ip, timestamp_list, [ga, gb, gi, gl, gp, gq, gn], sat_data])
	mutex.unlock()
	
func _handle_accel_packet(peer_ip: String, packet: PackedByteArray):
	var timestamp_list = _parse_timestamp_slice(packet.slice(4, 11))
	mutex.lock()
	signal_queue.append(["on_time_data_received", timestamp_list])
	mutex.unlock()
	var neg = packet.decode_u8(11)
	var x = (packet.decode_s32(12) / 1000000.0) * (-1 ** neg)
	neg = packet.decode_u8(16)
	var y = (packet.decode_s32(17) / 1000000.0) * (-1 ** neg)
	neg = packet.decode_u8(21)
	var z = (packet.decode_s32(22) / 1000000.0) * (-1 ** neg)
	
	neg = packet.decode_u8(26)
	var lin_x = (packet.decode_s32(27) / 1000000.0) * (-1 ** neg)
	neg = packet.decode_u8(31)
	var lin_y = (packet.decode_s32(32) / 1000000.0) * (-1 ** neg)
	neg = packet.decode_u8(36)
	var lin_z = (packet.decode_s32(37) / 1000000.0) * (-1 ** neg)
	
	neg = packet.decode_u8(41)
	var g_x = (packet.decode_s32(42) / 1000000.0) * (-1 ** neg)
	neg = packet.decode_u8(46)
	var g_y = (packet.decode_s32(47) / 1000000.0) * (-1 ** neg)
	neg = packet.decode_u8(51)
	var g_z = (packet.decode_s32(52) / 1000000.0) * (-1 ** neg)
	
	mutex.lock()
	signal_queue.append(["accelerometer_data_received", peer_ip, timestamp_list, Vector3(x, y, z), Vector3(lin_x, lin_y, lin_z), Vector3(g_x, g_y, g_z)])
	mutex.unlock()
	
func _handle_magnetic_packet(peer_ip: String, packet: PackedByteArray):
	var timestamp_list = _parse_timestamp_slice(packet.slice(4, 11))
	mutex.lock()
	signal_queue.append(["on_time_data_received", timestamp_list])
	mutex.unlock()
	var neg = packet.decode_u8(11)
	var x = (packet.decode_u32(12) / 1000.0) * (-1 ** neg)
	neg = packet.decode_u8(16)
	var y = (packet.decode_u32(17) / 1000.0) * (-1 ** neg)
	neg = packet.decode_u8(21)
	var z = (packet.decode_u32(22) / 1000.0) * (-1 ** neg)
	mutex.lock()
	signal_queue.append(["magnetometer_data_received", peer_ip, timestamp_list, Vector3(x, y, z)])
	mutex.unlock()

func _handle_lidar_2d_packet(peer_ip: String, packet: PackedByteArray):
	var timestamp_list = _parse_timestamp_slice(packet.slice(4, 11))
	mutex.lock()
	signal_queue.append(["on_time_data_received", timestamp_list])
	mutex.unlock()
	var point_count = packet.decode_u8(11)
	var point_data = []
	for idx in range(point_count):
		var offset = idx * 7
		var angle: float = float(packet.decode_u32(12 + offset)) / 10000
		var distance: float = float(packet.decode_u16(12 + offset + 4)) / 1000
		var intensity: int = packet.decode_u8(12 + offset + 6)
		point_data.append([angle, distance, intensity])
	signal_queue.append(["lidar_2d_data_received", peer_ip, timestamp_list, point_count, point_data])

func _handle_public_key_packet(peer_ip: String, packet: PackedByteArray):
	var timestamp_list = _parse_timestamp_slice(packet.slice(4, 11))
	mutex.lock()
	signal_queue.append(["on_time_data_received", timestamp_list])
	mutex.unlock()
	
	
func _handle_euler_packet(peer_ip: String, packet: PackedByteArray):
	var timestamp_list = _parse_timestamp_slice(packet.slice(4, 11))
	mutex.lock()
	signal_queue.append(["on_time_data_received", timestamp_list])
	mutex.unlock()
	
	
func _handle_quaternion_packet(peer_ip: String, packet: PackedByteArray):
	var timestamp_list = _parse_timestamp_slice(packet.slice(4, 11))
	mutex.lock()
	signal_queue.append(["on_time_data_received", timestamp_list])
	mutex.unlock()
	

func _handle_racebox_message(delta, packet):
	#print(packet.size())
	var year = packet.decode_u16(10)
	var month = packet.decode_u8(12)
	var day = packet.decode_u8(13)
	var hour = packet.decode_u8(14)
	var minute = packet.decode_u8(15)
	var second = packet.decode_u8(16)
	var dt_valid_flags = _to_binary(packet.decode_u8(17))
	var valid_date = int(dt_valid_flags[-1]) == 1
	var valid_time = int(dt_valid_flags[-2]) == 1
	var dt_fully_resolved = int(dt_valid_flags[-3]) == 1
	var valid_mag_dec = int(dt_valid_flags[-4]) == 1
	var time_accuracy_ns = packet.decode_u32(18)
	var nano_second = packet.decode_u32(22)
	var fix_status_flags = _to_binary(packet.decode_u8(27))
	var dt_flags = _to_binary(packet.decode_u8(28))
	var num_sv = packet.decode_u8(29)
	var lon_deg = float(packet.decode_u32(30)) / (10**7)
	var lat_deg = float(packet.decode_u32(34)) / (10**7)
	var wgs_alt_m = float(packet.decode_u32(38)) / (10**3) + 6378.137
	var msl_alt_m = float(packet.decode_u32(42)) / (10**3) + 6378.137
	var horz_accuracy_m = float(packet.decode_u32(46)) / (10**3)
	var vert_accuracy_m = float(packet.decode_u32(50)) / (10**3)
	var speed_mps = float(packet.decode_s32(54)) / (10**3)
	var heading_deg = float(packet.decode_s32(58)) / (10**5)
	var speed_accuracy_mps = float(packet.decode_u32(62)) / (10**3)
	var heading_accuracy_deg = float(packet.decode_u32(66)) / (10**5)
	var pdop = float(packet.decode_u16(70)) / 100
	var lat_lon_flags = _to_binary(packet.decode_u8(72))
	var battery_status = _to_binary(packet.decode_u8(73))
	var g_force_x = packet.decode_s16(74) # milli-g
	var g_force_y = packet.decode_s16(76) # milli-g
	var g_force_z = packet.decode_s16(78) # milli-g
	var rot_rate_x_dps = float(packet.decode_s16(80)) / 100
	var rot_rate_y_dps = float(packet.decode_s16(82)) / 100
	var rot_rate_z_dps = float(packet.decode_s16(84)) / 100
	if num_sv > 0:
		var lon_rad = deg_to_rad(lon_deg)
		var lat_rad = deg_to_rad(lat_deg)
		var sensor_pos_x = wgs_alt_m * sin(lon_rad) * cos(lat_rad)
		var sensor_pos_y = wgs_alt_m * sin(lon_rad) * sin(lat_rad)
		var sensor_pos_z = wgs_alt_m * cos(lon_rad)
		var camera_pos_x = (wgs_alt_m+1) * sin(lon_rad) * cos(lat_rad)
		var camera_pos_y = (wgs_alt_m+1) * sin(lon_rad) * sin(lat_rad)
		var camera_pos_z = (wgs_alt_m+1) * cos(lon_rad)
		camera_position_updated.emit(camera_pos_x, camera_pos_y, camera_pos_z)
		camera_rotation_updated.emit(deg_to_rad(rot_rate_x_dps) * delta, deg_to_rad(rot_rate_y_dps) * delta, deg_to_rad(rot_rate_z_dps) * delta)
		sensor_point_generated.emit(sensor_pos_x, sensor_pos_y, sensor_pos_z)
	else:
		print(Time.get_datetime_string_from_system())
		print("GPS has no sources")

func _to_binary(intValue: int) -> String:
	var bin_str: String = ""
	while intValue > 0:
		bin_str = str(intValue & 1) + bin_str
		intValue = intValue >> 1
	return bin_str