|
| 1 | +""" |
| 2 | +MIT License |
| 3 | +
|
| 4 | +Copyright DragonDreams GmbH 2024 |
| 5 | +
|
| 6 | +Permission is hereby granted, free of charge, to any person obtaining a copy |
| 7 | +of this software and associated documentation files (the "Software"), to deal |
| 8 | +in the Software without restriction, including without limitation the rights |
| 9 | +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
| 10 | +copies of the Software, and to permit persons to whom the Software is |
| 11 | +furnished to do so, subject to the following conditions: |
| 12 | +
|
| 13 | +The above copyright notice and this permission notice shall be included in all |
| 14 | +copies or substantial portions of the Software. |
| 15 | +
|
| 16 | +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
| 17 | +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
| 18 | +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
| 19 | +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
| 20 | +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
| 21 | +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE |
| 22 | +SOFTWARE. |
| 23 | +""" |
| 24 | + |
| 25 | +import multiprocessing.queues |
| 26 | +import multiprocessing |
| 27 | +import queue as pqueue |
| 28 | +import traceback |
| 29 | +import platform |
| 30 | +import logging |
| 31 | +from struct import pack, unpack |
| 32 | +import cv2 |
| 33 | +import numpy as np |
| 34 | +from vivefacialtracker.camera import FTCamera |
| 35 | +from vivefacialtracker.vivetracker import ViveTracker |
| 36 | + |
| 37 | + |
| 38 | +class FTCameraController: |
| 39 | + """Opens a camera grabbing frames as numpy arrays.""" |
| 40 | + |
| 41 | + _logger = logging.getLogger("evcta.FTCameraController") |
| 42 | + |
| 43 | + def __init__(self: 'FTCameraController', index: int) -> None: |
| 44 | + """Create camera grabber. |
| 45 | +
|
| 46 | + The camera is not yet opened. Set "callback_frame" then call |
| 47 | + "open()" to open the device and "start_read()" to start capturing. |
| 48 | +
|
| 49 | + Keyword arguments: |
| 50 | + index -- Index of the camera. Under Linux this uses the device |
| 51 | + file "/dev/video{index}". |
| 52 | + """ |
| 53 | + self.is_open = False |
| 54 | + self._index: int = index |
| 55 | + self._proc_read: multiprocessing.Process = None |
| 56 | + self._proc_queue: multiprocessing.queues.Queue = None |
| 57 | + |
| 58 | + def close(self: 'FTCameraController') -> None: |
| 59 | + """Closes the device if open. |
| 60 | +
|
| 61 | + If capturing stops capturing first. |
| 62 | + """ |
| 63 | + self.is_open = False |
| 64 | + FTCameraController._logger.info("FTCameraController.close: index {}".format(self._index)) |
| 65 | + self._stop_read() |
| 66 | + |
| 67 | + def open(self: 'FTCameraController') -> None: |
| 68 | + """Start capturing frames if not capturing and device is open.""" |
| 69 | + if self._proc_read is not None: |
| 70 | + return |
| 71 | + |
| 72 | + self.is_open = True |
| 73 | + FTCameraController._logger.info("FTCameraController.open: start process") |
| 74 | + self._proc_queue = multiprocessing.Queue(maxsize=1) |
| 75 | + self._proc_read = multiprocessing.Process(target=self._read_process, args=(self._proc_queue,)) |
| 76 | + self._proc_read.start() |
| 77 | + |
| 78 | + def _reopen(self: 'FTCameraController') -> None: |
| 79 | + FTCameraController._logger.info("FTCameraController._reopen") |
| 80 | + self.close() |
| 81 | + self.open() |
| 82 | + |
| 83 | + def get_image(self: 'FTCameraController') -> np.ndarray: |
| 84 | + """Get next image or None.""" |
| 85 | + try: |
| 86 | + # timeout of 1s is a bit short. 2s is safer |
| 87 | + frame = self._proc_queue.get(True, 2) |
| 88 | + shape = unpack('HHH', frame[0:6]) |
| 89 | + image = np.frombuffer(frame[6:], dtype=np.uint8).reshape(shape) |
| 90 | + return image |
| 91 | + except pqueue.Empty: |
| 92 | + # FTCameraController._logger.info("FTCameraController.get_image: timeout, reopen device") |
| 93 | + # self._reopen() |
| 94 | + return None |
| 95 | + except Exception: |
| 96 | + FTCameraController._logger.exception( |
| 97 | + "FTCameraController.get_image: Failed getting image") |
| 98 | + print(traceback.format_exc()) |
| 99 | + return None |
| 100 | + |
| 101 | + def _stop_read(self: 'FTCameraController') -> None: |
| 102 | + """Stop capturing frames if capturing.""" |
| 103 | + if self._proc_read is None: |
| 104 | + return |
| 105 | + FTCameraController._logger.info("FTCameraController._stop_read: stop process") |
| 106 | + self._proc_read.terminate() # sends a SIGTERM |
| 107 | + self._proc_read.join(1) |
| 108 | + |
| 109 | + if self._proc_read.exitcode is not None: |
| 110 | + FTCameraController._logger.info( |
| 111 | + "FTCameraController.stop_read: process terminated") |
| 112 | + else: |
| 113 | + FTCameraController._logger.info( |
| 114 | + "FTCameraController._stop_read: process not responding, killing it") |
| 115 | + self._proc_read.kill() # sends a SIGKILL |
| 116 | + self._proc_read.join(1) |
| 117 | + FTCameraController._logger.info( |
| 118 | + "FTCameraController._stop_read: process killed") |
| 119 | + self._proc_read = None |
| 120 | + |
| 121 | + def _read_process(self: 'FTCameraController', |
| 122 | + queue: multiprocessing.connection.Connection) -> None: |
| 123 | + """Read process function.""" |
| 124 | + |
| 125 | + """ |
| 126 | + logging.basicConfig(filename='ViveFaceTracker-ReadThread.log', filemode='w', |
| 127 | + encoding='utf-8', level=logging.INFO) |
| 128 | + """ |
| 129 | + |
| 130 | + FTCameraController._logger.info("FTCameraController._read_process: ENTER") |
| 131 | + class Helper(FTCamera.Processor): |
| 132 | + """Helper.""" |
| 133 | + def __init__(self: 'FTCameraController.Helper', |
| 134 | + queue: multiprocessing.connection.Connection) -> None: |
| 135 | + self.camera: FTCamera = None |
| 136 | + self.tracker: ViveTracker = None |
| 137 | + self._queue = queue |
| 138 | + |
| 139 | + def open_camera(self: 'FTCameraController.Helper', index: int, |
| 140 | + queue: multiprocessing.connection.Connection) -> None: |
| 141 | + """Open camera.""" |
| 142 | + self.camera = FTCamera(index) |
| 143 | + self.camera.terminator = FTCamera.Terminator() |
| 144 | + self.camera.processor = self |
| 145 | + self.camera.queue = queue |
| 146 | + self.camera.open() |
| 147 | + |
| 148 | + def open_tracker(self: 'FTCameraController.Helper') -> None: |
| 149 | + """Open tracker.""" |
| 150 | + if platform.system() == 'Linux': |
| 151 | + self.tracker = ViveTracker(self.camera.device.fileno()) |
| 152 | + else: |
| 153 | + self.tracker = ViveTracker(self.camera.device, self.camera.device_index) |
| 154 | + |
| 155 | + def close(self: 'FTCameraController.Helper') -> None: |
| 156 | + """Close tracker and camera.""" |
| 157 | + if self.tracker is not None: |
| 158 | + self.tracker.dispose() |
| 159 | + self.tracker = None |
| 160 | + if self.camera is not None: |
| 161 | + self.camera.close() |
| 162 | + self.camera.processor = None |
| 163 | + self.camera.terminator = None |
| 164 | + self.camera.queue = None |
| 165 | + self.camera = None |
| 166 | + |
| 167 | + def process(self, frame) -> None: |
| 168 | + """Process frame.""" |
| 169 | + channel = cv2.split(frame)[0] |
| 170 | + frame = cv2.merge((channel, channel, channel)) |
| 171 | + if self.tracker is not None: |
| 172 | + frame = self.tracker.process_frame(frame) |
| 173 | + self._queue.put(pack('HHH', *frame.shape) + frame.tobytes()) |
| 174 | + |
| 175 | + helper: Helper = Helper(queue) |
| 176 | + try: |
| 177 | + FTCameraController._logger.info( |
| 178 | + "FTCameraController._read_process: open device") |
| 179 | + helper.open_camera(self._index, queue) |
| 180 | + |
| 181 | + if not ViveTracker.is_camera_vive_tracker(helper.camera.device): |
| 182 | + FTCameraController._logger.exception( |
| 183 | + "FTCameraController._read_process: not a VIVE Facial Tracker") |
| 184 | + raise RuntimeError("not a VIVE Facial Tracker") |
| 185 | + |
| 186 | + helper.open_tracker() |
| 187 | + |
| 188 | + FTCameraController._logger.info( |
| 189 | + "FTCameraController._read_process: start reading") |
| 190 | + helper.camera.read() |
| 191 | + except Exception: |
| 192 | + FTCameraController._logger.exception( |
| 193 | + "FTCameraController._read_process: failed open device") |
| 194 | + print(traceback.format_exc()) |
| 195 | + finally: |
| 196 | + helper.close() |
| 197 | + |
| 198 | + FTCameraController._logger.info("FTCameraController._read_process: EXIT") |
0 commit comments