From 9decd613dd30fc356c543f005aaa53a18780d8e6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pl=C3=BCss=20Roland?= Date: Fri, 13 Sep 2024 20:05:39 +0200 Subject: [PATCH] adding vive facial tracker handling from https://github.com/LordOfDragons/vivefacialtracker. linux so far --- .gitignore | 1 + BabbleApp/camera.py | 74 +- BabbleApp/vivefacialtracker/camera.py | 613 +++++++++++++++ BabbleApp/vivefacialtracker/vivetracker.py | 821 +++++++++++++++++++++ 4 files changed, 1504 insertions(+), 5 deletions(-) create mode 100644 BabbleApp/vivefacialtracker/camera.py create mode 100644 BabbleApp/vivefacialtracker/vivetracker.py diff --git a/.gitignore b/.gitignore index 5ea6d78..47203ee 100644 --- a/.gitignore +++ b/.gitignore @@ -21,3 +21,4 @@ scripts/installer.iss Glia_cap.py lazyass_minecraft_script.py scripts/example_build_app_and_installer.bat +*.kdev* diff --git a/BabbleApp/camera.py b/BabbleApp/camera.py index 19c3626..cc00967 100644 --- a/BabbleApp/camera.py +++ b/BabbleApp/camera.py @@ -5,10 +5,14 @@ import serial import serial.tools.list_ports import threading import time +import traceback +import platform from colorama import Fore from config import BabbleConfig, BabbleSettingsConfig from enum import Enum +from vivefacialtracker.camera import FTCamera +from vivefacialtracker.vivetracker import ViveTracker WAIT_TIME = 0.1 @@ -50,6 +54,10 @@ class Camera: self.cancellation_event = cancellation_event self.current_capture_source = config.capture_source self.cv2_camera: "cv2.VideoCapture" = None + self.vft_camera: FTCamera = None + self.vft_camera_image: np.ndarray = None + self.vft_camera_lock = threading.Lock() + self.vft_tracker: ViveTracker = None self.serial_connection = None self.last_frame_time = time.time() @@ -76,6 +84,12 @@ class Camera: while True: if self.cancellation_event.is_set(): print(f"{Fore.CYAN}[INFO] Exiting Capture thread{Fore.RESET}") + if self.vft_tracker is not None: + self.vft_tracker.dispose() + self.vft_tracker = None + if self.vft_camera is not None: + self.vft_camera.close_sync() + self.vft_camera = None return should_push = True # If things aren't open, retry until they are. Don't let read requests come in any earlier @@ -93,6 +107,39 @@ class Camera: port = self.config.capture_source self.current_capture_source = port self.start_serial_connection(port) + elif str(self.config.capture_source).startswith("VFT"): + if self.vft_camera is None\ + or self.config.capture_source != self.current_capture_source: + print(self.error_message.format(self.config.capture_source)) + if self.cancellation_event.wait(WAIT_TIME): + return + self.vft_camera_image = None + try: + self.vft_camera = FTCamera(int(str(self.config.capture_source)[3:])) + self.vft_camera.open() + self.vft_camera.callback_frame = self.vft_process_frame + self.vft_camera.start_read() + should_push = False + except Exception: + print(traceback.format_exc()) + if self.vft_camera is not None: + self.vft_camera.close_sync() + self.vft_camera = None + pass + + if self.vft_camera is not None\ + and ViveTracker.is_camera_vive_tracker(self.vft_camera.device): + try: + if platform.system() == 'Linux': + self.vft_tracker = ViveTracker( + self.vft_camera.device.fileno()) + else: + self.vft_tracker = ViveTracker( + self.vft_camera.device, + self.vft_camera.device_index) + except Exception: + print(traceback.format_exc()) + else: if ( self.cv2_camera is None @@ -133,13 +180,30 @@ class Camera: # if we get all the way down here, consider ourselves connected self.camera_status = CameraState.CONNECTED + def vft_process_frame(self, data: np.ndarray) -> None: + with self.vft_camera_lock: + self.vft_camera_image = data + def get_cv2_camera_picture(self, should_push): try: - ret, image = self.cv2_camera.read() - if not ret: - self.cv2_camera.set(cv2.CAP_PROP_POS_FRAMES, 0) - raise RuntimeError("Problem while getting frame") - frame_number = self.cv2_camera.get(cv2.CAP_PROP_POS_FRAMES) + if self.vft_camera is not None: + image = None + with self.vft_camera_lock: + image = self.vft_camera_image + self.vft_camera_image = None + if image is None: + return + channel = cv2.split(image)[0] + image = cv2.merge((channel, channel, channel)) + if self.vft_tracker: + image = self.vft_tracker.process_frame(image) + frame_number = self.frame_number + else: + ret, image = self.cv2_camera.read() + if not ret: + self.cv2_camera.set(cv2.CAP_PROP_POS_FRAMES, 0) + raise RuntimeError("Problem while getting frame") + frame_number = self.cv2_camera.get(cv2.CAP_PROP_POS_FRAMES) # Calculate the fps. yeah = time.time() delta_time = yeah - self.last_frame_time diff --git a/BabbleApp/vivefacialtracker/camera.py b/BabbleApp/vivefacialtracker/camera.py new file mode 100644 index 0000000..52a1b88 --- /dev/null +++ b/BabbleApp/vivefacialtracker/camera.py @@ -0,0 +1,613 @@ +""" +MIT License + +Copyright DragonDreams GmbH 2024 + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +""" + +import platform +import numpy as np +import PIL +import asyncio as aio +import cv2 as cv +import traceback +import time +import logging +import threading +from enum import Enum + +isLinux = platform.system() == 'Linux' + +if isLinux: + import v4l2py as v4l + import v4l2py.device as v4ld +else: + import pygrabber.dshow_graph as pgdsg + import pygrabber.dshow_ids as pgdsi + + +class FTCamera: + """Opens a camera grabbing frames as numpy arrays.""" + class ControlType(Enum): + """Type of the control.""" + Integer = 'int' + Boolean = 'bool' + Select = 'select' + + if isLinux: + class Control: + """Control defined by the hardware.""" + def __init__(self: "FTCamera.ControlInfo", + control: v4ld.BaseControl) -> None: + self._control = control + self.name = control.name + self.type = None + self.minimum: int = 0 + self.maximum: int = 0 + self.step: int = 1 + self.default: int = 0 + self.clipping: bool = False + self.choices: dict[int: str] = {} + match control.type: + case v4ld.ControlType.INTEGER: + self.type = FTCamera.ControlType.Integer + self.minimum = control.minimum + self.maximum = control.maximum + self.step = control.step + self.default = control.default + self.clipping = control.clipping + case v4ld.ControlType.BOOLEAN: + self.type = FTCamera.ControlType.Boolean + self.default = control.default + + case v4ld.ControlType.MENU: + self.type = FTCamera.ControlType.Select + self.choices = dict(control.data) + self.default = control.default + + @property + def value(self: "FTCamera.ControlInfo") -> int | bool: + return self._control.value + + @value.setter + def value(self: "FTCamera.ControlInfo", new_value: int | bool): + self._control.value = new_value + + @property + def is_writeable(self: "FTCamera.ControlInfo") -> bool: + return self._control.is_writeable + else: + class Control: + """Control defined by the hardware.""" + def __init__(self: "FTCamera.ControlInfo") -> None: + raise Exception("Not supported") + + @property + def value(self: "FTCamera.ControlInfo") -> int | bool: + return 0 + + @value.setter + def value(self: "FTCamera.ControlInfo", new_value: int | bool): + pass + + @property + def is_writeable(self: "FTCamera.ControlInfo") -> bool: + pass + + class FrameSize: + def __init__(self: 'FTCamera.FrameSize', index: int, + width: int, height: int, min_fps: int) -> None: + self.index = index + self.width = width + self.height = height + self.min_fps = min_fps + + def __repr__(self: 'FTCamera.FrameSize') -> str: + return "(width={}, height={}, fps={})".format( + self.width, self.height, self.min_fps) + + class FrameFormat: + def __init__(self: 'FTCamera.FrameFormat', pixel_format: str, + description: str) -> None: + self.pixel_format = pixel_format + self.description = description + + def __repr__(self: 'FTCamera.FrameFormat') -> str: + return "(pixel_format={}, description='{}')".format( + self.pixel_format, self.description) + + _logger = logging.getLogger("evcta.FTCamera") + + def __init__(self: 'FTCamera', index: int) -> None: + """Create camera grabber. + + The camera is not yet opened. Set "callback_frame" then call + "open()" to open the device and "start_read()" to start capturing. + + Keyword arguments: + index -- Index of the camera. Under Linux this uses the device + file "/dev/video{index}". + """ + self._index: int = index + if isLinux: + self._device: v4l.Device = None + else: + self._device: pgdsg.VideoInput = None + + self._controls: "list[FRCamera.Control]" = [] + self._task_read: aio.Task = None + self._thread_read: threading.Thread = None + self._thread_lock: threading.Lock = None + self._arr_data: np.ndarray = None + self._arr_c2: np.ndarray = None + self._arr_c3: np.ndarray = None + self._arr_merge: np.ndarray = None + + self.callback_frame = None + """Callback to send captured frame data to. + + Has to be a callable object with the signature + "callback(data: np.ndarray) -> None". If callback_frame + is None no image is grabbed nor processed. + + The image send to the callback is a numpy array of shape + (height, width, 3). Channel format is YUV. + + Callback function can be changed while capturing. + """ + + def open(self: 'FTCamera') -> None: + """Open device if closed. + + This opens the device using Video4Linux. Finds frame size and + format to use. Also finds all supported controls. + + This method does not start recording. + + Throws "Exception" if: + - Device can not be found + - Device can not be opened + - Device has no format supporting capturing + - Device has no YUV format + - Device has no size with YUV format and at least 30 frame rate + """ + if self._device: + return + FTCamera._logger.info("FTCamera.open: index {}".format(self._index)) + if isLinux: + self._device = v4l.Device.from_id(self._index) + self._device.open() + else: + self._filter_graph = pgdsg.FilterGraph() + + self._filter_graph.add_video_input_device(self._index) + self._filter_video = self._filter_graph.get_input_device() + FTCamera._logger.info("Video input filter: {}".format( + self._filter_video.Name)) + self._device = self._filter_video + + self._filter_graph.add_sample_grabber(self._async_grabber) + self._filter_grabber = self._filter_graph.filters[ + pgdsg.FilterType.sample_grabber] + + self._filter_graph.add_null_render() + self._find_format() + self._find_frame_size() + self._set_frame_format() + self._init_arrays() + self._find_controls() + + if not isLinux: + self._filter_graph.prepare_preview_graph() + # self._filter_graph.print_debug_info() + + def _find_format(self: 'FTCamera') -> None: + """Logs all formats supported by camera and picks best one. + + Picks the first format which is YUV and supports capturing. + + Throws "Exception" if no suitable format is found. + """ + FTCamera._logger.info("formats:") + if isLinux: + for x in self._device.info.formats: + FTCamera._logger.info("- {}".format(x)) + self._format = next(x for x in self._device.info.formats + if x.pixel_format == v4l.PixelFormat.YUYV + and x.type == v4ld.BufferType.VIDEO_CAPTURE) + else: + for x in self._filter_video.get_formats(): + FTCamera._logger.info(x) + fmt = next(x for x in self._filter_video.get_formats() + if x['media_type_str'] in ['YUY2']) + self._format = FTCamera.FrameFormat( + fmt['media_type_str'], fmt['media_type_str']) + FTCamera._logger.info("using format: {}".format(self._format)) + + def _find_frame_size(self: 'FTCamera') -> None: + """Logs all sizes supported by camera and picks best one. + + Picks the first size with YUV format and a minimum FPS of 30. + + Throws "Exception" if no suitable size is found. + """ + if isLinux: + FTCamera._logger.info("sizes:") + for x in self._device.info.frame_sizes: + FTCamera._logger.info("- {}".format(x)) + self._frame_size = next(x for x in self._device.info.frame_sizes + if x.pixel_format == v4l.PixelFormat.YUYV + and x.min_fps >= 30) + else: + fsize = next(x for x in self._filter_video.get_formats() + if x['media_type_str'] == self._format.pixel_format + and x['min_framerate'] >= 30) + self._frame_size = FTCamera.FrameSize( + fsize['index'], fsize['width'], fsize['height'], + int(fsize['max_framerate'])) + + FTCamera._logger.info("using frame size : {}".format(self._frame_size)) + self._frame_width = self._frame_size.width + self._frame_height = self._frame_size.height + self._pixel_count = self._frame_width * self._frame_height + self._half_pixel_count = self._pixel_count // 2 + self._half_frame_width = self._frame_width // 2 + self._half_frame_height = self._frame_height // 2 + + def _set_frame_format(self: 'FTCamera') -> None: + """Activates the found format and size.""" + if isLinux: + self._device.set_format( + buffer_type=v4ld.BufferType.VIDEO_CAPTURE, + width=self._frame_size.width, + height=self._frame_size.height, + pixel_format=self._format.pixel_format) + else: + self._filter_video.set_format(self._frame_size.index) + + # make grabber accept YUV2 not RGB24 + guidYUV2 = '{32595559-0000-0010-8000-00AA00389B71}' + self._filter_grabber.set_media_type( + pgdsg.MediaTypes.Video, guidYUV2) + + # by changing the format we have to replace the callback + # handler too. this is required since the original callback + # handler expects a 3-channel image and YUV2 delivers + # 2 channels. this crashes python_grabber + class SampleGrabberYUV2(pgdsg.SampleGrabberCallback): + def __init__(self: 'SampleGrabberYUV2', + callback: pgdsg.Callable[[pgdsg.Mat], None]): + super(SampleGrabberYUV2, self).__init__(callback) + + def BufferCB(self: 'SampleGrabberYUV2', this, SampleTime, + pBuffer: pgdsg.NPBUFFER, BufferLen: int) -> int: + if self.keep_photo: + self.keep_photo = False + w = self.image_resolution[0] + h = self.image_resolution[1] + img = np.ctypeslib.as_array(pBuffer, shape=(h, w, 2)) + # this is brain-dead. the video is returned with the + # x and y axis flipped. this totally breaks the YUV + # decoding. switching axes fixes this mess + img = np.moveaxis(img, 0, 1) + self.callback(img) + return 0 + + self._filter_grabber.set_callback( + SampleGrabberYUV2(self._async_grabber), 1) + + # an alternative is pgdsi.GUID_NULL accepting everything + + def _init_arrays(self: 'FTCamera') -> None: + """Create numpy arrays to fill during capturing.""" + self._arr_data = np.zeros([self._pixel_count * 2], dtype=np.uint8) + self._arr_merge = np.zeros([self._pixel_count, 3], dtype=np.uint8) + self._arr_c2 = np.empty([self._half_pixel_count], np.uint8) + self._arr_c3 = np.empty([self._half_pixel_count], np.uint8) + + def _find_controls(self: 'FTCamera') -> None: + """Logs all controls and stores them for use.""" + self._controls = [] + FTCamera._logger.info("controls:") + if isLinux: + for x in self._device.controls.values(): + FTCamera._logger.info("- {}".format(x)) + control = FTCamera.Control(x) + if not control.type: + continue + self._controls.append(control) + + @property + def device_index(self: 'FTCamera') -> int: + return self._index + + if isLinux: + @property + def device(self: 'FTCamera') -> v4l.Device: + """Video4Linux device if open or None if closed.""" + return self._device + else: + @property + def device(self: 'FTCamera') -> pgdsg.VideoInput: + """Device if open or None if closed.""" + return self._device + + @property + def frame_width(self: 'FTCamera') -> int: + """Width in pixels of captured frames. + + Only valid if device is open.""" + return self._frame_width + + @property + def frame_height(self: 'FTCamera') -> int: + """Height in pixels of captured frames. + + Only valid if device is open.""" + return self._frame_height + + @property + def frame_fps(self: 'FTCamera') -> float: + """Capture frame rate. + + Only valid if device is open.""" + return float(self._frame_size.min_fps) + + @property + def frame_format(self: 'FTCamera') -> str: + """Capture pixel format. + + Only valid if device is open.""" + return self._frame_size.pixel_format.name + + @property + def frame_format_description(self: 'FTCamera') -> str: + """Capture pixel format description. + + Only valid if device is open.""" + return self._format.description + + @property + def controls(self: 'FTCamera') -> "list[FTCamera.Control]": + """List of all supported controls. + + Only valid if device is open.""" + return self._controls + + async def close(self: 'FTCamera') -> None: + """Closes the device if open. + + If capturing stops capturing first. + """ + await self.stop_read() + if not self._device: + return + FTCamera._logger.info("FTCamera.close: index {}".format(self._index)) + try: + if isLinux: + self._device.close() + else: + self._filter_graph.stop() + self._filter_graph.remove_filters() + self._filter_grabber = None + self._filter_video = None + self._filter_graph = None + except Exception: + pass + self._device = None + + def close_sync(self: 'FTCamera') -> None: + aio.run(self.close()) + + def _has_asyncio_loop(self: 'FTCamera') -> bool: + try: + aio.get_running_loop() + return True + except RuntimeError: + return False + + def start_read(self: 'FTCamera') -> None: + """Start capturing frames if not capturing and device is open.""" + if self._task_read or self._thread_read or not self._device: + return + FTCamera._logger.info("FTCamera.start_read: start read task") + if isLinux: + if self._has_asyncio_loop(): + self._task_read = aio.create_task(self._async_read()) + else: + self._thread_read_stop = False + self._thread_lock = threading.Lock() + self._thread_read = threading.Thread(target=self._async_read_thread) + self._thread_read.start() + else: + self._has_frame = False + self._read_frame = None + self._thread_read_stop = False + self._task_lock = threading.Lock() + self._filter_graph.run() + self._thread_read = threading.Thread(target=self._async_read) + self._thread_read.start() + self._task_process = aio.create_task(self._async_process()) + + async def stop_read(self: 'FTCamera') -> None: + """Stop capturing frames if capturing.""" + if not self._task_read or not self._device: + return + FTCamera._logger.info("FTCamera.stop_read: stop read task") + if isLinux: + if self._thread_read is not None: + with self._thread_lock: + self._thread_read_stop = True + self._thread_read.join(1) + self._thread_read = None + self._thread_lock = None + else: + self._task_read.cancel() + try: + await self._task_read + except aio.CancelledError: + FTCamera._logger.info("FTCamera.stop_read: read task stopped") + self._task_read = None + else: + self._filter_graph.stop() + self._thread_read_stop = True + self._task_process.cancel() + try: + await self._task_process + except aio.CancelledError: + FTCamera._logger.info("FTCamera.stop_read: read task stopped") + self._task_process = None + self._thread_read.join(0.5) + self._thread_read = None + self._task_lock = None + + if isLinux: + async def _async_read(self: 'FTCamera') -> None: + async for frame in self._device: + if not self._process_frame(frame): + break + + def _async_read_thread(self: 'FTCamera') -> None: + try: + for frame in self._device: + with self._thread_lock: + if self._thread_read_stop: + return + if not self._process_frame(frame): + break + except Exception: + pass + else: + def _async_read(self: 'FTCamera') -> None: + while not self._thread_read_stop: + self._filter_graph.grab_frame() + time.sleep(0.001) + + def _async_grabber(self: 'FTCamera', image: np.ndarray) -> None: + with self._task_lock: + self._read_frame = image + self._has_frame = True + + async def _async_process(self: 'FTCamera') -> None: + while True: + has_frame = False + frame = None + with self._task_lock: + has_frame = self._has_frame + self._has_frame = False + frame = self._read_frame + self._read_frame = None + + if has_frame: + if not self._process_frame(frame): + break + else: + await aio.sleep(0.001) + + if isLinux: + def _process_frame(self: 'FTCamera', frame: v4l.Frame) -> bool: + """Process captured frames. + + Operates only on YUV422 format right now. Calls _decode_yuv422 + for processing the frame. See _decode_yuv422_y_only for an + optimized version producing only Y grayscale frame. + + The captured frame is reshaped to (height, width, 3) before + sending it to "callback_frame". + """ + if not self.callback_frame or len(frame.data) == 0: + return True + + try: + match frame.pixel_format: + case v4l.PixelFormat.YUYV: + self._decode_yuv422(frame.data) + case _: + FTCamera._logger.error("Unsupported pixel format: {}". + format(frame.pixel_format)) + return False + self.callback_frame(self._arr_merge.reshape( + [frame.height, frame.width, 3])) + + except aio.CancelledError: + raise + except Exception: + FTCamera._logger.error(traceback.format_exc()) + return False + return True + else: + def _process_frame(self: 'FTCamera', frame: np.ndarray) -> bool: + if not self.callback_frame or len(frame) == 0: + return True + try: + match self._format.pixel_format: + case 'YUY2': + self._decode_yuv422(frame) + case _: + FTCamera._logger.error( + "Unsupported pixel format: {}".format( + self._format.pixel_format)) + return False + self.callback_frame( + self._arr_merge.reshape([self._frame_size.height, + self._frame_size.width, 3])) + except aio.CancelledError: + raise + except Exception: + FTCamera._logger.error(traceback.format_exc()) + return False + return True + + if isLinux: + def _decode_yuv422(self: 'FTCamera', frame: list[bytes]) -> None: + """Decode YUV422 frame into YUV444 frame.""" + self._arr_data[:] = np.frombuffer(frame, dtype=np.uint8) + + self._arr_merge[:, 0] = np.array(self._arr_data[0::2]) + self._arr_c2[:] = np.array(self._arr_data[1::4]) + self._arr_c3[:] = np.array(self._arr_data[3::4]) + + self._arr_merge[0:self._pixel_count:2, 1] = self._arr_c2 + self._arr_merge[1:self._pixel_count:2, 1] = self._arr_c2 + self._arr_merge[0:self._pixel_count:2, 2] = self._arr_c3 + self._arr_merge[1:self._pixel_count:2, 2] = self._arr_c3 + else: + def _decode_yuv422(self: 'FTCamera', frame: np.ndarray) -> None: + self._arr_merge[:, 0] = frame[:, :, 0].ravel(order='F') + + self._arr_c2[:] = np.array(frame[:, :, 1:].ravel(order='F')[0::2]) + self._arr_c3[:] = np.array(frame[:, :, 1:].ravel(order='F')[1::2]) + + self._arr_merge[0:self._pixel_count:2, 1] = self._arr_c2 + self._arr_merge[1:self._pixel_count:2, 1] = self._arr_c2 + self._arr_merge[0:self._pixel_count:2, 2] = self._arr_c3 + self._arr_merge[1:self._pixel_count:2, 2] = self._arr_c3 + + def _decode_yuv422_y_only(self: 'FTCamera', frame: list[bytes]) -> None: + """Fast version of _decode_yuv422. + + This version is faster since it only copies the Y channel + of the image data. The result is thus a single channel + image (grayscale image). This is suitible for cameras + like the VIVE that output the same image on all channels + """ + self._arr_data[:] = np.frombuffer(frame, dtype=np.uint8) + + self._arr_merge[:, 0] = np.array(self._arr_data[0::2]) diff --git a/BabbleApp/vivefacialtracker/vivetracker.py b/BabbleApp/vivefacialtracker/vivetracker.py new file mode 100644 index 0000000..e26a1d5 --- /dev/null +++ b/BabbleApp/vivefacialtracker/vivetracker.py @@ -0,0 +1,821 @@ +""" +MIT License + +Copyright DragonDreams GmbH 2024 + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +""" + +import platform +import logging +import ctypes +import time +import cv2 as cv +import numpy as np +from timeit import default_timer as timer + +isLinux = platform.system() == 'Linux' + +if isLinux: + import fcntl + + _IOC_NRBITS = 8 + _IOC_TYPEBITS = 8 + _IOC_SIZEBITS = 14 + _IOC_DIRBITS = 2 + + _IOC_NRSHIFT = 0 + _IOC_TYPESHIFT = _IOC_NRSHIFT + _IOC_NRBITS + _IOC_SIZESHIFT = _IOC_TYPESHIFT + _IOC_TYPEBITS + _IOC_DIRSHIFT = _IOC_SIZESHIFT + _IOC_SIZEBITS + + _IOC_WRITE = 1 + _IOC_READ = 2 + + def _IOC(dir_, type_, nr, size): + return ( + ctypes.c_int32(dir_ << _IOC_DIRSHIFT).value + | ctypes.c_int32(ord(type_) << _IOC_TYPESHIFT).value + | ctypes.c_int32(nr << _IOC_NRSHIFT).value + | ctypes.c_int32(size << _IOC_SIZESHIFT).value + ) + + def _IOC_TYPECHECK(t): + return ctypes.sizeof(t) + + def _IOWR(type_, nr, size): + return _IOC(_IOC_READ | _IOC_WRITE, type_, nr, _IOC_TYPECHECK(size)) + +else: + import pygrabber.dshow_graph as pgdsg + import comtypes as comt + import ctypes.wintypes as ctwt + + c_void_p = ctypes.c_void_p + c_wchar = ctypes.c_wchar + c_wchar_p = ctypes.c_wchar_p + c_ulong = ctypes.c_ulong + c_uint = ctypes.c_uint + c_uint8 = ctypes.c_uint8 + c_uint16 = ctypes.c_uint16 + c_enum = ctypes.c_int + Structure = ctypes.Structure + POINTER = ctypes.POINTER + DWORD = ctwt.DWORD + COMMETHOD = comt.COMMETHOD + GUID = comt.GUID + REFIID = POINTER(GUID) + IUnknown = comt.IUnknown + HRESULT = ctypes.HRESULT + + KSNODETYPE_DEV_SPECIFIC = GUID('{941C7AC0-C559-11D0-8A2B-00A0C9255AC1}') + GUID_EXT_CTRL_UNIT = GUID('{2ccb0bda-6331-4fdb-850e-79054dbd5671}') + + class KSPROPERTY(Structure): + _fields_ = [ + ('Set', GUID), + ('Id', c_ulong), + ('Flags', c_ulong) + ] + + class KSP_NODE(Structure): + _fields_ = [ + ('Property', KSPROPERTY), + ('NodeId', ctypes.c_ulong), + ('Reserved', ctypes.c_ulong) + ] + + class KSTOPOLOGY_CONNECTION(Structure): + _fields_ = [ + ('FromNode', c_ulong), + ('FromNodePin', c_ulong), + ('ToNode', c_ulong), + ('ToNodePin', c_ulong) + ] + + class IExtensionUnit(IUnknown): + _case_insensitive_ = True + 'IExtensionUnit Interface' + _iid_ = GUID_EXT_CTRL_UNIT + _idlflags_ = [] + _methods_ = [ + COMMETHOD( + [], HRESULT, 'get_InfoSize', + (['out'], POINTER(c_ulong), 'pulSize')), + COMMETHOD( + [], HRESULT, 'get_Info', + (['in'], c_ulong, 'ulSize'), + (['in', 'out'], POINTER(c_uint8), 'pInfo')), + COMMETHOD( + [], HRESULT, 'get_PropertySize', + (['in'], c_ulong, 'PropertyId'), + (['out'], POINTER(c_ulong), 'pulSize')), + COMMETHOD( + [], HRESULT, 'get_Property', + (['in'], c_ulong, 'PropertyId'), + (['in'], c_ulong, 'ulSize'), + (['in', 'out'], POINTER(c_uint8), 'pValue')), + COMMETHOD( + [], HRESULT, 'put_Property', + (['in'], c_ulong, 'PropertyId'), + (['in'], c_ulong, 'ulSize'), + (['in', 'out'], POINTER(c_uint8), 'pValue')), + COMMETHOD( + [], HRESULT, 'get_PropertyRange', + (['in'], c_ulong, 'PropertyId'), + (['in'], c_ulong, 'ulSize'), + (['in', 'out'], POINTER(c_uint8), 'pMin'), + (['in', 'out'], POINTER(c_uint8), 'pMax'), + (['in', 'out'], POINTER(c_uint8), 'pSteppingDelta'), + (['in', 'out'], POINTER(c_uint8), 'pDefault')) + ] + + class IKsTopologyInfo(IUnknown): + _case_insensitive_ = True + 'IKsTopologyInfo Interface' + _iid_ = GUID('{720D4AC0-7533-11D0-A5D6-28DB04C10000}') + _idlflags_ = [] + _methods_ = [ + COMMETHOD( + [], HRESULT, 'get_NumCategories', + (['out'], POINTER(DWORD), 'pdwNumCategories')), + COMMETHOD( + [], HRESULT, 'get_Category', + (['in'], DWORD, 'dwIndex'), + (['out'], POINTER(GUID), 'pCategory')), + COMMETHOD( + [], HRESULT, 'get_NumConnections', + (['out'], POINTER(DWORD), 'pdwNumConnections')), + COMMETHOD( + [], HRESULT, 'get_ConnectionInfo', + (['in'], DWORD, 'dwIndex'), + (['out'], POINTER(KSTOPOLOGY_CONNECTION), 'pConnectionInfo')), + COMMETHOD( + [], HRESULT, 'get_NodeName', + (['in'], DWORD, 'dwNodeId'), + # pwchNodeName is actually 'out' but not possible to + # be declared like this in comtypes + (['in'], c_wchar_p, 'pwchNodeName'), + (['in'], DWORD, 'dwBufSize'), + (['out'], POINTER(DWORD), 'pdwNameLen')), + COMMETHOD( + [], HRESULT, 'get_NumNodes', + (['out'], POINTER(DWORD), 'pdwNumNodes')), + COMMETHOD( + [], HRESULT, 'get_NodeType', + (['in'], DWORD, 'dwNodeId'), + (['out'], POINTER(GUID), 'pNodeType')), + COMMETHOD( + [], HRESULT, 'CreateNodeInstance', + (['in'], DWORD, 'dwNodeId'), + (['in'], REFIID, 'iid'), + (['out'], POINTER(POINTER(IUnknown)), 'ppvObject'))] + + class KSPROPERTY(Structure): + _fields_ = [ + ('Set', GUID), + ('Id', c_ulong), + ('Flags', c_ulong) + ] + + class KSMETHOD(Structure): + _fields_ = [ + ('Set', GUID), + ('Id', c_ulong), + ('Flags', c_ulong) + ] + + class KSEVENT(Structure): + _fields_ = [ + ('Set', GUID), + ('Id', c_ulong), + ('Flags', c_ulong) + ] + + class KSP_NODE(Structure): + _fields_ = [ + ('Property', KSPROPERTY), + ('NodeId', c_ulong), + ('Reserved', c_ulong) + ] + + class IKsControl(IUnknown): + _case_insensitive_ = True + 'IKsControl Interface' + _iid_ = GUID('{28F54685-06FD-11D2-B27A-00A0C9223196}') + _idlflags_ = [] + _methods_ = [ + COMMETHOD( + [], HRESULT, 'KsProperty', + # (['in'], POINTER(KSPROPERTY), 'Property'), + (['in'], POINTER(KSP_NODE), 'Property'), + (['in'], c_ulong, 'PropertyLength'), + (['in'], c_void_p, 'PropertyData'), + (['in'], c_ulong, 'DataLength'), + (['in'], POINTER(c_ulong), 'BytesReturned')), + COMMETHOD( + [], HRESULT, 'KsMethod', + (['in'], POINTER(KSMETHOD), 'Method'), + (['in'], c_ulong, 'MethodLength'), + (['in', 'out'], c_void_p, 'MethodData'), + (['in'], c_ulong, 'DataLength'), + (['out'], POINTER(c_ulong), 'BytesReturned')), + COMMETHOD( + [], HRESULT, 'KsEvent', + (['in'], POINTER(KSEVENT), 'Event'), + (['in'], c_ulong, 'EventLength'), + (['in', 'out'], c_void_p, 'EventData'), + (['in'], c_ulong, 'DataLength'), + (['out'], POINTER(c_ulong), 'BytesReturned')) + ] + + def _find_extension_node(topo: IKsTopologyInfo, guid: GUID) -> int | None: + count = topo.get_NumNodes() + return next(i for i in range(count) if topo.get_NodeType(i) == guid) + + def _list_all_nodes(topo: IKsTopologyInfo) -> None: + count = topo.get_NumNodes() + name_buf = ctypes.create_unicode_buffer(255) + for i in range(count): + topo.get_NodeName(i, name_buf, 255) + guid = topo.get_NodeType(i) + print("- '{}' '{}'".format(name_buf.value, guid)) + + KSPROPERTY_EXTENSION_UNIT_INFO = 0 + KSPROPERTY_TYPE_GET = 0x1 + KSPROPERTY_TYPE_SET = 0x2 + KSPROPERTY_TYPE_TOPOLOGY = 0x10000000 + + def _control_propery_request(control: IKsControl, index: int, + node: int, data: list[c_uint8]) -> int: + extprop = KSP_NODE( + KSPROPERTY(GUID_EXT_CTRL_UNIT, index, + KSPROPERTY_TYPE_GET | KSPROPERTY_TYPE_TOPOLOGY), + node, 0) + bytes_returned = ctypes.c_ulong(0) + """ + print(["{}={}".format(n, getattr(extprop, n)) + for n, t in extprop._fields_]) + print(["{}={}".format(n, getattr(extprop.Property, n)) + for n, t in extprop.Property._fields_]) + print(("_control_propery_request: index={} node={}," + + " len(data)={}, prop_size={}").format( + index, node, len(data), _control_propery_request_len( + control, index, node))) + """ + control.KsProperty(extprop, ctypes.sizeof(extprop), + data, len(data), bytes_returned) + # print("GetControlProperty: rec {}".format(bytes_returned.value)) + return bytes_returned.value + + def _control_propery_request_len(control: IKsControl, index: int, + node: int) -> int: + extprop = KSP_NODE( + KSPROPERTY(GUID_EXT_CTRL_UNIT, index, + KSPROPERTY_TYPE_GET | KSPROPERTY_TYPE_TOPOLOGY), + node, 0) + bytes_returned = ctypes.c_ulong(0) + try: + control.KsProperty(extprop, ctypes.sizeof(extprop), + None, 0, bytes_returned) + except comt.COMError as e: + if e.hresult == -2147024662: # more data available + return bytes_returned.value + return 0 + + class KSPROPXUINFO(Structure): + _fields_ = [ + ('Length', c_uint8), + ('DescriptorType', c_uint8), + ('DescriptorSubtype', c_uint8), + ('bUnitID', c_uint8), + ('guidExtensionCode', GUID), + ('bNumControls', c_uint8), + ('bNrInPins', c_uint8) + # ('baSourceID', c_uint8 * 64) + ] + + del COMMETHOD, GUID, HRESULT, POINTER, DWORD, REFIID + del c_void_p, c_wchar, c_wchar_p, c_ulong, c_uint, c_uint8 + del c_uint16, c_enum, Structure + + +class ViveTracker: + """Provides support to activate data steam on VIVE Facial Tracker camera.""" + _XU_TASK_SET = 0x50 + _XU_TASK_GET = 0x51 + _XU_REG_SENSOR = 0xab + + if isLinux: + _UVC_SET_CUR = 0x01 + _UVC_GET_CUR = 0x81 + _UVC_GET_MIN = 0x82 + _UVC_GET_MAX = 0x83 + _UVC_GET_RES = 0x84 + _UVC_GET_LEN = 0x85 + _UVC_GET_INFO = 0x86 + _UVC_GET_DEF = 0x87 + + class _uvc_xu_control_query(ctypes.Structure): + _fields_ = [ + ('unit', ctypes.c_uint8), + ('selector', ctypes.c_uint8), + ('query', ctypes.c_uint8), + ('size', ctypes.c_uint16), + ('data', ctypes.POINTER(ctypes.c_uint8)), + ] + + _UVCIOC_CTRL_QUERY = _IOWR('u', 0x21, _uvc_xu_control_query) + + _logger = logging.getLogger("evcta.ViveTracker") + + if isLinux: + def __init__(self: 'ViveTracker', fd: int) -> None: + """Create VIVE Face Tracker instance. + + Constructor tries first to detect if this is a VIVE Face Tracker. + Then device parameters are set and the data stream eventually + activated. + + Make sure to call "dispose()" once the tracker is no more needed + to deactivate the data stream. + + Keyword arguments: + fd --- File descriptor of device. Using Video4Linux device use + "device.fileno()" for this argument. Using FTCamera use + "ftcamera.device.fileno()" for this argument. + """ + ViveTracker._logger.info("create vive tracker") + if not fd: + raise Exception("Missing camera file descriptor") + self._fd: int = fd + self._init_common() + else: + def __init__(self: 'ViveTracker', device: pgdsg.VideoInput, + index: int) -> None: + """Create VIVE Face Tracker instance. + + Constructor tries first to detect if this is a VIVE Face Tracker. + Then device parameters are set and the data stream eventually + activated. + + Make sure to call "dispose()" once the tracker is no more needed + to deactivate the data stream. + + Keyword arguments: + device --- DirectShow device + """ + self._device = device + self._device_index = index + self._xu_control: IKsControl = None + + ViveTracker._logger.info("create vive tracker") + + try: + self._open_controller() + self._init_common() + except Exception: + self.dispose() + + def _init_common(self: 'ViveTracker') -> None: + self._bufferSend: list[ctypes.c_uint8] = (ctypes.c_uint8 * 384)() + self._bufferReceive: list[ctypes.c_uint8] = (ctypes.c_uint8 * 384)() + self._bufferRegister: list[ctypes.c_uint8] = (ctypes.c_uint8 * 17)() + + self._dataTest: list[ctypes.c_uint8] = (ctypes.c_uint8 * 384)() + self._dataTest[0] = 0x51 + self._dataTest[1] = 0x52 + self._dataTest[254] = 0x53 + self._dataTest[255] = 0x54 + + self._debug = False + + self._detect_vive_tracker() + self._activate_tracker() + + if isLinux: + @staticmethod + def is_camera_vive_tracker(device: 'v4l.Device') -> bool: + """Detect if this is a VIVE Face Tracker. + + This is done right now by looking at the human readable device + description which might not be fool proof. Better would be to + check the vendor-id(0x0bb4) and device-id (0x0321). But these + can be only found by querying full USB descriptor. Left for + the reader as excercise. + """ + check = "HTC Multimedia Camera" in device.info.card + ViveTracker._logger.info("is_camera_vive_tracker: '{}' -> {}". + format(device.info.card, check)) + return check + else: + @staticmethod + def is_camera_vive_tracker(device: 'pgdsg.VideoInput') -> bool: + check = "HTC Multimedia Camera" in device.Name + ViveTracker._logger.info("is_camera_vive_tracker: '{}' -> {}". + format(device.Name, check)) + return check + + def dispose(self: 'ViveTracker') -> None: + """Dispose of tracker. + + Deactivates data stream.""" + ViveTracker._logger.info("dispose vive tracker") + + if isLinux: + self._deactivate_tracker() + else: + self._deactivate_tracker() + self._close_controller() + + def process_frame(self: 'ViveTracker', data: np.ndarray) -> np.ndarray: + """Process a captured frame. + + Right now this applies a median blur but other manipulations + are possible to improve the image if desired. + + Keyword arguments: + data --- Frame to process + """ + data = cv.medianBlur(data, 5) + return data + + if not isLinux: + def _open_controller(self: 'ViveTracker') -> None: + if self._xu_control: + return + + logger = ViveTracker._logger + + sdenum = pgdsg.SystemDeviceEnum().system_device_enum + filenum = sdenum.CreateClassEnumerator( + comt.GUID(pgdsg.DeviceCategories.VideoInputDevice), dwFlags=0) + moniker, count = filenum.Next(1) + i = 0 + while i != self._device_index and count > 0: + moniker, count = filenum.Next(1) + i = i + 1 + + """ + ipb = comt.persist.IPropertyBag + propbag = moniker.BindToStorage(0, 0, ipb._iid_).QueryInterface(ipb) + self._dev_name = propbag.Read("FriendlyName", pErrorLog=None) + try: + self._dev_desc = propbag.Read("Description", pErrorLog=None) + except Exception: + self._dev_desc = "" + self._dev_path = propbag.Read("DevicePath", pErrorLog=None) + logger.info("device info:") + logger.info("- description: {}".format(self._dev_desc)) + logger.info("- friendly name: {}".format(self._dev_name)) + logger.info("- device path: {}".format(self._dev_path)) + """ + + topo = self._device.instance.QueryInterface(IKsTopologyInfo) + + # _list_all_nodes(topo) + self._xu_node_index = _find_extension_node( + topo, KSNODETYPE_DEV_SPECIFIC) + + xu_node: IUnknown = topo.CreateNodeInstance( + self._xu_node_index, IUnknown._iid_) + + self._xu_control = xu_node.QueryInterface(IKsControl) + + """ + data = (ctypes.c_uint8 * 250)(0) + length = _control_propery_request( + self._xu_control, KSPROPERTY_EXTENSION_UNIT_INFO, + self._xu_node_index, data) + xupi = KSPROPXUINFO.from_buffer_copy(data) + # print(["{:02x}".format(x) for x in data[:length]]) + print(["{}={}".format(n, getattr(xupi, n)) + for n, t in xupi._fields_]) + """ + + def _close_controller(self: 'ViveTracker') -> None: + self._xu_control = None + + def _xu_get_len(self: 'ViveTracker', selector: int) -> int: + """Send GET_LEN command to device extension unit. + + Keyword arguments: + selector --- Selector + """ + if isLinux: + length = (ctypes.c_uint8 * 2)(0, 0) + c = ViveTracker._uvc_xu_control_query( + 4, selector, ViveTracker._UVC_GET_LEN, 2, length) + fcntl.ioctl(self._fd, ViveTracker._UVCIOC_CTRL_QUERY, c) + return (length[1] << 8) + length[0] + else: + return _control_propery_request_len( + self._xu_control, 2, self._xu_node_index) + + def _xu_get_cur(self: 'ViveTracker', selector: int, + data: list[ctypes.c_uint8]) -> None: + """Send GET_CUR command to device extension unit. + + Keyword arguments: + selector --- Selector + data -- Buffer to store response to. Has to be 384 bytes long. + """ + if isLinux: + c = ViveTracker._uvc_xu_control_query( + 4, selector, ViveTracker._UVC_GET_CUR, len(data), data) + fcntl.ioctl(self._fd, ViveTracker._UVCIOC_CTRL_QUERY, c) + else: + xuprop = KSP_NODE( + KSPROPERTY(GUID_EXT_CTRL_UNIT, selector, + KSPROPERTY_TYPE_GET | KSPROPERTY_TYPE_TOPOLOGY), + self._xu_node_index, 0) + received = ctypes.c_ulong(0) + """ + print(["{}={}".format(n, getattr(xuprop, n)) + for n, t in xuprop._fields_]) + print(["{}={}".format(n, getattr(xuprop.Property, n)) + for n, t in xuprop.Property._fields_]) + print("_xu_get_cur: index={} node={}, len(data)={}, prop_size={}". + format(selector, self._xu_node_index, len(data), + _control_propery_request_len( + self._xu_control, selector, self._xu_node_index))) + """ + self._xu_control.KsProperty(xuprop, ctypes.sizeof(xuprop), + data, len(data), received) + # print("received: {}".format(received.value)) + + def _xu_set_cur(self: 'ViveTracker', selector: int, + data: list[ctypes.c_uint8]) -> None: + """Send SET_CUR command to device extension unit. + + Keyword arguments: + selector --- Selector + data -- Data to send. Has to be 384 bytes long. + """ + if isLinux: + c = ViveTracker._uvc_xu_control_query( + 4, selector, ViveTracker._UVC_SET_CUR, len(data), data) + fcntl.ioctl(self._fd, ViveTracker._UVCIOC_CTRL_QUERY, c) + else: + xuprop = KSP_NODE( + KSPROPERTY(GUID_EXT_CTRL_UNIT, selector, + KSPROPERTY_TYPE_SET | KSPROPERTY_TYPE_TOPOLOGY), + self._xu_node_index, 0) + received = ctypes.c_ulong(0) + """ + print(["{}={}".format(n, getattr(xuprop, n)) + for n, t in xuprop._fields_]) + print(["{}={}".format(n, getattr(xuprop.Property, n)) + for n, t in xuprop.Property._fields_]) + print("_xu_set_cur: index={} node={}, len(data)={}, prop_size={}". + format(selector, self._xu_node_index, len(data), + _control_propery_request_len( + self._xu_control, selector, self._xu_node_index))) + """ + self._xu_control.KsProperty(xuprop, ctypes.sizeof(xuprop), + data, len(data), received) + + def _get_len(self: 'ViveTracker') -> int: + """Get buffer length of device.""" + return self._xu_get_len(2) + + def _set_cur(self: 'ViveTracker', command: list[ctypes.c_uint8], + timeout: float = 0.5) -> None: + """Send SET_CUR command to device extension unit with proper handling. + + Sends SET_CUR command to the device. Then sends GET_CUR commands to + device until the "command finished" response is found. + + Keyword arguments: + command --- Command to send. + timeout -- Timeout in seconds. + """ + length = len(command) + self._bufferSend[:length] = command + self._xu_set_cur(2, self._bufferSend) + if self._debug: + ViveTracker._logger.debug("set_cur({})".format( + [hex(x) for x in command[:16]])) + lenbuf = len(self._bufferReceive) + stime = timer() + while True: + self._bufferReceive[:] = (ctypes.c_uint8 * lenbuf)(0) + self._xu_get_cur(2, self._bufferReceive) + if self._bufferReceive[0] == 0x55: + # command not finished yet + if self._debug: + ViveTracker._logger.debug("-> getCur: pending") + elif self._bufferReceive[0] == 0x56: + # the full command is repeated minus the last byte. + # we check only the first 16 bytes here + if self._bufferReceive[1:17] == self._bufferSend[0:16]: + if self._debug: + ViveTracker._logger.debug("-> getCur: finished") + return # command finished + else: + raise Exception( + "set_cur({}): response not matching command". + format([hex(x) for x in command[:16]])) + else: + raise Exception("set_cur({}): invalid response: {}".format( + [hex(x) for x in command[:16]], + [hex(x) for x in self._bufferReceive[:16]])) + + elapsed = (timer() - stime) + if self._debug: + ViveTracker._logger.debug("-> elasped {:d}ms".format( + int(elapsed * 1000))) + if elapsed > timeout: + raise Exception("set_cur({}): timeout".format( + [hex(x) for x in command[:16]])) + + def _set_cur_no_resp(self: 'ViveTracker', + command: list[ctypes.c_uint8]) -> None: + """Send SET_CUR command to device without proper handling. + + Keyword arguments: + command --- Command to send. + """ + self._bufferSend[:len(command)] = command + self._xu_set_cur(2, self._bufferSend) + if self._debug: + ViveTracker._logger.debug("set_cur_no_resp({})".format( + [hex(x) for x in command[:16]])) + + def _init_register(self: 'ViveTracker', command: int, reg: int, + address: int, address_len: int, + value: int, value_len: int) -> None: + """Init buffer for manipulating a register. + + Keyword arguments: + command --- Command + reg --- Register + address --- Address + address_len --- Length of address in bytes + value --- Value + value_len --- Length of value in bytes + """ + br = self._bufferRegister + br[0] = ctypes.c_uint8(command) + br[1] = ctypes.c_uint8(reg) + br[2] = ctypes.c_uint8(0x60) + br[3] = ctypes.c_uint8(address_len) # address width in bytes + br[4] = ctypes.c_uint8(value_len) # data width in bytes + + # address + br[5] = ctypes.c_uint8((address > 24) & 0xff) + br[6] = ctypes.c_uint8((address > 16) & 0xff) + br[7] = ctypes.c_uint8((address > 8) & 0xff) + br[8] = ctypes.c_uint8(address & 0xff) + + # page address + br[9] = ctypes.c_uint8(0x90) + br[10] = ctypes.c_uint8(0x01) + br[11] = ctypes.c_uint8(0x00) + br[12] = ctypes.c_uint8(0x01) + + # value + br[13] = ctypes.c_uint8((value > 24) & 0xff) + br[14] = ctypes.c_uint8((value > 16) & 0xff) + br[15] = ctypes.c_uint8((value > 8) & 0xff) + br[16] = ctypes.c_uint8(value & 0xff) + + def _set_register(self: 'ViveTracker', reg: int, address: int, + value: int, timeout: float = 0.5) -> None: + """Set device register. + + Keyword arguments: + reg --- Register to manipulate + address --- Address to manipulate + value --- Value to set + timeout --- Timeout in seconds. Use 0 to send register without + proper request handling + """ + self._init_register(ViveTracker._XU_TASK_SET, reg, address, 1, value, 1) + if timeout > 0: + self._set_cur(self._bufferRegister, timeout) + else: + self._set_cur_no_resp(self._bufferRegister) + + def _get_register(self: 'ViveTracker', reg: int, address: int, + timeout: float = 0.5) -> int: + """Get device register. + + Keyword arguments: + reg --- Register to fetch + address --- Address to fetch + timeout --- Timeout in seconds + """ + self._init_register(ViveTracker._XU_TASK_GET, reg, address, 1, 0, 1) + self._set_cur(self._bufferRegister, timeout) + return int(self._bufferReceive[17]) + + def _set_register_sensor(self: 'ViveTracker', address: int, value: int, + timeout: float = 0.5) -> None: + """Set device sensor register. + + Keyword arguments: + address --- Address to manipulate + value --- Value to set + timeout --- Timeout in seconds. Use 0 to send register without + proper request handling + """ + self._set_register(ViveTracker._XU_REG_SENSOR, address, value, timeout) + + def _get_register_sensor(self: 'ViveTracker', address: int, + timeout: float = 0.5) -> int: + """Get device sensor register. + + Keyword arguments: + address --- Address to fetch + timeout --- Timeout in seconds + """ + return self._get_register(ViveTracker._XU_REG_SENSOR, address, timeout) + + def _set_enable_stream(self: 'ViveTracker', enable: bool) -> None: + """Enable or disable data stream. + + Keyword arguments: + enable --- Enable or disable data stream. + """ + buf = (ctypes.c_uint8 * 4)(ViveTracker._XU_TASK_SET, 0x14, 0x00, + 0x01 if enable else 0x00) + self._set_cur_no_resp(buf) + + def _detect_vive_tracker(self: 'ViveTracker') -> None: + """Try to detect if this is a VIVE Face Tracker device. + + uses GET_LEN to get the data buffer length. VIVE Face Tracker + uses 384. If this is not the case then this is most probebly + something else but not a VIVE Face Tracker. + """ + if isLinux: + length = self._get_len() + else: + length = _control_propery_request_len( + self._xu_control, 2, self._xu_node_index) + if length != 384: + raise Exception("length check failed: {} instead of 384". + format(length)) + ViveTracker._logger.info("vive tracker detected") + + def _activate_tracker(self: 'ViveTracker') -> None: + """Activate tracker. + + Sets parameters and enables data stream.""" + ViveTracker._logger.info("activate vive tracker") + + ViveTracker._logger.info("-> disable stream") + self._set_cur(self._dataTest) + self._set_enable_stream(False) + time.sleep(0.25) + + ViveTracker._logger.info("-> set camera parameters") + self._set_cur(self._dataTest) + self._set_register_sensor(0x00, 0x40) + self._set_register_sensor(0x08, 0x01) + self._set_register_sensor(0x70, 0x00) + self._set_register_sensor(0x02, 0xff) + self._set_register_sensor(0x03, 0xff) + self._set_register_sensor(0x04, 0xff) + self._set_register_sensor(0x0e, 0x00) + self._set_register_sensor(0x05, 0xb2) + self._set_register_sensor(0x06, 0xb2) + self._set_register_sensor(0x07, 0xb2) + self._set_register_sensor(0x0f, 0x03) + + ViveTracker._logger.info("-> enable stream") + self._set_cur(self._dataTest) + self._set_enable_stream(True) + time.sleep(0.25) + + def _deactivate_tracker(self: 'ViveTracker') -> None: + """Deactivate tracker. + + Disables data stream. + """ + ViveTracker._logger.info("deactivate vive tracker") + + ViveTracker._logger.info("SKIPPED") + """ + ViveTracker._logger.info("-> disable stream") + self._set_cur(self._dataTest) + self._set_enable_stream(False) + time.sleep(0.25) + """