adding vive facial tracker handling from

https://github.com/LordOfDragons/vivefacialtracker.
linux so far
This commit is contained in:
Plüss Roland committed 2024-09-13 20:05:39 +02:00
1 parent 9f596ebf0d
commit 9decd613dd
4 files changed
+1504 -5

No files matched your search

+1
View File
@@ -21,3 +21,4 @@ scripts/installer.iss
Glia_cap.py
lazyass_minecraft_script.py
scripts/example_build_app_and_installer.bat
*.kdev*
+69 -5
View File
@@ -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
+613
View File
@@ -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])
+821
View File
@@ -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)
"""