mirror of
https://github.com/Project-Babble/ProjectBabble.git
synced 2026-11-20 00:51:16 -08:00
adding vive facial tracker handling from
https://github.com/LordOfDragons/vivefacialtracker. linux so far
This commit is contained in:
1 parent
9f596ebf0d
commit
9decd613dd
4 files changed
+1504
-5
No files matched your search
@@ -21,3 +21,4 @@ scripts/installer.iss
|
||||
Glia_cap.py
|
||||
lazyass_minecraft_script.py
|
||||
scripts/example_build_app_and_installer.bat
|
||||
*.kdev*
|
||||
+69
-5
@@ -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
|
||||
|
||||
@@ -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])
|
||||
@@ -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)
|
||||
"""
|
||||
Reference in new issue
Block a user