97 lines
3.3 KiB
Python
97 lines
3.3 KiB
Python
"""Pure geofence geometry tests (no GPS hardware, no threads)."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import pytest
|
|
|
|
from config import GeoFenceConfig
|
|
from gowelcome.geo.geofence import GeoFence, bearing_deg, haversine_m, normalize_deg
|
|
|
|
_M_PER_DEG = 111_194.0 # ~metres per degree of latitude
|
|
|
|
|
|
def test_haversine_known_distances():
|
|
assert haversine_m(0, 0, 0, 0) == pytest.approx(0.0, abs=1e-6)
|
|
assert haversine_m(0, 0, 1, 0) == pytest.approx(_M_PER_DEG, rel=0.01)
|
|
assert haversine_m(0, 0, 0, 1) == pytest.approx(_M_PER_DEG, rel=0.01)
|
|
|
|
|
|
def test_bearing_cardinals():
|
|
assert bearing_deg(0, 0, 1, 0) == pytest.approx(0.0, abs=0.5) # north
|
|
assert bearing_deg(0, 0, 0, 1) == pytest.approx(90.0, abs=0.5) # east
|
|
assert bearing_deg(0, 0, -1, 0) == pytest.approx(180.0, abs=0.5) # south
|
|
assert bearing_deg(0, 0, 0, -1) == pytest.approx(270.0, abs=0.5) # west
|
|
|
|
|
|
def test_normalize_deg():
|
|
assert normalize_deg(270) == pytest.approx(-90.0)
|
|
assert normalize_deg(-270) == pytest.approx(90.0)
|
|
assert normalize_deg(0) == pytest.approx(0.0)
|
|
assert normalize_deg(190) == pytest.approx(-170.0)
|
|
|
|
|
|
def _fence(**over) -> GeoFence:
|
|
cfg = GeoFenceConfig(radius_m=15.0, margin_m=3.0, release_m=2.0)
|
|
for k, v in over.items():
|
|
setattr(cfg, k, v)
|
|
return GeoFence(cfg)
|
|
|
|
|
|
def test_center_capture_onstart():
|
|
f = _fence(center_mode="onstart")
|
|
assert not f.has_center
|
|
f.maybe_capture_center(10.0, 20.0)
|
|
assert f.has_center and f.center == (10.0, 20.0)
|
|
f.maybe_capture_center(11.0, 21.0) # ignored once set
|
|
assert f.center == (10.0, 20.0)
|
|
|
|
|
|
def test_center_fixed_mode():
|
|
f = _fence(center_mode="fixed", center_lat=1.0, center_lon=2.0)
|
|
assert f.has_center and f.center == (1.0, 2.0)
|
|
|
|
|
|
def test_breached_and_cleared_hysteresis():
|
|
f = _fence(center_mode="onstart")
|
|
f.maybe_capture_center(0.0, 0.0)
|
|
# radius 15, margin 3 -> breached at >=12 m; cleared at <=10 m.
|
|
near = 9.0 / _M_PER_DEG # ~9 m north
|
|
edge = 13.0 / _M_PER_DEG # ~13 m north
|
|
assert not f.breached(near, 0.0)
|
|
assert f.cleared(near, 0.0)
|
|
assert f.breached(edge, 0.0)
|
|
assert not f.cleared(edge, 0.0)
|
|
|
|
|
|
def test_homing_yaw_sign_steers_toward_center():
|
|
f = _fence(center_mode="onstart")
|
|
f.maybe_capture_center(0.0, 0.0)
|
|
east = 0.001 # ~111 m east of centre -> centre is to the WEST
|
|
west = -0.001 # west of centre -> centre is to the EAST
|
|
# Heading north (course 0). East of centre -> turn LEFT (vyaw>0) to face west.
|
|
assert f.homing_yaw(0.0, east, course_deg=0.0) > 0.0
|
|
# West of centre -> turn RIGHT (vyaw<0) to face east.
|
|
assert f.homing_yaw(0.0, west, course_deg=0.0) < 0.0
|
|
|
|
|
|
def test_homing_yaw_no_course_gentle_turn():
|
|
f = _fence(center_mode="onstart")
|
|
f.maybe_capture_center(0.0, 0.0)
|
|
v = f.homing_yaw(0.0, 0.001, course_deg=None)
|
|
assert 0.0 < v <= f.cfg.max_homing_yaw
|
|
|
|
|
|
def test_homing_yaw_clamped():
|
|
f = _fence(center_mode="onstart", homing_kp=100.0, max_homing_yaw=0.9)
|
|
f.maybe_capture_center(0.0, 0.0)
|
|
v = f.homing_yaw(0.0, 0.001, course_deg=90.0)
|
|
assert abs(v) <= 0.9 + 1e-9
|
|
|
|
|
|
def test_no_center_returns_none():
|
|
f = _fence(center_mode="onstart")
|
|
assert f.distance_from_center(0, 0) is None
|
|
assert f.bearing_to_center(0, 0) is None
|
|
assert not f.breached(0, 0)
|
|
assert f.cleared(0, 0) # nothing to enforce -> treated as clear
|