Fixing aruco_pipeline lags.

7 files changed, 232 insertions, 34 deletions

diff --git a/src/argaze/ArFeatures.py b/src/argaze/ArFeatures.py
index 6b4b182..b70cc40 100644
--- a/src/argaze/ArFeatures.py
+++ b/src/argaze/ArFeatures.py
@@ -24,6 +24,7 @@ import sys
 import importlib
 import threading
 import time
+import math
 
 from argaze import DataFeatures, GazeFeatures
 from argaze.AreaOfInterest import *
@@ -1344,6 +1345,7 @@ class ArCamera(ArFrame):
 
 # Define default ArContext image parameters
 DEFAULT_ARCONTEXT_IMAGE_PARAMETERS = {
+	"draw_times": True,
 	"draw_exceptions": True
 }
 
@@ -1408,7 +1410,7 @@ class ArContext(DataFeatures.PipelineStepObject):
 		Define abstract __enter__ method to use device as a context.
 
 		!!! warning
-			This method is called provided that the PipelineInput is created as a context using a with statement.
+			This method is called provided that the ArContext is created using a with statement.
 		"""
 		return self
 
@@ -1417,14 +1419,14 @@ class ArContext(DataFeatures.PipelineStepObject):
 		Define abstract __exit__ method to use device as a context.
 
 		!!! warning
-			This method is called provided that the PipelineInput is created as a context using a with statement.
+			This method is called provided that the ArContext is created using a with statement.
 		"""
 		pass
 
 	def _process_gaze_position(self, timestamp: int|float, x: int|float = None, y: int|float = None, precision: int|float = None):
 		"""Request pipeline to process new gaze position at a timestamp."""
 
-		logging.debug('%s._process_gaze_position timestamp: %f', type(self).__name__, timestamp)
+		logging.debug('%s._process_gaze_position', type(self).__name__)
 
 		if issubclass(type(self.__pipeline), ArFrame):
 
@@ -1451,7 +1453,7 @@ class ArContext(DataFeatures.PipelineStepObject):
 	def _process_camera_image(self, timestamp: int|float, image: numpy.ndarray):
 		"""Request pipeline to process new camera image at a timestamp."""
 
-		logging.debug('%s._process_camera_image timestamp: %f', type(self).__name__, timestamp)
+		logging.debug('%s._process_camera_image', type(self).__name__)
 
 		if issubclass(type(self.__pipeline), ArCamera):
 
@@ -1460,7 +1462,7 @@ class ArContext(DataFeatures.PipelineStepObject):
 			# Compare image size with ArCamera frame size
 			if width != self.__pipeline.size[0] or height != self.__pipeline.size[1]:
 
-				logging.warning('image size (%i x %i) is different of ArCamera frame size (%i x %i)', width, height, self.__pipeline.size[0], self.__pipeline.size[1])
+				logging.warning('%s._process_camera_image: image size (%i x %i) is different of ArCamera frame size (%i x %i)', type(self).__name__ , width, height, self.__pipeline.size[0], self.__pipeline.size[1])
 				return
 
 			try:
@@ -1471,7 +1473,7 @@ class ArContext(DataFeatures.PipelineStepObject):
 
 			except DataFeatures.TimestampedException as e:
 
-				logging.warning('%s', e)
+				logging.warning('%s._process_camera_image: %s', type(self).__name__, e)
 
 				self.__exceptions.append(e)
 
@@ -1479,7 +1481,7 @@ class ArContext(DataFeatures.PipelineStepObject):
 
 			raise(TypeError('Pipeline is not ArCamera instance.'))
 
