# License: Apache 2.0. See LICENSE file in root directory. # Copyright(c) 2021 Intel Corporation. All Rights Reserved. # This library is part of validation testing wrapper import collections import csv import logging import os import threading import time from queue import Queue, Full from datetime import datetime import pyrealsense2 as rs class ModifiedLogger(logging.LoggerAdapter): def process(self, msg, kwargs): return "{serial}: {msg}".format(serial=self.extra['serial'], msg=msg), kwargs class LRSFrameQueueManager: """Manage a queue structure for drop-safe and high-performance frame handling using pyrealsense. The queue structure consist of 2 queues: lrs frame_queue and python threading queue. The lrs frame_queue is filled with frames arriving from lrs while the threading queue get frames from it and send it for post-process. Each frame which is inserted to the lrs.frame_queue is marked with keep=True to prevent frame drops due to slow frame processing. Frames from the lrs frame_queue are sent for post-process using producer-consumer threads; the producer thread get frames from the lrs frame_queue and put it in the threading queue, while the consumer thread get frames from the threading queue and sent it for post-process. Attributes: lrs_queue: pyrealsesne.frame_queue through it frames arrive from lrs Args: callback (function): Post-process callback to be called on each frame in the post process queue. max_queue_size (int): Max size of a queue in the frame queues structure. statistics (bool): Whether to collect performance statistics or not. If collected, will be saved in a CSV file under '~/queue_.csv serial_number (str): The serial number of the camera of which the queues belongs to. """ class Event(object): TERMINATE_CONSUMER = 'terminate_consumer' START_COLLECTING = 'start_collecting' STOP_COLLECTING = 'stop_collecting' def __init__(self, callback=None, max_queue_size=100000, statistics=False, serial_number=None): self._id = serial_number self._max_queue_size = max_queue_size self._logger = ModifiedLogger(logging.getLogger('test'), {'serial': self._id}, ) self._process = callback self.lrs_queue = rs.frame_queue(capacity=self._max_queue_size, keep_frames=True) self._post_process_queue = Queue(maxsize=self._max_queue_size) self._producer_thread = None self._consumer_thread = None self._block_queue_event = threading.Event() self.block() # ignoring frames until self.start is called self._terminate_producer_event = threading.Event() self.statistics = statistics self._producing_times = collections.deque() self._producer_queue_sizes = collections.deque() self._consuming_times = collections.deque() self._consumer_queue_sizes = collections.deque() self._memory_samples = collections.deque() def start(self): """Initialize the queue structure and starting the producer-consumer threads first, unblocking the threading queue and clear all thread's events and then initialize the producer-consumer threads. """ self.unblock() self._terminate_producer_event.clear() if self.lrs_queue is None: self.lrs_queue = rs.frame_queue(capacity=self._max_queue_size, keep_frames=True) if self._post_process_queue is None: self._post_process_queue = Queue(maxsize=self._max_queue_size) # in case self.start is being called multiple times in a row and self.stop is not being called between. if self._producer_thread is None: self._logger.info("initializing a producer thread") self._producer_thread = threading.Thread(target=self._produce_frames, name="producer_thread(LRSFrameQueueManager)") self._producer_thread.setDaemon(True) self._logger.info("starting the producer thread") self._producer_thread.start() # # in case self.start is being called multiple times in a row and self.stop is not being called between. if self._consumer_thread is None: self._logger.info("initializing a consumer thread") self._consumer_thread = threading.Thread(target=self._consume_frames, name="consumer_thread(LRSFrameQueueManager)") self._consumer_thread.setDaemon(True) self._logger.info("starting the consumer thread") self._consumer_thread.start() def stop(self, timeout=3 * 60): """Kill the producer-consumer threads and deconstruct the queues. Args: timeout (float): max time to kill consumer/producer thread. """ # saving the post-process queue state so it will be returned to its initial state after it will be blocked leave_unblocked = not self._block_queue_event.is_set() self.block() self._logger.info("waiting for frames queue to be empty and frames consuming thread to be terminated") self._logger.info("inserting 'termination' object to frame queue") self._post_process_queue.put((LRSFrameQueueManager.Event.TERMINATE_CONSUMER, time.time())) self._consumer_thread.join(timeout=timeout) if self._consumer_thread.is_alive(): raise RuntimeError("frame queue hasn't been empty and frame consuming thread hans't been terminated within {} seconds".format(timeout)) self._logger.info("frames queue is empty and consuming thread was terminated") self._consumer_thread = None self._logger.info("killing producer thread") self._terminate_producer_event.set() self._producer_thread.join(timeout=timeout) if self._producer_thread.is_alive(): raise RuntimeError("frame producing thread han't been terminated within {} seconds".format(timeout)) self._logger.info("producing thread was terminated") self._producer_thread = None del self.lrs_queue self.lrs_queue = None del self._post_process_queue self._post_process_queue = None if leave_unblocked: self.unblock() def block(self): """Stop the producer thread from putting the frames from lrs frame_queue in the threading queue. """ self._logger.info("blocking the post-process queue") self._block_queue_event.set() def unblock(self): """set the producer thread to put the frames from lrs frame_queue in the threading queue. """ self._logger.info("unblocking the post-process queue") self._block_queue_event.clear() def join(self): """Block until the threading queue is empty. """ self._logger.info("waiting fot the post-process queue to be empty") if not self._post_process_queue: # in case LRSFrameQueueManager.join() was called after LRSFrameQueueManager.stop() return self._post_process_queue.join() def register_callback(self, callback): """Register a post-process callback to be called on each frame in the post process queue. Args: callback: post-process callback to be called on each frame in the post process queue. """ self._logger.info("registering post-process callback") self._process = callback def _produce_frames(self, timeout=1): while True: start = time.time() if self._terminate_producer_event.is_set(): self._logger.debug("producer thread have been set to be terminated, returning") return # self._logger.debug("waiting for a frame in lrs_queue") try: lrs_frame = self.lrs_queue.wait_for_frame(timeout_ms=timeout * 1000) frame_ts = time.time() * 1000.0 # [milliseconds] except Exception as ex: self._logger.error(ex) continue # self._logger.debug("got a frame from lrs_queue") if self._block_queue_event.is_set(): self._logger.debug("queue is blocked, dropped frame #{} of stream {}".format(lrs_frame.get_frame_number(), lrs_frame.get_profile().stream_type())) continue # self._logger.debug("putting the frame in the queue") try: self._post_process_queue.put((lrs_frame, frame_ts), block=True, timeout=timeout) except Full: self._logger.error("frame queue is full for more than {} seconds, dropped frame #{}".format(timeout, lrs_frame.get_frame_number())) continue produce_time = (time.time() - start) * 1000.0 queue_size = self._post_process_queue.qsize() if self.statistics: self._producing_times.append(produce_time) self._producer_queue_sizes.append(queue_size) self._logger.debug("added frame to the queue within: {} ms, queue size: {}".format(produce_time, queue_size)) def _consume_frames(self): while True: # self._logger.debug("getting a frame from the queue, queue size: {}".format(self._post_process_queue.qsize())) element, ts = self._post_process_queue.get(block=True) start = time.time() if element == LRSFrameQueueManager.Event.TERMINATE_CONSUMER: self._logger.debug("consuming thread has been set to be terminated, returning") return lrs_frame = element # self._logger.debug("working on the frame") try: if self._process: self._process(lrs_frame, ts) else: self._logger.debug("no post-process callback is configured, dropping frame #{} of stream {}".format(lrs_frame.get_frame_number(), lrs_frame.get_profile().stream_type())) continue except Exception as ex: self._logger.exception(ex) del lrs_frame consume_time = (time.time() - start) * 1000.0 queue_size = self._post_process_queue.qsize() # self._logger.debug("marking the frame as a done task") self._post_process_queue.task_done() self._logger.debug("consumed a frame from the queue within {} ms, queue size: {}".format(consume_time, queue_size)) if self.statistics: self._consumer_queue_sizes.append(queue_size) self._consuming_times.append(consume_time) def to_csv(self): """Save internal performance statistics gathered by the producer and consumer threads in a CSV file under '~/queue_.csv' """ if self.statistics: with open(os.path.join(os.path.expanduser('~'), 'queue_{}.csv'.format(datetime.now().strftime("%d-%m-%Y-%H-%M-%S.%f"))), 'wb') as csv_file: fieldnames = ['producing_time', 'producer_queue_size', 'consuming_time', 'consumer_queue_size', 'memory'] writer = csv.DictWriter(csv_file, fieldnames=fieldnames) writer.writeheader() for producing_time, producer_queue_size, consuming_time, consumer_queue_size, memory_sample in zip(self._producing_times, self._producer_queue_sizes, self._consuming_times, self._consumer_queue_sizes, self._memory_samples): writer.writerow({'producing_time': producing_time, 'producer_queue_size': producer_queue_size, 'consuming_time': consuming_time, 'consumer_queue_size': consumer_queue_size, 'memory': memory_sample})