335 lines
12 KiB
Python
335 lines
12 KiB
Python
"""GPS sources for the geofence.
|
|
|
|
The Go2 has no built-in GPS, so this reads an EXTERNAL receiver on the onboard
|
|
computer. Three sources, selected by ``cfg.gps.source``:
|
|
|
|
* ``gpsd`` -- talk to a running ``gpsd`` over its JSON socket (no extra dep).
|
|
* ``serial`` -- parse NMEA (GGA/RMC) straight off a serial receiver (pyserial).
|
|
* ``mock`` -- a simulated receiver that integrates the commanded velocity into
|
|
a fake lat/lon, so the geofence + BOUNDARY behaviour can be
|
|
exercised entirely off-robot.
|
|
|
|
Each source runs a background thread and exposes the most recent good fix via
|
|
:meth:`GpsSource.latest` (which returns ``None`` when the fix is stale). Quality
|
|
gating (min sats / max HDOP) is applied before a fix is published.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import json
|
|
import logging
|
|
import math
|
|
import socket
|
|
import threading
|
|
import time
|
|
from abc import ABC, abstractmethod
|
|
from dataclasses import dataclass
|
|
from typing import Optional
|
|
|
|
from config import GoWelcomeConfig, GpsConfig
|
|
|
|
logger = logging.getLogger(__name__)
|
|
|
|
_EARTH_RADIUS_M = 6_371_000.0
|
|
|
|
|
|
@dataclass
|
|
class GpsFix:
|
|
"""A single GPS reading. Angles in degrees, distances in metres/SI."""
|
|
|
|
lat: float
|
|
lon: float
|
|
ts: float # time.monotonic() when received
|
|
course_deg: Optional[float] = None # course over ground, CW from north
|
|
speed_mps: Optional[float] = None
|
|
num_sats: Optional[int] = None
|
|
hdop: Optional[float] = None
|
|
|
|
|
|
class GpsSource(ABC):
|
|
"""Background GPS reader exposing the latest (non-stale) fix."""
|
|
|
|
@abstractmethod
|
|
def start(self) -> None: ...
|
|
|
|
@abstractmethod
|
|
def stop(self) -> None: ...
|
|
|
|
@abstractmethod
|
|
def latest(self) -> Optional[GpsFix]:
|
|
"""Most recent good fix, or ``None`` if unavailable/stale."""
|
|
|
|
def on_command(self, vx: float, vy: float, vyaw: float) -> None:
|
|
"""Hook so simulated sources can integrate the robot's motion. No-op
|
|
for real receivers."""
|
|
|
|
|
|
class _ThreadGpsSource(GpsSource):
|
|
"""Shared thread + quality-gating + staleness machinery for real sources."""
|
|
|
|
def __init__(self, cfg: GpsConfig) -> None:
|
|
self.cfg = cfg
|
|
self._fix: Optional[GpsFix] = None
|
|
self._lock = threading.Lock()
|
|
self._stop = threading.Event()
|
|
self._thread: Optional[threading.Thread] = None
|
|
|
|
def start(self) -> None:
|
|
self._thread = threading.Thread(target=self._loop, name="GpsSource", daemon=True)
|
|
self._thread.start()
|
|
|
|
def stop(self) -> None:
|
|
self._stop.set()
|
|
|
|
def latest(self) -> Optional[GpsFix]:
|
|
with self._lock:
|
|
fix = self._fix
|
|
if fix is None:
|
|
return None
|
|
if (time.monotonic() - fix.ts) > self.cfg.stale_after:
|
|
return None
|
|
return fix
|
|
|
|
def _publish(self, fix: GpsFix) -> None:
|
|
if fix.num_sats is not None and fix.num_sats < self.cfg.min_satellites:
|
|
return
|
|
if fix.hdop is not None and fix.hdop > self.cfg.max_hdop:
|
|
return
|
|
with self._lock:
|
|
self._fix = fix
|
|
|
|
@abstractmethod
|
|
def _loop(self) -> None: ...
|
|
|
|
|
|
class GpsdSource(_ThreadGpsSource):
|
|
"""Read fixes from a running ``gpsd`` over its JSON protocol (stdlib socket)."""
|
|
|
|
def _loop(self) -> None:
|
|
while not self._stop.is_set():
|
|
try:
|
|
with socket.create_connection(
|
|
(self.cfg.gpsd_host, self.cfg.gpsd_port), timeout=5.0
|
|
) as sock:
|
|
sock.sendall(b'?WATCH={"enable":true,"json":true}\n')
|
|
buf = b""
|
|
sock.settimeout(2.0)
|
|
while not self._stop.is_set():
|
|
try:
|
|
chunk = sock.recv(4096)
|
|
except socket.timeout:
|
|
continue
|
|
if not chunk:
|
|
break
|
|
buf += chunk
|
|
while b"\n" in buf:
|
|
line, buf = buf.split(b"\n", 1)
|
|
self._handle_line(line)
|
|
except Exception as exc: # pragma: no cover - hardware path
|
|
logger.debug("gpsd connection error: %s; retrying", exc)
|
|
self._stop.wait(2.0)
|
|
|
|
def _handle_line(self, line: bytes) -> None:
|
|
try:
|
|
obj = json.loads(line.decode("ascii", "replace"))
|
|
except Exception:
|
|
return
|
|
if obj.get("class") != "TPV":
|
|
return
|
|
if obj.get("mode", 0) < 2: # need at least a 2D fix
|
|
return
|
|
lat, lon = obj.get("lat"), obj.get("lon")
|
|
if lat is None or lon is None:
|
|
return
|
|
self._publish(GpsFix(
|
|
lat=float(lat), lon=float(lon), ts=time.monotonic(),
|
|
course_deg=_opt_float(obj.get("track")),
|
|
speed_mps=_opt_float(obj.get("speed")),
|
|
num_sats=None, # gpsd reports sats in SKY reports; not gated here
|
|
hdop=_opt_float(obj.get("hdop")),
|
|
))
|
|
|
|
|
|
class SerialNmeaSource(_ThreadGpsSource):
|
|
"""Parse NMEA GGA/RMC off a serial GPS receiver (lazy pyserial)."""
|
|
|
|
def _loop(self) -> None:
|
|
try:
|
|
import serial # type: ignore
|
|
except ImportError: # pragma: no cover - off-robot
|
|
logger.error("SerialNmeaSource needs pyserial: pip install pyserial")
|
|
return
|
|
while not self._stop.is_set():
|
|
try:
|
|
with serial.Serial(self.cfg.serial_port, self.cfg.serial_baud, timeout=1.0) as ser:
|
|
last_course = None
|
|
last_speed = None
|
|
while not self._stop.is_set():
|
|
raw = ser.readline().decode("ascii", "replace").strip()
|
|
if not raw or not raw.startswith("$"):
|
|
continue
|
|
kind = raw[3:6]
|
|
if kind == "RMC":
|
|
c, s = _parse_rmc(raw)
|
|
if c is not None:
|
|
last_course = c
|
|
if s is not None:
|
|
last_speed = s
|
|
elif kind == "GGA":
|
|
parsed = _parse_gga(raw)
|
|
if parsed is not None:
|
|
lat, lon, sats, hdop = parsed
|
|
self._publish(GpsFix(
|
|
lat=lat, lon=lon, ts=time.monotonic(),
|
|
course_deg=last_course, speed_mps=last_speed,
|
|
num_sats=sats, hdop=hdop,
|
|
))
|
|
except Exception as exc: # pragma: no cover - hardware path
|
|
logger.debug("serial GPS error: %s; retrying", exc)
|
|
self._stop.wait(2.0)
|
|
|
|
|
|
class MockGpsSource(GpsSource):
|
|
"""Simulated receiver: integrates the commanded velocity into a fake fix.
|
|
|
|
Lets the geofence + BOUNDARY homing be demonstrated/tested with no hardware.
|
|
Heading is tracked CW-from-north so it matches a real GPS course-over-ground.
|
|
"""
|
|
|
|
def __init__(self, cfg: GpsConfig, rate_hz: float = 10.0) -> None:
|
|
self.cfg = cfg
|
|
self._lat0 = cfg.mock_start_lat
|
|
self._lon0 = cfg.mock_start_lon
|
|
self._north_m = 0.0
|
|
self._east_m = 0.0
|
|
self._heading_cw = 0.0 # degrees, CW from north
|
|
self._vx = 0.0
|
|
self._vyaw = 0.0
|
|
self._period = 1.0 / max(1.0, rate_hz)
|
|
self._lock = threading.Lock()
|
|
self._stop = threading.Event()
|
|
self._thread: Optional[threading.Thread] = None
|
|
self._fix: Optional[GpsFix] = None
|
|
|
|
def start(self) -> None:
|
|
self._thread = threading.Thread(target=self._loop, name="MockGps", daemon=True)
|
|
self._thread.start()
|
|
|
|
def stop(self) -> None:
|
|
self._stop.set()
|
|
|
|
def on_command(self, vx: float, vy: float, vyaw: float) -> None:
|
|
with self._lock:
|
|
self._vx = vx
|
|
self._vyaw = vyaw
|
|
|
|
def latest(self) -> Optional[GpsFix]:
|
|
with self._lock:
|
|
return self._fix
|
|
|
|
def _loop(self) -> None:
|
|
dt = self._period
|
|
while not self._stop.is_set():
|
|
with self._lock:
|
|
vx, vyaw = self._vx, self._vyaw
|
|
# +vyaw is CCW (left); CW-from-north heading decreases on a left turn.
|
|
self._heading_cw = (self._heading_cw - math.degrees(vyaw) * dt) % 360.0
|
|
h = math.radians(self._heading_cw)
|
|
self._north_m += vx * math.cos(h) * dt
|
|
self._east_m += vx * math.sin(h) * dt
|
|
lat = self._lat0 + math.degrees(self._north_m / _EARTH_RADIUS_M)
|
|
lon = self._lon0 + math.degrees(
|
|
self._east_m / (_EARTH_RADIUS_M * math.cos(math.radians(self._lat0)))
|
|
)
|
|
speed = abs(vx)
|
|
fix = GpsFix(
|
|
lat=lat, lon=lon, ts=time.monotonic(),
|
|
course_deg=self._heading_cw if speed >= 0.05 else None,
|
|
speed_mps=speed, num_sats=12, hdop=0.8,
|
|
)
|
|
with self._lock:
|
|
self._fix = fix
|
|
self._stop.wait(dt)
|
|
|
|
|
|
# --------------------------------------------------------------------------- #
|
|
# Helpers
|
|
# --------------------------------------------------------------------------- #
|
|
def _opt_float(v) -> Optional[float]:
|
|
try:
|
|
return float(v)
|
|
except (TypeError, ValueError):
|
|
return None
|
|
|
|
|
|
def _nmea_deg(value: str, hemi: str) -> Optional[float]:
|
|
"""Convert an NMEA ddmm.mmmm + hemisphere field to signed decimal degrees."""
|
|
if not value:
|
|
return None
|
|
try:
|
|
dot = value.index(".")
|
|
deg = int(value[: dot - 2])
|
|
minutes = float(value[dot - 2 :])
|
|
dec = deg + minutes / 60.0
|
|
if hemi in ("S", "W"):
|
|
dec = -dec
|
|
return dec
|
|
except (ValueError, IndexError):
|
|
return None
|
|
|
|
|
|
def _parse_gga(sentence: str):
|
|
"""Return ``(lat, lon, num_sats, hdop)`` from a GGA sentence, or ``None``."""
|
|
sentence = sentence.split("*", 1)[0] # drop the trailing *checksum
|
|
f = sentence.split(",")
|
|
if len(f) < 10:
|
|
return None
|
|
if f[6] in ("", "0"): # fix quality 0 == no fix
|
|
return None
|
|
lat = _nmea_deg(f[2], f[3])
|
|
lon = _nmea_deg(f[4], f[5])
|
|
if lat is None or lon is None:
|
|
return None
|
|
sats = int(f[7]) if f[7].isdigit() else None
|
|
hdop = _opt_float(f[8])
|
|
return lat, lon, sats, hdop
|
|
|
|
|
|
def _parse_rmc(sentence: str):
|
|
"""Return ``(course_deg, speed_mps)`` from an RMC sentence (or ``None``s)."""
|
|
sentence = sentence.split("*", 1)[0] # drop the trailing *checksum
|
|
f = sentence.split(",")
|
|
if len(f) < 9 or f[2] != "A": # status A == valid
|
|
return None, None
|
|
speed = _opt_float(f[7]) # knots
|
|
course = _opt_float(f[8])
|
|
return course, (speed * 0.514444 if speed is not None else None)
|
|
|
|
|
|
def build_gps_source(cfg: GoWelcomeConfig) -> Optional[GpsSource]:
|
|
"""Construct the GPS source named by ``cfg.gps`` (or ``None`` if disabled).
|
|
|
|
``auto`` probes gpsd first, then falls back to a serial NMEA receiver.
|
|
"""
|
|
g = cfg.gps
|
|
if not g.enabled:
|
|
return None
|
|
src = (g.source or "auto").strip().lower()
|
|
|
|
if src == "mock" or cfg.mock:
|
|
logger.info("GPS: using simulated MockGpsSource")
|
|
return MockGpsSource(g)
|
|
if src == "gpsd":
|
|
return GpsdSource(g)
|
|
if src == "serial":
|
|
return SerialNmeaSource(g)
|
|
|
|
# auto: prefer gpsd if it answers, else serial.
|
|
try:
|
|
with socket.create_connection((g.gpsd_host, g.gpsd_port), timeout=0.5):
|
|
logger.info("GPS: auto-detected gpsd at %s:%d", g.gpsd_host, g.gpsd_port)
|
|
return GpsdSource(g)
|
|
except OSError:
|
|
logger.info("GPS: gpsd not found; using serial NMEA on %s", g.serial_port)
|
|
return SerialNmeaSource(g)
|