diff --git a/PIPELINE.md b/PIPELINE.md index e472517..351ab97 100644 --- a/PIPELINE.md +++ b/PIPELINE.md @@ -42,12 +42,12 @@ repo currently implements the **bold** ones; the rest are documented for later. | endpoint | method | agent | status | |---|---|---|---| -| **`/api/v1/fleet/ingest/telemetry`** | POST | G1 (`g1t`), R1, Go2 | ✅ implemented | -| **`/api/v1/fleet/ingest/{sn}/map`** | POST | G1 (`g1`) | ✅ implemented | +| **`/api/v1/fleet/ingest/telemetry`** | POST | G1, R1, Go2 | ✅ implemented (~2 s; includes `map` status) | +| **`/api/v1/fleet/ingest/{sn}/map`** | POST | G1, R1, Go2 | ✅ implemented (once per content; rtabmap `.db` + slam_toolbox) | +| **`/api/v1/fleet/ingest/{sn}/alert`** | POST | G1, R1, Go2 | ✅ implemented (each NEW fault, rising edge, string body) | +| **`/api/v1/fleet/ingest/{sn}/logs`** | POST | G1, R1, Go2 | ✅ implemented (agent log lines every `LOGS_INTERVAL`, default 60 s) | | `/api/v1/fleet/ingest/{sn}/commands` | GET | — | ⏳ spec'd, not built | | `/api/v1/fleet/ingest/commands/{id}/ack` | POST | — | ⏳ | -| `/api/v1/fleet/ingest/{sn}/alert` | POST | — | ⏳ (critical faults; ordinary ones ride in telemetry `faults[]`) | -| `/api/v1/fleet/ingest/{sn}/logs` | POST | — | ⏳ | | `/api/v1/fleet/ingest/{sn}/remote` | POST | — | ⏳ (tunnel/SSH registration) | Auth header (all): `Authorization: Bearer `. diff --git a/README.md b/README.md index ef948c2..df0658d 100644 --- a/README.md +++ b/README.md @@ -61,10 +61,6 @@ Project/fleet/ │ ├── requirements.txt │ ├── docker-compose.yml (manual local use only) │ └── .env.example - ├── g1t/ ← TELEMETRY (G1, unitree_hg) - │ ├── sanad_api_g1t.py - │ ├── Dockerfile (DDS) + vendor/ - │ └── .env.example ├── r1/ ← TELEMETRY (unitree_hg) │ ├── sanad_api_r1.py │ ├── Dockerfile (DDS: CycloneDDS + unitree_sdk2py) @@ -87,14 +83,18 @@ agent to the robot; you never edit files on the robot. | agent (type) | robot | endpoint | what it sends | cadence | |---|---|---|---|---| | `sanad_api_g1` (`g1`) | Unitree G1 | `POST /api/v1/fleet/ingest/{sn}/map` | RTAB-Map `.db` + places (points) | on change (~30 s scan) | -| `sanad_api_g1t` (`g1t`) | Unitree G1 | `POST /api/v1/fleet/ingest/telemetry` | battery, charging, status, position, faults, mac | every ~2 s | | `sanad_api_r1` (`r1`) | Unitree R1 EDU | `POST /api/v1/fleet/ingest/telemetry` | same telemetry | every ~2 s | | `sanad_api_go2` (`go2`) | Unitree Go2 | `POST /api/v1/fleet/ingest/telemetry` | same telemetry | every ~2 s | -The G1 runs **both** `g1` (map) and `g1t` (telemetry) — two independent -containers/services, both keyed by the **same `sn`** (same physical robot). `g1t` -uses the same `unitree_hg` DDS as R1, and reads `position` from the firmware odom -topic (`rt/lf/odommodestate`) over DDS. +The `g1` agent is ONE service doing BOTH jobs: telemetry every ~2 s **and** a +30 s map loop that checks the Sanad dashboard's saved maps and uploads each one +ONE time (re-upload only on content change). The map result is embedded in every +telemetry post as the `map` field (`uploaded/no_map/failed` + reason), so a +missing map is always visible on the server. Supported map formats: +**RTAB-Map `.db`** and **slam_toolbox sets** (`.yaml` + `.pgm` +[+ `.posegraph`/`.data`]) — slam maps are converted to the spec's image JSON +(PNG + resolution + origin + width/height). Faults are sent as **strings** +(the ingest 500s on fault objects). All requests carry `Authorization: Bearer `. Full payload schemas and the data pipeline are in [PIPELINE.md](PIPELINE.md). @@ -140,7 +140,7 @@ cd Project/fleet ./fleet_install.sh # …or scripted: -./fleet_install.sh install r1 10.255.254.82 --sn r1_82 \ +./fleet_install.sh install r1 10.255.254.82 --sn E39N4000Q6D7E70F --name r1_82 \ --server-ip 10.255.254.83 --port 8799 --token # See what it's sending, tail logs, check the service: @@ -371,8 +371,8 @@ bridge). | robot | agent(s) | IP | SSH | arch | DDS iface | SN | |---|---|---|---|---|---|---| -| G1 | map (`g1`) + telemetry (`g1t`) | `10.255.254.58` | `unitree` (key) | arm64 | `eth0` | `g1_7892` | -| R1 | telemetry (`r1`) | `10.255.254.82` | `unitree` (key) | arm64 | `eth10` | `r1_82` | +| G1 | telemetry + map (`g1`) | `10.255.254.58` | `unitree` (key) | arm64 | `eth0` | `E21D6000PB89GF88` (name `g1_58`) | +| R1 | telemetry (`r1`) | `10.255.254.82` | `unitree` (key) | arm64 | `eth10` | `E39N4000Q6D7E70F` (name `r1_82`) | | Go2 | telemetry (`go2`) | *(TBD)* | `unitree` | arm64 | `eth0` | — | Workstation (deploy host + test fleet server): **`10.255.254.83`** (`wlp4s0`). diff --git a/agents/g1/.env.example b/agents/g1/.env.example index 306ae7f..e9d3b39 100644 --- a/agents/g1/.env.example +++ b/agents/g1/.env.example @@ -41,3 +41,15 @@ POLL_INTERVAL=30 # Verify the fleet server's TLS cert (1 recommended; 0 only for self-signed dev). VERIFY_TLS=1 HTTP_TIMEOUT=30 + +# ── logs + alerts ───────────────────────────────────────────────────────────── +LOGS_INTERVAL=60 + +# ── project logs (shared alongside agent logs) ─────────────────────────────── +# auto (default) = find a RUNNING Sanad project container (sanadr1, sanad-p4, +# sanad*) via the /host mount and tail its docker json-log, labeled +# "-logs" in the shipped lines; telemetry shows project_logs (null=none). +# Or pin a container name / an explicit log file (PROJECT_LOG_PATH, via /host). +PROJECT_LOG_CONTAINER=auto +PROJECT_LOG_PATH= +PROJECT_LOG_LABEL= diff --git a/agents/g1/sanad_api_g1.py b/agents/g1/sanad_api_g1.py index 5b30534..bdd8593 100644 --- a/agents/g1/sanad_api_g1.py +++ b/agents/g1/sanad_api_g1.py @@ -48,6 +48,7 @@ from __future__ import annotations import argparse import base64 +import datetime as _dt import hashlib import json import logging @@ -90,6 +91,14 @@ def _env_bool(name: str, default: bool) -> bool: return _env(name, "1" if default else "0").lower() in ("1", "true", "yes", "on") +def _now_str() -> str: + """Full local date+time with UTC offset (e.g. 2026-07-13 14:20:33+04:00). + TZ_OFFSET_HOURS (default +4, Dubai) keeps container clocks honest without tzdata.""" + off = float(_env("TZ_OFFSET_HOURS", "4")) + tz = _dt.timezone(_dt.timedelta(hours=off)) + return _dt.datetime.now(tz).isoformat(sep=" ", timespec="seconds") + + # --------------------------------------------------------------------------- # # config # --------------------------------------------------------------------------- # @@ -125,7 +134,17 @@ class Config: map_upload_mode: str map_endpoint_tmpl: str map_poll_interval: float + map_max_upload_mb: float state_dir: Path + # logs + alerts + alert_endpoint: str + logs_endpoint: str + logs_interval: float + # project logs (e.g. the robot's Sanad app) shipped alongside agent logs + project_log_container: str + project_log_path: str + project_log_label: str + project_log_backfill: int # transport verify_tls: bool http_timeout: float @@ -169,7 +188,15 @@ class Config: map_upload_mode=_env("MAP_UPLOAD_MODE", "multipart").lower(), map_endpoint_tmpl=_env("MAP_ENDPOINT", "/api/v1/fleet/ingest/{sn}/map"), map_poll_interval=float(_env("MAP_POLL_INTERVAL", "30")), + map_max_upload_mb=float(_env("MAP_MAX_UPLOAD_MB", "7")), state_dir=Path(_env("STATE_DIR", "/data/state")), + alert_endpoint=_env("ALERT_ENDPOINT", "/api/v1/fleet/ingest/{sn}/alert"), + logs_endpoint=_env("LOGS_ENDPOINT", "/api/v1/fleet/ingest/{sn}/logs"), + logs_interval=float(_env("LOGS_INTERVAL", "60")), + project_log_container=_env("PROJECT_LOG_CONTAINER", "auto"), + project_log_path=_env("PROJECT_LOG_PATH", ""), + project_log_label=_env("PROJECT_LOG_LABEL", ""), + project_log_backfill=int(_env("PROJECT_LOG_BACKFILL", "100")), verify_tls=_env_bool("VERIFY_TLS", True), http_timeout=float(_env("HTTP_TIMEOUT", "30")), ) @@ -180,6 +207,12 @@ class Config: def map_url(self) -> str: return self.server_url + self.map_endpoint_tmpl.format(sn=self.sn) + def alert_url(self) -> str: + return self.server_url + self.alert_endpoint.format(sn=self.sn) + + def logs_url(self) -> str: + return self.server_url + self.logs_endpoint.format(sn=self.sn) + def auth_headers(self) -> Dict[str, str]: return {"Authorization": f"Bearer {self.device_token}"} @@ -836,17 +869,27 @@ def map_sync_once(cfg: Config, session: requests.Session, return 0 state = load_state(cfg) - uploaded = failed = 0 + uploaded = failed = unstable = too_large = current = 0 last_err: Optional[str] = None now = time.time() for m in maps: prev = state.get(str(m.path.resolve())) if not force and prev == m.fingerprint(): + current += 1 continue # already uploaded this exact content — one-time rule # stability guard: a db modified in the last 120 s is still being # written (active mapping) — wait until it settles before uploading if not force and (now - m.mtime) < 120: log.info("map %s still changing (mapping in progress) — waiting to settle", m.name) + unstable += 1 + continue + # server rejects bodies over ~8 MB (client_max_body_size) — don't burn + # bandwidth on uploads that will 413. Raster (slam_toolbox) maps are tiny. + if m.fmt != "slam_toolbox" and (m.size / 1048576) > cfg.map_max_upload_mb: + log.warning("map %s is %.0f MB — exceeds server upload cap (~%.0f MB), skipping " + "(export a raster map or raise the server limit)", + m.name, m.size / 1048576, cfg.map_max_upload_mb) + too_large += 1 continue m.sha256 = (_sha256_set(list(m.files.values())) if m.fmt == "slam_toolbox" else _sha256(m.path)) @@ -866,8 +909,27 @@ def map_sync_once(cfg: Config, session: requests.Session, if failed: _set_map_status(state="failed", uploaded=False, maps_found=len(maps), last_map=maps[0].stem, error=last_err) + elif uploaded or current: + # at least one map is on the server (just now or previously); note skips + note = None + if too_large: + note = f"{too_large} map(s) skipped: exceed server upload cap (~{cfg.map_max_upload_mb:.0f} MB)" + elif unstable: + note = "newer map still being written (mapping in progress)" + _set_map_status(state="uploaded", uploaded=True, maps_found=len(maps), + last_map=maps[0].stem, error=note) + elif too_large: + _set_map_status(state="failed", uploaded=False, maps_found=len(maps), + last_map=maps[0].stem, + error=f"map exceeds server upload cap (~{cfg.map_max_upload_mb:.0f} MB) — " + "export a raster map or raise the server limit") + elif unstable: + # newest content is still being written (active mapping) — be honest + _set_map_status(state="pending", uploaded=False, maps_found=len(maps), + last_map=maps[0].stem, + error="map still being written (mapping in progress) — " + "will upload when it settles") else: - # every discovered map is on the server (just now or previously) _set_map_status(state="uploaded", uploaded=True, maps_found=len(maps), last_map=maps[0].stem, error=None) return uploaded @@ -882,10 +944,247 @@ def map_loop(cfg: Config, session: requests.Session) -> None: time.sleep(cfg.map_poll_interval) +# --------------------------------------------------------------------------- # +# logs + alerts (spec: POST /{sn}/logs periodically, POST /{sn}/alert on events) +# --------------------------------------------------------------------------- # +class _RingLogHandler(logging.Handler): + """Buffers the agent's own log lines so they can be shipped to the server.""" + + def __init__(self, maxlen: int = 400): + super().__init__(level=logging.INFO) + from collections import deque + self._buf: Any = deque(maxlen=maxlen) + self._blk = threading.Lock() + + def emit(self, record: logging.LogRecord) -> None: + try: + with self._blk: + self._buf.append(self.format(record)) + except Exception: + pass + + def drain(self) -> List[str]: + with self._blk: + lines = list(self._buf) + self._buf.clear() + return lines + + def requeue(self, lines: List[str]) -> None: + """Put unshipped lines back (front of the ring) so they retry next cycle + instead of being lost — bounded by maxlen, oldest evicted first.""" + with self._blk: + self._buf.extendleft(reversed(lines)) + + +_LOG_RING = _RingLogHandler() + +# shipped-status shown in every telemetry post ("logs" / "alerts" fields) +_LOGS_STAT: Dict[str, Any] = {"last_sent": None, "lines_sent": 0, "ok": None} +_ALERTS_STAT: Dict[str, Any] = {"sent": 0, "last": None, "last_time": None, "ok": None} + +# start times ("started_at" = this run, "last_start" = previous run) +_STARTED: Dict[str, Any] = {"now": None, "prev": None, "mono": time.monotonic()} + + +def _init_start_times(cfg: Config) -> None: + """Record this agent start; remember the previous one (persisted in STATE_DIR).""" + f = cfg.state_dir / "agent_state.json" + prev = (_read_json(f, {}) or {}).get("started_at") + now_s = _now_str() + try: + cfg.state_dir.mkdir(parents=True, exist_ok=True) + f.write_text(json.dumps({"started_at": now_s})) + except Exception as e: + log.debug("could not persist start time: %s", e) + _STARTED.update(now=now_s, prev=prev, mono=time.monotonic()) + + +class ProjectLogTail: + """Tails the robot's main PROJECT logs (e.g. the sanadr1 / sanad-p4 app) + and feeds them into the shipped log lines, labeled "[-logs] …". + + Sources, in priority order: + PROJECT_LOG_PATH explicit log file (or dir -> newest *.log) via /host + PROJECT_LOG_CONTAINER a docker container name; "auto" (default) scans the + host's docker metadata (/host/var/lib/docker) for a + RUNNING Sanad project (sanadr1, sanad-p4, sanad*) + Reads the container's json-log through the read-only /:/host mount — no + docker socket needed, read-only, cannot disturb the project.""" + + KNOWN = ("sanadr1", "sanad-p4", "sanadv3", "sanad") + + def __init__(self, cfg: Config): + self.label: Optional[str] = None + self._cur: Optional[Path] = None + self._pos = 0 + self._backfill = max(0, cfg.project_log_backfill) + self.active = False + try: + self._resolve(cfg) + except Exception as e: + log.debug("project-log resolve failed: %s", e) + if self.active: + log.info("project logs: sharing '%s' (%s)", self.label, self._cur) + else: + log.info("project logs: none found (project_logs=null)") + + def _resolve(self, cfg: Config) -> None: + # explicit file/dir + if cfg.project_log_path: + p = Path(cfg.project_log_path) + if p.is_dir(): + logs = sorted(p.glob("*.log"), key=lambda f: f.stat().st_mtime, reverse=True) + p = logs[0] if logs else None + if p and p.exists(): + self._start(p, cfg.project_log_label or f"{p.stem}-logs") + return + # docker container json-log via /host + base = Path("/host/var/lib/docker/containers") + if not base.exists(): + return + want = cfg.project_log_container + candidates: List[Any] = [] + for cf in base.glob("*/config.v2.json"): + try: + d = json.loads(cf.read_text()) + except Exception: + continue + name = (d.get("Name") or "").lstrip("/") + running = bool((d.get("State") or {}).get("Running")) + lp = d.get("LogPath") or "" + if not name or not lp: + continue + if want != "auto": + if name == want: + candidates.append((0, name, lp, running)) + elif running and "sanad" in name.lower() and not name.startswith("sanad-api"): + # rank known Sanad projects first + rank = self.KNOWN.index(name) if name in self.KNOWN else len(self.KNOWN) + candidates.append((rank, name, lp, running)) + if not candidates: + return + candidates.sort(key=lambda c: c[0]) + _, name, lp, _ = candidates[0] + p = Path("/host" + lp) if not lp.startswith("/host") else Path(lp) + if p.exists(): + self._start(p, cfg.project_log_label or f"{name}-logs") + + def _start(self, p: Path, label: str) -> None: + self._cur = p + size = p.stat().st_size + self._pos = size # default: only NEW lines ship + # backfill: start N lines before EOF so recent history ships at startup + if self._backfill and size: + try: + take = min(size, 512 * 1024) + with p.open("rb") as f: + f.seek(size - take) + tail = f.read(take) + parts = tail.splitlines(keepends=True)[-self._backfill:] + self._pos = size - sum(len(x) for x in parts) + except Exception: + self._pos = size + self.label = label + self.active = True + + def poll(self) -> List[str]: + """New lines since last poll (docker json-log unwrapped), labeled.""" + if not self.active or self._cur is None: + return [] + out: List[str] = [] + try: + st = self._cur.stat() + if st.st_size < self._pos: # log rotated + self._pos = 0 + if st.st_size > self._pos: + with self._cur.open("rb") as f: + f.seek(self._pos) + chunk = f.read(min(st.st_size - self._pos, 256 * 1024)) + self._pos = f.tell() + for ln in chunk.decode("utf-8", "replace").splitlines(): + ln = ln.strip() + if ln.startswith("{"): + try: + ln = (json.loads(ln).get("log") or "").rstrip() + except Exception: + pass + if ln: + out.append(f"[{self.label}] {ln}") + except Exception as e: + log.debug("project-log poll failed: %s", e) + return out[-100:] # cap per cycle + + +_PROJECT_TAIL: Optional[ProjectLogTail] = None + + +def ship_logs(cfg: Config, session: requests.Session) -> None: + """POST buffered agent log lines (+ project logs) to /{sn}/logs. + Best-effort: failures are logged at DEBUG only (below the ring's level -> + no feedback loop).""" + lines = _LOG_RING.drain() + if _PROJECT_TAIL is not None and _PROJECT_TAIL.active: + lines.extend(_PROJECT_TAIL.poll()) + if not lines: + return + try: + r = session.post(cfg.logs_url(), + json={"sn": cfg.sn, "name": cfg.name, + "lines": lines, "ts": int(time.time())}, + headers=cfg.auth_headers(), + timeout=cfg.http_timeout, verify=cfg.verify_tls) + if r.ok: + _LOGS_STAT.update(last_sent=_now_str(), ok=True) + _LOGS_STAT["lines_sent"] += len(lines) + log.debug("logs shipped: %d lines -> HTTP %s", len(lines), r.status_code) + else: + _LOGS_STAT["ok"] = False + _LOG_RING.requeue(lines) # retry next cycle (server keeps 500ing) + log.debug("logs ship failed: HTTP %s (%d lines requeued)", r.status_code, len(lines)) + except requests.RequestException as e: + _LOGS_STAT["ok"] = False + _LOG_RING.requeue(lines) + log.debug("logs ship failed (%d lines requeued): %s", len(lines), e) + + +def logs_loop(cfg: Config, session: requests.Session) -> None: + while True: + time.sleep(cfg.logs_interval) + try: + ship_logs(cfg, session) + except Exception: + pass + + +_ALERT_SEEN: set = set() + + +def send_alerts(cfg: Config, session: requests.Session, faults: List[str]) -> None: + """POST each NEW fault (rising edge) to /{sn}/alert. Faults are strings.""" + global _ALERT_SEEN + current = set(faults) + new = current - _ALERT_SEEN + _ALERT_SEEN = current + for f in sorted(new): + try: + r = session.post(cfg.alert_url(), + json={"sn": cfg.sn, "name": cfg.name, + "alert": f, "message": f, "ts": int(time.time())}, + headers=cfg.auth_headers(), + timeout=cfg.http_timeout, verify=cfg.verify_tls) + _ALERTS_STAT.update(last=f, last_time=_now_str(), ok=bool(r.ok)) + if r.ok: + _ALERTS_STAT["sent"] += 1 + log.info("alert sent: %s -> HTTP %s", f, r.status_code) + except requests.RequestException as e: + _ALERTS_STAT.update(last=f, last_time=_now_str(), ok=False) + log.debug("alert send failed (%s): %s", f, e) + + # --------------------------------------------------------------------------- # # telemetry assembly # --------------------------------------------------------------------------- # -def derive_faults(cfg: Config, snap: Dict[str, Any]) -> List[Dict[str, Any]]: +def derive_faults(cfg: Config, snap: Dict[str, Any]) -> List[str]: # STRINGS, not objects — the fleet ingest 500s on fault objects. faults: List[str] = [] bms = snap.get("bms") @@ -962,6 +1261,15 @@ def build_telemetry(cfg: Config, mac: str, reader: Optional[DDSReader], "position": position, # null when no odom/localization source "faults": faults, "map": get_map_status(), # SHOWS whether the saved map made it to the server + "logs": dict(_LOGS_STAT), # log-shipping status (last_sent, lines_sent, ok) + "project_logs": (_PROJECT_TAIL.label + if _PROJECT_TAIL is not None and _PROJECT_TAIL.active + else None), # e.g. "sanadr1-logs"; null = no project found + "alerts": dict(_ALERTS_STAT), # alert status (sent, last, last_time, ok) + "time": _now_str(), # full date+time of this post + "started_at": _STARTED["now"], # when this agent run started + "last_start": _STARTED["prev"], # previous agent start (null on first ever) + "uptime_s": int(time.monotonic() - _STARTED["mono"]), "ts": int(time.time()), } @@ -1019,6 +1327,9 @@ def main(argv: Optional[List[str]] = None) -> int: logging.basicConfig(level=logging.DEBUG if args.verbose else logging.INFO, format="%(asctime)s %(levelname)s %(name)s: %(message)s") + # buffer our own log lines for shipping to /{sn}/logs + _LOG_RING.setFormatter(logging.Formatter("%(asctime)s %(levelname)s %(name)s: %(message)s")) + logging.getLogger().addHandler(_LOG_RING) _load_dotenv() cfg = Config.from_env() if args.interval is not None: @@ -1028,6 +1339,9 @@ def main(argv: Optional[List[str]] = None) -> int: cmd_list(cfg) return 0 + _init_start_times(cfg) + global _PROJECT_TAIL + _PROJECT_TAIL = ProjectLogTail(cfg) mac = read_mac(cfg.mac_interface) log.info("sanad_api_g1 — sn=%s name=%s mac=%s server=%s iface=%s pos=%s " "map_dir=%s map_every=%.0fs%s", @@ -1054,6 +1368,7 @@ def main(argv: Optional[List[str]] = None) -> int: log.info("[dry-run] %s", json.dumps(payload)) else: post_telemetry(cfg, payload, session) + send_alerts(cfg, session, payload.get("faults") or []) tick += 1 if args.once or args.dry_run: @@ -1061,16 +1376,18 @@ def main(argv: Optional[List[str]] = None) -> int: map_sync_once(cfg, session, force=args.force, dry_run=args.dry_run) if args.once: one_telemetry() + ship_logs(cfg, session) return 0 for _ in range(3): one_telemetry() time.sleep(min(cfg.poll_interval, 1.0)) return 0 - # loop mode: map sync in a background thread, telemetry in the main loop + # loop mode: map sync + log shipping in background threads threading.Thread(target=map_loop, args=(cfg, session), daemon=True).start() - log.info("telemetry every %.1fs; map check every %.0fs (Ctrl-C to stop)", - cfg.poll_interval, cfg.map_poll_interval) + threading.Thread(target=logs_loop, args=(cfg, session), daemon=True).start() + log.info("telemetry every %.1fs; map check every %.0fs; logs every %.0fs (Ctrl-C to stop)", + cfg.poll_interval, cfg.map_poll_interval, cfg.logs_interval) while True: try: one_telemetry() diff --git a/agents/go2/.env.example b/agents/go2/.env.example index d958996..bb2613c 100644 --- a/agents/go2/.env.example +++ b/agents/go2/.env.example @@ -35,3 +35,27 @@ ROBOT_MODEL=go2 # Optional: Sanad data dir whose size is reported in storage (as /host/ # when the installer's read-only /:/host mount is used). Empty = omit. STORAGE_DATA_PATH= + +# ── map sync (uploaded ONCE per content; status shown in telemetry "map") ──── +# The installer mounts the robot's real maps dir at /data/maps (probed on the +# robot: rtabmap*.db / slam_toolbox yaml+pgm sets). Fallback = agent's own dir. +MAPS_DIR=/data/maps +DATA_DIR=/data/web_data +STATE_DIR=/data/state +MAP_SELECT=all +MAP_UPLOAD_MODE=multipart +MAP_POLL_INTERVAL=30 + +# ── logs + alerts ───────────────────────────────────────────────────────────── +# agent log lines shipped to /{sn}/logs every LOGS_INTERVAL; new faults POSTed +# to /{sn}/alert immediately (rising edge). +LOGS_INTERVAL=60 + +# ── project logs (shared alongside agent logs) ─────────────────────────────── +# auto (default) = find a RUNNING Sanad project container (sanadr1, sanad-p4, +# sanad*) via the /host mount and tail its docker json-log, labeled +# "-logs" in the shipped lines; telemetry shows project_logs (null=none). +# Or pin a container name / an explicit log file (PROJECT_LOG_PATH, via /host). +PROJECT_LOG_CONTAINER=auto +PROJECT_LOG_PATH= +PROJECT_LOG_LABEL= diff --git a/agents/go2/sanad_api_go2.py b/agents/go2/sanad_api_go2.py index 5542c78..f4d73f8 100644 --- a/agents/go2/sanad_api_go2.py +++ b/agents/go2/sanad_api_go2.py @@ -37,15 +37,19 @@ CLI: --simulate | --once | --dry-run | --interval N | -v from __future__ import annotations import argparse +import base64 +import datetime as _dt +import hashlib import json import logging +import math import os import shutil import sys import threading import time import uuid -from dataclasses import dataclass +from dataclasses import dataclass, field from pathlib import Path from typing import Any, Dict, List, Optional @@ -74,6 +78,15 @@ def _env_bool(name: str, default: bool) -> bool: return _env(name, "1" if default else "0").lower() in ("1", "true", "yes", "on") +def _now_str() -> str: + """Full local date+time with UTC offset (e.g. 2026-07-13 14:20:33+04:00). + TZ_OFFSET_HOURS (default +4, Dubai) keeps container clocks honest without tzdata.""" + off = float(_env("TZ_OFFSET_HOURS", "4")) + tz = _dt.timezone(_dt.timedelta(hours=off)) + return _dt.datetime.now(tz).isoformat(sep=" ", timespec="seconds") + + + @dataclass class Config: server_url: str @@ -94,6 +107,27 @@ class Config: motor_temp_max: float poll_interval: float endpoint: str + # map sync + robot: str + maps_dir: Path + web_data_dir: Optional[Path] + legacy_places: Optional[Path] + web_nav3_url: str + map_select: str + map_upload_mode: str + map_endpoint_tmpl: str + map_poll_interval: float + map_max_upload_mb: float + state_dir: Path + # logs + alerts + alert_endpoint: str + logs_endpoint: str + logs_interval: float + # project logs (e.g. the robot's Sanad app) shipped alongside agent logs + project_log_container: str + project_log_path: str + project_log_label: str + project_log_backfill: int verify_tls: bool http_timeout: float @@ -105,6 +139,8 @@ class Config: if missing: raise SystemExit(f"[config] missing required env: {', '.join(missing)}") iface = _env("DDS_INTERFACE", "eth0") + data_dir = _env("DATA_DIR") + legacy = _env("LEGACY_PLACES") return cls( server_url=server, device_token=token, sn=_env("SN", "go2_0000"), @@ -122,6 +158,24 @@ class Config: motor_temp_max=float(_env("MOTOR_TEMP_MAX", "85")), poll_interval=float(_env("POLL_INTERVAL", "2")), endpoint=_env("TELEMETRY_ENDPOINT", "/api/v1/fleet/ingest/telemetry"), + robot=_env("ROBOT", "sanad"), + maps_dir=Path(_env("MAPS_DIR", "/data/maps")), + web_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(), + map_upload_mode=_env("MAP_UPLOAD_MODE", "multipart").lower(), + map_endpoint_tmpl=_env("MAP_ENDPOINT", "/api/v1/fleet/ingest/{sn}/map"), + map_poll_interval=float(_env("MAP_POLL_INTERVAL", "30")), + map_max_upload_mb=float(_env("MAP_MAX_UPLOAD_MB", "7")), + state_dir=Path(_env("STATE_DIR", "/data/state")), + alert_endpoint=_env("ALERT_ENDPOINT", "/api/v1/fleet/ingest/{sn}/alert"), + logs_endpoint=_env("LOGS_ENDPOINT", "/api/v1/fleet/ingest/{sn}/logs"), + logs_interval=float(_env("LOGS_INTERVAL", "60")), + project_log_container=_env("PROJECT_LOG_CONTAINER", "auto"), + project_log_path=_env("PROJECT_LOG_PATH", ""), + project_log_label=_env("PROJECT_LOG_LABEL", ""), + project_log_backfill=int(_env("PROJECT_LOG_BACKFILL", "100")), verify_tls=_env_bool("VERIFY_TLS", True), http_timeout=float(_env("HTTP_TIMEOUT", "10")), ) @@ -129,6 +183,15 @@ class Config: def telemetry_url(self) -> str: return self.server_url + self.endpoint + def map_url(self) -> str: + return self.server_url + self.map_endpoint_tmpl.format(sn=self.sn) + + def alert_url(self) -> str: + return self.server_url + self.alert_endpoint.format(sn=self.sn) + + def logs_url(self) -> str: + return self.server_url + self.logs_endpoint.format(sn=self.sn) + def auth_headers(self) -> Dict[str, str]: return {"Authorization": f"Bearer {self.device_token}"} @@ -345,6 +408,705 @@ class RosbridgePosition: return dict(self._xy) if self._xy else None +# --------------------------------------------------------------------------- # +# map sync (web_nav3 saved maps → fleet server, uploaded ONCE per content) +# --------------------------------------------------------------------------- # +@dataclass +class MapArtifact: + path: Path # .db (rtabmap) or .yaml (slam_toolbox set) + name: str + stem: str + size: int + mtime: int + fmt: str = "rtabmap_db" # rtabmap_db | slam_toolbox + files: Dict[str, Path] = field(default_factory=dict) # slam set: pgm/yaml/posegraph/data + description: str = "" + sha256: str = "" + points: List[Dict[str, Any]] = field(default_factory=list) + + def fingerprint(self) -> str: + return f"{self.fmt}:{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() + + +def _sha256_set(paths: List[Path]) -> str: + h = hashlib.sha256() + for p in sorted(paths): + with p.open("rb") as f: + for chunk in iter(lambda: f.read(1024 * 1024), b""): + h.update(chunk) + return h.hexdigest() + + +# ---- slam_toolbox map set (office.yaml + office.pgm [+ .posegraph .data]) ---- +def _parse_map_yaml(path: Path) -> Dict[str, Any]: + """Tiny parser for a ROS map_server yaml (image/resolution/origin) — no pyyaml.""" + out: Dict[str, Any] = {} + for line in path.read_text().splitlines(): + line = line.split("#", 1)[0].strip() + if ":" not in line: + continue + k, _, v = line.partition(":") + k, v = k.strip(), v.strip() + if k == "image": + out["image"] = v + elif k == "resolution": + try: + out["resolution"] = float(v) + except ValueError: + pass + elif k == "origin": + try: + nums = [float(x) for x in v.strip("[]").split(",")] + out["origin"] = {"x": nums[0], "y": nums[1], + "yaw": nums[2] if len(nums) > 2 else 0.0} + except Exception: + pass + return out + + +def _read_pgm(path: Path) -> Optional[Dict[str, Any]]: + """Parse a binary PGM (P5): returns {width, height, maxval, pixels(bytes)}.""" + try: + data = path.read_bytes() + if not data.startswith(b"P5"): + return None + # tokenize header (magic, width, height, maxval), skipping comments + tokens: List[bytes] = [] + i = 2 + while len(tokens) < 3 and i < len(data): + c = data[i:i + 1] + if c in b" \t\r\n": + i += 1 + elif c == b"#": + i = data.index(b"\n", i) + 1 + else: + j = i + while j < len(data) and data[j:j + 1] not in b" \t\r\n": + j += 1 + tokens.append(data[i:j]) + i = j + w, h, maxval = int(tokens[0]), int(tokens[1]), int(tokens[2]) + pixels = data[i + 1: i + 1 + w * h] + if len(pixels) < w * h: + return None + return {"width": w, "height": h, "maxval": maxval, "pixels": pixels} + except Exception: + return None + + +def _pgm_to_png_b64(pgm: Dict[str, Any]) -> str: + """Grayscale 8-bit PNG from parsed PGM — pure stdlib (zlib + struct).""" + import struct + import zlib + + w, h, pixels = pgm["width"], pgm["height"], pgm["pixels"] + + def chunk(tag: bytes, body: bytes) -> bytes: + return (struct.pack(">I", len(body)) + tag + body + + struct.pack(">I", zlib.crc32(tag + body) & 0xFFFFFFFF)) + + ihdr = struct.pack(">IIBBBBB", w, h, 8, 0, 0, 0, 0) # 8-bit grayscale + raw = b"".join(b"\x00" + pixels[y * w:(y + 1) * w] for y in range(h)) + png = (b"\x89PNG\r\n\x1a\n" + chunk(b"IHDR", ihdr) + + chunk(b"IDAT", zlib.compress(raw, 6)) + chunk(b"IEND", b"")) + return base64.b64encode(png).decode("ascii") + + +def _discover_slam_sets(cfg: Config) -> List[MapArtifact]: + """Find slam_toolbox / map_server map sets: .yaml + .pgm + (+ optional .posegraph/.data) under the maps roots and maps_slam/.""" + roots = [cfg.maps_dir, cfg.maps_dir / cfg.robot, cfg.maps_dir / "maps_slam"] + seen: set = set() + out: List[MapArtifact] = [] + for root in roots: + if not root.exists(): + continue + for y in sorted(root.glob("*.yaml")): + meta = _parse_map_yaml(y) + img = meta.get("image", "") + pgm = (y.parent / img) if img else y.with_suffix(".pgm") + if not pgm.exists(): + pgm = y.with_suffix(".pgm") + if not pgm.exists(): + continue # yaml without a raster — not a map set + rp = str(y.resolve()) + if rp in seen: + continue + seen.add(rp) + files: Dict[str, Path] = {"yaml": y, "pgm": pgm} + for ext in ("posegraph", "data"): + p = y.with_suffix("." + ext) + if p.exists(): + files[ext] = p + size = sum(p.stat().st_size for p in files.values()) + mtime = max(int(p.stat().st_mtime) for p in files.values()) + out.append(MapArtifact(path=y, name=y.name, stem=y.stem, + size=size, mtime=mtime, + fmt="slam_toolbox", files=files)) + return out + + +def _map_key(stem: str) -> str: + 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: + 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]: + out: List[Path] = [] + key = _map_key(stem) + if cfg.web_data_dir: + out.append(cfg.web_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]]: + 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 + return pts + return [] + + +def discover_maps(cfg: Config) -> List[MapArtifact]: + 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 = 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", ""), + )) + # slam_toolbox map sets (office.yaml + office.pgm …) live alongside + out.extend(_discover_slam_sets(cfg)) + out.sort(key=lambda m: m.mtime, reverse=True) + return out + + +def _active_map_name(cfg: Config) -> Optional[str]: + 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: + 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 + return maps[:1] + return maps # "all" + + +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 map state: %s", e) + + +def build_meta(cfg: Config, m: MapArtifact) -> Dict[str, Any]: + return { + "sn": cfg.sn, + "name": m.stem, + "file": m.name, + "format": m.fmt, + "size_bytes": m.size, + "sha256": m.sha256, + "mtime": m.mtime, + "description": m.description, + "points": m.points, + } + + +def _upload_slam_map(cfg: Config, m: MapArtifact, session: requests.Session) -> bool: + """slam_toolbox map → the spec's image JSON: PNG (from the pgm) + resolution + + origin + width/height + points. This is what the dashboard renders.""" + url = cfg.map_url() + ymeta = _parse_map_yaml(m.files["yaml"]) + pgm = _read_pgm(m.files["pgm"]) + if pgm is None: + log.error("map %s: cannot parse %s (not binary P5?)", m.stem, m.files["pgm"].name) + return False + body = build_meta(cfg, m) + body.update({ + "resolution": ymeta.get("resolution"), + "origin": ymeta.get("origin"), + "width": pgm["width"], + "height": pgm["height"], + "image_base64": _pgm_to_png_b64(pgm), + }) + try: + resp = session.post(url, json=body, headers=cfg.auth_headers(), + timeout=cfg.http_timeout, verify=cfg.verify_tls) + except requests.RequestException as e: + log.error("map upload %s FAILED (transport): %s", m.stem, e) + return False + if not resp.ok: + log.error("map upload %s FAILED: HTTP %s %s", m.stem, resp.status_code, resp.text[:300]) + return False + log.info("map uploaded: %s (slam_toolbox %dx%d @ %sm, %d points) -> HTTP %s", + m.stem, pgm["width"], pgm["height"], ymeta.get("resolution"), + len(m.points), resp.status_code) + return True + + +def upload_map(cfg: Config, m: MapArtifact, session: requests.Session) -> bool: + if m.fmt == "slam_toolbox": + return _upload_slam_map(cfg, m, session) + url = cfg.map_url() + meta = build_meta(cfg, m) + try: + if cfg.map_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("map upload %s FAILED (transport): %s", m.name, e) + return False + if not resp.ok: + log.error("map upload %s FAILED: HTTP %s %s", m.name, resp.status_code, resp.text[:300]) + return False + log.info("map uploaded: %s (%.2f MB, %d points) -> HTTP %s", + m.name, m.size / 1024 / 1024, len(m.points), resp.status_code) + return True + + +# Shared map status — SHOWN in every telemetry post ("map" field). +_MAP_STATUS_LOCK = threading.Lock() +_MAP_STATUS: Dict[str, Any] = { + "uploaded": False, "state": "pending", "maps_found": 0, + "last_map": None, "error": None, "checked_ts": None, +} + + +def _set_map_status(**kw: Any) -> None: + with _MAP_STATUS_LOCK: + _MAP_STATUS.update(kw) + _MAP_STATUS["checked_ts"] = int(time.time()) + + +def get_map_status() -> Dict[str, Any]: + with _MAP_STATUS_LOCK: + return dict(_MAP_STATUS) + + +def map_sync_once(cfg: Config, session: requests.Session, + force: bool = False, dry_run: bool = False) -> int: + """One map pass: scan the Sanad dashboard maps and upload anything new. + Always updates the shared map status (visible in telemetry).""" + try: + maps = select_maps(cfg, discover_maps(cfg)) + except Exception as e: + _set_map_status(state="failed", uploaded=False, error=f"map scan failed: {e}") + return 0 + if not maps: + _set_map_status(state="no_map", uploaded=False, maps_found=0, last_map=None, + error=f"no saved map found in Sanad dashboard " + f"(maps_dir={cfg.maps_dir}, robot={cfg.robot})") + return 0 + + state = load_state(cfg) + uploaded = failed = unstable = too_large = current = 0 + last_err: Optional[str] = None + now = time.time() + for m in maps: + prev = state.get(str(m.path.resolve())) + if not force and prev == m.fingerprint(): + current += 1 + continue # already uploaded this exact content — one-time rule + # stability guard: a db modified in the last 120 s is still being + # written (active mapping) — wait until it settles before uploading + if not force and (now - m.mtime) < 120: + log.info("map %s still changing (mapping in progress) — waiting to settle", m.name) + unstable += 1 + continue + # server rejects bodies over ~8 MB (client_max_body_size) — don't burn + # bandwidth on uploads that will 413. Raster (slam_toolbox) maps are tiny. + if m.fmt != "slam_toolbox" and (m.size / 1048576) > cfg.map_max_upload_mb: + log.warning("map %s is %.0f MB — exceeds server upload cap (~%.0f MB), skipping " + "(export a raster map or raise the server limit)", + m.name, m.size / 1048576, cfg.map_max_upload_mb) + too_large += 1 + continue + m.sha256 = (_sha256_set(list(m.files.values())) + if m.fmt == "slam_toolbox" else _sha256(m.path)) + m.points = load_points(cfg, m.stem) + if dry_run: + log.info("[dry-run] would upload map %s (%.2f MB, %d points)", + m.name, m.size / 1024 / 1024, len(m.points)) + continue + if upload_map(cfg, m, session): + state[str(m.path.resolve())] = m.fingerprint() + save_state(cfg, state) + uploaded += 1 + else: + failed += 1 + last_err = f"upload failed for {m.name} (see agent log)" + + if failed: + _set_map_status(state="failed", uploaded=False, maps_found=len(maps), + last_map=maps[0].stem, error=last_err) + elif uploaded or current: + # at least one map is on the server (just now or previously); note skips + note = None + if too_large: + note = f"{too_large} map(s) skipped: exceed server upload cap (~{cfg.map_max_upload_mb:.0f} MB)" + elif unstable: + note = "newer map still being written (mapping in progress)" + _set_map_status(state="uploaded", uploaded=True, maps_found=len(maps), + last_map=maps[0].stem, error=note) + elif too_large: + _set_map_status(state="failed", uploaded=False, maps_found=len(maps), + last_map=maps[0].stem, + error=f"map exceeds server upload cap (~{cfg.map_max_upload_mb:.0f} MB) — " + "export a raster map or raise the server limit") + elif unstable: + # newest content is still being written (active mapping) — be honest + _set_map_status(state="pending", uploaded=False, maps_found=len(maps), + last_map=maps[0].stem, + error="map still being written (mapping in progress) — " + "will upload when it settles") + else: + _set_map_status(state="uploaded", uploaded=True, maps_found=len(maps), + last_map=maps[0].stem, error=None) + return uploaded + + +def map_loop(cfg: Config, session: requests.Session) -> None: + while True: + try: + map_sync_once(cfg, session) + except Exception as e: + log.exception("map pass failed: %s", e) + time.sleep(cfg.map_poll_interval) + + +# --------------------------------------------------------------------------- # +# logs + alerts (spec: POST /{sn}/logs periodically, POST /{sn}/alert on events) +# --------------------------------------------------------------------------- # +class _RingLogHandler(logging.Handler): + """Buffers the agent's own log lines so they can be shipped to the server.""" + + def __init__(self, maxlen: int = 400): + super().__init__(level=logging.INFO) + from collections import deque + self._buf: Any = deque(maxlen=maxlen) + self._blk = threading.Lock() + + def emit(self, record: logging.LogRecord) -> None: + try: + with self._blk: + self._buf.append(self.format(record)) + except Exception: + pass + + def drain(self) -> List[str]: + with self._blk: + lines = list(self._buf) + self._buf.clear() + return lines + + def requeue(self, lines: List[str]) -> None: + """Put unshipped lines back (front of the ring) so they retry next cycle + instead of being lost — bounded by maxlen, oldest evicted first.""" + with self._blk: + self._buf.extendleft(reversed(lines)) + + +_LOG_RING = _RingLogHandler() + +# shipped-status shown in every telemetry post ("logs" / "alerts" fields) +_LOGS_STAT: Dict[str, Any] = {"last_sent": None, "lines_sent": 0, "ok": None} +_ALERTS_STAT: Dict[str, Any] = {"sent": 0, "last": None, "last_time": None, "ok": None} + +# start times ("started_at" = this run, "last_start" = previous run) +_STARTED: Dict[str, Any] = {"now": None, "prev": None, "mono": time.monotonic()} + + +def _init_start_times(cfg: Config) -> None: + """Record this agent start; remember the previous one (persisted in STATE_DIR).""" + f = cfg.state_dir / "agent_state.json" + prev = (_read_json(f, {}) or {}).get("started_at") + now_s = _now_str() + try: + cfg.state_dir.mkdir(parents=True, exist_ok=True) + f.write_text(json.dumps({"started_at": now_s})) + except Exception as e: + log.debug("could not persist start time: %s", e) + _STARTED.update(now=now_s, prev=prev, mono=time.monotonic()) + + +class ProjectLogTail: + """Tails the robot's main PROJECT logs (e.g. the sanadr1 / sanad-p4 app) + and feeds them into the shipped log lines, labeled "[-logs] …". + + Sources, in priority order: + PROJECT_LOG_PATH explicit log file (or dir -> newest *.log) via /host + PROJECT_LOG_CONTAINER a docker container name; "auto" (default) scans the + host's docker metadata (/host/var/lib/docker) for a + RUNNING Sanad project (sanadr1, sanad-p4, sanad*) + Reads the container's json-log through the read-only /:/host mount — no + docker socket needed, read-only, cannot disturb the project.""" + + KNOWN = ("sanadr1", "sanad-p4", "sanadv3", "sanad") + + def __init__(self, cfg: Config): + self.label: Optional[str] = None + self._cur: Optional[Path] = None + self._pos = 0 + self._backfill = max(0, cfg.project_log_backfill) + self.active = False + try: + self._resolve(cfg) + except Exception as e: + log.debug("project-log resolve failed: %s", e) + if self.active: + log.info("project logs: sharing '%s' (%s)", self.label, self._cur) + else: + log.info("project logs: none found (project_logs=null)") + + def _resolve(self, cfg: Config) -> None: + # explicit file/dir + if cfg.project_log_path: + p = Path(cfg.project_log_path) + if p.is_dir(): + logs = sorted(p.glob("*.log"), key=lambda f: f.stat().st_mtime, reverse=True) + p = logs[0] if logs else None + if p and p.exists(): + self._start(p, cfg.project_log_label or f"{p.stem}-logs") + return + # docker container json-log via /host + base = Path("/host/var/lib/docker/containers") + if not base.exists(): + return + want = cfg.project_log_container + candidates: List[Any] = [] + for cf in base.glob("*/config.v2.json"): + try: + d = json.loads(cf.read_text()) + except Exception: + continue + name = (d.get("Name") or "").lstrip("/") + running = bool((d.get("State") or {}).get("Running")) + lp = d.get("LogPath") or "" + if not name or not lp: + continue + if want != "auto": + if name == want: + candidates.append((0, name, lp, running)) + elif running and "sanad" in name.lower() and not name.startswith("sanad-api"): + # rank known Sanad projects first + rank = self.KNOWN.index(name) if name in self.KNOWN else len(self.KNOWN) + candidates.append((rank, name, lp, running)) + if not candidates: + return + candidates.sort(key=lambda c: c[0]) + _, name, lp, _ = candidates[0] + p = Path("/host" + lp) if not lp.startswith("/host") else Path(lp) + if p.exists(): + self._start(p, cfg.project_log_label or f"{name}-logs") + + def _start(self, p: Path, label: str) -> None: + self._cur = p + size = p.stat().st_size + self._pos = size # default: only NEW lines ship + # backfill: start N lines before EOF so recent history ships at startup + if self._backfill and size: + try: + take = min(size, 512 * 1024) + with p.open("rb") as f: + f.seek(size - take) + tail = f.read(take) + parts = tail.splitlines(keepends=True)[-self._backfill:] + self._pos = size - sum(len(x) for x in parts) + except Exception: + self._pos = size + self.label = label + self.active = True + + def poll(self) -> List[str]: + """New lines since last poll (docker json-log unwrapped), labeled.""" + if not self.active or self._cur is None: + return [] + out: List[str] = [] + try: + st = self._cur.stat() + if st.st_size < self._pos: # log rotated + self._pos = 0 + if st.st_size > self._pos: + with self._cur.open("rb") as f: + f.seek(self._pos) + chunk = f.read(min(st.st_size - self._pos, 256 * 1024)) + self._pos = f.tell() + for ln in chunk.decode("utf-8", "replace").splitlines(): + ln = ln.strip() + if ln.startswith("{"): + try: + ln = (json.loads(ln).get("log") or "").rstrip() + except Exception: + pass + if ln: + out.append(f"[{self.label}] {ln}") + except Exception as e: + log.debug("project-log poll failed: %s", e) + return out[-100:] # cap per cycle + + +_PROJECT_TAIL: Optional[ProjectLogTail] = None + + +def ship_logs(cfg: Config, session: requests.Session) -> None: + """POST buffered agent log lines (+ project logs) to /{sn}/logs. + Best-effort: failures are logged at DEBUG only (below the ring's level -> + no feedback loop).""" + lines = _LOG_RING.drain() + if _PROJECT_TAIL is not None and _PROJECT_TAIL.active: + lines.extend(_PROJECT_TAIL.poll()) + if not lines: + return + try: + r = session.post(cfg.logs_url(), + json={"sn": cfg.sn, "name": cfg.name, + "lines": lines, "ts": int(time.time())}, + headers=cfg.auth_headers(), + timeout=cfg.http_timeout, verify=cfg.verify_tls) + if r.ok: + _LOGS_STAT.update(last_sent=_now_str(), ok=True) + _LOGS_STAT["lines_sent"] += len(lines) + log.debug("logs shipped: %d lines -> HTTP %s", len(lines), r.status_code) + else: + _LOGS_STAT["ok"] = False + _LOG_RING.requeue(lines) # retry next cycle (server keeps 500ing) + log.debug("logs ship failed: HTTP %s (%d lines requeued)", r.status_code, len(lines)) + except requests.RequestException as e: + _LOGS_STAT["ok"] = False + _LOG_RING.requeue(lines) + log.debug("logs ship failed (%d lines requeued): %s", len(lines), e) + + +def logs_loop(cfg: Config, session: requests.Session) -> None: + while True: + time.sleep(cfg.logs_interval) + try: + ship_logs(cfg, session) + except Exception: + pass + + +_ALERT_SEEN: set = set() + + +def send_alerts(cfg: Config, session: requests.Session, faults: List[str]) -> None: + """POST each NEW fault (rising edge) to /{sn}/alert. Faults are strings.""" + global _ALERT_SEEN + current = set(faults) + new = current - _ALERT_SEEN + _ALERT_SEEN = current + for f in sorted(new): + try: + r = session.post(cfg.alert_url(), + json={"sn": cfg.sn, "name": cfg.name, + "alert": f, "message": f, "ts": int(time.time())}, + headers=cfg.auth_headers(), + timeout=cfg.http_timeout, verify=cfg.verify_tls) + _ALERTS_STAT.update(last=f, last_time=_now_str(), ok=bool(r.ok)) + if r.ok: + _ALERTS_STAT["sent"] += 1 + log.info("alert sent: %s -> HTTP %s", f, r.status_code) + except requests.RequestException as e: + _ALERTS_STAT.update(last=f, last_time=_now_str(), ok=False) + log.debug("alert send failed (%s): %s", f, e) + + +# --------------------------------------------------------------------------- # +# telemetry assembly +# --------------------------------------------------------------------------- # def derive_faults(cfg: Config, snap: Dict[str, Any]) -> List[Dict[str, Any]]: # STRINGS, not objects — the fleet ingest 500s on fault objects. faults: List[str] = [] @@ -417,7 +1179,18 @@ def build_telemetry(cfg: Config, mac: str, reader: Optional[DDSReader], "motor_temp": motor_temp, # null = not receiving "storage": read_storage(cfg), "status": status, - "position": position, "faults": faults, "ts": int(time.time()), + "position": position, "faults": faults, + "map": get_map_status(), # SHOWS whether the saved map made it to the server + "logs": dict(_LOGS_STAT), # log-shipping status (last_sent, lines_sent, ok) + "project_logs": (_PROJECT_TAIL.label + if _PROJECT_TAIL is not None and _PROJECT_TAIL.active + else None), # e.g. "sanadr1-logs"; null = no project found + "alerts": dict(_ALERTS_STAT), # alert status (sent, last, last_time, ok) + "time": _now_str(), # full date+time of this post + "started_at": _STARTED["now"], # when this agent run started + "last_start": _STARTED["prev"], # previous agent start (null on first ever) + "uptime_s": int(time.monotonic() - _STARTED["mono"]), + "ts": int(time.time()), } @@ -446,22 +1219,45 @@ def _sim_state(i: int) -> Dict[str, Any]: "position": {"x": round(1.0 + 0.1 * i, 2), "y": round(2.0 - 0.05 * i, 2)}} +def cmd_list(cfg: Config) -> None: + maps = discover_maps(cfg) + if not maps: + print(f"(no 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) + print(f" {m.name:<28} {m.size/1024/1024:6.2f} MB {len(pts):>3} points {m.description}") + + def main(argv: Optional[List[str]] = None) -> int: ap = argparse.ArgumentParser(description="Go2 fleet telemetry agent") ap.add_argument("--simulate", action="store_true") ap.add_argument("--once", action="store_true") ap.add_argument("--dry-run", action="store_true") + ap.add_argument("--force", action="store_true", help="re-upload maps even if unchanged") + ap.add_argument("--list", action="store_true", help="list discovered maps and exit") ap.add_argument("--interval", type=float, default=None) 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") + _LOG_RING.setFormatter(logging.Formatter("%(asctime)s %(levelname)s %(name)s: %(message)s")) + logging.getLogger().addHandler(_LOG_RING) _load_dotenv() cfg = Config.from_env() if args.interval is not None: cfg.poll_interval = args.interval + if args.list: + cmd_list(cfg) + return 0 + + _init_start_times(cfg) + global _PROJECT_TAIL + _PROJECT_TAIL = ProjectLogTail(cfg) + mac = read_mac(cfg.mac_interface) log.info("sanad_api_go2 telemetry — sn=%s mac=%s server=%s iface=%s domain=%d%s", cfg.sn, mac, cfg.server_url, cfg.dds_interface, cfg.dds_domain, @@ -486,16 +1282,22 @@ def main(argv: Optional[List[str]] = None) -> int: log.info("[dry-run] %s", json.dumps(payload)) else: post_telemetry(cfg, payload, session) + send_alerts(cfg, session, payload.get("faults") or []) tick += 1 + if args.once or args.dry_run: + map_sync_once(cfg, session, force=args.force, dry_run=args.dry_run) if args.once: - one(); return 0 + one(); ship_logs(cfg, session); return 0 if args.dry_run: for _ in range(3): one(); time.sleep(min(cfg.poll_interval, 1.0)) return 0 - log.info("loop every %.1fs (Ctrl-C to stop)", cfg.poll_interval) + threading.Thread(target=map_loop, args=(cfg, session), daemon=True).start() + threading.Thread(target=logs_loop, args=(cfg, session), daemon=True).start() + log.info("loop every %.1fs; map every %.0fs; logs every %.0fs (Ctrl-C to stop)", + cfg.poll_interval, cfg.map_poll_interval, cfg.logs_interval) while True: try: one() diff --git a/agents/r1/.env.example b/agents/r1/.env.example index 077d7fc..60c4bb2 100644 --- a/agents/r1/.env.example +++ b/agents/r1/.env.example @@ -41,3 +41,27 @@ ROBOT_MODEL=r1 # Optional: Sanad data dir whose size is reported in storage (as /host/ # when the installer's read-only /:/host mount is used). Empty = omit. STORAGE_DATA_PATH= + +# ── map sync (uploaded ONCE per content; status shown in telemetry "map") ──── +# The installer mounts the robot's real maps dir at /data/maps (probed on the +# robot: rtabmap*.db / slam_toolbox yaml+pgm sets). Fallback = agent's own dir. +MAPS_DIR=/data/maps +DATA_DIR=/data/web_data +STATE_DIR=/data/state +MAP_SELECT=all +MAP_UPLOAD_MODE=multipart +MAP_POLL_INTERVAL=30 + +# ── logs + alerts ───────────────────────────────────────────────────────────── +# agent log lines shipped to /{sn}/logs every LOGS_INTERVAL; new faults POSTed +# to /{sn}/alert immediately (rising edge). +LOGS_INTERVAL=60 + +# ── project logs (shared alongside agent logs) ─────────────────────────────── +# auto (default) = find a RUNNING Sanad project container (sanadr1, sanad-p4, +# sanad*) via the /host mount and tail its docker json-log, labeled +# "-logs" in the shipped lines; telemetry shows project_logs (null=none). +# Or pin a container name / an explicit log file (PROJECT_LOG_PATH, via /host). +PROJECT_LOG_CONTAINER=auto +PROJECT_LOG_PATH= +PROJECT_LOG_LABEL= diff --git a/agents/r1/sanad_api_r1.py b/agents/r1/sanad_api_r1.py index 3cb4e47..f3309d6 100644 --- a/agents/r1/sanad_api_r1.py +++ b/agents/r1/sanad_api_r1.py @@ -1,71 +1,65 @@ #!/usr/bin/env python3 -"""sanad_api_r1 — R1 fleet TELEMETRY agent. +"""sanad_api_r1 — R1 fleet agent: TELEMETRY + MAP sync in ONE service. -Scope (this build): push the R1's live status to the YS Lootah fleet server. -This is the "Main statuses" row of the fleet spec (NOT the map — the R1 map is -skipped by request): +The single R1 agent (one container, one systemd service) that: - POST {SERVER_URL}/api/v1/fleet/ingest/telemetry (Bearer device token) - body: { "sn", "mac", "battery", "charging", "status", "position":{x,y}, "faults":[] } + 1. TELEMETRY — every ~2 s POSTs the robot's live status: + POST {SERVER_URL}/api/v1/fleet/ingest/telemetry + { sn, name, mac, brand, type, model, battery, charging, battery_detail, + motor_temp, storage, status, position, faults, map, ts } -Sent every ~2 s. Per the spec: if state can't be read, still send a heartbeat so -the robot stays "online". + 2. MAP — a background loop (every MAP_POLL_INTERVAL, default 30 s) checks the + Sanad dashboard's saved maps (web_nav3: RTAB-Map .db + places) and uploads + each map ONE time (re-upload only if its content changes): + POST {SERVER_URL}/api/v1/fleet/ingest/{sn}/map (multipart: db + meta) + + The map result is SHOWN inside every telemetry post as the "map" field: + "map": { "uploaded": true|false, + "state": "uploaded" | "no_map" | "failed" | "pending", + "maps_found": N, "last_map": "floor-1"|null, + "error": "no saved map found …"|null, "checked_ts": … } + so the server always sees whether the map made it — and why not. DATA SOURCES (Unitree R1 EDU, unitree_hg DDS — same family as the G1) --------------------------------------------------------------------- - battery / charging : rt/lf/bmsstate (BmsState_) soc 0-100; charging = current>+0.05A - (mirrors SanadR1 motion/arm_controller.get_battery) - faults / liveness : rt/lowstate (LowState_) motor temps + message staleness - status : R1 loco FSM via GET RPC 7001 (ids 0 ZeroTorque / 1 Damp / - 4 Locked-Standing / 811 Gait-Running) — READ-ONLY, optional - (R1_READ_FSM=1). Default derives status from BMS + motion. - position {x,y} : OPTIONAL. R1 localizes with stereo VSLAM (ROS side); this - agent has no ROS, so position is read over rosbridge /odom - only when R1_POSITION_SOURCE=rosbridge, else omitted. - mac : primary NIC hardware address. +----------------------------------------- + battery / charging : rt/lf/bmsstate (BmsState_) soc, current, voltage, temp, soh, cycles + faults / motor temp: rt/lowstate (LowState_) per-motor temps + staleness + position {x,y} : rt/lf/odommodestate (SportModeState_) firmware odom over DDS + status : derived (charging/moving/idle/offline); optional FSM GET RPC + (R1 ids: 0 zero-torque / 1 damping / 4 standing / 811 gait-running) + storage : host disk via the read-only /:/host mount (+ Sanad data dir size) + maps : MAPS_DIR//*.db (+ maps_meta.json), places under + DATA_DIR//places/.json — the web_nav3 stores -SAFETY: never commands motion. Only GET RPCs are ever issued to the R1. +Read-only toward the robot: never commands motion. Degrades to heartbeats if +unitree_sdk2py is unavailable; --simulate fakes only the DDS side (the map scan +stays real). -NO ROS. DDS via unitree_sdk2py (net=host + the robot interface). If unitree_sdk2py -is unavailable it degrades to heartbeats. --simulate feeds synthetic state so the -upload path is testable without a robot. +CONFIG — environment (see .env.example). Key vars: + SERVER_URL, DEVICE_TOKEN, SN (required at install), ROBOT_NAME, + DDS_INTERFACE (eth10), POLL_INTERVAL (2), TELEMETRY_ENDPOINT, + ROBOT (web_nav3 robot name, default sanad), MAPS_DIR, DATA_DIR, STATE_DIR, + MAP_SELECT (all|active|newest), MAP_UPLOAD_MODE (multipart|base64json), + MAP_ENDPOINT, MAP_POLL_INTERVAL (30), VERIFY_TLS, HTTP_TIMEOUT -CONFIG — environment (see .env.example) ---------------------------------------- - SERVER_URL, DEVICE_TOKEN fleet base URL + bearer token (required) - SN this robot's fleet id default r1_0000 - DDS_INTERFACE robot network iface for DDS default eth0 - DDS_DOMAIN DDS domain id default 0 - MAC_INTERFACE iface whose MAC to report default = DDS_INTERFACE - R1_READ_FSM 1 = read loco FSM for status default 0 - R1_POSITION_SOURCE none | rosbridge default none - ROSBRIDGE_URL ws://127.0.0.1:9090 (position) default ws://127.0.0.1:9090 - LOW_SOC % below which -> LOW_BATTERY fault default 15 - MOTOR_TEMP_MAX °C above which -> OVERTEMP fault default 85 - POLL_INTERVAL seconds between telemetry posts default 2 - VERIFY_TLS / HTTP_TIMEOUT TLS verify (1) / per-req timeout (10) - -CLI ---- - python sanad_api_r1.py # real DDS loop (default) - python sanad_api_r1.py --simulate # synthetic state (no robot) — for testing - python sanad_api_r1.py --once # one read+post, then exit - python sanad_api_r1.py --dry-run # build telemetry, print it, never POST +CLI: --simulate | --once | --dry-run | --force (map re-upload) | --list (maps) | -v """ from __future__ import annotations import argparse +import base64 +import datetime as _dt +import hashlib import json import logging import math import os import shutil -import socket import sys import threading import time import uuid -from dataclasses import dataclass +from dataclasses import dataclass, field from pathlib import Path from typing import Any, Dict, List, Optional @@ -97,6 +91,14 @@ def _env_bool(name: str, default: bool) -> bool: return _env(name, "1" if default else "0").lower() in ("1", "true", "yes", "on") +def _now_str() -> str: + """Full local date+time with UTC offset (e.g. 2026-07-13 14:20:33+04:00). + TZ_OFFSET_HOURS (default +4, Dubai) keeps container clocks honest without tzdata.""" + off = float(_env("TZ_OFFSET_HOURS", "4")) + tz = _dt.timezone(_dt.timedelta(hours=off)) + return _dt.datetime.now(tz).isoformat(sep=" ", timespec="seconds") + + # --------------------------------------------------------------------------- # # config # --------------------------------------------------------------------------- # @@ -111,6 +113,7 @@ class Config: model: str storage_path: str data_path: str + # telemetry / DDS dds_interface: str dds_domain: int mac_interface: str @@ -120,7 +123,29 @@ class Config: low_soc: int motor_temp_max: float poll_interval: float - endpoint: str + telemetry_endpoint: str + # map sync + robot: str + maps_dir: Path + web_data_dir: Optional[Path] + legacy_places: Optional[Path] + web_nav3_url: str + map_select: str + map_upload_mode: str + map_endpoint_tmpl: str + map_poll_interval: float + map_max_upload_mb: float + state_dir: Path + # logs + alerts + alert_endpoint: str + logs_endpoint: str + logs_interval: float + # project logs (e.g. the robot's Sanad app) shipped alongside agent logs + project_log_container: str + project_log_path: str + project_log_label: str + project_log_backfill: int + # transport verify_tls: bool http_timeout: float @@ -132,6 +157,8 @@ class Config: if missing: raise SystemExit(f"[config] missing required env: {', '.join(missing)}") iface = _env("DDS_INTERFACE", "eth0") + data_dir = _env("DATA_DIR") + legacy = _env("LEGACY_PLACES") return cls( server_url=server, device_token=token, @@ -151,26 +178,49 @@ class Config: low_soc=int(_env("LOW_SOC", "15")), motor_temp_max=float(_env("MOTOR_TEMP_MAX", "85")), poll_interval=float(_env("POLL_INTERVAL", "2")), - endpoint=_env("TELEMETRY_ENDPOINT", "/api/v1/fleet/ingest/telemetry"), + telemetry_endpoint=_env("TELEMETRY_ENDPOINT", "/api/v1/fleet/ingest/telemetry"), + robot=_env("ROBOT", "sanad"), + maps_dir=Path(_env("MAPS_DIR", "/data/maps")), + web_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(), + map_upload_mode=_env("MAP_UPLOAD_MODE", "multipart").lower(), + map_endpoint_tmpl=_env("MAP_ENDPOINT", "/api/v1/fleet/ingest/{sn}/map"), + map_poll_interval=float(_env("MAP_POLL_INTERVAL", "30")), + map_max_upload_mb=float(_env("MAP_MAX_UPLOAD_MB", "7")), + state_dir=Path(_env("STATE_DIR", "/data/state")), + alert_endpoint=_env("ALERT_ENDPOINT", "/api/v1/fleet/ingest/{sn}/alert"), + logs_endpoint=_env("LOGS_ENDPOINT", "/api/v1/fleet/ingest/{sn}/logs"), + logs_interval=float(_env("LOGS_INTERVAL", "60")), + project_log_container=_env("PROJECT_LOG_CONTAINER", "auto"), + project_log_path=_env("PROJECT_LOG_PATH", ""), + project_log_label=_env("PROJECT_LOG_LABEL", ""), + project_log_backfill=int(_env("PROJECT_LOG_BACKFILL", "100")), verify_tls=_env_bool("VERIFY_TLS", True), - http_timeout=float(_env("HTTP_TIMEOUT", "10")), + http_timeout=float(_env("HTTP_TIMEOUT", "30")), ) def telemetry_url(self) -> str: - return self.server_url + self.endpoint + return self.server_url + self.telemetry_endpoint + + def map_url(self) -> str: + return self.server_url + self.map_endpoint_tmpl.format(sn=self.sn) + + def alert_url(self) -> str: + return self.server_url + self.alert_endpoint.format(sn=self.sn) + + def logs_url(self) -> str: + return self.server_url + self.logs_endpoint.format(sn=self.sn) def auth_headers(self) -> Dict[str, str]: return {"Authorization": f"Bearer {self.device_token}"} # --------------------------------------------------------------------------- # -# mac address +# mac + storage # --------------------------------------------------------------------------- # def read_mac(interface: str) -> str: - """Stable hardware MAC. Prefer the named NIC (/sys), fall back to uuid.getnode. - - NOTE: with docker network_mode: host the container shares the host net - namespace, so this is the real robot NIC MAC (not a virtual docker MAC).""" p = Path(f"/sys/class/net/{interface}/address") try: mac = p.read_text().strip() @@ -182,9 +232,6 @@ def read_mac(interface: str) -> str: return ":".join(f"{(n >> b) & 0xff:02x}" for b in range(40, -1, -8)) -# --------------------------------------------------------------------------- # -# storage (host disk usage; mount / at /host:ro in docker) -# --------------------------------------------------------------------------- # _data_size_cache: Dict[str, Any] = {"ts": 0.0, "kb": None} @@ -203,7 +250,6 @@ def read_storage(cfg: Config) -> Optional[Dict[str, Any]]: } except Exception: return None - # data-dir size is a directory walk — cache it (refresh every 60 s) if cfg.data_path and os.path.isdir(cfg.data_path): now = time.monotonic() if _data_size_cache["kb"] is None or now - _data_size_cache["ts"] > 60: @@ -224,21 +270,21 @@ def read_storage(cfg: Config) -> Optional[Dict[str, Any]]: # --------------------------------------------------------------------------- # -# DDS reader (optional — degrades if unitree_sdk2py is absent) +# DDS reader (telemetry side — degrades to heartbeats if SDK absent) # --------------------------------------------------------------------------- # class DDSReader: - """Subscribes rt/lf/bmsstate + rt/lowstate and (optionally) reads the loco - FSM. All reads are passive; the only RPC ever issued is GET_FSM_ID.""" + """Subscribes rt/lf/bmsstate + rt/lowstate (+ rt/lf/odommodestate for position). + Passive reads; the only RPC ever issued is GET_FSM_ID.""" def __init__(self, cfg: Config): self.cfg = cfg self._lock = threading.Lock() self._bms: Optional[Dict[str, Any]] = None self._bms_ts = 0.0 - self._low = None self._low_ts = 0.0 self._temps: List[float] = [] self._max_dq = 0.0 + self._xy: Optional[Dict[str, float]] = None self._loco = None self.ok = False self._start() @@ -252,10 +298,16 @@ class DDSReader: from unitree_sdk2py.idl.unitree_hg.msg.dds_ import BmsState_ except Exception: BmsState_ = None + SportModeState_ = None + if self.cfg.position_source == "odom": + try: + # G1 firmware publishes odom as unitree_go SportModeState_. + from unitree_sdk2py.idl.unitree_go.msg.dds_ import SportModeState_ + except Exception: + SportModeState_ = None except Exception as e: log.warning("unitree_sdk2py unavailable (%s) — telemetry runs in heartbeat mode", e) return - try: ChannelFactoryInitialize(self.cfg.dds_domain, self.cfg.dds_interface) self._low_sub = ChannelSubscriber("rt/lowstate", LowState_) @@ -265,33 +317,32 @@ class DDSReader: self._bms_sub.Init(self._on_bms, 10) else: log.warning("BmsState_ not in this unitree_sdk2py — battery will be null") + if SportModeState_ is not None: + self._odom_sub = ChannelSubscriber("rt/lf/odommodestate", SportModeState_) + self._odom_sub.Init(self._on_odom, 10) if self.cfg.read_fsm: self._init_loco() self.ok = True - log.info("DDS up: domain=%d iface=%s (rt/lowstate + rt/lf/bmsstate)", - self.cfg.dds_domain, self.cfg.dds_interface) + log.info("DDS up: domain=%d iface=%s (bmsstate + lowstate%s)", + self.cfg.dds_domain, self.cfg.dds_interface, + " + odom" if SportModeState_ is not None else "") except Exception as e: log.warning("DDS init failed (%s) — heartbeat mode", e) def _init_loco(self) -> None: - """Loco client for READ-ONLY FSM id (GET RPC 7001). Never sends motion.""" try: from unitree_sdk2py.rpc.client import Client # type: ignore except Exception as e: log.warning("loco RPC client unavailable (%s) — status from BMS/motion only", e) return try: - # R1 loco service ("loco"), GET_FSM_ID = 7001 (see R1 r1_loco_client). - c = Client("loco", 0) - c.Init() - c.SetTimeout(3.0) + c = Client("loco", 0); c.Init(); c.SetTimeout(3.0) self._loco = c log.info("loco FSM read enabled (GET-only, no motion)") except Exception as e: log.warning("loco client init failed (%s) — status from BMS/motion only", e) self._loco = None - # -- callbacks -- def _on_bms(self, msg) -> None: try: soc = int(getattr(msg, "soc", 0) or 0) @@ -321,16 +372,15 @@ class DDSReader: temp_c = max(vals) except Exception: temp_c = None - batt = { - "soc": max(0, min(100, soc)), - "current_a": round(cur_mA / 1000.0, 2), - "voltage_v": round(volt_mv / 1000.0, 1) if volt_mv else None, - "temp_c": temp_c, - "soh": int(getattr(msg, "soh", 0) or 0), - "cycles": int(getattr(msg, "cycle", 0) or 0), - } with self._lock: - self._bms = batt + self._bms = { + "soc": max(0, min(100, soc)), + "current_a": round(cur_mA / 1000.0, 2), + "voltage_v": round(volt_mv / 1000.0, 1) if volt_mv else None, + "temp_c": temp_c, + "soh": int(getattr(msg, "soh", 0) or 0), + "cycles": int(getattr(msg, "cycle", 0) or 0), + } self._bms_ts = time.monotonic() except Exception: pass @@ -339,12 +389,10 @@ class DDSReader: try: temps: List[float] = [] max_dq = 0.0 - ms = getattr(msg, "motor_state", None) or [] - for m in ms: + for m in (getattr(msg, "motor_state", None) or []): t = getattr(m, "temperature", None) if t is not None: try: - # temperature may be a scalar or a small array (surface/winding) vals = [float(x) for x in t] if hasattr(t, "__iter__") else [float(t)] # 0 = slot not reporting (unpopulated motor), not a real temp temps.extend(v for v in vals if 0 < v <= 200) @@ -357,23 +405,30 @@ class DDSReader: except Exception: pass with self._lock: - self._low = msg self._low_ts = time.monotonic() self._temps = temps self._max_dq = max_dq except Exception: pass - # -- reads -- + def _on_odom(self, msg) -> None: + try: + pos = getattr(msg, "position", None) + if pos is not None and len(pos) >= 2: + with self._lock: + self._xy = {"x": round(float(pos[0]), 3), "y": round(float(pos[1]), 3)} + except Exception: + pass + def snapshot(self) -> Dict[str, Any]: with self._lock: now = time.monotonic() return { "bms": dict(self._bms) if self._bms else None, - "bms_age": (now - self._bms_ts) if self._bms_ts else None, "low_age": (now - self._low_ts) if self._low_ts else None, "temps": list(self._temps), "max_dq": self._max_dq, + "xy": dict(self._xy) if self._xy else None, } def fsm_id(self) -> Optional[int]: @@ -382,18 +437,16 @@ class DDSReader: try: code, data = self._loco._Call(7001, "{}") # GET_FSM_ID — read-only if code == 0 and data: - return int(json.loads(data).get("data", json.loads(data)) if data.strip().startswith("{") else data) + return int(json.loads(data).get("data", data)) if data.strip().startswith("{") else int(data) except Exception as e: log.debug("fsm read failed: %s", e) return None +# R1 FSM ids (official R1 sport doc): 0 ZeroTorque, 1 Damping, 4 Locked-Standing, 811 Gait-Running. _FSM_STATUS = {0: "zero_torque", 1: "damping", 4: "standing", 811: "ready"} -# --------------------------------------------------------------------------- # -# optional position over rosbridge (/odom) -# --------------------------------------------------------------------------- # class RosbridgePosition: def __init__(self, cfg: Config): self.cfg = cfg @@ -401,9 +454,9 @@ class RosbridgePosition: self._lock = threading.Lock() self._stop = False try: - import websocket # noqa: F401 (websocket-client) + import websocket # noqa: F401 except Exception as e: - log.warning("websocket-client absent (%s) — position disabled", e) + log.warning("websocket-client absent (%s) — rosbridge position disabled", e) self._ok = False return self._ok = True @@ -432,6 +485,702 @@ class RosbridgePosition: return dict(self._xy) if self._xy else None +# --------------------------------------------------------------------------- # +# map sync (web_nav3 saved maps → fleet server, uploaded ONCE per content) +# --------------------------------------------------------------------------- # +@dataclass +class MapArtifact: + path: Path # .db (rtabmap) or .yaml (slam_toolbox set) + name: str + stem: str + size: int + mtime: int + fmt: str = "rtabmap_db" # rtabmap_db | slam_toolbox + files: Dict[str, Path] = field(default_factory=dict) # slam set: pgm/yaml/posegraph/data + description: str = "" + sha256: str = "" + points: List[Dict[str, Any]] = field(default_factory=list) + + def fingerprint(self) -> str: + return f"{self.fmt}:{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() + + +def _sha256_set(paths: List[Path]) -> str: + h = hashlib.sha256() + for p in sorted(paths): + with p.open("rb") as f: + for chunk in iter(lambda: f.read(1024 * 1024), b""): + h.update(chunk) + return h.hexdigest() + + +# ---- slam_toolbox map set (office.yaml + office.pgm [+ .posegraph .data]) ---- +def _parse_map_yaml(path: Path) -> Dict[str, Any]: + """Tiny parser for a ROS map_server yaml (image/resolution/origin) — no pyyaml.""" + out: Dict[str, Any] = {} + for line in path.read_text().splitlines(): + line = line.split("#", 1)[0].strip() + if ":" not in line: + continue + k, _, v = line.partition(":") + k, v = k.strip(), v.strip() + if k == "image": + out["image"] = v + elif k == "resolution": + try: + out["resolution"] = float(v) + except ValueError: + pass + elif k == "origin": + try: + nums = [float(x) for x in v.strip("[]").split(",")] + out["origin"] = {"x": nums[0], "y": nums[1], + "yaw": nums[2] if len(nums) > 2 else 0.0} + except Exception: + pass + return out + + +def _read_pgm(path: Path) -> Optional[Dict[str, Any]]: + """Parse a binary PGM (P5): returns {width, height, maxval, pixels(bytes)}.""" + try: + data = path.read_bytes() + if not data.startswith(b"P5"): + return None + # tokenize header (magic, width, height, maxval), skipping comments + tokens: List[bytes] = [] + i = 2 + while len(tokens) < 3 and i < len(data): + c = data[i:i + 1] + if c in b" \t\r\n": + i += 1 + elif c == b"#": + i = data.index(b"\n", i) + 1 + else: + j = i + while j < len(data) and data[j:j + 1] not in b" \t\r\n": + j += 1 + tokens.append(data[i:j]) + i = j + w, h, maxval = int(tokens[0]), int(tokens[1]), int(tokens[2]) + pixels = data[i + 1: i + 1 + w * h] + if len(pixels) < w * h: + return None + return {"width": w, "height": h, "maxval": maxval, "pixels": pixels} + except Exception: + return None + + +def _pgm_to_png_b64(pgm: Dict[str, Any]) -> str: + """Grayscale 8-bit PNG from parsed PGM — pure stdlib (zlib + struct).""" + import struct + import zlib + + w, h, pixels = pgm["width"], pgm["height"], pgm["pixels"] + + def chunk(tag: bytes, body: bytes) -> bytes: + return (struct.pack(">I", len(body)) + tag + body + + struct.pack(">I", zlib.crc32(tag + body) & 0xFFFFFFFF)) + + ihdr = struct.pack(">IIBBBBB", w, h, 8, 0, 0, 0, 0) # 8-bit grayscale + raw = b"".join(b"\x00" + pixels[y * w:(y + 1) * w] for y in range(h)) + png = (b"\x89PNG\r\n\x1a\n" + chunk(b"IHDR", ihdr) + + chunk(b"IDAT", zlib.compress(raw, 6)) + chunk(b"IEND", b"")) + return base64.b64encode(png).decode("ascii") + + +def _discover_slam_sets(cfg: Config) -> List[MapArtifact]: + """Find slam_toolbox / map_server map sets: .yaml + .pgm + (+ optional .posegraph/.data) under the maps roots and maps_slam/.""" + roots = [cfg.maps_dir, cfg.maps_dir / cfg.robot, cfg.maps_dir / "maps_slam"] + seen: set = set() + out: List[MapArtifact] = [] + for root in roots: + if not root.exists(): + continue + for y in sorted(root.glob("*.yaml")): + meta = _parse_map_yaml(y) + img = meta.get("image", "") + pgm = (y.parent / img) if img else y.with_suffix(".pgm") + if not pgm.exists(): + pgm = y.with_suffix(".pgm") + if not pgm.exists(): + continue # yaml without a raster — not a map set + rp = str(y.resolve()) + if rp in seen: + continue + seen.add(rp) + files: Dict[str, Path] = {"yaml": y, "pgm": pgm} + for ext in ("posegraph", "data"): + p = y.with_suffix("." + ext) + if p.exists(): + files[ext] = p + size = sum(p.stat().st_size for p in files.values()) + mtime = max(int(p.stat().st_mtime) for p in files.values()) + out.append(MapArtifact(path=y, name=y.name, stem=y.stem, + size=size, mtime=mtime, + fmt="slam_toolbox", files=files)) + return out + + +def _map_key(stem: str) -> str: + 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: + 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]: + out: List[Path] = [] + key = _map_key(stem) + if cfg.web_data_dir: + out.append(cfg.web_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]]: + 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 + return pts + return [] + + +def discover_maps(cfg: Config) -> List[MapArtifact]: + 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 = 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", ""), + )) + # slam_toolbox map sets (office.yaml + office.pgm …) live alongside + out.extend(_discover_slam_sets(cfg)) + out.sort(key=lambda m: m.mtime, reverse=True) + return out + + +def _active_map_name(cfg: Config) -> Optional[str]: + 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: + 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 + return maps[:1] + return maps # "all" + + +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 map state: %s", e) + + +def build_meta(cfg: Config, m: MapArtifact) -> Dict[str, Any]: + return { + "sn": cfg.sn, + "name": m.stem, + "file": m.name, + "format": m.fmt, + "size_bytes": m.size, + "sha256": m.sha256, + "mtime": m.mtime, + "description": m.description, + "points": m.points, + } + + +def _upload_slam_map(cfg: Config, m: MapArtifact, session: requests.Session) -> bool: + """slam_toolbox map → the spec's image JSON: PNG (from the pgm) + resolution + + origin + width/height + points. This is what the dashboard renders.""" + url = cfg.map_url() + ymeta = _parse_map_yaml(m.files["yaml"]) + pgm = _read_pgm(m.files["pgm"]) + if pgm is None: + log.error("map %s: cannot parse %s (not binary P5?)", m.stem, m.files["pgm"].name) + return False + body = build_meta(cfg, m) + body.update({ + "resolution": ymeta.get("resolution"), + "origin": ymeta.get("origin"), + "width": pgm["width"], + "height": pgm["height"], + "image_base64": _pgm_to_png_b64(pgm), + }) + try: + resp = session.post(url, json=body, headers=cfg.auth_headers(), + timeout=cfg.http_timeout, verify=cfg.verify_tls) + except requests.RequestException as e: + log.error("map upload %s FAILED (transport): %s", m.stem, e) + return False + if not resp.ok: + log.error("map upload %s FAILED: HTTP %s %s", m.stem, resp.status_code, resp.text[:300]) + return False + log.info("map uploaded: %s (slam_toolbox %dx%d @ %sm, %d points) -> HTTP %s", + m.stem, pgm["width"], pgm["height"], ymeta.get("resolution"), + len(m.points), resp.status_code) + return True + + +def upload_map(cfg: Config, m: MapArtifact, session: requests.Session) -> bool: + if m.fmt == "slam_toolbox": + return _upload_slam_map(cfg, m, session) + url = cfg.map_url() + meta = build_meta(cfg, m) + try: + if cfg.map_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("map upload %s FAILED (transport): %s", m.name, e) + return False + if not resp.ok: + log.error("map upload %s FAILED: HTTP %s %s", m.name, resp.status_code, resp.text[:300]) + return False + log.info("map uploaded: %s (%.2f MB, %d points) -> HTTP %s", + m.name, m.size / 1024 / 1024, len(m.points), resp.status_code) + return True + + +# Shared map status — SHOWN in every telemetry post ("map" field). +_MAP_STATUS_LOCK = threading.Lock() +_MAP_STATUS: Dict[str, Any] = { + "uploaded": False, "state": "pending", "maps_found": 0, + "last_map": None, "error": None, "checked_ts": None, +} + + +def _set_map_status(**kw: Any) -> None: + with _MAP_STATUS_LOCK: + _MAP_STATUS.update(kw) + _MAP_STATUS["checked_ts"] = int(time.time()) + + +def get_map_status() -> Dict[str, Any]: + with _MAP_STATUS_LOCK: + return dict(_MAP_STATUS) + + +def map_sync_once(cfg: Config, session: requests.Session, + force: bool = False, dry_run: bool = False) -> int: + """One map pass: scan the Sanad dashboard maps and upload anything new. + Always updates the shared map status (visible in telemetry).""" + try: + maps = select_maps(cfg, discover_maps(cfg)) + except Exception as e: + _set_map_status(state="failed", uploaded=False, error=f"map scan failed: {e}") + return 0 + if not maps: + _set_map_status(state="no_map", uploaded=False, maps_found=0, last_map=None, + error=f"no saved map found in Sanad dashboard " + f"(maps_dir={cfg.maps_dir}, robot={cfg.robot})") + return 0 + + state = load_state(cfg) + uploaded = failed = unstable = too_large = current = 0 + last_err: Optional[str] = None + now = time.time() + for m in maps: + prev = state.get(str(m.path.resolve())) + if not force and prev == m.fingerprint(): + current += 1 + continue # already uploaded this exact content — one-time rule + # stability guard: a db modified in the last 120 s is still being + # written (active mapping) — wait until it settles before uploading + if not force and (now - m.mtime) < 120: + log.info("map %s still changing (mapping in progress) — waiting to settle", m.name) + unstable += 1 + continue + # server rejects bodies over ~8 MB (client_max_body_size) — don't burn + # bandwidth on uploads that will 413. Raster (slam_toolbox) maps are tiny. + if m.fmt != "slam_toolbox" and (m.size / 1048576) > cfg.map_max_upload_mb: + log.warning("map %s is %.0f MB — exceeds server upload cap (~%.0f MB), skipping " + "(export a raster map or raise the server limit)", + m.name, m.size / 1048576, cfg.map_max_upload_mb) + too_large += 1 + continue + m.sha256 = (_sha256_set(list(m.files.values())) + if m.fmt == "slam_toolbox" else _sha256(m.path)) + m.points = load_points(cfg, m.stem) + if dry_run: + log.info("[dry-run] would upload map %s (%.2f MB, %d points)", + m.name, m.size / 1024 / 1024, len(m.points)) + continue + if upload_map(cfg, m, session): + state[str(m.path.resolve())] = m.fingerprint() + save_state(cfg, state) + uploaded += 1 + else: + failed += 1 + last_err = f"upload failed for {m.name} (see agent log)" + + if failed: + _set_map_status(state="failed", uploaded=False, maps_found=len(maps), + last_map=maps[0].stem, error=last_err) + elif uploaded or current: + # at least one map is on the server (just now or previously); note skips + note = None + if too_large: + note = f"{too_large} map(s) skipped: exceed server upload cap (~{cfg.map_max_upload_mb:.0f} MB)" + elif unstable: + note = "newer map still being written (mapping in progress)" + _set_map_status(state="uploaded", uploaded=True, maps_found=len(maps), + last_map=maps[0].stem, error=note) + elif too_large: + _set_map_status(state="failed", uploaded=False, maps_found=len(maps), + last_map=maps[0].stem, + error=f"map exceeds server upload cap (~{cfg.map_max_upload_mb:.0f} MB) — " + "export a raster map or raise the server limit") + elif unstable: + # newest content is still being written (active mapping) — be honest + _set_map_status(state="pending", uploaded=False, maps_found=len(maps), + last_map=maps[0].stem, + error="map still being written (mapping in progress) — " + "will upload when it settles") + else: + _set_map_status(state="uploaded", uploaded=True, maps_found=len(maps), + last_map=maps[0].stem, error=None) + return uploaded + + +def map_loop(cfg: Config, session: requests.Session) -> None: + while True: + try: + map_sync_once(cfg, session) + except Exception as e: + log.exception("map pass failed: %s", e) + time.sleep(cfg.map_poll_interval) + + +# --------------------------------------------------------------------------- # +# logs + alerts (spec: POST /{sn}/logs periodically, POST /{sn}/alert on events) +# --------------------------------------------------------------------------- # +class _RingLogHandler(logging.Handler): + """Buffers the agent's own log lines so they can be shipped to the server.""" + + def __init__(self, maxlen: int = 400): + super().__init__(level=logging.INFO) + from collections import deque + self._buf: Any = deque(maxlen=maxlen) + self._blk = threading.Lock() + + def emit(self, record: logging.LogRecord) -> None: + try: + with self._blk: + self._buf.append(self.format(record)) + except Exception: + pass + + def drain(self) -> List[str]: + with self._blk: + lines = list(self._buf) + self._buf.clear() + return lines + + def requeue(self, lines: List[str]) -> None: + """Put unshipped lines back (front of the ring) so they retry next cycle + instead of being lost — bounded by maxlen, oldest evicted first.""" + with self._blk: + self._buf.extendleft(reversed(lines)) + + +_LOG_RING = _RingLogHandler() + +# shipped-status shown in every telemetry post ("logs" / "alerts" fields) +_LOGS_STAT: Dict[str, Any] = {"last_sent": None, "lines_sent": 0, "ok": None} +_ALERTS_STAT: Dict[str, Any] = {"sent": 0, "last": None, "last_time": None, "ok": None} + +# start times ("started_at" = this run, "last_start" = previous run) +_STARTED: Dict[str, Any] = {"now": None, "prev": None, "mono": time.monotonic()} + + +def _init_start_times(cfg: Config) -> None: + """Record this agent start; remember the previous one (persisted in STATE_DIR).""" + f = cfg.state_dir / "agent_state.json" + prev = (_read_json(f, {}) or {}).get("started_at") + now_s = _now_str() + try: + cfg.state_dir.mkdir(parents=True, exist_ok=True) + f.write_text(json.dumps({"started_at": now_s})) + except Exception as e: + log.debug("could not persist start time: %s", e) + _STARTED.update(now=now_s, prev=prev, mono=time.monotonic()) + + +class ProjectLogTail: + """Tails the robot's main PROJECT logs (e.g. the sanadr1 / sanad-p4 app) + and feeds them into the shipped log lines, labeled "[-logs] …". + + Sources, in priority order: + PROJECT_LOG_PATH explicit log file (or dir -> newest *.log) via /host + PROJECT_LOG_CONTAINER a docker container name; "auto" (default) scans the + host's docker metadata (/host/var/lib/docker) for a + RUNNING Sanad project (sanadr1, sanad-p4, sanad*) + Reads the container's json-log through the read-only /:/host mount — no + docker socket needed, read-only, cannot disturb the project.""" + + KNOWN = ("sanadr1", "sanad-p4", "sanadv3", "sanad") + + def __init__(self, cfg: Config): + self.label: Optional[str] = None + self._cur: Optional[Path] = None + self._pos = 0 + self._backfill = max(0, cfg.project_log_backfill) + self.active = False + try: + self._resolve(cfg) + except Exception as e: + log.debug("project-log resolve failed: %s", e) + if self.active: + log.info("project logs: sharing '%s' (%s)", self.label, self._cur) + else: + log.info("project logs: none found (project_logs=null)") + + def _resolve(self, cfg: Config) -> None: + # explicit file/dir + if cfg.project_log_path: + p = Path(cfg.project_log_path) + if p.is_dir(): + logs = sorted(p.glob("*.log"), key=lambda f: f.stat().st_mtime, reverse=True) + p = logs[0] if logs else None + if p and p.exists(): + self._start(p, cfg.project_log_label or f"{p.stem}-logs") + return + # docker container json-log via /host + base = Path("/host/var/lib/docker/containers") + if not base.exists(): + return + want = cfg.project_log_container + candidates: List[Any] = [] + for cf in base.glob("*/config.v2.json"): + try: + d = json.loads(cf.read_text()) + except Exception: + continue + name = (d.get("Name") or "").lstrip("/") + running = bool((d.get("State") or {}).get("Running")) + lp = d.get("LogPath") or "" + if not name or not lp: + continue + if want != "auto": + if name == want: + candidates.append((0, name, lp, running)) + elif running and "sanad" in name.lower() and not name.startswith("sanad-api"): + # rank known Sanad projects first + rank = self.KNOWN.index(name) if name in self.KNOWN else len(self.KNOWN) + candidates.append((rank, name, lp, running)) + if not candidates: + return + candidates.sort(key=lambda c: c[0]) + _, name, lp, _ = candidates[0] + p = Path("/host" + lp) if not lp.startswith("/host") else Path(lp) + if p.exists(): + self._start(p, cfg.project_log_label or f"{name}-logs") + + def _start(self, p: Path, label: str) -> None: + self._cur = p + size = p.stat().st_size + self._pos = size # default: only NEW lines ship + # backfill: start N lines before EOF so recent history ships at startup + if self._backfill and size: + try: + take = min(size, 512 * 1024) + with p.open("rb") as f: + f.seek(size - take) + tail = f.read(take) + parts = tail.splitlines(keepends=True)[-self._backfill:] + self._pos = size - sum(len(x) for x in parts) + except Exception: + self._pos = size + self.label = label + self.active = True + + def poll(self) -> List[str]: + """New lines since last poll (docker json-log unwrapped), labeled.""" + if not self.active or self._cur is None: + return [] + out: List[str] = [] + try: + st = self._cur.stat() + if st.st_size < self._pos: # log rotated + self._pos = 0 + if st.st_size > self._pos: + with self._cur.open("rb") as f: + f.seek(self._pos) + chunk = f.read(min(st.st_size - self._pos, 256 * 1024)) + self._pos = f.tell() + for ln in chunk.decode("utf-8", "replace").splitlines(): + ln = ln.strip() + if ln.startswith("{"): + try: + ln = (json.loads(ln).get("log") or "").rstrip() + except Exception: + pass + if ln: + out.append(f"[{self.label}] {ln}") + except Exception as e: + log.debug("project-log poll failed: %s", e) + return out[-100:] # cap per cycle + + +_PROJECT_TAIL: Optional[ProjectLogTail] = None + + +def ship_logs(cfg: Config, session: requests.Session) -> None: + """POST buffered agent log lines (+ project logs) to /{sn}/logs. + Best-effort: failures are logged at DEBUG only (below the ring's level -> + no feedback loop).""" + lines = _LOG_RING.drain() + if _PROJECT_TAIL is not None and _PROJECT_TAIL.active: + lines.extend(_PROJECT_TAIL.poll()) + if not lines: + return + try: + r = session.post(cfg.logs_url(), + json={"sn": cfg.sn, "name": cfg.name, + "lines": lines, "ts": int(time.time())}, + headers=cfg.auth_headers(), + timeout=cfg.http_timeout, verify=cfg.verify_tls) + if r.ok: + _LOGS_STAT.update(last_sent=_now_str(), ok=True) + _LOGS_STAT["lines_sent"] += len(lines) + log.debug("logs shipped: %d lines -> HTTP %s", len(lines), r.status_code) + else: + _LOGS_STAT["ok"] = False + _LOG_RING.requeue(lines) # retry next cycle (server keeps 500ing) + log.debug("logs ship failed: HTTP %s (%d lines requeued)", r.status_code, len(lines)) + except requests.RequestException as e: + _LOGS_STAT["ok"] = False + _LOG_RING.requeue(lines) + log.debug("logs ship failed (%d lines requeued): %s", len(lines), e) + + +def logs_loop(cfg: Config, session: requests.Session) -> None: + while True: + time.sleep(cfg.logs_interval) + try: + ship_logs(cfg, session) + except Exception: + pass + + +_ALERT_SEEN: set = set() + + +def send_alerts(cfg: Config, session: requests.Session, faults: List[str]) -> None: + """POST each NEW fault (rising edge) to /{sn}/alert. Faults are strings.""" + global _ALERT_SEEN + current = set(faults) + new = current - _ALERT_SEEN + _ALERT_SEEN = current + for f in sorted(new): + try: + r = session.post(cfg.alert_url(), + json={"sn": cfg.sn, "name": cfg.name, + "alert": f, "message": f, "ts": int(time.time())}, + headers=cfg.auth_headers(), + timeout=cfg.http_timeout, verify=cfg.verify_tls) + _ALERTS_STAT.update(last=f, last_time=_now_str(), ok=bool(r.ok)) + if r.ok: + _ALERTS_STAT["sent"] += 1 + log.info("alert sent: %s -> HTTP %s", f, r.status_code) + except requests.RequestException as e: + _ALERTS_STAT.update(last=f, last_time=_now_str(), ok=False) + log.debug("alert send failed (%s): %s", f, e) + + # --------------------------------------------------------------------------- # # telemetry assembly # --------------------------------------------------------------------------- # @@ -450,10 +1199,7 @@ def derive_faults(cfg: Config, snap: Dict[str, Any]) -> List[str]: def derive_status(cfg: Config, snap: Dict[str, Any], fsm: Optional[int]) -> str: - if fsm is not None and fsm in _FSM_STATUS: - base = _FSM_STATUS[fsm] - else: - base = None + base = _FSM_STATUS.get(fsm) if fsm is not None else None bms = snap.get("bms") charging = bool(bms and bms.get("current_a", 0.0) > 0.05) alive = snap.get("low_age") is not None and snap["low_age"] <= 3.0 @@ -472,10 +1218,11 @@ def build_telemetry(cfg: Config, mac: str, reader: Optional[DDSReader], if sim is not None: snap = {"bms": {"soc": sim["battery"], "current_a": 0.5 if sim["charging"] else -0.3, "voltage_v": 47.5, "temp_c": 36, "soh": 100, "cycles": 45}, - "bms_age": 0.1, "low_age": 0.1, "temps": [sim.get("temp", 45)], "max_dq": sim.get("max_dq", 0.0)} + "low_age": 0.1, "temps": [sim.get("temp", 45)], "max_dq": sim.get("max_dq", 0.0), + "xy": sim.get("position")} fsm = sim.get("fsm") else: - snap = reader.snapshot() if reader else {"bms": None, "low_age": None, "temps": [], "max_dq": 0.0} + snap = reader.snapshot() if reader else {"bms": None, "low_age": None, "temps": [], "max_dq": 0.0, "xy": None} fsm = reader.fsm_id() if (reader and cfg.read_fsm) else None bms = snap.get("bms") @@ -484,49 +1231,52 @@ def build_telemetry(cfg: Config, mac: str, reader: Optional[DDSReader], status = derive_status(cfg, snap, fsm) faults = derive_faults(cfg, snap) - # Battery detail (voltage / current / pack temp / health / cycles). battery_detail = None if bms: battery_detail = {"voltage_v": bms.get("voltage_v"), "current_a": bms.get("current_a"), "temp_c": bms.get("temp_c"), "soh": bms.get("soh"), "cycles": bms.get("cycles")} - # Motor temperature stats; null = temps not receiving. temps = snap.get("temps") or [] motor_temp = ({"max": round(max(temps), 1), "avg": round(sum(temps) / len(temps), 1), "min": round(min(temps), 1)} if temps else None) - position = None - if sim is not None: - position = sim.get("position") - elif pos is not None: + position = snap.get("xy") + if position is None and pos is not None: position = pos.get() - payload: Dict[str, Any] = { + return { "sn": cfg.sn, "name": cfg.name, # friendly display name (e.g. r1_82) "mac": mac, "brand": cfg.brand, - "type": cfg.robot_type, # humanoid | dog - "model": cfg.model, # r1 | g1 | go2 + "type": cfg.robot_type, # humanoid + "model": cfg.model, # g1 "battery": battery, # null = couldn't read (heartbeat) "charging": charging, "battery_detail": battery_detail, "motor_temp": motor_temp, # null = not receiving "storage": read_storage(cfg), "status": status, - "position": position, # null when no localization source + "position": position, # null when no odom/localization source "faults": faults, + "map": get_map_status(), # SHOWS whether the saved map made it to the server + "logs": dict(_LOGS_STAT), # log-shipping status (last_sent, lines_sent, ok) + "project_logs": (_PROJECT_TAIL.label + if _PROJECT_TAIL is not None and _PROJECT_TAIL.active + else None), # e.g. "sanadr1-logs"; null = no project found + "alerts": dict(_ALERTS_STAT), # alert status (sent, last, last_time, ok) + "time": _now_str(), # full date+time of this post + "started_at": _STARTED["now"], # when this agent run started + "last_start": _STARTED["prev"], # previous agent start (null on first ever) + "uptime_s": int(time.monotonic() - _STARTED["mono"]), "ts": int(time.time()), } - return payload -def post_telemetry(cfg: Config, payload: Dict[str, Any], - session: requests.Session) -> bool: +def post_telemetry(cfg: Config, payload: Dict[str, Any], session: requests.Session) -> bool: try: - r = session.post(cfg.telemetry_url(), json=payload, - headers=cfg.auth_headers(), + r = session.post(cfg.telemetry_url(), json=payload, headers=cfg.auth_headers(), timeout=cfg.http_timeout, verify=cfg.verify_tls) except requests.RequestException as e: log.error("telemetry POST failed (transport): %s", e) @@ -534,50 +1284,69 @@ def post_telemetry(cfg: Config, payload: Dict[str, Any], if not r.ok: log.error("telemetry POST failed: HTTP %s %s", r.status_code, r.text[:200]) return False - log.info("telemetry ok: battery=%s charging=%s status=%s pos=%s faults=%d -> HTTP %s", + mp = payload.get("map") or {} + log.info("telemetry ok: battery=%s charging=%s status=%s pos=%s faults=%d map=%s -> HTTP %s", payload["battery"], payload["charging"], payload["status"], - payload["position"], len(payload["faults"]), r.status_code) + payload["position"], len(payload["faults"]), mp.get("state"), r.status_code) return True +def _sim_state(i: int) -> Dict[str, Any]: + charging = (i % 6) in (0, 1) + battery = max(5, 90 - (i % 40)) + moving = (i % 3) == 2 and not charging + return {"battery": battery, "charging": charging, "temp": 45 + (i % 10), + "max_dq": 0.4 if moving else 0.0, "fsm": 811 if moving else 4, + "position": {"x": round(1.0 + 0.1 * i, 2), "y": round(2.0 - 0.05 * i, 2)}} + + +def cmd_list(cfg: Config) -> None: + maps = discover_maps(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) + print(f" {m.name:<28} {m.size/1024/1024:6.2f} MB {len(pts):>3} points {m.description}") + + # --------------------------------------------------------------------------- # # main # --------------------------------------------------------------------------- # -def _sim_state(i: int) -> Dict[str, Any]: - """Deterministic-ish synthetic state that varies each tick (for testing).""" - charging = (i % 6) in (0, 1) - battery = max(5, 90 - (i % 40)) - moving = (i % 3) == 2 and not charging - return { - "battery": battery, - "charging": charging, - "temp": 45 + (i % 10), - "max_dq": 0.4 if moving else 0.0, - "fsm": 811 if moving else 4, - "position": {"x": round(1.0 + 0.1 * i, 2), "y": round(2.0 - 0.05 * i, 2)}, - } - - def main(argv: Optional[List[str]] = None) -> int: - ap = argparse.ArgumentParser(description="R1 fleet telemetry agent") - ap.add_argument("--simulate", action="store_true", help="synthetic state (no robot)") - ap.add_argument("--once", action="store_true", help="one read+post, then exit") - ap.add_argument("--dry-run", action="store_true", help="print telemetry, never POST") - ap.add_argument("--interval", type=float, default=None) + ap = argparse.ArgumentParser(description="R1 fleet agent: telemetry + map sync") + ap.add_argument("--simulate", action="store_true", help="synthetic DDS state (map scan stays real)") + ap.add_argument("--once", action="store_true", help="one map pass + one telemetry post, then exit") + ap.add_argument("--dry-run", action="store_true", help="build payloads, never POST") + ap.add_argument("--force", action="store_true", help="re-upload maps even if unchanged") + ap.add_argument("--list", action="store_true", help="list discovered maps and exit") + ap.add_argument("--interval", type=float, default=None, help="override telemetry POLL_INTERVAL") 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") + logging.basicConfig(level=logging.DEBUG if args.verbose else logging.INFO, + format="%(asctime)s %(levelname)s %(name)s: %(message)s") + # buffer our own log lines for shipping to /{sn}/logs + _LOG_RING.setFormatter(logging.Formatter("%(asctime)s %(levelname)s %(name)s: %(message)s")) + logging.getLogger().addHandler(_LOG_RING) _load_dotenv() cfg = Config.from_env() if args.interval is not None: cfg.poll_interval = args.interval + if args.list: + cmd_list(cfg) + return 0 + + _init_start_times(cfg) + global _PROJECT_TAIL + _PROJECT_TAIL = ProjectLogTail(cfg) mac = read_mac(cfg.mac_interface) - log.info("sanad_api_r1 telemetry — sn=%s mac=%s server=%s iface=%s domain=%d%s", - cfg.sn, mac, cfg.server_url, cfg.dds_interface, cfg.dds_domain, + log.info("sanad_api_r1 — sn=%s name=%s mac=%s server=%s iface=%s pos=%s " + "map_dir=%s map_every=%.0fs%s", + cfg.sn, cfg.name, mac, cfg.server_url, cfg.dds_interface, + cfg.position_source, cfg.maps_dir, cfg.map_poll_interval, " [SIMULATE]" if args.simulate else "") reader = None @@ -586,12 +1355,12 @@ def main(argv: Optional[List[str]] = None) -> int: reader = DDSReader(cfg) if cfg.position_source == "rosbridge": pos = RosbridgePosition(cfg) - time.sleep(1.0) # let first DDS messages land + time.sleep(1.0) session = requests.Session() tick = 0 - def one() -> None: + def one_telemetry() -> None: nonlocal tick sim = _sim_state(tick) if args.simulate else None payload = build_telemetry(cfg, mac, reader, pos, sim=sim) @@ -599,23 +1368,31 @@ def main(argv: Optional[List[str]] = None) -> int: log.info("[dry-run] %s", json.dumps(payload)) else: post_telemetry(cfg, payload, session) + send_alerts(cfg, session, payload.get("faults") or []) tick += 1 - if args.once or (args.dry_run and args.once): - one() - return 0 - if args.dry_run: + if args.once or args.dry_run: + # one map pass first so the telemetry "map" field reflects it + map_sync_once(cfg, session, force=args.force, dry_run=args.dry_run) + if args.once: + one_telemetry() + ship_logs(cfg, session) + return 0 for _ in range(3): - one() + one_telemetry() time.sleep(min(cfg.poll_interval, 1.0)) return 0 - log.info("loop every %.1fs (Ctrl-C to stop)", cfg.poll_interval) + # loop mode: map sync + log shipping in background threads + threading.Thread(target=map_loop, args=(cfg, session), daemon=True).start() + threading.Thread(target=logs_loop, args=(cfg, session), daemon=True).start() + log.info("telemetry every %.1fs; map check every %.0fs; logs every %.0fs (Ctrl-C to stop)", + cfg.poll_interval, cfg.map_poll_interval, cfg.logs_interval) while True: try: - one() + one_telemetry() except Exception as e: - log.exception("tick failed: %s", e) + log.exception("telemetry tick failed: %s", e) try: time.sleep(cfg.poll_interval) except KeyboardInterrupt: diff --git a/fleet_install.sh b/fleet_install.sh index d4a1861..86abff9 100755 --- a/fleet_install.sh +++ b/fleet_install.sh @@ -39,35 +39,39 @@ rmt_in(){ ssh "${SSH_OPTS[@]}" "$USER_@$IP" "$@"; } # stdin passthrough ( detect_server_ip(){ [ -n "$SERVER_IP" ] && { echo "$SERVER_IP"; return; } ip -o route get "$IP" 2>/dev/null | grep -oP 'src \K\S+' | head -1; } -# ---- per-type docker create args (volumes differ) ---- -# g1: find where THIS robot's Sanad dashboard keeps its maps/places on the host -# (native web_nav3 install, or Package_4's host-mounted nav data). Falls back to -# the agent's own dirs (agent then reports map: no_map until maps are exposed). -G1_MAPS_HOST=""; G1_DATA_HOST="" -probe_g1_dirs(){ - G1_MAPS_HOST="/home/$USER_/$(rdir_of g1)/maps" - for c in "/home/$USER_/marcus_nav2_test/maps" \ - "/home/$USER_/sanad_deploy/Sanad_Package_4/nav/data/ros"; do - if rmt "test -d $c" 2>/dev/null; then G1_MAPS_HOST="$c"; break; fi - done - G1_DATA_HOST="/home/$USER_/$(rdir_of g1)/web_data" - for c in "/home/$USER_/marcus_nav2_test/web/data" \ - "/home/$USER_/sanad_deploy/Sanad_Package_4/nav/data/web"; do - if rmt "test -d $c" 2>/dev/null; then G1_DATA_HOST="$c"; break; fi - done +# ---- per-type docker create args ---- +# Every agent now syncs maps: find where THIS robot's Sanad/SLAM stack keeps its +# maps on the host. Falls back to the agent's own dirs (agent then reports +# map: no_map until a map exists / is exposed). +MAPS_HOST=""; DATA_HOST="" +probe_maps_dirs(){ + local t="$1"; local rdir; rdir="$(rdir_of "$t")" + MAPS_HOST="/home/$USER_/$rdir/maps" + DATA_HOST="/home/$USER_/$rdir/web_data" + if [ "$t" = g1 ]; then + for c in "/home/$USER_/marcus_nav2_test/maps" \ + "/home/$USER_/sanad_deploy/Sanad_Package_4/nav/data/ros"; do + if rmt "test -d $c" 2>/dev/null; then MAPS_HOST="$c"; break; fi + done + for c in "/home/$USER_/marcus_nav2_test/web/data" \ + "/home/$USER_/sanad_deploy/Sanad_Package_4/nav/data/web"; do + if rmt "test -d $c" 2>/dev/null; then DATA_HOST="$c"; break; fi + done + else + # r1 (stereo VSLAM) / go2: find an rtabmap db or a map_server yaml set + local found + found=$(rmt "find /home/$USER_ -maxdepth 4 \( -name 'rtabmap*.db' -o -name '*.posegraph' \) 2>/dev/null | head -1" 2>/dev/null) + [ -n "$found" ] && MAPS_HOST="$(dirname "$found")" + fi } run_args(){ - # all agents: host root read-only at /host → real disk-usage stats - local base="--network host --env-file /home/$USER_/$(rdir_of "$1")/.env -v /:/host:ro" - if [ "$1" = g1 ]; then - # g1 also mounts the Sanad dashboard map stores + its upload-state dir - echo "$base -v ${G1_MAPS_HOST:-/home/$USER_/$(rdir_of g1)/maps}:/data/maps:ro \ - -v ${G1_DATA_HOST:-/home/$USER_/$(rdir_of g1)/web_data}:/data/web_data:ro \ - -v /home/$USER_/$(rdir_of g1)/state:/data/state" - else - echo "$base" - fi + # /host:ro → real disk stats; maps/web_data ro + state rw → map sync + local rdir; rdir="$(rdir_of "$1")" + echo "--network host --env-file /home/$USER_/$rdir/.env -v /:/host:ro \ + -v ${MAPS_HOST:-/home/$USER_/$rdir/maps}:/data/maps:ro \ + -v ${DATA_HOST:-/home/$USER_/$rdir/web_data}:/data/web_data:ro \ + -v /home/$USER_/$rdir/state:/data/state" } # Resolve the SERVER_URL + VERIFY_TLS for a deploy: a full --server-url (real @@ -107,10 +111,6 @@ DDS_INTERFACE=$iface DDS_DOMAIN=0 VERIFY_TLS=$vtls POLL_INTERVAL=2 -EOF - # g1 additionally syncs the Sanad-dashboard map (web_nav3 stores) - if [ "$t" = g1 ]; then - rmt_in "cat >> ~/$rdir/.env" <> Sanad dashboard maps dir: $G1_MAPS_HOST" - echo ">> places dir: $G1_DATA_HOST" - fi + probe_maps_dirs "$t" + echo ">> maps dir: $MAPS_HOST" + echo ">> places dir: $DATA_HOST" echo ">> docker build on robot (native arm64; R1/Go2 compile DDS on first build) ..." rmt "cd ~/$rdir && docker build -t $img:latest ." || die "docker build failed" echo ">> creating container (systemd will own its lifecycle) ..." @@ -216,21 +214,20 @@ do_test(){ local rc; rc=$(rmt "curl -s -o /dev/null -w '%{http_code}' --max-time 6 http://$sip:$PORT/ping" || echo 000) [ "$rc" = 200 ] || die "robot cannot reach workstation server (http $rc) — firewall on $sip:$PORT?" echo " reachability OK (HTTP $rc)" - if [ "$t" = g1 ]; then - probe_g1_dirs - if [ "$G1_MAPS_HOST" = "/home/$USER_/$(rdir_of g1)/maps" ]; then - # no real Sanad maps dir on this robot — seed a fixture to prove the path - rmt "mkdir -p ~/$(rdir_of g1)/maps/sanad ~/$(rdir_of g1)/web_data/sanad/places - head -c 4096 /dev/urandom > ~/$(rdir_of g1)/maps/sanad/floor-test.db - touch -d '10 minutes ago' ~/$(rdir_of g1)/maps/sanad/floor-test.db - printf '%s' '{\"dock\":{\"x\":1.2,\"y\":3.4,\"qz\":0,\"qw\":1}}' > ~/$(rdir_of g1)/web_data/sanad/places/floor-test.json - rm -f ~/$(rdir_of g1)/state/uploaded.json" - fi - rmt "docker run --rm $(run_args g1) $img:latest --once --force" || true - else - echo ">> real DDS --once:"; rmt "docker run --rm $(run_args "$t") $img:latest --once" || true - echo ">> simulate --once:"; rmt "docker run --rm $(run_args "$t") $img:latest --simulate --once" || true + probe_maps_dirs "$t" + local rdir; rdir="$(rdir_of "$t")" + if [ "$MAPS_HOST" = "/home/$USER_/$rdir/maps" ]; then + # no real maps dir on this robot — seed a fixture to prove the upload path + rmt "mkdir -p ~/$rdir/maps/sanad ~/$rdir/web_data/sanad/places + head -c 4096 /dev/urandom > ~/$rdir/maps/sanad/floor-test.db + touch -d '10 minutes ago' ~/$rdir/maps/sanad/floor-test.db + printf '%s' '{\"dock\":{\"x\":1.2,\"y\":3.4,\"qz\":0,\"qw\":1}}' > ~/$rdir/web_data/sanad/places/floor-test.json + rm -f ~/$rdir/state/uploaded.json" fi + echo ">> real state --once (--force map):" + rmt "docker run --rm $(run_args "$t") $img:latest --once --force" || true + echo ">> simulate --once:" + rmt "docker run --rm $(run_args "$t") $img:latest --simulate --once" || true sleep 1; echo; echo ">> workstation server received:" local reqlog; reqlog="$(cat /tmp/fleet_last_reqlog)"; local result=0 python3 - "$reqlog" "$t" <<'PY' && result=0 || result=$? @@ -242,8 +239,9 @@ for r in recs: print(" POST",r["path"],"auth="+("yes" if r["auth"].startswith("Bearer") else "NO")) if j: print(" ",{k:j[k] for k in j if k!="db_base64"}) if r.get("meta"): print(" meta:",r["meta"][:160]) -ok=any(x["path"].endswith("/map") and x["ctype"]=="multipart/form-data" for x in recs) if t=="g1" \ - else any(x["path"].endswith("/telemetry") for x in recs) +# every agent now does telemetry + map; PASS needs both to have arrived +ok=(any(x["path"].endswith("/telemetry") for x in recs) + and any(x["path"].endswith("/map") for x in recs)) print(f"\n RESULT: {'PASS' if ok else 'FAIL'} ({len(recs)} request(s))") sys.exit(0 if ok else 1) PY diff --git a/tests/map_export_once.py b/tests/map_export_once.py new file mode 100644 index 0000000..f3e12fb --- /dev/null +++ b/tests/map_export_once.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 +"""One-shot /map -> pgm+yaml exporter. Run INSIDE the robot's VSLAM/nav +container (read-only w.r.t. the robot; subscribes once, writes /data, exits). + + source /opt/ros/foxy/setup.bash && timeout 30 python3 map_export_once.py +""" +import math +import sys + +import rclpy +from nav_msgs.msg import OccupancyGrid +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSProfile, QoSReliabilityPolicy + +OUT = "/data/map_export" + + +class Grab(Node): + def __init__(self): + super().__init__("map_export_once") + qos = QoSProfile(depth=1, + reliability=QoSReliabilityPolicy.RELIABLE, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) + self.create_subscription(OccupancyGrid, "/map", self.cb, qos) + self.done = False + + def cb(self, msg): + if self.done: + return + self.done = True + w, h, res = msg.info.width, msg.info.height, msg.info.resolution + ox, oy = msg.info.origin.position.x, msg.info.origin.position.y + q = msg.info.origin.orientation + yaw = math.atan2(2 * (q.w * q.z + q.x * q.y), + 1 - 2 * (q.y * q.y + q.z * q.z)) + d = msg.data + # map_saver convention: unknown(-1)->205, free->254, occupied->0 + px = bytearray(w * h) + for i in range(w * h): + v = d[i] + px[i] = 205 if v < 0 else (0 if v >= 65 else 254) + with open(OUT + ".pgm", "wb") as f: + f.write(b"P5\n%d %d\n255\n" % (w, h)) + for y in range(h - 1, -1, -1): # grid row 0 = bottom -> flip + f.write(bytes(px[y * w:(y + 1) * w])) + with open(OUT + ".yaml", "w") as f: + f.write("image: map_export.pgm\nresolution: %.6f\n" + "origin: [%.6f, %.6f, %.6f]\nnegate: 0\n" + "occupied_thresh: 0.65\nfree_thresh: 0.196\n" + % (res, ox, oy, yaw)) + print("EXPORTED %dx%d @ %.3fm origin=(%.2f, %.2f, %.2f)" + % (w, h, res, ox, oy, yaw), flush=True) + rclpy.shutdown() + + +def main(): + rclpy.init() + rclpy.spin(Grab()) + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/tests/mock_capture.py b/tests/mock_capture.py new file mode 100644 index 0000000..1e25a51 --- /dev/null +++ b/tests/mock_capture.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python3 +"""Capturing mock fleet server for agent tests: logs one JSON line per POST.""" +import json +import os +from http.server import BaseHTTPRequestHandler, HTTPServer + +REQLOG = os.environ.get("REQLOG", "/tmp/reqs.jsonl") +open(REQLOG, "w").close() + + +class H(BaseHTTPRequestHandler): + def do_POST(self): + n = int(self.headers.get("Content-Length", 0)) + body = self.rfile.read(n) + ctype = (self.headers.get("Content-Type", "") or "").split(";")[0] + rec = {"path": self.path, "ctype": ctype, + "auth": self.headers.get("Authorization", ""), "len": n} + if ctype == "application/json": + try: + rec["json"] = json.loads(body) + except Exception as e: + rec["json_err"] = str(e) + else: + txt = body.decode("latin-1") + i = txt.find('name="meta"') + if i != -1: + rec["meta"] = txt[i:].split("\r\n\r\n", 1)[-1].split("\r\n", 1)[0] + with open(REQLOG, "a") as f: + f.write(json.dumps(rec) + "\n") + self.send_response(200) + self.end_headers() + self.wfile.write(b'{"ok":true}') + + def log_message(self, *a): + pass + + +if __name__ == "__main__": + HTTPServer(("127.0.0.1", 8799), H).serve_forever()