9b1f3ddaf0
Add phone/waypoint_logger.py — interactive Termux CLI for recording,
managing, and publishing GPS waypoints:
GPS acquisition
- termux-location with gps/network/passive provider selection
- Falls back to network provider on GPS timeout
- Optional --live-gps flag: subscribes to saltybot/phone/gps MQTT
topic (sensor_dashboard.py stream) to avoid redundant GPS calls
Waypoint operations
- Record: acquires GPS fix, prompts for name + tags, appends to route
- List: table with lat/lon/alt/accuracy/tags + inter-waypoint
distance (haversine) and bearing (8-point compass)
- Delete: by index with confirmation prompt
- Clear: entire route with confirmation
- Rename: route name
Persistence
- Routes saved as JSON to ~/saltybot_route.json (configurable)
- Auto-loads on startup; survives session restarts
MQTT publish (saltybot/phone/route, QoS 1, retained)
- Full waypoint list with metadata
- nav2_poses array: flat-earth x/y (metres from origin),
quaternion yaw facing next waypoint (last faces prev)
- Compatible with Nav2 FollowWaypoints action input
Geo maths
- haversine_m(): great-circle distance
- bearing_deg(): initial bearing with 8-point compass label
- flat_earth_xy(): ENU metres for Nav2 pose export (<1% error <100km)
Flags: --broker, --port, --file, --route, --provider, --live-gps,
--no-mqtt, --debug
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
706 lines
26 KiB
Python
706 lines
26 KiB
Python
#!/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()
|