GoWelcome/scripts/smoke_test.py

"""End-to-end off-robot smoke test for GoWelcome (real YOLO + cv2 + threads).

Run AFTER ``./requirement.sh`` (needs ultralytics + opencv installed), with NO
robot attached. It exercises the genuine perception + control stack on real
images so you catch any dependency/logic problem on the bench, not on the dog:

    conda activate welcome        # or: source the env however you like
    python scripts/smoke_test.py

Checks:
  1. YoloDetector on a people image and a vehicle image (bundled with ultralytics).
  2. RoadDetector HSV mask: flags asphalt-grey, ignores saturated grass.
  3. PerceptionThread + GoWelcomeStateMachine: a detected person drives
     WANDER -> APPROACH/GREET; a detected vehicle forces AVOID_DANGER.
  4. build_robot factory builds MockRobot; NullAudio + shutdown are clean.

Exits non-zero on the first failed assertion.
"""

from __future__ import annotations

import logging
import os
import sys
import time

# Dynamic: project root is this file's parent dir's parent (scripts/..).
_ROOT = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
if _ROOT not in sys.path:
    sys.path.insert(0, _ROOT)

logging.basicConfig(level=logging.WARNING, format="%(levelname)s %(name)s: %(message)s")

import numpy as np

try:
    import cv2
    from ultralytics.utils import ASSETS
except ImportError as exc:  # pragma: no cover
    print(f"ERROR: smoke test needs opencv + ultralytics installed ({exc}).")
    print("Run ./requirement.sh first, then activate the 'welcome' env.")
    sys.exit(2)

from config import default_config
from gowelcome.perception.detector import YoloDetector
from gowelcome.perception.road_mask import RoadDetector
from gowelcome.perception.vision_thread import PerceptionThread
from gowelcome.statemachine import GoWelcomeStateMachine
from gowelcome.robot.interface import RobotInterface
from gowelcome.robot.factory import build_robot


def banner(t: str) -> None:
    print(f"\n===== {t} =====")


class _StubRobot(RobotInterface):
    """Serves one fixed frame; records commands. No hardware."""

    def __init__(self, frame):
        self._f = frame
        self.drives, self.gestures, self.greets = [], [], 0

    def get_frame(self):
        return self._f

    def frame_size(self):
        return (self._f.shape[1], self._f.shape[0])

    def drive(self, vx, vy, vyaw):
        self.drives.append((vx, vy, vyaw))

    def stop(self):
        pass

    def set_avoidance(self, on):
        pass

    def balance_stand(self):
        pass

    def stand_up(self):
        pass

    def damp(self):
        pass

    def gesture(self, name):
        self.gestures.append(name)

    def play_greeting(self):
        self.greets += 1

    def shutdown(self):
        pass


def main() -> int:
    cfg = default_config()
    cfg.mock = True
    cfg.audio.backend = "null"
    cfg.perception.device = "cpu"
    w, h = cfg.camera.width, cfg.camera.height

    # ---------------------------------------------------------- 1. YOLO
    banner("1. YoloDetector on real images")
    det = YoloDetector(cfg.perception)

    zid = cv2.resize(cv2.imread(str(ASSETS / "zidane.jpg")), (w, h))
    zdets = det.detect(zid)
    print("zidane.jpg ->", [(d.label, round(d.conf, 2)) for d in zdets])
    assert any(d.label == "person" for d in zdets), "expected person(s) in zidane.jpg"

    bus = cv2.resize(cv2.imread(str(ASSETS / "bus.jpg")), (w, h))
    bdets = det.detect(bus)
    blabels = {d.label for d in bdets}
    print("bus.jpg ->", [(d.label, round(d.conf, 2)) for d in bdets])
    assert "person" in blabels, "expected person in bus.jpg"
    assert "bus" in blabels, "expected bus (danger class) in bus.jpg"
    print("OK: real YOLO detects persons and a vehicle")

    # ---------------------------------------------------------- 2. ROAD
    banner("2. RoadDetector HSV mask")
    grey = np.full((h, w, 3), 95, np.uint8)
    ri = RoadDetector(cfg.perception).analyze(grey)
    print(f"grey -> coverage={ri.coverage:.2f}  L/C/R={ri.left:.2f}/{ri.center:.2f}/{ri.right:.2f}")
    assert ri.coverage > 0.5, "asphalt-grey frame should read as mostly road"
    green = np.zeros((h, w, 3), np.uint8)
    green[:] = (40, 200, 40)
    rg = RoadDetector(cfg.perception).analyze(green)
    print(f"green -> coverage={rg.coverage:.2f}")
    assert rg.coverage < 0.2, "saturated green should not read as road"
    print("OK: road mask flags asphalt, ignores grass")

    # ---------------------------------------------------------- 3. PIPELINE
    banner("3. PerceptionThread + StateMachine (person -> APPROACH/GREET)")
    cfg.perception.road_enabled = False
    cfg.perception.person_conf = 0.5
    robot = _StubRobot(zid)
    pt = PerceptionThread(robot, cfg)
    pt.start()
    time.sleep(2.0)
    res = pt.latest()
    print("perception ->", "persons:", len(res.persons), "dangers:", len(res.dangers))
    assert res and res.persons, "perception thread should report a person"
    sm = GoWelcomeStateMachine(robot, pt, cfg)
    seq = [sm.step(1 / 15).value for _ in range(6)]
    pt.stop()
    print("states:", seq)
    assert any(s in ("APPROACH", "GREET") for s in seq), "should approach/greet a person"
    print("OK: perception->decision reacts to a real detection")

    # ---------------------------------------------------------- 3b. DANGER
    banner("3b. Danger override (vehicle -> AVOID_DANGER)")
    robot2 = _StubRobot(bus)
    pt2 = PerceptionThread(robot2, cfg)
    pt2.start()
    time.sleep(2.0)
    res2 = pt2.latest()
    print("perception ->", "persons:", len(res2.persons), "dangers:", len(res2.dangers))
    sm2 = GoWelcomeStateMachine(robot2, pt2, cfg)
    seq2 = [sm2.step(1 / 15).value for _ in range(4)]
    pt2.stop()
    print("states:", seq2)
    assert res2.dangers, "the bus should be classified as a danger"
    assert "AVOID_DANGER" in seq2, "a nearby vehicle must force AVOID_DANGER"
    print("OK: vehicle triggers the safety escape state")

    # ---------------------------------------------------------- 4. FACTORY
    banner("4. build_robot factory + NullAudio")
    bot = build_robot(cfg)
    print("build_robot ->", type(bot).__name__)
    assert type(bot).__name__ == "MockRobot"
    bot.play_greeting()
    bot.shutdown()
    print("OK: factory builds MockRobot; null audio + shutdown clean")

    print("\n" + "=" * 40)
    print("ALL SMOKE TESTS PASSED")
    print("=" * 40)
    return 0


if __name__ == "__main__":
    sys.exit(main())