Files
ProjectBabble/BabbleApp/camera.py
T

335 lines
15 KiB
Python

import cv2
import numpy as np
import queue
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
# Serial communication protocol:
# header-begin (2 bytes)
# header-type (2 bytes)
# packet-size (2 bytes)
# packet (packet-size bytes)
ETVR_HEADER = b'\xff\xa0'
ETVR_HEADER_FRAME = b'\xff\xa1'
ETVR_HEADER_LEN = 6
class CameraState(Enum):
CONNECTING = 0
CONNECTED = 1
DISCONNECTED = 2
class Camera:
def __init__(
self,
config: BabbleConfig,
camera_index: int,
cancellation_event: "threading.Event",
capture_event: "threading.Event",
camera_status_outgoing: "queue.Queue[CameraState]",
camera_output_outgoing: "queue.Queue(maxsize=2)",
settings: BabbleSettingsConfig,
):
self.camera_status = CameraState.CONNECTING
self.config = config
self.settings = settings
self.camera_index = camera_index
self.camera_address = config.capture_source
self.camera_status_outgoing = camera_status_outgoing
self.camera_output_outgoing = camera_output_outgoing
self.capture_event = capture_event
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()
self.frame_number = 0
self.fps = 0
self.bps = 0
self.start = True
self.buffer = b''
self.pf_fps = 0
self.prevft = 0
self.newft = 0
self.fl = [0]
self.error_message = f"{Fore.YELLOW}[WARN] Capture source {{}} not found, retrying...{Fore.RESET}"
def __del__(self):
if self.serial_connection is not None:
self.serial_connection.close()
def set_output_queue(self, camera_output_outgoing: "queue.Queue"):
self.camera_output_outgoing = camera_output_outgoing
def run(self):
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
# than this, otherwise we can deadlock ourselves.
if (
self.config.capture_source != None and self.config.capture_source != ""
):
self.current_capture_source = self.config.capture_source
if ("COM" in str(self.config.capture_source)):
if (
self.serial_connection is None
or self.camera_status == CameraState.DISCONNECTED
or self.config.capture_source != self.current_capture_source
):
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
or not self.cv2_camera.isOpened()
or self.camera_status == CameraState.DISCONNECTED
or self.config.capture_source != self.current_capture_source
):
print(self.error_message.format(self.config.capture_source))
# This requires a wait, otherwise we can error and possible screw up the camera
# firmware. Fickle things.
if self.cancellation_event.wait(WAIT_TIME):
return
self.current_capture_source = self.config.capture_source
if self.config.use_ffmpeg == True:
self.cv2_camera = cv2.VideoCapture(self.current_capture_source, cv2.CAP_FFMPEG)
else:
self.cv2_camera = cv2.VideoCapture(self.current_capture_source)
if not self.settings.gui_cam_resolution_x == 0: self.cv2_camera.set(cv2.CAP_PROP_FRAME_WIDTH, self.settings.gui_cam_resolution_x)
if not self.settings.gui_cam_resolution_y == 0: self.cv2_camera.set(cv2.CAP_PROP_FRAME_HEIGHT, self.settings.gui_cam_resolution_y)
if not self.settings.gui_cam_framerate == 0: self.cv2_camera.set(cv2.CAP_PROP_FPS, self.settings.gui_cam_framerate)
should_push = False
else:
# We don't have a capture source to try yet, wait for one to show up in the GUI.
if self.cancellation_event.wait(WAIT_TIME):
self.camera_status = CameraState.DISCONNECTED
return
# Assuming we can access our capture source, wait for another thread to request a capture.
# Cycle every so often to see if our cancellation token has fired. This basically uses a
# python event as a context-less, resettable one-shot channel.
if should_push and not self.capture_event.wait(timeout=0.02):
continue
if self.config.capture_source != None:
if ("COM" in str(self.config.capture_source)):
self.get_serial_camera_picture(should_push)
else:
self.get_cv2_camera_picture(should_push)
if not should_push:
# 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:
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
self.last_frame_time = yeah
if delta_time > 0:
self.bps = len(image) / delta_time
self.frame_number = self.frame_number + 1
self.fps = (self.fps + self.pf_fps) / 2
self.newft = time.time()
self.fps = 1 / (self.newft - self.prevft)
self.prevft = self.newft
self.fps = int(self.fps)
if len(self.fl) < 60:
self.fl.append(self.fps)
else:
self.fl.pop(0)
self.fl.append(self.fps)
self.fps = sum(self.fl) / len(self.fl)
#self.bps = image.nbytes
if should_push:
self.push_image_to_queue(image, frame_number, self.fps)
except:
print(f"{Fore.YELLOW}[WARN] Capture source problem, assuming camera disconnected, waiting for reconnect.{Fore.RESET}")
self.camera_status = CameraState.DISCONNECTED
pass
def get_next_packet_bounds(self):
beg = -1
while beg == -1:
self.buffer += self.serial_connection.read(2048)
beg = self.buffer.find(ETVR_HEADER + ETVR_HEADER_FRAME)
# Discard any data before the frame header.
if beg > 0:
self.buffer = self.buffer[beg:]
beg = 0
# We know exactly how long the jpeg packet is
end = int.from_bytes(self.buffer[4:6], signed=False, byteorder="little")
self.buffer += self.serial_connection.read(end - len(self.buffer))
return beg, end
def get_next_jpeg_frame(self):
beg, end = self.get_next_packet_bounds()
jpeg = self.buffer[beg+ETVR_HEADER_LEN:end+ETVR_HEADER_LEN]
self.buffer = self.buffer[end+ETVR_HEADER_LEN:]
return jpeg
def get_serial_camera_picture(self, should_push):
conn = self.serial_connection
if conn is None:
return
try:
if conn.in_waiting:
jpeg = self.get_next_jpeg_frame()
if jpeg:
# Create jpeg frame from byte string
image = cv2.imdecode(np.fromstring(jpeg, dtype=np.uint8), cv2.IMREAD_UNCHANGED)
if image is None:
print(f"{Fore.YELLOW}[WARN] Frame drop. Corrupted JPEG.{Fore.RESET}")
return
# Discard the serial buffer. This is due to the fact that it
# may build up some outdated frames. A bit of a workaround here tbh.
if conn.in_waiting >= 32768:
print(f"{Fore.CYAN}[INFO] Discarding the serial buffer ({conn.in_waiting} bytes){Fore.RESET}")
conn.reset_input_buffer()
self.buffer = b''
# Calculate the fps.
yeah = time.time()
delta_time = yeah - self.last_frame_time
self.last_frame_time = yeah
if delta_time > 0:
self.bps = len(jpeg) / delta_time
self.fps = (self.fps + self.pf_fps) / 2
self.newft = time.time()
self.fps = 1 / (self.newft - self.prevft)
self.prevft = self.newft
self.fps = int(self.fps)
if len(self.fl) < 60:
self.fl.append(self.fps)
else:
self.fl.pop(0)
self.fl.append(self.fps)
self.fps = sum(self.fl) / len(self.fl)
self.frame_number = self.frame_number + 1
if should_push:
self.push_image_to_queue(image, self.frame_number, self.fps)
except Exception:
print(f"{Fore.YELLOW}[WARN] Serial capture source problem, assuming camera disconnected, waiting for reconnect.{Fore.RESET}")
conn.close()
self.camera_status = CameraState.DISCONNECTED
pass
def start_serial_connection(self, port):
if self.serial_connection is not None and self.serial_connection.is_open:
# Do nothing. The connection is already open on this port.
if self.serial_connection.port == port:
return
# Otherwise, close the connection before trying to reopen.
self.serial_connection.close()
com_ports = [tuple(p) for p in list(serial.tools.list_ports.comports())]
# Do not try connecting if no such port i.e. device was unplugged.
if not any(p for p in com_ports if port in p):
return
try:
conn = serial.Serial(
baudrate = 3000000,
port = port,
xonxoff=False,
dsrdtr=False,
rtscts=False)
# Set explicit buffer size for serial.
conn.set_buffer_size(rx_size = 32768, tx_size = 32768)
print(f"{Fore.CYAN}[INFO] ETVR Serial Tracker device connected on {port}{Fore.RESET}")
self.serial_connection = conn
self.camera_status = CameraState.CONNECTED
except Exception:
print(f"{Fore.CYAN}[INFO] Failed to connect on {port}{Fore.RESET}")
self.camera_status = CameraState.DISCONNECTED
def push_image_to_queue(self, image, frame_number, fps):
# If there's backpressure, just yell. We really shouldn't have this unless we start getting
# some sort of capture event conflict though.
qsize = self.camera_output_outgoing.qsize()
if qsize > 1:
print(
f"{Fore.YELLOW}[WARN] CAPTURE QUEUE BACKPRESSURE OF {qsize}. CHECK FOR CRASH OR TIMING ISSUES IN ALGORITHM.{Fore.RESET}")
self.camera_output_outgoing.put((image, frame_number, fps))
self.capture_event.clear()