kassam/fleet
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
fleet/agents/g1/sanad_api_g1.py

486 lines
18 KiB
Python
Raw Blame History

#!/usr/bin/env python3
"""sanad_api_g1 — G1 fleet MAP uploader.
Scope (this build): upload the G1's navigation MAP to the YS Lootah fleet
server. Nothing else (no telemetry / commands / logs — those are separate
agents). It is the "Maps sync" row of the fleet spec:
POST {SERVER_URL}/api/v1/fleet/ingest/{sn}/map (Bearer device token)
WHAT IT UPLOADS
---------------
The G1's map is produced by the web_nav3 (Nav2 + RTAB-Map) stack and stored on
disk as a RTAB-Map SQLite ``.db`` file (per web_nav3/web/backend.py:
``MAPS_ROOT/<robot>/<name>.db`` + a ``maps_meta.json`` sidecar), with each map's
named places kept in ``web/data/<robot>/places/<name>.json``.
There is NO rendered PNG on disk — the dashboard draws the occupancy grid live
over rosbridge. By design choice this agent uploads the RAW ``.db`` (the actual
map artifact) plus its places, rather than rendering an image. That means the
FLEET SERVER must accept a ``rtabmap_db`` artifact on the map endpoint (see
MAP_UPLOAD_MODE for the two wire formats). The documented spec body
(image_base64 / resolution / origin) is for a rendered map; switch to the
telemetry-agent's live-rosbridge renderer if you need that instead.
NO ROS, NO DDS. Pure files + HTTPS, so it drops into any robot as its own
container. It reads the maps/places straight from mounted volumes; the web_nav3
HTTP API is only used (optionally) to learn which map is "active".
CHANGE DETECTION
----------------
Each map's ``.db`` is fingerprinted (size + mtime fast-path, then sha256). A map
is (re)uploaded only when its fingerprint changes — matching the spec's "send
the nav map on change". State persists in ``STATE_DIR/uploaded.json`` so a
restart doesn't re-push unchanged maps.
CONFIG — all via environment (see .env.example)
-----------------------------------------------
SERVER_URL base URL of the fleet server (required) e.g. https://fleet.example.com
DEVICE_TOKEN per-robot bearer token (required)
SN this robot's fleet id (path key) default g1_7892
ROBOT web_nav3 robot name (maps subdir + header) default sanad
MAPS_DIR mounted web_nav3 maps/ dir (contains <robot>/*.db and/or *.db)
DATA_DIR mounted web_nav3 web/data dir (per-map places live under <robot>/places/)
LEGACY_PLACES optional path to a legacy places.json (per-robot, no map scoping)
WEB_NAV3_URL optional http://127.0.0.1:8765 — used only to read the ACTIVE map
MAP_SELECT all | active | newest default all
MAP_UPLOAD_MODE multipart | base64json default multipart
MAP_ENDPOINT path template, {sn} substituted default /api/v1/fleet/ingest/{sn}/map
POLL_INTERVAL seconds between scans (loop mode) default 30
STATE_DIR writable dir for upload state default /data/state
VERIFY_TLS 1|0 verify server TLS default 1
HTTP_TIMEOUT per-request timeout seconds default 30
CLI
---
python sanad_api_g1.py # run the loop (default)
python sanad_api_g1.py --once # one scan+upload pass, then exit
python sanad_api_g1.py --list # list discovered maps (no upload)
python sanad_api_g1.py --dry-run # build payloads + report, never POST
python sanad_api_g1.py --force # ignore state; upload even if unchanged
"""
from __future__ import annotations
import argparse
import base64
import hashlib
import json
import logging
import math
import os
import sys
import time
from dataclasses import dataclass, field
from pathlib import Path
from typing import Any, Dict, List, Optional
import requests
log = logging.getLogger("sanad_api_g1")
# --------------------------------------------------------------------------- #
# tiny .env loader (so it also runs bare, outside docker) — no dependency
# --------------------------------------------------------------------------- #
def _load_dotenv(path: str = ".env") -> None:
p = Path(path)
if not p.exists():
return
for line in p.read_text().splitlines():
line = line.strip()
if not line or line.startswith("#") or "=" not in line:
continue
k, _, v = line.partition("=")
k, v = k.strip(), v.strip().strip('"').strip("'")
os.environ.setdefault(k, v)
def _env(name: str, default: str = "") -> str:
return os.environ.get(name, default).strip()
def _env_bool(name: str, default: bool) -> bool:
v = _env(name, "1" if default else "0").lower()
return v in ("1", "true", "yes", "on")
# --------------------------------------------------------------------------- #
# config
# --------------------------------------------------------------------------- #
@dataclass
class Config:
server_url: str
device_token: str
sn: str
robot: str
maps_dir: Path
data_dir: Optional[Path]
legacy_places: Optional[Path]
web_nav3_url: str
map_select: str
upload_mode: str
endpoint_tmpl: str
poll_interval: float
state_dir: Path
verify_tls: bool
http_timeout: float
@classmethod
def from_env(cls) -> "Config":
server = _env("SERVER_URL").rstrip("/")
token = _env("DEVICE_TOKEN")
missing = [n for n, v in (("SERVER_URL", server), ("DEVICE_TOKEN", token)) if not v]
if missing:
raise SystemExit(f"[config] missing required env: {', '.join(missing)}")
data_dir = _env("DATA_DIR")
legacy = _env("LEGACY_PLACES")
return cls(
server_url=server,
device_token=token,
sn=_env("SN", "g1_7892"),
robot=_env("ROBOT", "sanad"),
maps_dir=Path(_env("MAPS_DIR", "/data/maps")),
data_dir=Path(data_dir) if data_dir else None,
legacy_places=Path(legacy) if legacy else None,
web_nav3_url=_env("WEB_NAV3_URL", "").rstrip("/"),
map_select=_env("MAP_SELECT", "all").lower(),
upload_mode=_env("MAP_UPLOAD_MODE", "multipart").lower(),
endpoint_tmpl=_env("MAP_ENDPOINT", "/api/v1/fleet/ingest/{sn}/map"),
poll_interval=float(_env("POLL_INTERVAL", "30")),
state_dir=Path(_env("STATE_DIR", "/data/state")),
verify_tls=_env_bool("VERIFY_TLS", True),
http_timeout=float(_env("HTTP_TIMEOUT", "30")),
)
def map_url(self) -> str:
return self.server_url + self.endpoint_tmpl.format(sn=self.sn)
def auth_headers(self) -> Dict[str, str]:
return {"Authorization": f"Bearer {self.device_token}"}
# --------------------------------------------------------------------------- #
# map artifact
# --------------------------------------------------------------------------- #
@dataclass
class MapArtifact:
path: Path # absolute path to the .db
name: str # file name, e.g. floor-1.db
stem: str # name without .db, e.g. floor-1
size: int
mtime: int
description: str = ""
sha256: str = ""
points: List[Dict[str, Any]] = field(default_factory=list)
def fingerprint(self) -> str:
# cheap identity for the change check before we hash the whole file
return f"{self.size}:{self.mtime}"
def _sha256(path: Path) -> str:
h = hashlib.sha256()
with path.open("rb") as f:
for chunk in iter(lambda: f.read(1024 * 1024), b""):
h.update(chunk)
return h.hexdigest()
# --------------------------------------------------------------------------- #
# discovery — read maps + places straight from the mounted web_nav3 files
# --------------------------------------------------------------------------- #
def _map_key(stem: str) -> str:
"""Mirror backend._map_key: safe filename stem for the per-map places file."""
stem = Path(stem).name
if stem.endswith(".db"):
stem = stem[:-3]
return "".join(c for c in stem if c.isalnum() or c in "_-.")
def _read_json(path: Path, default: Any) -> Any:
try:
return json.loads(path.read_text() or "")
except Exception:
return default
def _yaw_from_pose(pose: Dict[str, Any]) -> float:
"""Yaw (rad) from a places-pose dict. Supports full quaternion, planar
(qz,qw), or an explicit yaw field."""
if "qw" in pose or "qz" in pose:
qx = float(pose.get("qx", 0.0)); qy = float(pose.get("qy", 0.0))
qz = float(pose.get("qz", 0.0)); qw = float(pose.get("qw", 1.0))
return math.atan2(2.0 * (qw * qz + qx * qy),
1.0 - 2.0 * (qy * qy + qz * qz))
return float(pose.get("yaw", 0.0))
def _places_files_for(cfg: Config, stem: str) -> List[Path]:
"""Candidate on-disk places files for a map, most-specific first."""
out: List[Path] = []
key = _map_key(stem)
if cfg.data_dir:
out.append(cfg.data_dir / cfg.robot / "places" / f"{key}.json")
if cfg.legacy_places:
out.append(cfg.legacy_places)
return out
def load_points(cfg: Config, stem: str) -> List[Dict[str, Any]]:
"""Return the map's saved places as fleet points: {name, type, x, y, yaw}.
Places store shape (web_nav3): {"<name>": {x,y,z,qx,qy,qz,qw}}.
"""
for pf in _places_files_for(cfg, stem):
data = _read_json(pf, None) if pf.exists() else None
if isinstance(data, dict) and data:
pts: List[Dict[str, Any]] = []
for name, pose in data.items():
if not isinstance(pose, dict):
continue
try:
pts.append({
"name": name,
"type": str(pose.get("type", "waypoint")),
"x": float(pose["x"]),
"y": float(pose["y"]),
"yaw": round(_yaw_from_pose(pose), 4),
})
except (KeyError, TypeError, ValueError):
continue
log.debug("points for %s: %d (from %s)", stem, len(pts), pf)
return pts
return []
def discover_maps(cfg: Config) -> List[MapArtifact]:
"""Find every .db under MAPS_DIR/<robot>/ and MAPS_DIR/ (legacy root)."""
roots = [cfg.maps_dir / cfg.robot, cfg.maps_dir]
meta: Dict[str, Any] = {}
meta_file = cfg.maps_dir / cfg.robot / "maps_meta.json"
if meta_file.exists():
meta = _read_json(meta_file, {}) or {}
seen: set[str] = set()
out: List[MapArtifact] = []
for root in roots:
if not root.exists():
continue
for p in sorted(root.glob("*.db")):
rp = str(p.resolve())
if rp in seen:
continue
seen.add(rp)
st = p.stat()
out.append(MapArtifact(
path=p,
name=p.name,
stem=p.stem,
size=st.st_size,
mtime=int(st.st_mtime),
description=(meta.get(p.name) or {}).get("description", ""),
))
out.sort(key=lambda m: m.mtime, reverse=True)
return out
def _active_map_name(cfg: Config) -> Optional[str]:
"""Ask web_nav3 which map is currently loaded (optional; None if not set/up)."""
if not cfg.web_nav3_url:
return None
try:
r = requests.get(cfg.web_nav3_url + "/api/status",
headers={"X-Robot-Name": cfg.robot},
timeout=min(cfg.http_timeout, 5))
r.raise_for_status()
am = (r.json() or {}).get("active_map")
return _map_key(am) if am else None
except requests.RequestException as e:
log.debug("active-map query failed: %s", e)
return None
def select_maps(cfg: Config, maps: List[MapArtifact]) -> List[MapArtifact]:
if not maps:
return []
if cfg.map_select == "newest":
return maps[:1]
if cfg.map_select == "active":
active = _active_map_name(cfg)
if active:
picked = [m for m in maps if _map_key(m.stem) == active]
if picked:
return picked
log.warning("active map %r not found on disk; falling back to newest", active)
return maps[:1]
return maps # "all"
# --------------------------------------------------------------------------- #
# state (which fingerprints already uploaded)
# --------------------------------------------------------------------------- #
def _state_file(cfg: Config) -> Path:
return cfg.state_dir / "uploaded.json"
def load_state(cfg: Config) -> Dict[str, str]:
return _read_json(_state_file(cfg), {}) if _state_file(cfg).exists() else {}
def save_state(cfg: Config, state: Dict[str, str]) -> None:
try:
cfg.state_dir.mkdir(parents=True, exist_ok=True)
_state_file(cfg).write_text(json.dumps(state, indent=2))
except Exception as e:
log.warning("could not persist state: %s", e)
# --------------------------------------------------------------------------- #
# upload
# --------------------------------------------------------------------------- #
def build_meta(cfg: Config, m: MapArtifact) -> Dict[str, Any]:
return {
"sn": cfg.sn,
"name": m.stem,
"file": m.name,
"format": "rtabmap_db",
"size_bytes": m.size,
"sha256": m.sha256,
"mtime": m.mtime,
"description": m.description,
"points": m.points,
}
def upload_map(cfg: Config, m: MapArtifact, session: requests.Session) -> bool:
url = cfg.map_url()
meta = build_meta(cfg, m)
try:
if cfg.upload_mode == "base64json":
body = dict(meta)
body["db_base64"] = base64.b64encode(m.path.read_bytes()).decode("ascii")
resp = session.post(url, json=body, headers=cfg.auth_headers(),
timeout=cfg.http_timeout, verify=cfg.verify_tls)
else: # multipart (default)
with m.path.open("rb") as fh:
files = {"db": (m.name, fh, "application/octet-stream")}
data = {"meta": json.dumps(meta)}
resp = session.post(url, files=files, data=data,
headers=cfg.auth_headers(),
timeout=cfg.http_timeout, verify=cfg.verify_tls)
except requests.RequestException as e:
log.error("upload %s FAILED (transport): %s", m.name, e)
return False
if not resp.ok:
detail = resp.text[:300]
log.error("upload %s FAILED: HTTP %s %s", m.name, resp.status_code, detail)
return False
log.info("uploaded %s (%.2f MB, %d points) -> HTTP %s",
m.name, m.size / 1024 / 1024, len(m.points), resp.status_code)
return True
# --------------------------------------------------------------------------- #
# one pass
# --------------------------------------------------------------------------- #
def run_once(cfg: Config, *, force: bool, dry_run: bool,
session: requests.Session) -> int:
maps = select_maps(cfg, discover_maps(cfg))
if not maps:
log.info("no .db maps found under %s (robot=%s)", cfg.maps_dir, cfg.robot)
return 0
state = load_state(cfg)
uploaded = 0
for m in maps:
prev = state.get(str(m.path.resolve()))
if not force and prev == m.fingerprint():
log.debug("unchanged, skip: %s", m.name)
continue
# confirm change with a real content hash (mtime can shift without edits)
m.sha256 = _sha256(m.path)
m.points = load_points(cfg, m.stem)
if dry_run:
log.info("[dry-run] would upload %s (%.2f MB, sha=%s…, %d points)",
m.name, m.size / 1024 / 1024, m.sha256[:12], len(m.points))
uploaded += 1
continue
if upload_map(cfg, m, session):
state[str(m.path.resolve())] = m.fingerprint()
save_state(cfg, state)
uploaded += 1
if uploaded == 0:
log.info("nothing to upload (%d map(s) already current)", len(maps))
return uploaded
def cmd_list(cfg: Config) -> None:
maps = discover_maps(cfg)
active = _active_map_name(cfg)
if not maps:
print(f"(no .db maps under {cfg.maps_dir} for robot '{cfg.robot}')")
return
print(f"{len(maps)} map(s) under {cfg.maps_dir} (robot={cfg.robot}):")
for m in maps:
pts = load_points(cfg, m.stem)
flag = " <-- active" if active and _map_key(m.stem) == active else ""
print(f" {m.name:<28} {m.size/1024/1024:6.2f} MB {len(pts):>3} points"
f" {m.description}{flag}")
# --------------------------------------------------------------------------- #
# main
# --------------------------------------------------------------------------- #
def main(argv: Optional[List[str]] = None) -> int:
ap = argparse.ArgumentParser(description="G1 fleet map uploader")
ap.add_argument("--once", action="store_true", help="one scan+upload pass, then exit")
ap.add_argument("--list", action="store_true", help="list discovered maps and exit")
ap.add_argument("--dry-run", action="store_true", help="build payloads but never POST")
ap.add_argument("--force", action="store_true", help="upload even if unchanged")
ap.add_argument("--interval", type=float, default=None, help="override POLL_INTERVAL seconds")
ap.add_argument("-v", "--verbose", action="store_true")
args = ap.parse_args(argv)
logging.basicConfig(
level=logging.DEBUG if args.verbose else logging.INFO,
format="%(asctime)s %(levelname)s %(name)s: %(message)s",
)
_load_dotenv()
cfg = Config.from_env()
if args.interval is not None:
cfg.poll_interval = args.interval
log.info("sanad_api_g1 map uploader — sn=%s robot=%s server=%s mode=%s select=%s",
cfg.sn, cfg.robot, cfg.server_url, cfg.upload_mode, cfg.map_select)
log.info("maps_dir=%s data_dir=%s web_nav3=%s",
cfg.maps_dir, cfg.data_dir, cfg.web_nav3_url or "(disabled)")
if args.list:
cmd_list(cfg)
return 0
session = requests.Session()
if args.once or args.dry_run:
run_once(cfg, force=args.force, dry_run=args.dry_run, session=session)
return 0
log.info("loop every %.0fs (Ctrl-C to stop)", cfg.poll_interval)
while True:
try:
run_once(cfg, force=args.force, dry_run=False, session=session)
except Exception as e: # never let the loop die
log.exception("pass failed: %s", e)
try:
time.sleep(cfg.poll_interval)
except KeyboardInterrupt:
log.info("stopped")
return 0
if __name__ == "__main__":
sys.exit(main())
Reference in New Issue View Git Blame Copy Permalink