145 lines
5.7 KiB
Python
145 lines
5.7 KiB
Python
"""Unit tests for :mod:`gowelcome.control.servoing` (VisualServo).
|
|
|
|
Constructs ``Detection`` objects directly and a ``ServoConfig`` from the
|
|
project config; asserts the velocity *signs and inequalities* rather than exact
|
|
floats, so the tests survive minor gain re-tuning. No cv2 / SDK / hardware.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
from config import ServoConfig, default_config
|
|
from gowelcome.control.servoing import VisualServo
|
|
from gowelcome.types import Detection
|
|
|
|
|
|
FRAME_W = 640
|
|
FRAME_H = 480
|
|
DT = 1.0 / 15.0
|
|
|
|
|
|
def _centered_box(frame_w: int, frame_h: int, h_px: int, w_px: int = 80) -> Detection:
|
|
"""A person box centred horizontally, with the given pixel height."""
|
|
cx = frame_w / 2.0
|
|
cy = frame_h / 2.0
|
|
x1 = int(cx - w_px / 2)
|
|
x2 = int(cx + w_px / 2)
|
|
y1 = int(cy - h_px / 2)
|
|
y2 = int(cy + h_px / 2)
|
|
return Detection(label="person", conf=0.95, x1=x1, y1=y1, x2=x2, y2=y2)
|
|
|
|
|
|
def test_servoconfig_from_default_config():
|
|
"""ServoConfig is reachable from the top-level default config."""
|
|
cfg = default_config()
|
|
assert isinstance(cfg.servo, ServoConfig)
|
|
assert cfg.servo.yaw_sign == -1.0 # contract default
|
|
|
|
|
|
def test_centered_far_target_drives_forward_with_small_yaw():
|
|
"""Centred + small box (far) -> vx > 0 and |vyaw| small (near zero)."""
|
|
cfg = default_config().servo
|
|
servo = VisualServo(cfg)
|
|
# Small box -> far away -> below stop_height_ratio.
|
|
far_box = _centered_box(FRAME_W, FRAME_H, h_px=60) # ratio 0.125 << 0.50
|
|
vx, vyaw, arrived = servo.compute(far_box, FRAME_W, FRAME_H, DT)
|
|
assert not arrived
|
|
assert vx > 0.0
|
|
# Perfectly centred -> within the deadband -> essentially no turn.
|
|
assert abs(vyaw) < 1e-6
|
|
|
|
|
|
def test_far_right_target_yaw_sign_is_correct():
|
|
"""Target far to the RIGHT (norm_err>0) with yaw_sign=-1 -> vyaw < 0."""
|
|
cfg = default_config().servo
|
|
assert cfg.yaw_sign == -1.0
|
|
servo = VisualServo(cfg)
|
|
# Push the box well to the right of centre (cx ~ 0.9 * frame_w).
|
|
box = Detection(label="person", conf=0.9, x1=540, y1=200, x2=620, y2=300)
|
|
assert box.horizontal_offset(FRAME_W) > 0.0 # confirm "right of centre"
|
|
_vx, vyaw, _arrived = servo.compute(box, FRAME_W, FRAME_H, DT)
|
|
assert vyaw < 0.0 # turn right (CW) to centre a right-side target
|
|
|
|
|
|
def test_far_left_target_yaw_sign_is_correct():
|
|
"""Target far to the LEFT (norm_err<0) with yaw_sign=-1 -> vyaw > 0."""
|
|
cfg = default_config().servo
|
|
servo = VisualServo(cfg)
|
|
box = Detection(label="person", conf=0.9, x1=20, y1=200, x2=100, y2=300)
|
|
assert box.horizontal_offset(FRAME_W) < 0.0 # left of centre
|
|
_vx, vyaw, _arrived = servo.compute(box, FRAME_W, FRAME_H, DT)
|
|
assert vyaw > 0.0 # turn left (CCW) to centre a left-side target
|
|
|
|
|
|
def test_arrived_when_box_fills_stop_height_ratio():
|
|
"""Box height >= stop_height_ratio of the frame -> arrived True and vx == 0."""
|
|
cfg = default_config().servo
|
|
servo = VisualServo(cfg)
|
|
# Box taller than stop_height_ratio * frame_h (0.50 * 480 = 240).
|
|
h_px = int(cfg.stop_height_ratio * FRAME_H) + 40
|
|
near_box = _centered_box(FRAME_W, FRAME_H, h_px=h_px)
|
|
assert near_box.height_ratio(FRAME_H) >= cfg.stop_height_ratio
|
|
vx, _vyaw, arrived = servo.compute(near_box, FRAME_W, FRAME_H, DT)
|
|
assert arrived is True
|
|
assert vx == 0.0
|
|
|
|
|
|
def test_deadband_zeroes_yaw_near_centre():
|
|
"""A target inside the yaw deadband produces ~0 yaw."""
|
|
cfg = default_config().servo
|
|
servo = VisualServo(cfg)
|
|
# Offset the centre by less than yaw_deadband (0.06) of the half-width.
|
|
# half-width = 320; 0.06 * 320 ~= 19 px, so nudge by ~10 px.
|
|
cx = FRAME_W / 2.0 + 10
|
|
box = Detection(
|
|
label="person", conf=0.9,
|
|
x1=int(cx - 40), y1=200, x2=int(cx + 40), y2=300,
|
|
)
|
|
assert abs(box.horizontal_offset(FRAME_W)) < cfg.yaw_deadband
|
|
_vx, vyaw, _arrived = servo.compute(box, FRAME_W, FRAME_H, DT)
|
|
assert abs(vyaw) < 1e-6
|
|
|
|
|
|
def test_no_derivative_kick_crossing_into_deadband():
|
|
"""Crossing from off-centre INTO the deadband must not spike the yaw.
|
|
|
|
Regression for the derivative-kick: with a non-zero kd, a target that was
|
|
off-centre last tick and is centred this tick used to inject a one-tick
|
|
derivative spike. The PID's own deadband (fed a pre-zeroed in-band error)
|
|
must suppress it, so the centred tick yields ~0 yaw.
|
|
"""
|
|
cfg = default_config().servo
|
|
assert cfg.kd_yaw > 0.0 # the kick only exists with a derivative term
|
|
servo = VisualServo(cfg)
|
|
|
|
# Tick 1: clearly off-centre (outside the deadband) -> a real turn.
|
|
off = Detection(label="person", conf=0.9, x1=560, y1=200, x2=620, y2=300)
|
|
assert off.horizontal_offset(FRAME_W) > cfg.yaw_deadband
|
|
_vx1, vyaw1, _ = servo.compute(off, FRAME_W, FRAME_H, DT)
|
|
assert abs(vyaw1) > 0.0
|
|
|
|
# Tick 2: now centred (inside the deadband) -> must settle to ~0, no kick.
|
|
centred = _centered_box(FRAME_W, FRAME_H, h_px=120)
|
|
assert abs(centred.horizontal_offset(FRAME_W)) < cfg.yaw_deadband
|
|
_vx2, vyaw2, _ = servo.compute(centred, FRAME_W, FRAME_H, DT)
|
|
assert abs(vyaw2) < 1e-6
|
|
|
|
|
|
def test_forward_throttled_off_axis():
|
|
"""A far box far off-axis should command less forward than the same box centred."""
|
|
cfg = default_config().servo
|
|
|
|
far_h = 60 # well below the stop ratio for both cases
|
|
|
|
servo_centre = VisualServo(cfg)
|
|
centred = _centered_box(FRAME_W, FRAME_H, h_px=far_h)
|
|
vx_centre, _, _ = servo_centre.compute(centred, FRAME_W, FRAME_H, DT)
|
|
|
|
servo_side = VisualServo(cfg)
|
|
off_axis = Detection(label="person", conf=0.9, x1=580, y1=210,
|
|
x2=620, y2=210 + far_h)
|
|
vx_side, _, _ = servo_side.compute(off_axis, FRAME_W, FRAME_H, DT)
|
|
|
|
# Heading-error throttle (exp(-falloff*|err|)) makes the off-axis forward
|
|
# speed no greater than the centred one.
|
|
assert vx_side <= vx_centre
|