refractor capture to a module
This commit is contained in:
ykcheungab
+123
-162
@@ -6,13 +6,12 @@ optical tactile sensor data with support for stitching, optical flow tracking,
|
||||
and Helmholtz-Hodge decomposition.
|
||||
"""
|
||||
|
||||
import os
|
||||
# import os # only used by commented-out stitch_cal
|
||||
import numpy as np
|
||||
import cv2
|
||||
import time
|
||||
import threading
|
||||
from enum import Enum, auto
|
||||
from typing import Tuple
|
||||
from typing import Optional, Tuple
|
||||
# from threading import Thread, Event
|
||||
# from queue import Queue, Empty
|
||||
|
||||
@@ -24,6 +23,14 @@ from .get_depth import Contact3D
|
||||
from .backend_availability import (check_cupy_available, check_opencl_available)
|
||||
from .frame_processors import create_frame_processor
|
||||
from .signal_processing import DEFAULT_SAMPLE_RATE_HZ, LowpassFilter
|
||||
from .capture import (
|
||||
CaptureConfig,
|
||||
CaptureWorker,
|
||||
ReconnectPolicy,
|
||||
is_live_camera,
|
||||
normalize_source,
|
||||
open_capture,
|
||||
)
|
||||
|
||||
class Sensor:
|
||||
"""
|
||||
@@ -61,7 +68,9 @@ class Sensor:
|
||||
|
||||
def __init__(self, vid_src, cuda=True, isstitch=True, verbose=True, debug=False,
|
||||
config_name="prototype", cal_path=None, override=False, box=None, mirror=True,
|
||||
backend='auto'):
|
||||
backend='auto', capture_config: Optional[CaptureConfig] = None,
|
||||
auto_reconnect: bool = False, capture_mode: str = "latest",
|
||||
capture_queue_size: int = 1, capture_overflow: str = "drop_oldest"):
|
||||
"""
|
||||
Initialize Sensor with video source and configuration.
|
||||
|
||||
@@ -79,8 +88,11 @@ class Sensor:
|
||||
"cupy": Use CuPy for GPU acceleration
|
||||
"opencl": Use OpenCL for GPU acceleration
|
||||
"cpu": Use CPU for computation
|
||||
hhd_downsample: Downsample factor for flow field before HHD (default: 1, no downsampling).
|
||||
Factor of 2 halves each dimension, giving ~4x speedup in Poisson solve.
|
||||
capture_config: Optional ``CaptureConfig`` overriding capture kwargs below.
|
||||
auto_reconnect: Enable USB disconnect auto-reconnect for live cameras.
|
||||
capture_mode: ``"latest"`` (low latency, default) or ``"queue"``.
|
||||
capture_queue_size: Bounded queue size for queue mode (1..10).
|
||||
capture_overflow: ``drop_oldest`` | ``drop_newest`` | ``block``.
|
||||
"""
|
||||
|
||||
self.CUPY_AVAILABLE = check_cupy_available()
|
||||
@@ -92,7 +104,7 @@ class Sensor:
|
||||
self._external_only = isinstance(vid_src, str) and vid_src.lower() == "external"
|
||||
|
||||
# Store settings
|
||||
self.vid_src = vid_src
|
||||
self.vid_src = normalize_source(vid_src) if not self._external_only else vid_src
|
||||
self.isstitch = isstitch
|
||||
self.cuda = cuda
|
||||
self.verbose = verbose
|
||||
@@ -100,6 +112,11 @@ class Sensor:
|
||||
self.config_name = config_name
|
||||
self.cal_path = cal_path
|
||||
self.mirror = mirror
|
||||
self._capture_config_override = capture_config
|
||||
self._auto_reconnect = bool(auto_reconnect)
|
||||
self._capture_mode = capture_mode
|
||||
self._capture_queue_size = int(capture_queue_size)
|
||||
self._capture_overflow = capture_overflow
|
||||
|
||||
# Load configuration
|
||||
self.__config_cam = None
|
||||
@@ -194,21 +211,16 @@ class Sensor:
|
||||
self.masked_poisson_max_iters = self.__config_tracker.get("masked_poisson_max_iters", 3)
|
||||
self.masked_poisson_tol = self.__config_tracker.get("masked_poisson_tol", 1e-4)
|
||||
self.masked_poisson_reg = self.__config_tracker.get("masked_poisson_reg", 1e-8)
|
||||
self._use_pyav = False
|
||||
self._av_container = None
|
||||
self._av_stream = None
|
||||
self._av_decoder = None
|
||||
self.cap = None
|
||||
# Legacy PyAV capture state (unused after orisys.capture refactor).
|
||||
# self._use_pyav = False
|
||||
# self._av_container = None
|
||||
# self._av_stream = None
|
||||
# self._av_decoder = None
|
||||
self.cap = None # compat alias for _capture_worker.source.cap
|
||||
self._capture_worker: Optional[CaptureWorker] = None
|
||||
self._is_live_camera = (
|
||||
isinstance(self.vid_src, int)
|
||||
or (isinstance(self.vid_src, str) and self.vid_src.startswith("/dev/video"))
|
||||
) and not self._external_only
|
||||
self._capture_thread = None
|
||||
self._capture_running = False
|
||||
self._latest_frame = None
|
||||
self._latest_frame_seq = 0
|
||||
self._last_consumed_seq = -1
|
||||
self._latest_frame_lock = threading.Lock()
|
||||
is_live_camera(self.vid_src) and not self._external_only
|
||||
)
|
||||
|
||||
# Setup video capture unless this sensor is fed externally.
|
||||
if self._external_only:
|
||||
@@ -679,23 +691,30 @@ class Sensor:
|
||||
|
||||
def disconnect(self):
|
||||
"""Release resources and stop async threads."""
|
||||
self._capture_running = False
|
||||
if self._capture_thread is not None:
|
||||
self._capture_thread.join(timeout=1.0)
|
||||
self._capture_thread = None
|
||||
|
||||
if self._use_pyav:
|
||||
if self._av_container is not None:
|
||||
self._av_container.close()
|
||||
self._av_container = None
|
||||
self._av_stream = None
|
||||
self._av_decoder = None
|
||||
if self.verbose:
|
||||
print("Sensor disconnected.")
|
||||
elif self.cap is not None and self.cap.isOpened():
|
||||
self.cap.release()
|
||||
if self._capture_worker is not None:
|
||||
self._capture_worker.close()
|
||||
self._capture_worker = None
|
||||
self.cap = None
|
||||
if self.verbose:
|
||||
print("Sensor disconnected.")
|
||||
# Legacy PyAV / direct-cap fallbacks (unused after orisys.capture refactor).
|
||||
# elif self._use_pyav:
|
||||
# if self._av_container is not None:
|
||||
# self._av_container.close()
|
||||
# self._av_container = None
|
||||
# self._av_stream = None
|
||||
# self._av_decoder = None
|
||||
# if self.verbose:
|
||||
# print("Sensor disconnected.")
|
||||
# elif self.cap is not None:
|
||||
# try:
|
||||
# if self.cap.isOpened():
|
||||
# self.cap.release()
|
||||
# except Exception:
|
||||
# pass
|
||||
# self.cap = None
|
||||
# if self.verbose:
|
||||
# print("Sensor disconnected.")
|
||||
cv2.destroyAllWindows()
|
||||
|
||||
# =========================================================================
|
||||
@@ -830,12 +849,13 @@ class Sensor:
|
||||
"compound_eye" if "stitcher" in self._config else "single_camera",
|
||||
)
|
||||
self.__config_cam = self._config["camera"]
|
||||
self._config_resolution_raw = self.__config_cam["resolution_raw"]
|
||||
self._config_fps = self.__config_cam["fps"]
|
||||
# Capture resolution/fps now applied via CaptureConfig.from_camera_dict(__config_cam).
|
||||
# self._config_resolution_raw = self.__config_cam["resolution_raw"]
|
||||
# self._config_fps = self.__config_cam["fps"]
|
||||
self._config_units = self.__config_cam.get("units", 1)
|
||||
self._config_resolution_stitch = self.__config_cam["resolution_stitch"]
|
||||
|
||||
self._config_dis_preset = self._config["tracker"]["dis_preset"]
|
||||
# self._config_dis_preset = self._config["tracker"]["dis_preset"]
|
||||
|
||||
self.__config_tracker = self._config["tracker"]
|
||||
self.__config_stitcher = self._config.get("stitcher")
|
||||
@@ -850,71 +870,34 @@ class Sensor:
|
||||
|
||||
def _open_video_source(self, override, box):
|
||||
"""Initialize video capture and calibration."""
|
||||
# _is_camera_index = isinstance(self.vid_src, int)
|
||||
# _is_v4l2_path = isinstance(self.vid_src, str) and self.vid_src.startswith("/dev/video")
|
||||
# use_linux_camera_pyav = (
|
||||
# platform.system() == "Linux"
|
||||
# and (_is_camera_index or _is_v4l2_path)
|
||||
# and av is not None
|
||||
# )
|
||||
|
||||
# if use_linux_camera_pyav:
|
||||
# device = self.vid_src if _is_v4l2_path else f"/dev/video{self.vid_src}"
|
||||
# if self.verbose:
|
||||
# print(f"Using PyAV Linux camera backend: {device}")
|
||||
# try:
|
||||
# self._av_container = av.open(
|
||||
# device,
|
||||
# format="v4l2",
|
||||
# options={
|
||||
# "input_format": "mjpeg",
|
||||
# "video_size": f"{self._config_resolution_raw[0]}x{self._config_resolution_raw[1]}",
|
||||
# "framerate": str(self._config_fps),
|
||||
# },
|
||||
# )
|
||||
# self._av_stream = self._av_container.streams.video[0]
|
||||
# self._av_decoder = self._av_container.decode(self._av_stream)
|
||||
# self._use_pyav = True
|
||||
# except Exception as exc:
|
||||
# if self.verbose:
|
||||
# print(f"PyAV camera open failed ({exc}), fallback to OpenCV backend.")
|
||||
# self._use_pyav = False
|
||||
|
||||
self.use_pyav = False
|
||||
if not self._use_pyav:
|
||||
self.cap = cv2.VideoCapture(self.vid_src)
|
||||
if self._is_live_camera:
|
||||
self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc('M', 'J', 'P', 'G'))
|
||||
self.cap.set(cv2.CAP_PROP_FPS, self._config_fps)
|
||||
self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self._config_resolution_raw[0])
|
||||
self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self._config_resolution_raw[1])
|
||||
|
||||
|
||||
if self.verbose:
|
||||
print("Connecting to video source...")
|
||||
|
||||
|
||||
|
||||
if self._capture_config_override is not None:
|
||||
capture_config = self._capture_config_override
|
||||
else:
|
||||
capture_config = CaptureConfig.from_camera_dict(
|
||||
self.vid_src,
|
||||
self.__config_cam,
|
||||
mode=self._capture_mode,
|
||||
queue_size=self._capture_queue_size,
|
||||
overflow=self._capture_overflow,
|
||||
reconnect=ReconnectPolicy(enabled=self._auto_reconnect),
|
||||
verbose=self.verbose,
|
||||
start_worker=False,
|
||||
)
|
||||
|
||||
try:
|
||||
self._capture_worker = open_capture(capture_config, start_worker=False)
|
||||
except Exception as exc:
|
||||
raise Exception("Failed to open video source.") from exc
|
||||
|
||||
self.cap = self._capture_worker.source.cap
|
||||
# self._use_pyav = False
|
||||
|
||||
if not self._is_source_open():
|
||||
raise Exception("Failed to open video source.")
|
||||
|
||||
if self.verbose and not self._use_pyav and self.cap is not None:
|
||||
fourcc_int = int(self.cap.get(cv2.CAP_PROP_FOURCC))
|
||||
fourcc_bytes = [(fourcc_int >> (8 * i)) & 0xFF for i in range(4)]
|
||||
# Show printable FOURCC only; some backends return 0 or non-printable bytes.
|
||||
if fourcc_int <= 0:
|
||||
fourcc = "N/A"
|
||||
else:
|
||||
fourcc = "".join(chr(b) if 32 <= b <= 126 else "." for b in fourcc_bytes)
|
||||
width = int(self.cap.get(cv2.CAP_PROP_FRAME_WIDTH))
|
||||
height = int(self.cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
|
||||
fps = self.cap.get(cv2.CAP_PROP_FPS)
|
||||
backend_id = int(self.cap.get(cv2.CAP_PROP_BACKEND))
|
||||
print(
|
||||
f"OpenCV negotiated stream: FOURCC='{fourcc}' "
|
||||
f"(int={fourcc_int}, hex=0x{fourcc_int:08X}), "
|
||||
f"size={width}x{height}, fps={fps:.2f}, backend_id={backend_id}"
|
||||
)
|
||||
|
||||
# Wait for first valid frame
|
||||
frame = None
|
||||
while frame is None:
|
||||
@@ -936,10 +919,9 @@ class Sensor:
|
||||
img = self.frame_processor.process(frame)
|
||||
self.initial_img = self._make_initial_gray(img)
|
||||
|
||||
# For live cameras, start a latest-frame capture thread to avoid latency buildup.
|
||||
if self._is_live_camera:
|
||||
self._start_capture_thread()
|
||||
|
||||
# For live cameras, start a capture worker to avoid latency buildup.
|
||||
if self._is_live_camera and self._capture_worker is not None:
|
||||
self._capture_worker.start()
|
||||
|
||||
def _make_initial_gray(self, img_bgr):
|
||||
gray = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2GRAY)
|
||||
@@ -978,73 +960,52 @@ class Sensor:
|
||||
print(f"Sensor: {status}")
|
||||
return n_boxes
|
||||
|
||||
def _extract_tiles(self, img_src):
|
||||
"""Extract tiles from source image."""
|
||||
self.stitcher.get_stitch_unit(img_src)
|
||||
self.stitcher.extract_tiles()
|
||||
return True
|
||||
# Unused after frame_processor owns stitch extract/process.
|
||||
# def _extract_tiles(self, img_src):
|
||||
# """Extract tiles from source image."""
|
||||
# self.stitcher.get_stitch_unit(img_src)
|
||||
# self.stitcher.extract_tiles()
|
||||
# return True
|
||||
|
||||
def _is_source_open(self):
|
||||
if self._use_pyav:
|
||||
return self._av_container is not None
|
||||
return self.cap is not None and self.cap.isOpened()
|
||||
if self._capture_worker is not None:
|
||||
return self._capture_worker.source.is_open()
|
||||
return False
|
||||
# Legacy PyAV / direct-cap fallbacks (unused after orisys.capture refactor).
|
||||
# if self._use_pyav:
|
||||
# return self._av_container is not None
|
||||
# return self.cap is not None and self.cap.isOpened()
|
||||
|
||||
def _read_frame(self):
|
||||
if self._capture_running:
|
||||
# Startup race guard: wait briefly until capture thread publishes first frame.
|
||||
if self._latest_frame is None:
|
||||
deadline = time.perf_counter() + 1.0
|
||||
while time.perf_counter() < deadline:
|
||||
with self._latest_frame_lock:
|
||||
if self._latest_frame is not None:
|
||||
break
|
||||
time.sleep(0.001)
|
||||
|
||||
# Return latest frame immediately (do not wait for next camera tick).
|
||||
# This keeps end-to-end latency minimal; caller may receive repeated
|
||||
# frames if processing runs faster than camera FPS.
|
||||
with self._latest_frame_lock:
|
||||
frame = self._latest_frame
|
||||
seq = self._latest_frame_seq
|
||||
if frame is not None:
|
||||
self._last_consumed_seq = seq
|
||||
return True, frame.copy()
|
||||
return False, None
|
||||
if self._capture_worker is not None and self._capture_worker.running:
|
||||
return self._capture_worker.read(copy=True, wait_s=1.0)
|
||||
return self._read_frame_raw()
|
||||
|
||||
def _read_frame_raw(self):
|
||||
if self._use_pyav:
|
||||
if self._av_decoder is None:
|
||||
return False, None
|
||||
try:
|
||||
frame = next(self._av_decoder)
|
||||
except StopIteration:
|
||||
return False, None
|
||||
except Exception:
|
||||
return False, None
|
||||
return True, frame.to_ndarray(format="bgr24")
|
||||
if self.cap is None:
|
||||
return False, None
|
||||
return self.cap.read()
|
||||
if self._capture_worker is not None:
|
||||
return self._capture_worker.source.read_raw()
|
||||
return False, None
|
||||
# Legacy PyAV / direct-cap fallbacks (unused after orisys.capture refactor).
|
||||
# if self._use_pyav:
|
||||
# if self._av_decoder is None:
|
||||
# return False, None
|
||||
# try:
|
||||
# frame = next(self._av_decoder)
|
||||
# except StopIteration:
|
||||
# return False, None
|
||||
# except Exception:
|
||||
# return False, None
|
||||
# return True, frame.to_ndarray(format="bgr24")
|
||||
# if self.cap is None:
|
||||
# return False, None
|
||||
# return self.cap.read()
|
||||
|
||||
def _start_capture_thread(self):
|
||||
if self._capture_thread is not None:
|
||||
return
|
||||
self._capture_running = True
|
||||
self._capture_thread = threading.Thread(target=self._capture_loop, daemon=True)
|
||||
self._capture_thread.start()
|
||||
if self.verbose:
|
||||
print("Latest-frame capture thread started.")
|
||||
|
||||
def _capture_loop(self):
|
||||
while self._capture_running:
|
||||
ret, frame = self._read_frame_raw()
|
||||
if not ret:
|
||||
time.sleep(0.001)
|
||||
continue
|
||||
with self._latest_frame_lock:
|
||||
self._latest_frame = frame
|
||||
self._latest_frame_seq += 1
|
||||
@property
|
||||
def capture_status(self):
|
||||
"""Return capture health snapshot, or None in external-frame mode."""
|
||||
if self._capture_worker is None:
|
||||
return None
|
||||
return self._capture_worker.source.status
|
||||
|
||||
|
||||
# =========================================================================
|
||||
|
||||
Reference in New Issue
Block a user