184 lines
6.8 KiB
Python
184 lines
6.8 KiB
Python
"""Off-robot robot backend for development and testing.
|
|
|
|
:class:`MockRobot` implements :class:`RobotInterface` against a laptop webcam or
|
|
a video file (via OpenCV) instead of a real Go2. Locomotion / posture / gesture
|
|
commands are merely logged -- nothing moves. This lets you exercise the
|
|
perception pipeline and state machine on a workstation with no hardware.
|
|
|
|
All optional/heavy imports (``cv2``) are performed lazily inside ``__init__`` so
|
|
the package stays importable on a machine without OpenCV.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import logging
|
|
from typing import TYPE_CHECKING, Optional, Tuple
|
|
|
|
from config import GoWelcomeConfig
|
|
from gowelcome.robot.audio import build_audio_backend
|
|
from gowelcome.robot.interface import GESTURES, RobotInterface
|
|
|
|
if TYPE_CHECKING:
|
|
import numpy as np
|
|
|
|
logger = logging.getLogger(__name__)
|
|
|
|
|
|
class MockRobot(RobotInterface):
|
|
"""A :class:`RobotInterface` backed by a webcam / video file, no hardware.
|
|
|
|
Frames come from ``cv2.VideoCapture(cfg.camera.mock_source)`` (an integer
|
|
webcam index when the source is all digits, otherwise a file path). If the
|
|
capture cannot be opened, the robot operates *frameless* -- :meth:`get_frame`
|
|
returns ``None`` -- so the rest of the stack can still run.
|
|
|
|
Locomotion / posture / gesture calls are logged with a ``[MOCK]`` prefix.
|
|
Greeting audio is dispatched through whatever backend
|
|
:func:`gowelcome.robot.audio.build_audio_backend` selects for ``cfg``.
|
|
"""
|
|
|
|
def __init__(self, cfg: GoWelcomeConfig) -> None:
|
|
self._cfg = cfg
|
|
self._cap = None
|
|
self._is_file = False
|
|
self._frame_size: Tuple[int, int] = (0, 0)
|
|
self._avoidance_on = cfg.safety.use_lidar_avoidance
|
|
|
|
# Lazy OpenCV import -- keep the package importable without cv2.
|
|
try:
|
|
import cv2 # type: ignore
|
|
except ImportError:
|
|
logger.warning(
|
|
"MockRobot: OpenCV (cv2) not available "
|
|
"(pip install opencv-python) -- running frameless.",
|
|
)
|
|
cv2 = None
|
|
self._cv2 = cv2
|
|
|
|
# Audio backend (NullAudio for mock by default, but honour cfg.audio).
|
|
self.audio = build_audio_backend(cfg)
|
|
|
|
if self._cv2 is not None:
|
|
self._open_capture()
|
|
|
|
# --- capture helpers --------------------------------------------------
|
|
def _open_capture(self) -> None:
|
|
"""Open the configured camera/video source, logging on failure."""
|
|
cv2 = self._cv2
|
|
src_str = str(self._cfg.camera.mock_source)
|
|
if src_str.isdigit():
|
|
source = int(src_str)
|
|
self._is_file = False
|
|
else:
|
|
source = src_str
|
|
self._is_file = True
|
|
|
|
try:
|
|
cap = cv2.VideoCapture(source)
|
|
except Exception as exc: # noqa: BLE001
|
|
logger.warning("MockRobot: failed to create VideoCapture(%r): %s", source, exc)
|
|
self._cap = None
|
|
return
|
|
|
|
if not cap.isOpened():
|
|
logger.warning(
|
|
"MockRobot: could not open camera source %r -- running frameless.",
|
|
source,
|
|
)
|
|
self._cap = None
|
|
return
|
|
|
|
self._cap = cap
|
|
logger.info("MockRobot: opened frame source %r (is_file=%s)", source, self._is_file)
|
|
|
|
# --- perception input -------------------------------------------------
|
|
def get_frame(self) -> "Optional[np.ndarray]":
|
|
"""Return the latest BGR frame resized to the configured size.
|
|
|
|
Video files loop (seek to frame 0 on EOF). Returns ``None`` when no
|
|
capture is available or a read genuinely fails.
|
|
"""
|
|
if self._cap is None or self._cv2 is None:
|
|
return None
|
|
cv2 = self._cv2
|
|
|
|
ok, frame = self._cap.read()
|
|
if not ok or frame is None:
|
|
# End of a video file -> loop back to the start and retry once.
|
|
if self._is_file:
|
|
self._cap.set(cv2.CAP_PROP_POS_FRAMES, 0)
|
|
ok, frame = self._cap.read()
|
|
if not ok or frame is None:
|
|
logger.debug("MockRobot: frame read failed")
|
|
return None
|
|
|
|
w, h = self._cfg.camera.width, self._cfg.camera.height
|
|
try:
|
|
frame = cv2.resize(frame, (w, h))
|
|
except Exception as exc: # noqa: BLE001
|
|
logger.warning("MockRobot: resize failed: %s", exc)
|
|
return None
|
|
|
|
self._frame_size = (w, h)
|
|
return frame
|
|
|
|
def frame_size(self) -> Tuple[int, int]:
|
|
"""Current ``(width, height)``; ``(0, 0)`` until the first frame."""
|
|
return self._frame_size
|
|
|
|
# --- locomotion -------------------------------------------------------
|
|
def drive(self, vx: float, vy: float, vyaw: float) -> None:
|
|
"""Log a body-frame velocity command (no hardware motion)."""
|
|
logger.info("[MOCK] drive vx=%.3f vy=%.3f vyaw=%.3f", vx, vy, vyaw)
|
|
|
|
def stop(self) -> None:
|
|
"""Log a zero-velocity (stay-standing) command."""
|
|
logger.info("[MOCK] stop")
|
|
|
|
def set_avoidance(self, on: bool) -> None:
|
|
"""Log a change to the (virtual) LiDAR avoidance toggle."""
|
|
self._avoidance_on = on
|
|
logger.info("[MOCK] set_avoidance on=%s", on)
|
|
|
|
# --- posture / expression --------------------------------------------
|
|
def balance_stand(self) -> None:
|
|
"""Log entering balanced standing."""
|
|
logger.info("[MOCK] balance_stand")
|
|
|
|
def stand_up(self) -> None:
|
|
"""Log a stiff stand-up."""
|
|
logger.info("[MOCK] stand_up")
|
|
|
|
def damp(self) -> None:
|
|
"""Log an emergency soft-stop (limp)."""
|
|
logger.info("[MOCK] damp")
|
|
|
|
def gesture(self, name: str) -> None:
|
|
"""Log an expressive gesture; warn if the name is unknown."""
|
|
if name not in GESTURES:
|
|
logger.warning("[MOCK] gesture: unknown gesture %r (known: %s)", name, GESTURES)
|
|
return
|
|
logger.info("[MOCK] gesture %s", name)
|
|
|
|
# --- greeting payload -------------------------------------------------
|
|
def play_greeting(self) -> None:
|
|
"""Dispatch the configured greeting clip through the audio backend."""
|
|
wav_path = self._cfg.greet.wav_path
|
|
logger.info("[MOCK] play_greeting %s", wav_path)
|
|
self.audio.play(wav_path)
|
|
|
|
# --- lifecycle --------------------------------------------------------
|
|
def shutdown(self) -> None:
|
|
"""Release the capture and audio backend. Idempotent."""
|
|
logger.info("[MOCK] shutdown")
|
|
if self._cap is not None:
|
|
try:
|
|
self._cap.release()
|
|
except Exception as exc: # noqa: BLE001
|
|
logger.debug("MockRobot: capture release failed: %s", exc)
|
|
self._cap = None
|
|
try:
|
|
self.audio.close()
|
|
except Exception as exc: # noqa: BLE001
|
|
logger.debug("MockRobot: audio close failed: %s", exc)
|