-	def __image(self, draw_exceptions: bool):
+	def __image(self, draw_times: bool, draw_exceptions: bool):
 		"""
 		Get pipeline image with execution informations.
 
@@ -1493,6 +1495,38 @@ class ArContext(DataFeatures.PipelineStepObject):
 
 		logging.debug('\t> get image (%i x %i)', width, height)
 
+		info_stack = 0
+
+		if draw_times:
+
+			
+
+			if issubclass(type(self.__pipeline), ArCamera):
+
+				try:
+
+					watch_time = int(self.__pipeline.execution_times['watch'])
+
+				except KeyError:
+
+					watch_time = math.nan
+
+				info_stack += 1
+				cv2.putText(image, f'Watch {watch_time}ms', (20, info_stack * 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
+
+			if issubclass(type(self.__pipeline), ArFrame):
+
+				try:
+
+					look_time = self.__pipeline.execution_times['look']
+
+				except KeyError:
+
+					look_time = math.nan
+
+				info_stack += 1
+				cv2.putText(image, f'Look {look_time:.2f}ms', (20, info_stack * 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
+
 		if draw_exceptions:
 
 			# Write exceptions
@@ -1511,7 +1545,7 @@ class ArContext(DataFeatures.PipelineStepObject):
 		Get pipeline image.
 
 		Parameters:
-			kwargs: PipelineInput.__image parameters
+			kwargs: ArContext.__image parameters
 		"""
 		# Use image_parameters attribute if no kwargs
 		if kwargs:
diff --git a/src/argaze/ArUcoMarkers/ArUcoCamera.py b/src/argaze/ArUcoMarkers/ArUcoCamera.py
index dda55be..80e3f94 100644
--- a/src/argaze/ArUcoMarkers/ArUcoCamera.py
+++ b/src/argaze/ArUcoMarkers/ArUcoCamera.py
@@ -244,4 +244,4 @@ class ArUcoCamera(ArFeatures.ArCamera):
 
 			return self.__image(**kwargs)
 
-		return self.__image(**self.image_parameters)
+		return self.__image(**self._image_parameters)
diff --git a/src/argaze/utils/demo/aruco_markers_pipeline.json b/src/argaze/utils/demo/aruco_markers_pipeline.json
index a4fe400..b64dde3 100644
--- a/src/argaze/utils/demo/aruco_markers_pipeline.json
+++ b/src/argaze/utils/demo/aruco_markers_pipeline.json
@@ -1,6 +1,6 @@
 {
     "argaze.ArUcoMarkers.ArUcoCamera.ArUcoCamera": {
-        "name": "demo_camera",
+        "name": "Head-mounted camera",
         "size": [1920, 1080],
         "aruco_detector": {
             "dictionary": "DICT_APRILTAG_16h5",
@@ -54,7 +54,67 @@
                     }
                 },
                 "frames": {
-                    "GrayRectangle": "gaze_analysis_pipeline.json"
+                    "GrayRectangle": {
+                        "size": [1920, 1149],
+                        "background": "frame_background.jpg",
+                        "gaze_movement_identifier": {
+                            "argaze.GazeAnalysis.DispersionThresholdIdentification.GazeMovementIdentifier": {
+                                "deviation_max_threshold": 50,
+                                "duration_min_threshold": 200
+                            }
+                        },
+                        "scan_path": {
+                            "duration_max": 10000
+                        },
+                        "layers": {
+                            "demo_layer": {
+                                "aoi_scene": "aoi_2d_scene.json",
+                                "aoi_matcher": {
+                                    "argaze.GazeAnalysis.FocusPointInside.AOIMatcher": {}
+                                }
+                            }
+                        },
+                        "image_parameters": {
+                            "background_weight": 1,
+                            "draw_scan_path": {
+                                "draw_fixations": {
+                                    "deviation_circle_color": [255, 0, 255],
+                                    "duration_border_color": [127, 0, 127],
+                                    "duration_factor": 1e-2
+                                }, 
+                                "draw_saccades": {
+                                    "line_color": [255, 0, 255]
+                                }
+                            },
+                            "draw_layers": {
+                                "demo_layer": {
+                                    "draw_aoi_scene": {
+                                        "draw_aoi": {
+                                            "color": [255, 255, 255],
+                                            "border_size": 1
+                                        }
+                                    },
+                                    "draw_aoi_matching": {
+                                        "draw_looked_aoi": {
+                                            "color": [0, 255, 255],
+                                            "border_size": 10
+                                        },
+                                        "looked_aoi_name_color": [255, 255, 255],
+                                        "looked_aoi_name_offset": [10, 10]
+                                    }
+                                }
+                            }, 
+                            "draw_fixations": {
+                                "deviation_circle_color": [255, 255, 255],
+                                "duration_border_color": [127, 0, 127],
+                                "duration_factor": 1e-2
+                            }, 
+                            "draw_gaze_positions": {
+                                "color": [0, 255, 255],
+                                "size": 2
+                            }
+                        }
+                    }
                 },
                 "angle_tolerance": 15.0,
                 "distance_tolerance": 2.54
diff --git a/src/argaze/utils/demo/eyetracker_setup.json b/src/argaze/utils/demo/eyetracker_setup.json
index 70f85e4..8d47542 100644
--- a/src/argaze/utils/demo/eyetracker_setup.json
+++ b/src/argaze/utils/demo/eyetracker_setup.json
@@ -15,6 +15,8 @@
         },
         "pipeline": "aruco_markers_pipeline.json",
         "image_parameters": {
+            "draw_something": false,
+            "draw_times": true,
             "draw_exceptions": true
         }
     }
