Buffering gaze positions while the pipeline is busy. Improving lock managment in ArUcoCamera.watch.

2 files changed, 76 insertions, 59 deletions

diff --git a/src/argaze/ArFeatures.py b/src/argaze/ArFeatures.py
index 4515ae1..b05cfba 100644
--- a/src/argaze/ArFeatures.py
+++ b/src/argaze/ArFeatures.py
@@ -1176,14 +1176,16 @@ class ArCamera(ArFrame):
 
 		for layer_name, layer in self.layers.items():
 
-			# Initialize layer if needed
-			if layer.aoi_scene is None:
+			with layer._lock:
 
-				layer.aoi_scene = AOI2DScene.AOI2DScene()
+				# Initialize layer if needed
+				if layer.aoi_scene is None:
 
-			else:
+					layer.aoi_scene = AOI2DScene.AOI2DScene()
+
+				else:
 
-				layer.aoi_scene.clear()
+					layer.aoi_scene.clear()
 
 	def _write_projection_cache(self, timestamp: int|float, exception = None):
 		"""Write layers aoi scene into the projection cache.
@@ -1493,6 +1495,7 @@ class ArContext(DataFeatures.PipelineStepObject):
 		# Init private attributes
 		self.__pipeline = None
 		self.__exceptions = DataFeatures.TimestampedExceptions()
+		self.__gaze_positions_buffer = GazeFeatures.TimeStampedGazePositions()
 
 		# Init protected attributes
 		self._stop_event = threading.Event()
@@ -1543,23 +1546,33 @@ class ArContext(DataFeatures.PipelineStepObject):
 
 		if issubclass(type(self.__pipeline), ArFrame):
 
-			try:
+			# Edit gaze position
+			if x is None and y is None:
 
-				if x is None and y is None:
+				gaze_position = GazeFeatures.GazePosition(timestamp=timestamp)
 
-					# Edit empty gaze position
-					self.__pipeline.look(GazeFeatures.GazePosition(timestamp=timestamp))
+			else:
 
-				else:
+				gaze_position = GazeFeatures.GazePosition((x, y), precision=precision, timestamp=timestamp)
+
+			# Buffer gaze position
+			self.__gaze_positions_buffer.append(gaze_position)
+
+			# Process all buffered gaze positions while the pipeline is available
+			while not self.__pipeline.busy() and self.__gaze_positions_buffer:
+
+				gaze_position = self.__gaze_positions_buffer.pop(0)
+
+				try:
 
 					# Edit gaze position
-					self.__pipeline.look(GazeFeatures.GazePosition((x, y), precision=precision, timestamp=timestamp))
+					self.__pipeline.look(gaze_position)
 
-			except DataFeatures.TimestampedException as e:
+				except DataFeatures.TimestampedException as e:
 
-				logging.warning('%s._process_gaze_position: %s', DataFeatures.get_class_path(self), e)
+					logging.warning('%s._process_gaze_position: %s', DataFeatures.get_class_path(self), e)
 
-				self.__exceptions.append(e)
+					self.__exceptions.append(e)
 
 		else:
 
diff --git a/src/argaze/ArUcoMarker/ArUcoCamera.py b/src/argaze/ArUcoMarker/ArUcoCamera.py
index 8ae8cb8..4fa8b83 100644
--- a/src/argaze/ArUcoMarker/ArUcoCamera.py
+++ b/src/argaze/ArUcoMarker/ArUcoCamera.py
@@ -119,61 +119,63 @@ class ArUcoCamera(ArFeatures.ArCamera):
 
 		logging.debug('ArUcoCamera.watch')
 
-		# Use camera frame lock feature
-		with self._lock:
+		# Draw black rectangles to mask sides
+		if self.__sides_mask > 0:
+
+			logging.debug('\t> drawing sides mask (%i px)', self.__sides_mask)
 
-			# Draw black rectangles to mask sides
-			if self.__sides_mask > 0:
-				logging.debug('\t> drawing sides mask (%i px)', self.__sides_mask)
+			height, width, _ = image.shape
 
-				height, width, _ = image.shape
+			cv2.rectangle(image, (0, 0), (self.__sides_mask, height), (0, 0, 0), -1)
+			cv2.rectangle(image, (width - self.__sides_mask, 0), (width, height), (0, 0, 0), -1)
 
-				cv2.rectangle(image, (0, 0), (self.__sides_mask, height), (0, 0, 0), -1)
-				cv2.rectangle(image, (width - self.__sides_mask, 0), (width, height), (0, 0, 0), -1)
+		# Fill camera frame background with timestamped image
+		with self._lock:
 
-			# Fill camera frame background with timestamped image
 			self.background = image
 
-			# Read projection from the cache if required
-			if not self._read_projection_cache(image.timestamp):
+		# Read projection from the cache if required
+		if not self._read_projection_cache(image.timestamp):
 
-				# Detect aruco markers
-				logging.debug('\t> detect markers')
+			# Detect aruco markers
+			logging.debug('\t> detect markers')
 
-				self.__aruco_detector.detect_markers(image)
+			self.__aruco_detector.detect_markers(image)
 
-				# Clear former layers projection into camera frame
-				self._clear_projection()
+			# Clear former layers projection into camera frame
+			self._clear_projection()
 
-				# Project each aoi 3d scene into camera frame
-				for scene_name, scene in self.scenes.items():
+			# Project each aoi 3d scene into camera frame
+			for scene_name, scene in self.scenes.items():
 
-					''' TODO: Enable aruco_aoi processing
-					if scene.aruco_aoi:
+				''' TODO: Enable aruco_aoi processing
+				if scene.aruco_aoi:
 
-						try:
+					try:
 
-							# Build AOI scene directly from detected ArUco marker corners
-							self.layers[??].aoi_2d_scene |= scene.build_aruco_aoi_scene(self.__aruco_detector.detected_markers())
+						# Build AOI scene directly from detected ArUco marker corners
+						self.layers[??].aoi_2d_scene |= scene.build_aruco_aoi_scene(self.__aruco_detector.detected_markers())
 
-						except ArFeatures.PoseEstimationFailed:
+					except ArFeatures.PoseEstimationFailed:
 
-							pass
-					'''
+						pass
+				'''
 
-					# Estimate scene pose from detected scene markers
-					logging.debug('\t> estimate %s scene pose', scene_name)
+				# Estimate scene pose from detected scene markers
+				logging.debug('\t> estimate %s scene pose', scene_name)
 
-					try:
+				try:
+
+					tvec, rmat, _ = scene.estimate_pose(self.__aruco_detector.detected_markers(), timestamp=image.timestamp)
 
-						tvec, rmat, _ = scene.estimate_pose(self.__aruco_detector.detected_markers(), timestamp=image.timestamp)
+					# Project scene into camera frame according estimated pose
+					for layer_name, layer_projection in scene.project(tvec, rmat, self.visual_hfov, self.visual_vfov, timestamp=image.timestamp):
 
-						# Project scene into camera frame according estimated pose
-						for layer_name, layer_projection in scene.project(tvec, rmat, self.visual_hfov, self.visual_vfov, timestamp=image.timestamp):
+						logging.debug('\t> project %s scene %s layer', scene_name, layer_name)
 
-							logging.debug('\t> project %s scene %s layer', scene_name, layer_name)
+						try:
 
-							try:
+							with self.layers[layer_name]._lock:
 
 								# Update camera layer aoi
 								self.layers[layer_name].aoi_scene |= layer_projection
@@ -181,23 +183,25 @@ class ArUcoCamera(ArFeatures.ArCamera):
 								# Timestamp camera layer
 								self.layers[layer_name].timestamp = image.timestamp
 
-							except KeyError:
+						except KeyError:
+
+							pass
 
-								pass
+					# Write projection into the cache if required
+					self._write_projection_cache(image.timestamp)
 
-						# Write projection into the cache if required
-						self._write_projection_cache(image.timestamp)
+				except DataFeatures.TimestampedException as e:
 
-					except DataFeatures.TimestampedException as e:
+					# Write exception into the cache if required
+					self._write_projection_cache(image.timestamp, e)
 
-						# Write exception into the cache if required
-						self._write_projection_cache(image.timestamp, e)
+					# Raise exception
+					raise e
 
-						# Raise exception
-						raise e
+		# Copy camera frame background into scene frames background if required
+		if self.copy_background_into_scenes_frames:
 
-			# Copy camera frame background into scene frames background if required
-			if self.copy_background_into_scenes_frames:
+			with self._lock:
 
 				self._copy_background_into_scenes_frames()