From 09e9b4bd14c67b764f18d661c6da8779a9b8b314 Mon Sep 17 00:00:00 2001
From: Damien Mouratille
Date: Wed, 7 Aug 2024 16:24:54 +0200
Subject: Add Tobii Pro Glasses3

---
 src/argaze/utils/contexts/PupilLabs.py             |   1 +
 src/argaze/utils/contexts/TobiiProGlasses3.py      | 128 +++++++++++++++++++++
 .../utils/demo/tobii_g3_live_stream_context.json   |   6 +
 3 files changed, 135 insertions(+)
 create mode 100644 src/argaze/utils/contexts/TobiiProGlasses3.py
 create mode 100644 src/argaze/utils/demo/tobii_g3_live_stream_context.json

diff --git a/src/argaze/utils/contexts/PupilLabs.py b/src/argaze/utils/contexts/PupilLabs.py
index 1bfb658..035915b 100644
--- a/src/argaze/utils/contexts/PupilLabs.py
+++ b/src/argaze/utils/contexts/PupilLabs.py
@@ -21,6 +21,7 @@ __license__ = "GPLv3"
 import sys
 import logging
 import time
+
 import threading
 from dataclasses import dataclass
 
diff --git a/src/argaze/utils/contexts/TobiiProGlasses3.py b/src/argaze/utils/contexts/TobiiProGlasses3.py
new file mode 100644
index 0000000..7deb4d5
--- /dev/null
+++ b/src/argaze/utils/contexts/TobiiProGlasses3.py
@@ -0,0 +1,128 @@
+"""Handle network connection to Tobii Pro G3 devices. 
+	Based on Tobii Realtime Python API.
+	g3pylib must be installed
+"""
+
+"""
+This program is free software: you can redistribute it and/or modify it under
+the terms of the GNU General Public License as published by the Free Software
+Foundation, either version 3 of the License, or (at your option) any later
+version.
+This program is distributed in the hope that it will be useful, but WITHOUT
+ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+You should have received a copy of the GNU General Public License along with
+this program. If not, see <https://www.gnu.org/licenses/>.
+
+"""
+
+__author__ = "Damien Mouratille"
+__credits__ = []
+__copyright__ = "Copyright 2024, Ecole Nationale de l'Aviation Civile (ENAC)"
+__license__ = "GPLv3"
+
+import sys
+import logging
+import time
+import dill
+import threading
+from dataclasses import dataclass
+import numpy
+import cv2
+import asyncio
+import os
+
+from argaze import ArFeatures, DataFeatures, GazeFeatures
+from argaze.utils import UtilsFeatures
+
+
+from g3pylib import connect_to_glasses
+
+
+class LiveStream(ArFeatures.DataCaptureContext):
+
+	@DataFeatures.PipelineStepInit
+	def __init__(self, **kwargs):
+
+		# Init DataCaptureContext class
+		super().__init__()
+
+	def __enter__(self):
+
+		logging.info('Tobii Pro G3 connexion starts...')
+
+		# Init timestamp
+		self.__start_time = time.time()
+
+		self.__loop = asyncio.new_event_loop()
+		self.__loop.run_until_complete(self.__stream_rtsp())
+
+		return self
+
+	async def __stream_rtsp(self):
+		"""Stream video and gaze."""
+
+		logging.info('Stream gaze from Tobii Pro G3')
+
+		while self.is_running():
+
+			try:
+				async with connect_to_glasses.with_zeroconf(True,10000) as g3:
+					async with g3.stream_rtsp(scene_camera=True, gaze=True) as streams:
+						async with streams.gaze.decode() as gaze_stream, streams.scene_camera.decode() as scene_stream:
+							while True:
+								frame, frame_timestamp = await scene_stream.get()
+								gaze, gaze_timestamp = await gaze_stream.get()
+								while gaze_timestamp is None or frame_timestamp is None:
+									if frame_timestamp is None:
+										frame, frame_timestamp = await scene_stream.get()
+									if gaze_timestamp is None:
+										gaze, gaze_timestamp = await gaze_stream.get()
+								while gaze_timestamp < frame_timestamp:
+									gaze, gaze_timestamp = await gaze_stream.get()
+									while gaze_timestamp is None:
+										gaze, gaze_timestamp = await gaze_stream.get()
+
+								scene_frame = frame.to_ndarray(format="bgr24")
+
+								gaze_timestamp = int((gaze_timestamp - self.__start_time) * 1e3)
+
+								logging.debug('Gaze received at %i timestamp', gaze_timestamp)
+
+								# If given gaze data
+								if "gaze2d" in gaze:
+									gaze2d = gaze["gaze2d"]
+									# Convert rational (x,y) to pixel location (x,y)
+									h, w = scene_frame.shape[:2]
+									gaze_scene = (int(gaze2d[0] * w), int(gaze2d[1] * h))
+
+
+									self._process_gaze_position(
+											timestamp=gaze_timestamp,
+											x=gaze_scene[0],
+											y=gaze_scene[1])
+								else:
+									# Process empty gaze position
+									logging.debug('Not worn at %i timestamp', gaze_timestamp)
+
+								scene_timestamp = int((frame_timestamp - self.__start_time) * 1e3)
+
+								logging.debug('Video received at %i timestamp', scene_timestamp)
+
+								self._process_camera_image(
+									timestamp=scene_timestamp,
+									image=scene_frame)
+
+			except KeyboardInterrupt:
+				pass
+
+
+
+	@DataFeatures.PipelineStepExit
+	def __exit__(self, exception_type, exception_value, exception_traceback):
+
+		logging.debug('Tobii Pro G3 context stops...')
+
+		# Close data stream
+		self.stop()
+
diff --git a/src/argaze/utils/demo/tobii_g3_live_stream_context.json b/src/argaze/utils/demo/tobii_g3_live_stream_context.json
new file mode 100644
index 0000000..4619b3c
--- /dev/null
+++ b/src/argaze/utils/demo/tobii_g3_live_stream_context.json
@@ -0,0 +1,6 @@
+{
+    "argaze.utils.contexts.TobiiProGlasses3.LiveStream" : {
+        "name": "TobiiProGlasses3",
+        "pipeline": "aruco_markers_pipeline.json"
+    }
+}
\ No newline at end of file
-- 
cgit v1.1