127 lines
5.2 KiB
Python
127 lines
5.2 KiB
Python
"""Pure geofence geometry -- haversine distance + bearings + the keep-in fence.
|
|
|
|
No I/O, no threads, no heavy deps (stdlib ``math`` only), so it unit-tests
|
|
off-robot. The :class:`GeoFence` holds a centre (lat/lon) and a radius, decides
|
|
when the dog has strayed too close to the edge, and computes the yaw needed to
|
|
home back toward the centre using GPS course-over-ground (no compass required).
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import math
|
|
from typing import Optional, Tuple
|
|
|
|
from config import GeoFenceConfig
|
|
|
|
_EARTH_RADIUS_M = 6_371_000.0
|
|
|
|
|
|
def haversine_m(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
|
|
"""Great-circle distance between two lat/lon points, in metres."""
|
|
p1 = math.radians(lat1)
|
|
p2 = math.radians(lat2)
|
|
dphi = math.radians(lat2 - lat1)
|
|
dlam = math.radians(lon2 - lon1)
|
|
a = (
|
|
math.sin(dphi / 2.0) ** 2
|
|
+ math.cos(p1) * math.cos(p2) * math.sin(dlam / 2.0) ** 2
|
|
)
|
|
return 2.0 * _EARTH_RADIUS_M * math.asin(min(1.0, math.sqrt(a)))
|
|
|
|
|
|
def bearing_deg(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
|
|
"""Initial bearing from point 1 to point 2, degrees clockwise from north [0,360)."""
|
|
p1 = math.radians(lat1)
|
|
p2 = math.radians(lat2)
|
|
dlam = math.radians(lon2 - lon1)
|
|
y = math.sin(dlam) * math.cos(p2)
|
|
x = math.cos(p1) * math.sin(p2) - math.sin(p1) * math.cos(p2) * math.cos(dlam)
|
|
return (math.degrees(math.atan2(y, x)) + 360.0) % 360.0
|
|
|
|
|
|
def normalize_deg(angle: float) -> float:
|
|
"""Wrap an angle to ``[-180, 180)`` degrees."""
|
|
return (angle + 180.0) % 360.0 - 180.0
|
|
|
|
|
|
class GeoFence:
|
|
"""A circular keep-in fence around a centre point.
|
|
|
|
Distances use the haversine metric; the homing yaw is derived from the GPS
|
|
course-over-ground so no magnetometer/compass is needed.
|
|
"""
|
|
|
|
def __init__(self, cfg: GeoFenceConfig) -> None:
|
|
self.cfg = cfg
|
|
self._center: Optional[Tuple[float, float]] = None
|
|
if cfg.center_mode == "fixed" and (cfg.center_lat or cfg.center_lon):
|
|
self._center = (cfg.center_lat, cfg.center_lon)
|
|
|
|
# --- centre management -------------------------------------------------
|
|
@property
|
|
def has_center(self) -> bool:
|
|
return self._center is not None
|
|
|
|
@property
|
|
def center(self) -> Optional[Tuple[float, float]]:
|
|
return self._center
|
|
|
|
def set_center(self, lat: float, lon: float) -> None:
|
|
"""Explicitly (re)set the fence centre (e.g. dashboard 'set here')."""
|
|
self._center = (float(lat), float(lon))
|
|
|
|
def maybe_capture_center(self, lat: float, lon: float) -> None:
|
|
"""In ``onstart`` mode, capture the first good fix as the centre."""
|
|
if self._center is None and self.cfg.center_mode == "onstart":
|
|
self._center = (float(lat), float(lon))
|
|
|
|
# --- queries -----------------------------------------------------------
|
|
def distance_from_center(self, lat: float, lon: float) -> Optional[float]:
|
|
if self._center is None:
|
|
return None
|
|
return haversine_m(self._center[0], self._center[1], lat, lon)
|
|
|
|
def distance_to_edge(self, lat: float, lon: float) -> Optional[float]:
|
|
"""Metres from the current position to the fence edge (negative = outside)."""
|
|
d = self.distance_from_center(lat, lon)
|
|
return None if d is None else (self.cfg.radius_m - d)
|
|
|
|
def breached(self, lat: float, lon: float) -> bool:
|
|
"""True once within ``margin_m`` of the edge (or beyond) -> start homing."""
|
|
d = self.distance_from_center(lat, lon)
|
|
if d is None:
|
|
return False
|
|
return d >= (self.cfg.radius_m - self.cfg.margin_m)
|
|
|
|
def cleared(self, lat: float, lon: float) -> bool:
|
|
"""True once safely back inside (hysteresis) -> may resume normal behaviour."""
|
|
d = self.distance_from_center(lat, lon)
|
|
if d is None:
|
|
return True
|
|
return d <= (self.cfg.radius_m - self.cfg.margin_m - self.cfg.release_m)
|
|
|
|
def bearing_to_center(self, lat: float, lon: float) -> Optional[float]:
|
|
if self._center is None:
|
|
return None
|
|
return bearing_deg(lat, lon, self._center[0], self._center[1])
|
|
|
|
def homing_yaw(self, lat: float, lon: float, course_deg: Optional[float]) -> float:
|
|
"""Yaw rate (rad/s) to steer toward the centre given current GPS course.
|
|
|
|
``vyaw = homing_kp * heading_error_rad``, clamped, where ``heading_error``
|
|
is ``normalize(bearing_to_centre - course)``. ``+`` yaw = turn left/CCW.
|
|
When ``course_deg`` is unknown (too slow for a valid GPS track), returns
|
|
a gentle constant turn so the dog moves and re-acquires a course.
|
|
"""
|
|
target = self.bearing_to_center(lat, lon)
|
|
if target is None:
|
|
return 0.0
|
|
if course_deg is None:
|
|
# No reliable course yet: turn gently (one direction) to gain a track.
|
|
return min(self.cfg.max_homing_yaw, 0.4)
|
|
# Compass bearings are clockwise-from-north; a positive (clockwise) error
|
|
# means "turn right" = negative yaw in the body convention, hence the sign.
|
|
err_deg = normalize_deg(target - course_deg)
|
|
vyaw = -math.radians(err_deg) * self.cfg.homing_kp
|
|
return max(-self.cfg.max_homing_yaw, min(self.cfg.max_homing_yaw, vyaw))
|