ykcheungab
124 lines
3.9 KiB
Python
124 lines
3.9 KiB
Python
"""Finger tactile acquisition loop for example dashboards."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import time
|
|
|
|
import numpy as np
|
|
from orisys.finger import FingerRuntime
|
|
|
|
from viz import FrameRecorder, build_uv_surface_mesh, resolve_viz_pose_path
|
|
|
|
from .config import PipelineConfig
|
|
from .frame import FingerFrameView
|
|
|
|
|
|
class FingerPipeline:
|
|
"""Owns FingerRuntime and produces FingerFrameView snapshots per tick."""
|
|
|
|
def __init__(self, config: PipelineConfig):
|
|
self.config = config
|
|
self.runtime: FingerRuntime | None = None
|
|
self.recorder = FrameRecorder()
|
|
self._start_time: float | None = None
|
|
self.uv_mesh = None
|
|
self.world_to_cam: np.ndarray | None = None
|
|
self.mesh_lookat: np.ndarray | None = None
|
|
|
|
@property
|
|
def is_open(self) -> bool:
|
|
return self.runtime is not None
|
|
|
|
@property
|
|
def tactile(self):
|
|
if self.runtime is None:
|
|
raise RuntimeError("FingerPipeline is not open")
|
|
return self.runtime.tactile
|
|
|
|
def open(self) -> None:
|
|
if self.runtime is not None:
|
|
return
|
|
self.runtime = FingerRuntime.open(
|
|
config=self.config.config,
|
|
calib=self.config.calib,
|
|
video=self.config.video,
|
|
verbose=self.config.verbose,
|
|
)
|
|
self._prepare_mesh_assets()
|
|
self.runtime.warmup(color=self.config.use_color)
|
|
self._start_time = time.time()
|
|
|
|
def _prepare_mesh_assets(self) -> None:
|
|
if self.runtime is None:
|
|
return
|
|
domain = self.runtime.domain
|
|
if domain.surface is None or getattr(domain.surface, "world_xyz", None) is None:
|
|
return
|
|
self.uv_mesh = build_uv_surface_mesh(domain.surface.world_xyz, domain.valid_mask)
|
|
if self.config.viz_pose.lower() != "none":
|
|
self.world_to_cam = np.load(resolve_viz_pose_path(self.config.viz_pose))
|
|
self.mesh_lookat = self.uv_mesh.rest_vertices.mean(axis=0)
|
|
|
|
def close(self) -> None:
|
|
if self.runtime is not None:
|
|
self.runtime.close()
|
|
self.runtime = None
|
|
self._start_time = None
|
|
|
|
def reset(self) -> None:
|
|
if self.runtime is not None:
|
|
self.runtime.tactile.reset()
|
|
|
|
def tick(self) -> FingerFrameView | None:
|
|
if self.runtime is None or self._start_time is None:
|
|
return None
|
|
|
|
ok, unrolled = self.runtime.grab_and_process(color=self.config.use_color)
|
|
if not ok:
|
|
return None
|
|
|
|
tactile = self.runtime.tactile
|
|
self.recorder.write_if_recording(tactile.frame)
|
|
|
|
tactile.compute_deformation(decompose=True, check_motion=True, threshold=0)
|
|
tactile.compute_contact()
|
|
|
|
fps, flow, vnormal, img, contour, centroid, depth_map, fnormal, fshearx, fsheary = tactile.read_info(
|
|
tactile.info.FPS,
|
|
tactile.info.VRAW,
|
|
tactile.info.VNORMAL,
|
|
tactile.info.IMG,
|
|
tactile.info.CONTOUR,
|
|
tactile.info.CENTROID,
|
|
tactile.info.DEPTH,
|
|
tactile.info.FNORMAL,
|
|
tactile.info.FSHEARX,
|
|
tactile.info.FSHEARY,
|
|
)
|
|
|
|
return FingerFrameView(
|
|
timestamp=time.time() - self._start_time,
|
|
fps=float(fps),
|
|
unrolled=unrolled,
|
|
flow=flow,
|
|
vnormal=vnormal,
|
|
depth_map=depth_map,
|
|
img=img,
|
|
contour=contour,
|
|
centroid=centroid,
|
|
fnormal=float(fnormal),
|
|
fshearx=float(fshearx),
|
|
fsheary=float(fsheary),
|
|
centroid_found=bool(tactile.raw_sensor.centroid_found),
|
|
mask_coverage=float(tactile.mask_stats["coverage"]),
|
|
raw_frame=tactile.frame,
|
|
)
|
|
|
|
def start_recording(self) -> bool:
|
|
if self.runtime is None:
|
|
return False
|
|
return self.recorder.start(self.runtime.tactile.frame)
|
|
|
|
def stop_recording(self) -> None:
|
|
self.recorder.stop()
|