281 lines
9.6 KiB
Python
281 lines
9.6 KiB
Python
"""State-machine behaviour with the GPS geofence, dog-play, and dashboard controls.
|
|
|
|
Driven off-robot with a recording fake robot, a scripted fake GPS, and a fake
|
|
perception source. Stdlib only.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import time
|
|
from typing import List, Optional, Tuple
|
|
|
|
from config import default_config
|
|
from gowelcome.geo.gps import GpsFix
|
|
from gowelcome.statemachine import GoWelcomeStateMachine
|
|
from gowelcome.types import Detection, PerceptionResult, RoadInfo, State
|
|
from gowelcome.robot.interface import RobotInterface
|
|
|
|
FRAME_W, FRAME_H = 640, 480
|
|
DT = 1.0 / 15.0
|
|
|
|
|
|
class FakeRobot(RobotInterface):
|
|
def __init__(self) -> None:
|
|
self.drives: List[Tuple[float, float, float]] = []
|
|
self.stops = 0
|
|
self.gestures: List[str] = []
|
|
self.greetings = 0
|
|
self.balance_stands = 0
|
|
self.damps = 0
|
|
|
|
def get_frame(self):
|
|
return None
|
|
|
|
def frame_size(self):
|
|
return (FRAME_W, FRAME_H)
|
|
|
|
def drive(self, vx, vy, vyaw):
|
|
self.drives.append((vx, vy, vyaw))
|
|
|
|
def stop(self):
|
|
self.stops += 1
|
|
|
|
def set_avoidance(self, on):
|
|
pass
|
|
|
|
def balance_stand(self):
|
|
self.balance_stands += 1
|
|
|
|
def stand_up(self):
|
|
pass
|
|
|
|
def damp(self):
|
|
self.damps += 1
|
|
|
|
def gesture(self, name):
|
|
self.gestures.append(name)
|
|
|
|
def play_greeting(self):
|
|
self.greetings += 1
|
|
|
|
def shutdown(self):
|
|
pass
|
|
|
|
|
|
class FakeGps:
|
|
"""Returns a settable fix; records commanded motion."""
|
|
|
|
def __init__(self, fix: Optional[GpsFix] = None) -> None:
|
|
self._fix = fix
|
|
self.commands: List[Tuple[float, float, float]] = []
|
|
|
|
def set_fix(self, fix: Optional[GpsFix]) -> None:
|
|
self._fix = fix
|
|
|
|
def latest(self) -> Optional[GpsFix]:
|
|
return self._fix
|
|
|
|
def on_command(self, vx, vy, vyaw):
|
|
self.commands.append((vx, vy, vyaw))
|
|
|
|
|
|
class FakePerception:
|
|
def __init__(self, result: PerceptionResult) -> None:
|
|
self._result = result
|
|
|
|
def latest(self) -> PerceptionResult:
|
|
self._result.ts = time.monotonic()
|
|
return self._result
|
|
|
|
|
|
def _empty_perc() -> PerceptionResult:
|
|
return PerceptionResult(frame_w=FRAME_W, frame_h=FRAME_H, persons=[], dangers=[],
|
|
road=None, ts=time.monotonic(), seq=1)
|
|
|
|
|
|
def _fix(lat, lon, course=None) -> GpsFix:
|
|
return GpsFix(lat=lat, lon=lon, ts=time.monotonic(), course_deg=course,
|
|
speed_mps=0.3, num_sats=10, hdop=0.8)
|
|
|
|
|
|
# --------------------------------------------------------------------------- #
|
|
# Geofence
|
|
# --------------------------------------------------------------------------- #
|
|
def test_breach_enters_boundary_and_homes_forward():
|
|
cfg = default_config()
|
|
cfg.geofence.enabled = True
|
|
cfg.geofence.center_mode = "onstart"
|
|
robot = FakeRobot()
|
|
gps = FakeGps(_fix(0.0, 0.0, course=0.0)) # at centre
|
|
sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg, gps_source=gps)
|
|
|
|
sm.step(DT) # captures centre, stays inside
|
|
assert sm.state is State.WANDER
|
|
|
|
gps.set_fix(_fix(0.0, 0.001, course=90.0)) # ~111 m east -> way outside
|
|
state = sm.step(DT)
|
|
assert state is State.BOUNDARY
|
|
vx, _vy, vyaw = robot.drives[-1]
|
|
assert vx > 0.0 # driving back toward centre
|
|
assert abs(vyaw) > 0.0 # steering toward centre
|
|
|
|
|
|
def test_boundary_low_speed_ignores_garbage_course():
|
|
"""Near-stationary GPS course is noise -> homing must use the safe gentle
|
|
re-acquire turn, not steer on the (possibly wrong-way) reported course."""
|
|
import pytest
|
|
cfg = default_config()
|
|
cfg.geofence.enabled = True
|
|
cfg.geofence.center_mode = "onstart" # min_speed_for_course default 0.15
|
|
robot = FakeRobot()
|
|
gps = FakeGps(_fix(0.0, 0.0, course=0.0))
|
|
sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg, gps_source=gps)
|
|
sm.step(DT) # capture centre
|
|
# Breached + near-stationary + a course pointing the WRONG way (east, away).
|
|
gps.set_fix(GpsFix(lat=0.0, lon=0.001, ts=time.monotonic(), course_deg=90.0,
|
|
speed_mps=0.0, num_sats=10, hdop=0.8))
|
|
sm.step(DT)
|
|
assert sm.state is State.BOUNDARY
|
|
vx, _vy, vyaw = robot.drives[-1]
|
|
assert vx > 0.0
|
|
# course gated out -> gentle re-acquire turn (~+0.4), NOT the garbage hard turn.
|
|
assert vyaw == pytest.approx(0.4, abs=1e-6)
|
|
|
|
|
|
def test_gps_lost_holds():
|
|
cfg = default_config()
|
|
cfg.geofence.enabled = True
|
|
cfg.geofence.no_fix_behavior = "stop"
|
|
robot = FakeRobot()
|
|
gps = FakeGps(_fix(0.0, 0.0))
|
|
sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg, gps_source=gps)
|
|
sm.step(DT) # capture centre
|
|
drives_before = len(robot.drives)
|
|
gps.set_fix(None) # GPS lost
|
|
sm.step(DT)
|
|
assert robot.stops >= 1
|
|
assert len(robot.drives) == drives_before # never drove blind
|
|
|
|
|
|
def test_no_center_holds_until_first_fix():
|
|
cfg = default_config()
|
|
cfg.geofence.enabled = True
|
|
robot = FakeRobot()
|
|
gps = FakeGps(None) # never a fix
|
|
sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg, gps_source=gps)
|
|
sm.step(DT)
|
|
assert robot.stops >= 1
|
|
assert not robot.drives
|
|
|
|
|
|
def test_geofence_disabled_when_no_gps_source():
|
|
cfg = default_config()
|
|
robot = FakeRobot()
|
|
sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg, gps_source=None)
|
|
sm.step(DT)
|
|
assert sm.state is State.WANDER
|
|
assert robot.drives and robot.drives[-1][0] > 0.0 # roams normally
|
|
|
|
|
|
# --------------------------------------------------------------------------- #
|
|
# Vision-only containment (default: no GPS) -- stay away from road + cars
|
|
# --------------------------------------------------------------------------- #
|
|
def _perc(road=None, dets=None) -> PerceptionResult:
|
|
dets = dets or []
|
|
dangers = [d for d in dets if d.height_ratio(FRAME_H) >= 0.25 and d.label != "person"]
|
|
return PerceptionResult(frame_w=FRAME_W, frame_h=FRAME_H, persons=[],
|
|
dangers=dangers, detections=dets, road=road,
|
|
ts=time.monotonic(), seq=1)
|
|
|
|
|
|
def test_default_is_vision_only_no_gps_hold():
|
|
"""Default config has no GPS -> the dog roams (vision-only), never holds."""
|
|
cfg = default_config()
|
|
assert cfg.geofence.enabled is False and cfg.gps.enabled is False
|
|
robot = FakeRobot()
|
|
sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg, gps_source=None)
|
|
sm.step(DT)
|
|
assert sm.state is State.WANDER
|
|
assert robot.drives and robot.drives[-1][0] > 0.0 # cruising, not holding
|
|
|
|
|
|
def test_road_repulsion_steers_away_and_slows():
|
|
"""Soft band road on the LEFT -> veer RIGHT (vyaw<0) and slow down."""
|
|
cfg = default_config()
|
|
cfg.wander.yaw_sweep_rate = 0.0 # isolate the repulsion term
|
|
robot = FakeRobot()
|
|
# road heavier on the left, centre below the hard AVOID trigger (0.35)
|
|
road = RoadInfo(coverage=0.2, left=0.30, center=0.2, right=0.03)
|
|
sm = GoWelcomeStateMachine(robot, FakePerception(_perc(road=road)), cfg)
|
|
sm.step(DT)
|
|
assert sm.state is State.WANDER # soft band -> not hard AVOID_DANGER
|
|
vx, _vy, vyaw = robot.drives[-1]
|
|
assert vyaw < 0.0 # turn right, away from the left road
|
|
assert 0.0 < vx < cfg.wander.forward_speed # slowed but still moving
|
|
|
|
|
|
def test_car_repulsion_steers_away():
|
|
"""A (far/small) car on the RIGHT -> veer LEFT (vyaw>0) away from it."""
|
|
cfg = default_config()
|
|
cfg.wander.yaw_sweep_rate = 0.0
|
|
robot = FakeRobot()
|
|
car = Detection(label="car", conf=0.5, x1=520, y1=210, x2=600, y2=260) # right, small
|
|
sm = GoWelcomeStateMachine(robot, FakePerception(_perc(dets=[car])), cfg)
|
|
sm.step(DT)
|
|
assert sm.state is State.WANDER # small/far car -> soft steer, not hard avoid
|
|
vx, _vy, vyaw = robot.drives[-1]
|
|
assert vyaw > 0.0 # turn left, away from the right-side car
|
|
|
|
|
|
# --------------------------------------------------------------------------- #
|
|
# Dog-play + dashboard controls
|
|
# --------------------------------------------------------------------------- #
|
|
def test_play_fires_in_wander():
|
|
cfg = default_config()
|
|
cfg.play.moderate_interval = 0.0
|
|
cfg.play.jitter = 0.0
|
|
robot = FakeRobot()
|
|
sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg, gps_source=None)
|
|
sm.step(DT)
|
|
assert sm.state is State.WANDER
|
|
assert robot.gestures, "a dog-play gesture should have fired"
|
|
assert robot.gestures[-1] in cfg.play.actions
|
|
|
|
|
|
def test_set_play_mode_and_status():
|
|
cfg = default_config()
|
|
sm = GoWelcomeStateMachine(FakeRobot(), FakePerception(_empty_perc()), cfg)
|
|
assert sm.play_mode() == "moderate"
|
|
assert sm.set_play_mode("playful") is True
|
|
assert sm.play_mode() == "playful"
|
|
assert sm.set_play_mode("bogus") is False
|
|
s = sm.status()
|
|
assert s["play_mode"] == "playful"
|
|
assert s["state"] in {st.value for st in State}
|
|
|
|
|
|
def test_pause_stops_robot():
|
|
cfg = default_config()
|
|
robot = FakeRobot()
|
|
sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg)
|
|
sm.set_paused(True)
|
|
sm.step(DT)
|
|
assert robot.stops >= 1 and not robot.drives
|
|
sm.set_paused(False)
|
|
sm.step(DT)
|
|
assert robot.drives # resumes
|
|
|
|
|
|
def test_estop_damps_and_stops():
|
|
cfg = default_config()
|
|
robot = FakeRobot()
|
|
sm = GoWelcomeStateMachine(robot, FakePerception(_empty_perc()), cfg)
|
|
sm.request_estop()
|
|
sm.step(DT)
|
|
assert robot.damps >= 1
|
|
assert robot.stops >= 1
|
|
assert not robot.drives
|
|
sm.clear_estop()
|
|
sm.step(DT)
|
|
assert robot.drives
|