Merge pull request 'feat: GPS waypoint logger (Issue #617)' (#620) from sl-android/issue-617-waypoint-logger into main
This commit is contained in:
commit
38df5b4ebb
705
phone/waypoint_logger.py
Normal file
705
phone/waypoint_logger.py
Normal file
@ -0,0 +1,705 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
waypoint_logger.py — GPS waypoint logger and route planner for SaltyBot (Issue #617)
|
||||
|
||||
Interactive CLI for recording, managing, and publishing GPS waypoints from
|
||||
an Android/Termux phone. Uses termux-location for GPS fixes (same provider
|
||||
as sensor_dashboard.py) and paho-mqtt to publish routes to the broker.
|
||||
|
||||
Features
|
||||
────────
|
||||
• Record GPS waypoint at current position (with name and optional tags)
|
||||
• List all saved waypoints with distance/bearing from previous
|
||||
• Delete a waypoint by index
|
||||
• Clear all waypoints in the active route
|
||||
• Publish the route as a Nav2-compatible waypoint list to MQTT
|
||||
• Subscribe to saltybot/phone/gps for live GPS if sensor_dashboard.py
|
||||
is already running (avoids double GPS call)
|
||||
• Load/save route to JSON file (persistent between sessions)
|
||||
|
||||
MQTT topics
|
||||
───────────
|
||||
Publish: saltybot/phone/route
|
||||
Subscribe: saltybot/phone/gps (live GPS from sensor_dashboard.py)
|
||||
|
||||
Route JSON published to MQTT
|
||||
─────────────────────────────
|
||||
{
|
||||
"route_name": "patrol_loop",
|
||||
"ts": 1710000000.0,
|
||||
"waypoints": [
|
||||
{
|
||||
"index": 0,
|
||||
"name": "start",
|
||||
"tags": ["patrol", "home"],
|
||||
"lat": 45.123, "lon": -73.456, "alt_m": 12.3,
|
||||
"accuracy_m": 2.1,
|
||||
"recorded_at": 1710000000.0
|
||||
}, ...
|
||||
]
|
||||
}
|
||||
|
||||
Nav2 PoseStamped list format (also included under "nav2_poses" key):
|
||||
Each entry has frame_id, position {x,y,z}, orientation {x,y,z,w}
|
||||
using a flat-earth approximation relative to the first waypoint.
|
||||
|
||||
Usage
|
||||
─────
|
||||
python3 phone/waypoint_logger.py [OPTIONS]
|
||||
|
||||
--broker HOST MQTT broker IP (default: 192.168.1.100)
|
||||
--port PORT MQTT port (default: 1883)
|
||||
--file PATH Route JSON file (default: ~/saltybot_route.json)
|
||||
--route NAME Route name (default: route)
|
||||
--provider PROV GPS provider: gps|network|passive (default: gps)
|
||||
--live-gps Subscribe to sensor_dashboard GPS instead of polling
|
||||
--no-mqtt Disable MQTT (log-only mode)
|
||||
--debug Verbose logging
|
||||
|
||||
Dependencies (Termux)
|
||||
──────────────────────
|
||||
pkg install termux-api python
|
||||
pip install paho-mqtt
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import logging
|
||||
import math
|
||||
import os
|
||||
import subprocess
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
from dataclasses import dataclass, field, asdict
|
||||
from datetime import datetime, timezone
|
||||
from typing import Optional
|
||||
|
||||
try:
|
||||
import paho.mqtt.client as mqtt
|
||||
MQTT_AVAILABLE = True
|
||||
except ImportError:
|
||||
MQTT_AVAILABLE = False
|
||||
|
||||
# ── Constants ─────────────────────────────────────────────────────────────────
|
||||
|
||||
TOPIC_ROUTE = "saltybot/phone/route"
|
||||
TOPIC_GPS_LIVE = "saltybot/phone/gps"
|
||||
|
||||
EARTH_RADIUS_M = 6_371_000.0 # mean Earth radius in metres
|
||||
|
||||
DEFAULT_FILE = os.path.expanduser("~/saltybot_route.json")
|
||||
|
||||
# ── Data model ────────────────────────────────────────────────────────────────
|
||||
|
||||
@dataclass
|
||||
class Waypoint:
|
||||
index: int
|
||||
name: str
|
||||
lat: float
|
||||
lon: float
|
||||
alt_m: float = 0.0
|
||||
accuracy_m: float = -1.0
|
||||
tags: list[str] = field(default_factory=list)
|
||||
recorded_at: float = field(default_factory=time.time)
|
||||
|
||||
def to_dict(self) -> dict:
|
||||
return asdict(self)
|
||||
|
||||
@staticmethod
|
||||
def from_dict(d: dict) -> "Waypoint":
|
||||
return Waypoint(
|
||||
index = int(d.get("index", 0)),
|
||||
name = str(d.get("name", "")),
|
||||
lat = float(d["lat"]),
|
||||
lon = float(d["lon"]),
|
||||
alt_m = float(d.get("alt_m", 0.0)),
|
||||
accuracy_m = float(d.get("accuracy_m", -1.0)),
|
||||
tags = list(d.get("tags", [])),
|
||||
recorded_at = float(d.get("recorded_at", 0.0)),
|
||||
)
|
||||
|
||||
|
||||
# ── Geo maths ─────────────────────────────────────────────────────────────────
|
||||
|
||||
def haversine_m(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
|
||||
"""Great-circle distance between two WGS84 points (metres)."""
|
||||
φ1, φ2 = math.radians(lat1), math.radians(lat2)
|
||||
Δφ = math.radians(lat2 - lat1)
|
||||
Δλ = math.radians(lon2 - lon1)
|
||||
a = math.sin(Δφ / 2) ** 2 + math.cos(φ1) * math.cos(φ2) * math.sin(Δλ / 2) ** 2
|
||||
return 2 * EARTH_RADIUS_M * math.asin(math.sqrt(a))
|
||||
|
||||
|
||||
def bearing_deg(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
|
||||
"""Initial bearing from point-1 to point-2 (degrees, 0=N, 90=E)."""
|
||||
φ1, φ2 = math.radians(lat1), math.radians(lat2)
|
||||
Δλ = math.radians(lon2 - lon1)
|
||||
x = math.sin(Δλ) * math.cos(φ2)
|
||||
y = math.cos(φ1) * math.sin(φ2) - math.sin(φ1) * math.cos(φ2) * math.cos(Δλ)
|
||||
return (math.degrees(math.atan2(x, y)) + 360) % 360
|
||||
|
||||
|
||||
def _compass(deg: float) -> str:
|
||||
"""Convert bearing degrees to 8-point compass label."""
|
||||
dirs = ["N", "NE", "E", "SE", "S", "SW", "W", "NW"]
|
||||
return dirs[round(deg / 45) % 8]
|
||||
|
||||
|
||||
def flat_earth_xy(origin_lat: float, origin_lon: float,
|
||||
lat: float, lon: float) -> tuple[float, float]:
|
||||
"""
|
||||
Simple flat-earth approximation — metres east/north from origin.
|
||||
Accurate within ~1% for distances < 100 km.
|
||||
"""
|
||||
lat_rad = math.radians((origin_lat + lat) / 2.0)
|
||||
x = math.radians(lon - origin_lon) * EARTH_RADIUS_M * math.cos(lat_rad)
|
||||
y = math.radians(lat - origin_lat) * EARTH_RADIUS_M
|
||||
return x, y
|
||||
|
||||
|
||||
# ── GPS acquisition ───────────────────────────────────────────────────────────
|
||||
|
||||
def get_gps_fix(provider: str = "gps", timeout: float = 12.0) -> Optional[dict]:
|
||||
"""
|
||||
Acquire a GPS fix via termux-location.
|
||||
Falls back to 'network' provider if 'gps' times out.
|
||||
Returns a dict with lat, lon, alt_m, accuracy_m or None on failure.
|
||||
"""
|
||||
def _try(prov: str, t: float) -> Optional[dict]:
|
||||
try:
|
||||
r = subprocess.run(
|
||||
["termux-location", "-p", prov, "-r", "once"],
|
||||
capture_output=True, text=True, timeout=t,
|
||||
)
|
||||
if r.returncode != 0 or not r.stdout.strip():
|
||||
return None
|
||||
raw = json.loads(r.stdout)
|
||||
lat = float(raw.get("latitude", 0.0))
|
||||
lon = float(raw.get("longitude", 0.0))
|
||||
if lat == 0.0 and lon == 0.0:
|
||||
return None
|
||||
return {
|
||||
"lat": lat,
|
||||
"lon": lon,
|
||||
"alt_m": float(raw.get("altitude", 0.0)),
|
||||
"accuracy_m": float(raw.get("accuracy", -1.0)),
|
||||
"provider": prov,
|
||||
}
|
||||
except (subprocess.TimeoutExpired, json.JSONDecodeError,
|
||||
FileNotFoundError, KeyError):
|
||||
return None
|
||||
|
||||
fix = _try(provider, timeout)
|
||||
if fix is None and provider == "gps":
|
||||
logging.debug("GPS timeout — retrying with network provider")
|
||||
fix = _try("network", 6.0)
|
||||
return fix
|
||||
|
||||
|
||||
# ── Live GPS subscriber (optional, uses sensor_dashboard.py stream) ──────────
|
||||
|
||||
class LiveGPS:
|
||||
"""
|
||||
Subscribe to saltybot/phone/gps on MQTT and cache the latest fix.
|
||||
Used when sensor_dashboard.py is already running to avoid double GPS calls.
|
||||
"""
|
||||
|
||||
def __init__(self, broker: str, port: int):
|
||||
self._lock = threading.Lock()
|
||||
self._latest: Optional[dict] = None
|
||||
|
||||
if not MQTT_AVAILABLE:
|
||||
return
|
||||
|
||||
self._client = mqtt.Client(client_id="wp-live-gps", clean_session=True)
|
||||
self._client.on_connect = lambda c, u, f, rc: c.subscribe(TOPIC_GPS_LIVE)
|
||||
self._client.on_message = self._on_msg
|
||||
try:
|
||||
self._client.connect(broker, port, keepalive=30)
|
||||
self._client.loop_start()
|
||||
except Exception as e:
|
||||
logging.warning("LiveGPS connect failed: %s", e)
|
||||
|
||||
def _on_msg(self, client, userdata, message) -> None:
|
||||
try:
|
||||
data = json.loads(message.payload)
|
||||
with self._lock:
|
||||
self._latest = data
|
||||
except json.JSONDecodeError:
|
||||
pass
|
||||
|
||||
def get(self, max_age_s: float = 3.0) -> Optional[dict]:
|
||||
with self._lock:
|
||||
d = self._latest
|
||||
if d is None:
|
||||
return None
|
||||
if time.time() - d.get("ts", 0.0) > max_age_s:
|
||||
return None
|
||||
return {
|
||||
"lat": d.get("lat", 0.0),
|
||||
"lon": d.get("lon", 0.0),
|
||||
"alt_m": d.get("alt_m", 0.0),
|
||||
"accuracy_m": d.get("accuracy_m", -1.0),
|
||||
"provider": d.get("provider", "live"),
|
||||
}
|
||||
|
||||
def stop(self) -> None:
|
||||
if MQTT_AVAILABLE and hasattr(self, "_client"):
|
||||
self._client.loop_stop()
|
||||
self._client.disconnect()
|
||||
|
||||
|
||||
# ── Route ─────────────────────────────────────────────────────────────────────
|
||||
|
||||
class Route:
|
||||
"""Ordered list of waypoints with persistence and Nav2 export."""
|
||||
|
||||
def __init__(self, name: str, path: str):
|
||||
self.name = name
|
||||
self._path = path
|
||||
self._waypoints: list[Waypoint] = []
|
||||
self._load()
|
||||
|
||||
# ── Persistence ───────────────────────────────────────────────────────────
|
||||
|
||||
def _load(self) -> None:
|
||||
if not os.path.exists(self._path):
|
||||
return
|
||||
try:
|
||||
with open(self._path, "r") as f:
|
||||
data = json.load(f)
|
||||
self.name = data.get("route_name", self.name)
|
||||
self._waypoints = [Waypoint.from_dict(w) for w in data.get("waypoints", [])]
|
||||
# Re-index to keep indices contiguous after deletes
|
||||
for i, wp in enumerate(self._waypoints):
|
||||
wp.index = i
|
||||
logging.info("Loaded %d waypoints from %s", len(self._waypoints), self._path)
|
||||
except (json.JSONDecodeError, KeyError, ValueError) as e:
|
||||
logging.warning("Could not load route file: %s", e)
|
||||
|
||||
def save(self) -> None:
|
||||
with open(self._path, "w") as f:
|
||||
json.dump(self.to_dict(), f, indent=2)
|
||||
logging.debug("Saved route to %s", self._path)
|
||||
|
||||
# ── Waypoint operations ───────────────────────────────────────────────────
|
||||
|
||||
def add(self, name: str, lat: float, lon: float, alt_m: float,
|
||||
accuracy_m: float, tags: list[str]) -> Waypoint:
|
||||
wp = Waypoint(
|
||||
index = len(self._waypoints),
|
||||
name = name,
|
||||
lat = lat,
|
||||
lon = lon,
|
||||
alt_m = alt_m,
|
||||
accuracy_m = accuracy_m,
|
||||
tags = tags,
|
||||
)
|
||||
self._waypoints.append(wp)
|
||||
self.save()
|
||||
return wp
|
||||
|
||||
def delete(self, index: int) -> bool:
|
||||
if not (0 <= index < len(self._waypoints)):
|
||||
return False
|
||||
self._waypoints.pop(index)
|
||||
for i, wp in enumerate(self._waypoints):
|
||||
wp.index = i
|
||||
self.save()
|
||||
return True
|
||||
|
||||
def clear(self) -> int:
|
||||
n = len(self._waypoints)
|
||||
self._waypoints.clear()
|
||||
self.save()
|
||||
return n
|
||||
|
||||
def __len__(self) -> int:
|
||||
return len(self._waypoints)
|
||||
|
||||
def __iter__(self):
|
||||
return iter(self._waypoints)
|
||||
|
||||
def get(self, index: int) -> Optional[Waypoint]:
|
||||
if 0 <= index < len(self._waypoints):
|
||||
return self._waypoints[index]
|
||||
return None
|
||||
|
||||
# ── Serialisation ─────────────────────────────────────────────────────────
|
||||
|
||||
def to_dict(self) -> dict:
|
||||
return {
|
||||
"route_name": self.name,
|
||||
"ts": time.time(),
|
||||
"waypoints": [wp.to_dict() for wp in self._waypoints],
|
||||
"nav2_poses": self._nav2_poses(),
|
||||
}
|
||||
|
||||
def _nav2_poses(self) -> list[dict]:
|
||||
"""
|
||||
Flat-earth Nav2 PoseStamped list (x east, y north) relative to
|
||||
the first waypoint. Yaw faces the next waypoint; last faces prev.
|
||||
"""
|
||||
if not self._waypoints:
|
||||
return []
|
||||
|
||||
origin = self._waypoints[0]
|
||||
poses = []
|
||||
|
||||
for i, wp in enumerate(self._waypoints):
|
||||
x, y = flat_earth_xy(origin.lat, origin.lon, wp.lat, wp.lon)
|
||||
|
||||
# Compute yaw: face toward next waypoint (or prev for last)
|
||||
if i + 1 < len(self._waypoints):
|
||||
nxt = self._waypoints[i + 1]
|
||||
nx, ny = flat_earth_xy(origin.lat, origin.lon, nxt.lat, nxt.lon)
|
||||
yaw = math.atan2(ny - y, nx - x)
|
||||
elif i > 0:
|
||||
prv = self._waypoints[i - 1]
|
||||
px, py = flat_earth_xy(origin.lat, origin.lon, prv.lat, prv.lon)
|
||||
yaw = math.atan2(y - py, x - px)
|
||||
else:
|
||||
yaw = 0.0
|
||||
|
||||
poses.append({
|
||||
"frame_id": "map",
|
||||
"position": {"x": round(x, 3), "y": round(y, 3), "z": round(wp.alt_m, 3)},
|
||||
"orientation": {
|
||||
"x": 0.0, "y": 0.0,
|
||||
"z": round(math.sin(yaw / 2), 6),
|
||||
"w": round(math.cos(yaw / 2), 6),
|
||||
},
|
||||
"waypoint_name": wp.name,
|
||||
})
|
||||
|
||||
return poses
|
||||
|
||||
|
||||
# ── MQTT publisher ────────────────────────────────────────────────────────────
|
||||
|
||||
class MQTTClient:
|
||||
"""Simple synchronous paho wrapper for route publishing."""
|
||||
|
||||
def __init__(self, broker: str, port: int):
|
||||
self._broker = broker
|
||||
self._port = port
|
||||
self._client = None
|
||||
self._connected = False
|
||||
|
||||
if not MQTT_AVAILABLE:
|
||||
return
|
||||
self._client = mqtt.Client(client_id="saltybot-waypoint-logger", clean_session=True)
|
||||
self._client.on_connect = self._on_connect
|
||||
self._client.on_disconnect = self._on_disconnect
|
||||
self._client.reconnect_delay_set(min_delay=3, max_delay=30)
|
||||
try:
|
||||
self._client.connect_async(broker, port, keepalive=60)
|
||||
self._client.loop_start()
|
||||
except Exception as e:
|
||||
logging.warning("MQTT connect failed: %s", e)
|
||||
|
||||
def _on_connect(self, c, u, f, rc) -> None:
|
||||
self._connected = (rc == 0)
|
||||
if rc == 0:
|
||||
logging.debug("MQTT connected")
|
||||
|
||||
def _on_disconnect(self, c, u, rc) -> None:
|
||||
self._connected = False
|
||||
|
||||
def publish(self, topic: str, payload: dict, qos: int = 1) -> bool:
|
||||
if self._client is None:
|
||||
return False
|
||||
# Wait briefly for connection
|
||||
deadline = time.monotonic() + 5.0
|
||||
while not self._connected and time.monotonic() < deadline:
|
||||
time.sleep(0.1)
|
||||
if not self._connected:
|
||||
return False
|
||||
info = self._client.publish(
|
||||
topic, json.dumps(payload, separators=(",", ":")), qos=qos, retain=True
|
||||
)
|
||||
return info.rc == 0
|
||||
|
||||
def stop(self) -> None:
|
||||
if self._client:
|
||||
self._client.loop_stop()
|
||||
self._client.disconnect()
|
||||
|
||||
|
||||
# ── CLI helpers ───────────────────────────────────────────────────────────────
|
||||
|
||||
def _fmt_ts(epoch: float) -> str:
|
||||
return datetime.fromtimestamp(epoch, tz=timezone.utc).strftime("%Y-%m-%d %H:%M:%S UTC")
|
||||
|
||||
|
||||
def _fmt_dist(metres: float) -> str:
|
||||
if metres < 1000:
|
||||
return f"{metres:.1f} m"
|
||||
return f"{metres / 1000:.3f} km"
|
||||
|
||||
|
||||
def _print_waypoints(route: Route) -> None:
|
||||
if not len(route):
|
||||
print(" (no waypoints)")
|
||||
return
|
||||
print(f"\n {'#':>3} {'Name':<18} {'Lat':>10} {'Lon':>11} {'Alt':>7} {'Acc':>7} {'Tags'}")
|
||||
print(" " + "─" * 75)
|
||||
prev = None
|
||||
for wp in route:
|
||||
dist_str = ""
|
||||
brg_str = ""
|
||||
if prev is not None:
|
||||
d = haversine_m(prev.lat, prev.lon, wp.lat, wp.lon)
|
||||
b = bearing_deg(prev.lat, prev.lon, wp.lat, wp.lon)
|
||||
dist_str = f" ↑ {_fmt_dist(d)} {_compass(b)}"
|
||||
tags = ",".join(wp.tags) if wp.tags else "—"
|
||||
print(f" {wp.index:>3} {wp.name:<18} {wp.lat:>10.6f} {wp.lon:>11.6f}"
|
||||
f" {wp.alt_m:>6.1f}m "
|
||||
f"{wp.accuracy_m:>5.1f}m {tags}{dist_str}")
|
||||
prev = wp
|
||||
print()
|
||||
|
||||
|
||||
def _acquire_fix(args, live_gps: Optional[LiveGPS]) -> Optional[dict]:
|
||||
"""Get a GPS fix — from live MQTT stream if available, else termux-location."""
|
||||
if live_gps is not None:
|
||||
fix = live_gps.get(max_age_s=3.0)
|
||||
if fix is not None:
|
||||
return fix
|
||||
print(" (live GPS stale — falling back to termux-location)")
|
||||
print(" Acquiring GPS fix...", end="", flush=True)
|
||||
fix = get_gps_fix(provider=args.provider)
|
||||
if fix:
|
||||
print(f" {fix['lat']:.6f}, {fix['lon']:.6f} ±{fix['accuracy_m']:.1f}m")
|
||||
else:
|
||||
print(" FAILED")
|
||||
return fix
|
||||
|
||||
|
||||
# ── Interactive menu ──────────────────────────────────────────────────────────
|
||||
|
||||
def _menu_record(route: Route, args, live_gps: Optional[LiveGPS]) -> None:
|
||||
fix = _acquire_fix(args, live_gps)
|
||||
if fix is None:
|
||||
print(" Could not get GPS fix.")
|
||||
return
|
||||
|
||||
default_name = f"wp{len(route)}"
|
||||
name = input(f" Waypoint name [{default_name}]: ").strip() or default_name
|
||||
tags_raw = input(" Tags (comma-separated, or Enter to skip): ").strip()
|
||||
tags = [t.strip() for t in tags_raw.split(",") if t.strip()] if tags_raw else []
|
||||
|
||||
wp = route.add(name, fix["lat"], fix["lon"], fix["alt_m"], fix["accuracy_m"], tags)
|
||||
print(f" ✓ Recorded #{wp.index}: {wp.name} ({wp.lat:.6f}, {wp.lon:.6f})")
|
||||
|
||||
|
||||
def _menu_list(route: Route) -> None:
|
||||
print(f"\n Route: '{route.name}' ({len(route)} waypoints)")
|
||||
_print_waypoints(route)
|
||||
|
||||
if len(route) >= 2:
|
||||
wps = list(route)
|
||||
total = sum(
|
||||
haversine_m(wps[i].lat, wps[i].lon, wps[i+1].lat, wps[i+1].lon)
|
||||
for i in range(len(wps) - 1)
|
||||
)
|
||||
print(f" Total route distance: {_fmt_dist(total)}")
|
||||
|
||||
|
||||
def _menu_delete(route: Route) -> None:
|
||||
if not len(route):
|
||||
print(" No waypoints to delete.")
|
||||
return
|
||||
_print_waypoints(route)
|
||||
raw = input(" Enter index to delete (or Enter to cancel): ").strip()
|
||||
if not raw:
|
||||
return
|
||||
try:
|
||||
idx = int(raw)
|
||||
except ValueError:
|
||||
print(" Invalid index.")
|
||||
return
|
||||
wp = route.get(idx)
|
||||
if wp is None:
|
||||
print(f" No waypoint at index {idx}.")
|
||||
return
|
||||
confirm = input(f" Delete '{wp.name}'? [y/N]: ").strip().lower()
|
||||
if confirm == "y":
|
||||
route.delete(idx)
|
||||
print(f" ✓ Deleted '{wp.name}'.")
|
||||
else:
|
||||
print(" Cancelled.")
|
||||
|
||||
|
||||
def _menu_publish(route: Route, mqtt_client: Optional[MQTTClient]) -> None:
|
||||
if not len(route):
|
||||
print(" No waypoints to publish.")
|
||||
return
|
||||
|
||||
payload = route.to_dict()
|
||||
print(f"\n Publishing {len(route)} waypoints to {TOPIC_ROUTE}...")
|
||||
|
||||
if mqtt_client is None:
|
||||
print(" (MQTT disabled — printing payload)")
|
||||
print(json.dumps(payload, indent=2))
|
||||
return
|
||||
|
||||
ok = mqtt_client.publish(TOPIC_ROUTE, payload, qos=1)
|
||||
if ok:
|
||||
print(f" ✓ Published to {TOPIC_ROUTE} (retained=true)")
|
||||
else:
|
||||
print(" ✗ MQTT publish failed — check broker connection.")
|
||||
|
||||
# Also print Nav2 summary
|
||||
poses = payload.get("nav2_poses", [])
|
||||
if poses:
|
||||
print(f"\n Nav2 poses ({len(poses)}):")
|
||||
for p in poses:
|
||||
pos = p["position"]
|
||||
print(f" {p['waypoint_name']:<18} x={pos['x']:>8.2f}m y={pos['y']:>8.2f}m")
|
||||
|
||||
|
||||
def _menu_clear(route: Route) -> None:
|
||||
if not len(route):
|
||||
print(" Route is already empty.")
|
||||
return
|
||||
confirm = input(f" Clear all {len(route)} waypoints? [y/N]: ").strip().lower()
|
||||
if confirm == "y":
|
||||
n = route.clear()
|
||||
print(f" ✓ Cleared {n} waypoints.")
|
||||
else:
|
||||
print(" Cancelled.")
|
||||
|
||||
|
||||
def _menu_rename(route: Route) -> None:
|
||||
new_name = input(f" New route name [{route.name}]: ").strip()
|
||||
if new_name:
|
||||
route.name = new_name
|
||||
route.save()
|
||||
print(f" ✓ Route renamed to '{route.name}'.")
|
||||
|
||||
|
||||
def _menu_info(route: Route) -> None:
|
||||
print(f"\n Route file: {route._path}")
|
||||
print(f" Route name: {route.name}")
|
||||
print(f" Waypoints: {len(route)}")
|
||||
if len(route):
|
||||
wps = list(route)
|
||||
print(f" First wp: {wps[0].name} ({_fmt_ts(wps[0].recorded_at)})")
|
||||
print(f" Last wp: {wps[-1].name} ({_fmt_ts(wps[-1].recorded_at)})")
|
||||
if len(route) >= 2:
|
||||
total = sum(
|
||||
haversine_m(wps[i].lat, wps[i].lon, wps[i+1].lat, wps[i+1].lon)
|
||||
for i in range(len(wps) - 1)
|
||||
)
|
||||
print(f" Total dist: {_fmt_dist(total)}")
|
||||
print()
|
||||
|
||||
|
||||
# ── Main loop ─────────────────────────────────────────────────────────────────
|
||||
|
||||
MENU = """
|
||||
┌─ SaltyBot Waypoint Logger ──────────────────────────┐
|
||||
│ r Record waypoint at current GPS position │
|
||||
│ l List all waypoints │
|
||||
│ d Delete a waypoint │
|
||||
│ p Publish route to MQTT │
|
||||
│ c Clear all waypoints │
|
||||
│ n Rename route │
|
||||
│ i Route info │
|
||||
│ q Quit │
|
||||
└─────────────────────────────────────────────────────┘
|
||||
"""
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(
|
||||
description="SaltyBot GPS waypoint logger (Issue #617)"
|
||||
)
|
||||
parser.add_argument("--broker", default="192.168.1.100",
|
||||
help="MQTT broker IP (default: 192.168.1.100)")
|
||||
parser.add_argument("--port", type=int, default=1883,
|
||||
help="MQTT port (default: 1883)")
|
||||
parser.add_argument("--file", default=DEFAULT_FILE,
|
||||
help=f"Route JSON file (default: {DEFAULT_FILE})")
|
||||
parser.add_argument("--route", default="route",
|
||||
help="Route name (default: route)")
|
||||
parser.add_argument("--provider", default="gps",
|
||||
choices=["gps", "network", "passive"],
|
||||
help="GPS provider (default: gps)")
|
||||
parser.add_argument("--live-gps", action="store_true",
|
||||
help="Subscribe to saltybot/phone/gps for live GPS")
|
||||
parser.add_argument("--no-mqtt", action="store_true",
|
||||
help="Disable MQTT (log-only mode)")
|
||||
parser.add_argument("--debug", action="store_true",
|
||||
help="Verbose logging")
|
||||
args = parser.parse_args()
|
||||
|
||||
logging.basicConfig(
|
||||
level=logging.DEBUG if args.debug else logging.WARNING,
|
||||
format="%(asctime)s [%(levelname)s] %(message)s",
|
||||
)
|
||||
|
||||
# Load or create route
|
||||
route = Route(args.route, args.file)
|
||||
|
||||
# MQTT client
|
||||
mqtt_client: Optional[MQTTClient] = None
|
||||
if not args.no_mqtt:
|
||||
if MQTT_AVAILABLE:
|
||||
mqtt_client = MQTTClient(args.broker, args.port)
|
||||
else:
|
||||
print("Warning: paho-mqtt not installed — MQTT disabled. Run: pip install paho-mqtt")
|
||||
|
||||
# Optional live GPS subscription
|
||||
live_gps: Optional[LiveGPS] = None
|
||||
if args.live_gps and MQTT_AVAILABLE:
|
||||
live_gps = LiveGPS(args.broker, args.port)
|
||||
print(f" Subscribed to live GPS on {TOPIC_GPS_LIVE}")
|
||||
|
||||
print(MENU)
|
||||
print(f" Route: '{route.name}' | {len(route)} waypoints loaded | file: {args.file}")
|
||||
print()
|
||||
|
||||
try:
|
||||
while True:
|
||||
try:
|
||||
choice = input(" > ").strip().lower()
|
||||
except EOFError:
|
||||
break
|
||||
|
||||
if choice in ("q", "quit", "exit"):
|
||||
break
|
||||
elif choice in ("r", "record"):
|
||||
_menu_record(route, args, live_gps)
|
||||
elif choice in ("l", "list"):
|
||||
_menu_list(route)
|
||||
elif choice in ("d", "delete"):
|
||||
_menu_delete(route)
|
||||
elif choice in ("p", "publish"):
|
||||
_menu_publish(route, mqtt_client)
|
||||
elif choice in ("c", "clear"):
|
||||
_menu_clear(route)
|
||||
elif choice in ("n", "rename"):
|
||||
_menu_rename(route)
|
||||
elif choice in ("i", "info"):
|
||||
_menu_info(route)
|
||||
elif choice == "":
|
||||
pass
|
||||
else:
|
||||
print(" Unknown command. Type r/l/d/p/c/n/i/q.")
|
||||
|
||||
except KeyboardInterrupt:
|
||||
print("\n Interrupted.")
|
||||
finally:
|
||||
if live_gps:
|
||||
live_gps.stop()
|
||||
if mqtt_client:
|
||||
mqtt_client.stop()
|
||||
|
||||
print(" Bye.")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Loading…
x
Reference in New Issue
Block a user