diff --git a/src/argaze/utils/demo/gaze_analysis_pipeline.json b/src/argaze/utils/demo/gaze_analysis_pipeline.json
index 07b7e78..737589d 100644
--- a/src/argaze/utils/demo/gaze_analysis_pipeline.json
+++ b/src/argaze/utils/demo/gaze_analysis_pipeline.json
@@ -24,7 +24,7 @@
             }
         },
         "heatmap": {
-            "size": [320, 240]
+            "size": [32, 24]
         },
         "layers": {
             "demo_layer": {
diff --git a/src/argaze/utils/eyetrackers/TobiiProGlasses2.py b/src/argaze/utils/eyetrackers/TobiiProGlasses2.py
index 94f31a7..f46ddd8 100644
--- a/src/argaze/utils/eyetrackers/TobiiProGlasses2.py
+++ b/src/argaze/utils/eyetrackers/TobiiProGlasses2.py
@@ -54,6 +54,11 @@ DEFAULT_PROJECT_NAME = 'DefaultProject'
 DEFAULT_PARTICIPANT_NAME = 'DefaultParticipant'
 DEFAULT_RECORD_NAME = 'DefaultRecord'
 
+# Define default Tobii image_parameters values
+DEFAULT_TOBII_IMAGE_PARAMETERS = {
+    "draw_something": False
+}
+
 # Define extra classes to support Tobii data parsing
 @dataclass
 class DirSig():
@@ -304,6 +309,8 @@ class LiveStream(ArFeatures.ArContext):
 
 		self.__parser = TobiiJsonDataParser()
 
+		self._image_parameters = {**ArFeatures.DEFAULT_ARCONTEXT_IMAGE_PARAMETERS, **DEFAULT_TOBII_IMAGE_PARAMETERS}
+		
 	@property
 	def address(self) -> str:
 		"""Network address where to find the device."""
@@ -465,18 +472,23 @@ class LiveStream(ArFeatures.ArContext):
 	def __enter__(self):
 
 		logging.info('Tobii Pro Glasses 2 connexion starts...')
-		logging.debug('TobiiProGlasses2.Provider.__enter__')
+		logging.debug('%s.__enter__', type(self).__name__)
 
 		# Update current configuration with configuration patch
 		logging.debug('> updating configuration')
 
-		configuration = self.__get_request('/api/system/conf')
-
+		# Update current configuration with configuration patch
 		if self.__configuration:
 
-			#configuration.update(self.__configuration)
+			logging.debug('> updating configuration')
 			configuration = self.__post_request('/api/system/conf', self.__configuration)
 
+		# Read current configuration
+		else:
+
+			logging.debug('> reading configuration')
+			configuration = self.__get_request('/api/system/conf')
+
 		# Log current configuration
 		logging.info('Tobii Pro Glasses 2 configuration:')
 
@@ -509,6 +521,10 @@ class LiveStream(ArFeatures.ArContext):
 		# Create stop event
 		self.__stop_event = threading.Event()
 
+		# Create a video buffer with a lock
+		self.__video_buffer = collections.OrderedDict()
+		self.__video_buffer_lock = threading.Lock()
+
 		# Open data stream
 		self.__data_socket = self.__make_socket()
 		self.__data_thread = threading.Thread(target = self.__stream_data)
@@ -520,15 +536,22 @@ class LiveStream(ArFeatures.ArContext):
 		# Open video stream
 		self.__video_socket = self.__make_socket()
 		self.__video_thread = threading.Thread(target = self.__stream_video)
-		self.__video_thread.daemon = True
+		self.__video_thread.daemon = False
 
 		logging.debug('> starting video thread...')
 		self.__video_thread.start()
 
+		# Open video buffer reader
+		self.__video_buffer_read_thread = threading.Thread(target = self.__video_buffer_read)
+		self.__video_buffer_read_thread.daemon = False
+
+		logging.debug('> starting video buffer reader thread...')
+		self.__video_buffer_read_thread.start()
+
 		# Keep connection alive
 		self.__keep_alive_msg = "{\"type\": \"live.data.unicast\", \"key\": \""+ str(uuid.uuid4()) +"\", \"op\": \"start\"}"
 		self.__keep_alive_thread = threading.Thread(target = self.__keep_alive)
-		self.__keep_alive_thread.daemon = True
+		self.__keep_alive_thread.daemon = False
 
 		logging.debug('> starting keep alive thread...')
 		self.__keep_alive_thread.start()
@@ -537,18 +560,53 @@ class LiveStream(ArFeatures.ArContext):
 
 	def __exit__(self, exception_type, exception_value, exception_traceback):
 
-		logging.debug('TobiiProGlasses2.Provider.__exit__')
+		logging.debug('%s.__exit__', type(self).__name__)
 		
 		# Close data stream
 		self.__stop_event.set()
 
 		# Stop keeping connection alive
 		threading.Thread.join(self.__keep_alive_thread)
-		self.__keep_alive_thread = None
 
 		# Stop data streaming
 		threading.Thread.join(self.__data_thread)
-		self.__data_thread = None
+
+		# Stop video buffer reading
+		threading.Thread.join(self.__video_buffer_read_thread)
+
+		# Stop video streaming
+		threading.Thread.join(self.__video_thread)
+
+	def __image(self, draw_something: bool, **kwargs: dict) -> numpy.array:
+		"""Get Tobbi visualisation.
+
+		Parameters:
+			kwargs: ArContext.image parameters
+		"""
+
+		# Get context image
+		image = super().image(**kwargs)
+
+		if draw_something:
+
+			cv2.putText(image, 'SOMETHING', (512, 512), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
+
+		return image
+
+	def image(self, **kwargs: dict) -> numpy.array:
+		"""
+		Get Tobbi visualisation.
+
+		Parameters:
+			kwargs: LiveStream.__image parameters
+		"""
+
+		# Use image_parameters attribute if no kwargs
+		if kwargs:
+
+			return self.__image(**kwargs)
+
+		return self.__image(**self._image_parameters)
 
 	def __make_socket(self):
 		"""Create a socket to enable network communication."""
@@ -582,7 +640,7 @@ class LiveStream(ArFeatures.ArContext):
 	def __stream_data(self):
 		"""Stream data from dedicated socket."""
 
-		logging.debug('TobiiProGlasses2.Provider.__stream_data')
+		logging.debug('%s.__stream_data', type(self).__name__)
 
 		while not self.__stop_event.is_set():
 
@@ -624,11 +682,10 @@ class LiveStream(ArFeatures.ArContext):
 	def __stream_video(self):
 		"""Stream video from dedicated socket."""
 
-		logging.debug('TobiiProGlasses2.Provider.__stream_video')
+		logging.debug('%s.__stream_video', type(self).__name__)
 
 		container = av.open(f'rtsp://{self.__address}:8554/live/scene', options={'rtsp_transport': 'tcp'})
 		self.__stream = container.streams.video[0]
-		self.__buffer = collections.OrderedDict()
 
 		for image in container.decode(self.__stream):
 
@@ -645,19 +702,66 @@ class LiveStream(ArFeatures.ArContext):
 				if image.time is not None:
 
 					timestamp = int(image.time * 1e6)
-					image = image.to_ndarray(format='bgr24')
 
-					logging.debug('> image timestamp: %f', timestamp)
+					logging.debug('> store image at %f timestamp', timestamp)
+
+					# Lock buffer access
+					self.__video_buffer_lock.acquire()
+
+					# Decode image and store it at time index
+					self.__video_buffer[timestamp] = image.to_ndarray(format='bgr24')
+
+					# Unlock buffer access
+					self.__video_buffer_lock.release()
+
+	def __video_buffer_read(self):
+		"""Read incoming buffered video images."""
+
+		logging.debug('%s.__video_buffer_read', type(self).__name__)
+
+		while not self.__stop_event.is_set():
+
+			# Can't read image while it is locked
+			while self.__video_buffer_lock.locked():
+
+				time.sleep(1e-6)
+
+			# Lock buffer access
+			self.__video_buffer_lock.acquire()
+
+			# Video buffer not empty
+			if len(self.__video_buffer) > 0:
+
+				# Get last stored image
+				try:
+
+					timestamp, image = self.__video_buffer.popitem(last=True)
+
+					logging.debug('> read image at %f timestamp', timestamp)
+
+					if len(self.__video_buffer) > 0:
+
+						logging.warning('skipping %i image', len(self.__video_buffer))
+
+					# Clear buffer
+					self.__video_buffer = collections.OrderedDict()
 
 					# Process camera image
 					self._process_camera_image(
 						timestamp = timestamp, 
 						image = image)
+					
+				except Exception as e:
+
+					logging.warning('%s.__video_buffer_read: %s', type(self).__name__, e)
+
+			# Unlock buffer access
+			self.__video_buffer_lock.release()
 
 	def __keep_alive(self):
 		"""Maintain network connection."""
 
-		logging.debug('TobiiProGlasses2.Provider.__keep_alive')
+		logging.debug('%s.__keep_alive', type(self).__name__)
 
 		while not self.__stop_event.is_set():
 
@@ -671,7 +775,7 @@ class LiveStream(ArFeatures.ArContext):
 
 		url = self.__base_url + api_action
 
-		logging.debug('TobiiProGlasses2.Provider.__get_request %s', url)
+		logging.debug('%s.__get_request %s', type(self).__name__, url)
 
 		res = urlopen(url).read()
 
@@ -683,16 +787,17 @@ class LiveStream(ArFeatures.ArContext):
 
 			data = None
 
-		logging.debug('TobiiProGlasses2.Provider.__get_request received: %s', data)
+		logging.debug('%s.__get_request received %s', type(self).__name__, data)
 
 		return data
 
 	def __post_request(self, api_action, data = None, wait_for_response = True) -> str:
 		"""Send a POST request and get result back."""
 
-		logging.debug('TobiiProGlasses2.Provider.__post_request %s', api_action)
-
 		url = self.__base_url + api_action
+
+		logging.debug('%s.__post_request %s', type(self).__name__, url)
+
 		req = Request(url)
 		req.add_header('Content-Type', 'application/json')
 		data = json.dumps(data)
diff --git a/src/argaze/utils/pipeline_run.py b/src/argaze/utils/pipeline_run.py
index dc9ef53..3a8640f 100644
--- a/src/argaze/utils/pipeline_run.py
+++ b/src/argaze/utils/pipeline_run.py
@@ -54,9 +54,6 @@ def main():
 			# Visualisation loop
 			while True:
 
-				# DEBUG
-				print("DISPLAY", context.name)
-
 				# Display context
 				cv2.imshow(context.name, context.image())