1841 lines
75 KiB
Python
1841 lines
75 KiB
Python
#!/usr/bin/env python3
|
|
"""sanad_api_go2 — Go2 fleet agent: TELEMETRY + MAP sync in ONE service.
|
|
|
|
The single Go2 agent (one container, one systemd service) that:
|
|
|
|
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 }
|
|
|
|
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 Go2, unitree_go DDS (⚠ UNVERIFIED on hardware))
|
|
-----------------------------------------
|
|
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
|
|
(G1 ids: 200 walk-ready / 4 stand / 2 squat / 702 lie2stand)
|
|
storage : host disk via the read-only /:/host mount (+ Sanad data dir size)
|
|
maps : MAPS_DIR/<robot>/*.db (+ maps_meta.json), places under
|
|
DATA_DIR/<robot>/places/<map>.json — the web_nav3 stores
|
|
|
|
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).
|
|
|
|
CONFIG — environment (see .env.example). Key vars:
|
|
SERVER_URL, DEVICE_TOKEN, SN (required at install), ROBOT_NAME,
|
|
DDS_INTERFACE (eth0), 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
|
|
|
|
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 sys
|
|
import threading
|
|
import time
|
|
import uuid
|
|
from dataclasses import dataclass, field
|
|
from pathlib import Path
|
|
from typing import Any, Dict, List, Optional
|
|
|
|
import requests
|
|
|
|
log = logging.getLogger("sanad_api_go2")
|
|
|
|
|
|
# --------------------------------------------------------------------------- #
|
|
# env helpers
|
|
# --------------------------------------------------------------------------- #
|
|
def _load_dotenv(path: str = ".env") -> None:
|
|
p = Path(path)
|
|
if not p.exists():
|
|
return
|
|
for line in p.read_text().splitlines():
|
|
line = line.strip()
|
|
if not line or line.startswith("#") or "=" not in line:
|
|
continue
|
|
k, _, v = line.partition("=")
|
|
os.environ.setdefault(k.strip(), v.strip().strip('"').strip("'"))
|
|
|
|
|
|
def _env(name: str, default: str = "") -> str:
|
|
return os.environ.get(name, default).strip()
|
|
|
|
|
|
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
|
|
# --------------------------------------------------------------------------- #
|
|
@dataclass
|
|
class Config:
|
|
server_url: str
|
|
device_token: str
|
|
sn: str
|
|
name: str
|
|
brand: str
|
|
robot_type: str
|
|
model: str
|
|
storage_path: str
|
|
data_path: str
|
|
# telemetry / DDS
|
|
dds_interface: str
|
|
dds_domain: int
|
|
mac_interface: str
|
|
read_fsm: bool
|
|
position_source: str
|
|
rosbridge_url: str
|
|
low_soc: int
|
|
motor_temp_max: float
|
|
alert_log_patterns: str
|
|
alert_log_cooldown: float
|
|
alert_scan_interval: float
|
|
alert_backfill_bytes: int
|
|
poll_interval: float
|
|
telemetry_endpoint: str
|
|
# map sync
|
|
robot: str
|
|
maps_dir: Path
|
|
extra_map_dirs: List[Path] # extra roots to scan for pgm+yaml sets (e.g. Nav2/Pudu maps)
|
|
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
|
|
# remote dashboard (register the Sanad UI URL for the fleet to embed)
|
|
remote_enable: bool
|
|
remote_endpoint: str
|
|
remote_kind: str
|
|
remote_host: str
|
|
remote_ports: str
|
|
remote_url: str
|
|
remote_interval: float
|
|
ssh_enable: bool
|
|
ssh_user: str
|
|
ssh_port: int
|
|
control_url: str
|
|
control_enable: bool
|
|
# 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
|
|
project_log_exclude: str
|
|
ros_distro: str
|
|
# transport
|
|
verify_tls: bool
|
|
http_timeout: float
|
|
|
|
@classmethod
|
|
def from_env(cls) -> "Config":
|
|
server = _env("SERVER_URL").rstrip("/")
|
|
token = _env("DEVICE_TOKEN")
|
|
missing = [n for n, v in (("SERVER_URL", server), ("DEVICE_TOKEN", token)) if not v]
|
|
if missing:
|
|
raise SystemExit(f"[config] missing required env: {', '.join(missing)}")
|
|
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"),
|
|
name=_env("ROBOT_NAME", "") or _env("SN", "go2_0000"),
|
|
brand=_env("ROBOT_BRAND", "unitree"),
|
|
robot_type=_env("ROBOT_TYPE", "dog"),
|
|
model=_env("ROBOT_MODEL", "go2"),
|
|
storage_path=_env("STORAGE_PATH", ""),
|
|
data_path=_env("STORAGE_DATA_PATH", ""),
|
|
dds_interface=iface,
|
|
dds_domain=int(_env("DDS_DOMAIN", "0")),
|
|
mac_interface=_env("MAC_INTERFACE", iface),
|
|
read_fsm=_env_bool("GO2_READ_FSM", False),
|
|
position_source=_env("GO2_POSITION_SOURCE", "none").lower(),
|
|
rosbridge_url=_env("ROSBRIDGE_URL", "ws://127.0.0.1:9090"),
|
|
low_soc=int(_env("LOW_SOC", "50")),
|
|
motor_temp_max=float(_env("MOTOR_TEMP_MAX", "85")),
|
|
# log-driven alerts: "CODE=regex" entries separated by ";;" (regex may
|
|
# contain '|'). Scanned against the robot's project logs (sanadr1).
|
|
# NOTE: matching is CASE-SENSITIVE (log levels are uppercase); use an
|
|
# inline (?i) prefix for case-insensitive text (Gemini messages).
|
|
alert_log_patterns=_env("ALERT_LOG_PATTERNS",
|
|
"GEMINI_BILLING=(?i)prepayment credits.{0,40}deplet|Please go to AI Studio|Failed to connect to Gemini"
|
|
";;ROBOT_ERROR=\\bERROR\\b|\\bCRITICAL\\b|^Traceback"),
|
|
alert_log_cooldown=float(_env("ALERT_LOG_COOLDOWN", "300")), # per-signature re-alert gap
|
|
alert_scan_interval=float(_env("ALERT_SCAN_INTERVAL", "10")),
|
|
alert_backfill_bytes=int(_env("ALERT_BACKFILL_BYTES", str(8 * 1024 * 1024))),
|
|
poll_interval=float(_env("POLL_INTERVAL", "2")),
|
|
telemetry_endpoint=_env("TELEMETRY_ENDPOINT", "/api/v1/fleet/ingest/telemetry"),
|
|
robot=_env("ROBOT", "sanad"),
|
|
maps_dir=Path(_env("MAPS_DIR", "/data/maps")),
|
|
# colon-separated extra roots (mounted Nav2/Pudu map dirs). Any *.yaml+*.pgm
|
|
# set found here is rendered to PNG and uploaded like a slam_toolbox map.
|
|
extra_map_dirs=[Path(p) for p in _env("EXTRA_MAP_DIRS", "").split(":") if p.strip()],
|
|
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")),
|
|
remote_enable=_env_bool("REMOTE_ENABLE", True),
|
|
remote_endpoint=_env("REMOTE_ENDPOINT", "/api/v1/fleet/ingest/{sn}/remote"),
|
|
remote_kind=_env("REMOTE_KIND", "web"),
|
|
remote_host=_env("REMOTE_HOST", ""),
|
|
remote_ports=_env("REMOTE_PORTS", "8001,8014,8011,8012,8013,8000,8080"),
|
|
remote_url=_env("REMOTE_URL", ""), # explicit URL (e.g. a public tunnel) wins
|
|
remote_interval=float(_env("REMOTE_INTERVAL", "60")),
|
|
ssh_enable=_env_bool("SSH_REGISTER", True),
|
|
ssh_user=_env("SSH_USER", "unitree"),
|
|
ssh_port=int(_env("SSH_PORT", "22")),
|
|
control_url=_env("CONTROL_STATUS_URL", ""), # Sanad /api/controller/status
|
|
control_enable=_env_bool("CONTROL_ENABLE", False), # remote mode-SWITCH (motion!) — off
|
|
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")),
|
|
# drop uvicorn/access-log noise so shipped lines match the project's
|
|
# own LIVE LOGS panel exactly (app-logger lines + tracebacks only)
|
|
project_log_exclude=_env("PROJECT_LOG_EXCLUDE",
|
|
r"^(INFO|WARNING|ERROR|DEBUG|CRITICAL):\s"),
|
|
ros_distro=_env("SOFTWARE_ROS", ""),
|
|
verify_tls=_env_bool("VERIFY_TLS", True),
|
|
http_timeout=float(_env("HTTP_TIMEOUT", "30")),
|
|
)
|
|
|
|
def telemetry_url(self) -> str:
|
|
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 remote_url_ep(self) -> str:
|
|
return self.server_url + self.remote_endpoint.format(sn=self.sn)
|
|
|
|
def auth_headers(self) -> Dict[str, str]:
|
|
return {"Authorization": f"Bearer {self.device_token}"}
|
|
|
|
|
|
# --------------------------------------------------------------------------- #
|
|
# mac + storage
|
|
# --------------------------------------------------------------------------- #
|
|
def read_mac(interface: str) -> str:
|
|
p = Path(f"/sys/class/net/{interface}/address")
|
|
try:
|
|
mac = p.read_text().strip()
|
|
if mac and mac != "00:00:00:00:00:00":
|
|
return mac.lower()
|
|
except Exception:
|
|
pass
|
|
n = uuid.getnode()
|
|
return ":".join(f"{(n >> b) & 0xff:02x}" for b in range(40, -1, -8))
|
|
|
|
|
|
_AGENT_VERSION = "2026.07.13"
|
|
|
|
_software_cache: Optional[Dict[str, Any]] = None
|
|
|
|
|
|
def read_software(cfg: Config) -> Dict[str, Any]:
|
|
"""Robot software/OS card: ROS distro, host OS (via /host), kernel, arch.
|
|
The container shares the HOST kernel; the host OS comes from
|
|
/host/etc/os-release (the read-only /:/host mount)."""
|
|
global _software_cache
|
|
if _software_cache is not None:
|
|
return _software_cache
|
|
sw: Dict[str, Any] = {}
|
|
# ROS distro: pinned via SOFTWARE_ROS, else detect host installs /opt/ros/*
|
|
ros = cfg.ros_distro
|
|
if not ros:
|
|
try:
|
|
for base in ("/host/opt/ros", "/opt/ros"):
|
|
p = Path(base)
|
|
if p.is_dir():
|
|
distros = sorted(d.name for d in p.iterdir() if d.is_dir())
|
|
if distros:
|
|
ros = ",".join(distros)
|
|
break
|
|
except Exception:
|
|
pass
|
|
sw["ros"] = ros or None
|
|
# host OS (firmware/OS card)
|
|
os_name = os_ver = None
|
|
for rel in ("/host/etc/os-release", "/etc/os-release"):
|
|
try:
|
|
kv = {}
|
|
for line in Path(rel).read_text().splitlines():
|
|
if "=" in line:
|
|
k, _, v = line.partition("=")
|
|
kv[k] = v.strip().strip('"')
|
|
os_name = kv.get("PRETTY_NAME") or kv.get("NAME")
|
|
os_ver = kv.get("VERSION_ID")
|
|
break
|
|
except Exception:
|
|
continue
|
|
sw["os"] = os_name # e.g. "Ubuntu 20.04.6 LTS"
|
|
sw["os_version"] = os_ver # e.g. "20.04"
|
|
u = os.uname()
|
|
sw["kernel"] = u.release # host kernel (shared with the container)
|
|
sw["arch"] = u.machine # e.g. aarch64
|
|
sw["python"] = ".".join(map(str, sys.version_info[:3]))
|
|
sw["agent"] = f"{log.name} {_AGENT_VERSION}"
|
|
_software_cache = sw
|
|
return sw
|
|
|
|
|
|
_firmware_cache: Optional[Dict[str, Any]] = None
|
|
|
|
|
|
def read_firmware_static() -> Dict[str, Any]:
|
|
"""Board-level firmware card (static, via /host): compute board model +
|
|
Jetson L4T/BSP release + kernel. DDS adds live robot/bms fw versions."""
|
|
global _firmware_cache
|
|
if _firmware_cache is not None:
|
|
return dict(_firmware_cache)
|
|
fw: Dict[str, Any] = {}
|
|
# board model, e.g. "NVIDIA Orin NX Developer Kit"
|
|
for p in ("/host/sys/firmware/devicetree/base/model",
|
|
"/host/proc/device-tree/model",
|
|
"/sys/firmware/devicetree/base/model",
|
|
"/proc/device-tree/model"):
|
|
try:
|
|
fw["board"] = Path(p).read_bytes().decode().strip("\x00 \n")
|
|
break
|
|
except Exception:
|
|
continue
|
|
# Jetson L4T/BSP: "# R35 (release), REVISION: 3.1, ..." -> "R35.3.1"
|
|
for p in ("/host/etc/nv_tegra_release", "/etc/nv_tegra_release"):
|
|
try:
|
|
head = Path(p).read_text().splitlines()[0]
|
|
import re
|
|
m = re.search(r"(R\d+).*?REVISION:\s*([\d.]+)", head)
|
|
fw["l4t"] = f"{m.group(1)}.{m.group(2)}" if m else head.lstrip("# ").strip()
|
|
break
|
|
except Exception:
|
|
continue
|
|
fw["kernel"] = os.uname().release
|
|
_firmware_cache = fw
|
|
return dict(fw)
|
|
|
|
|
|
_data_size_cache: Dict[str, Any] = {"ts": 0.0, "kb": None}
|
|
|
|
|
|
def read_storage(cfg: Config) -> Optional[Dict[str, Any]]:
|
|
"""Disk usage of the robot's root fs + optional Sanad data-dir size.
|
|
|
|
In docker, bind-mount the host root read-only at /host (the installer does)
|
|
so this reports the HOST disk, not the container overlay."""
|
|
root = cfg.storage_path or ("/host" if os.path.isdir("/host") else "/")
|
|
try:
|
|
du = shutil.disk_usage(root)
|
|
out: Dict[str, Any] = {
|
|
"total_gb": round(du.total / 1e9, 2),
|
|
"free_gb": round(du.free / 1e9, 2),
|
|
"used_percent": round(du.used / du.total * 100, 1),
|
|
}
|
|
except Exception:
|
|
return None
|
|
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:
|
|
try:
|
|
total = 0
|
|
for r, _, files in os.walk(cfg.data_path):
|
|
for f in files:
|
|
try:
|
|
total += os.path.getsize(os.path.join(r, f))
|
|
except OSError:
|
|
pass
|
|
_data_size_cache.update(ts=now, kb=round(total / 1024, 1))
|
|
except Exception:
|
|
pass
|
|
if _data_size_cache["kb"] is not None:
|
|
out["data_kb"] = _data_size_cache["kb"]
|
|
return out
|
|
|
|
|
|
# --------------------------------------------------------------------------- #
|
|
# DDS reader (telemetry side — degrades to heartbeats if SDK absent)
|
|
# --------------------------------------------------------------------------- #
|
|
class DDSReader:
|
|
"""Subscribes rt/lowstate (unitree_go LowState_); battery is NESTED in
|
|
LowState_.bms_state (Go2 has no separate rt/lf/bmsstate). Optional position
|
|
from rt/lf/sportmodestate. Passive reads only; the only RPC ever issued is
|
|
GET_FSM_ID (Go2 has no loco FSM RPC, so it stays off)."""
|
|
|
|
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_ts = 0.0
|
|
self._temps: List[float] = []
|
|
self._max_dq = 0.0
|
|
self._xy: Optional[Dict[str, float]] = None
|
|
self._fw: Dict[str, Any] = {}
|
|
self._loco = None
|
|
self.ok = False
|
|
self._start()
|
|
|
|
def _start(self) -> None:
|
|
try:
|
|
from unitree_sdk2py.core.channel import (
|
|
ChannelFactoryInitialize, ChannelSubscriber)
|
|
from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_
|
|
SportModeState_ = None
|
|
if self.cfg.position_source in ("sportmode", "odom"):
|
|
try:
|
|
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_)
|
|
self._low_sub.Init(self._on_low, 10)
|
|
if SportModeState_ is not None:
|
|
self._sport_sub = ChannelSubscriber("rt/lf/sportmodestate", SportModeState_)
|
|
self._sport_sub.Init(self._on_odom, 10)
|
|
self.ok = True
|
|
log.info("DDS up: domain=%d iface=%s (rt/lowstate; battery from bms_state%s)",
|
|
self.cfg.dds_domain, self.cfg.dds_interface,
|
|
" + sportmode" if SportModeState_ is not None else "")
|
|
except Exception as e:
|
|
log.warning("DDS init failed (%s) — heartbeat mode", e)
|
|
|
|
def _init_loco(self) -> None:
|
|
return # Go2 has no r1/g1-style loco FSM RPC
|
|
|
|
def _on_low(self, msg) -> None:
|
|
try:
|
|
# battery from the nested BMS state
|
|
bms = getattr(msg, "bms_state", None) or getattr(msg, "bms", None)
|
|
if bms is not None:
|
|
soc = int(getattr(bms, "soc", 0) or 0)
|
|
cur = int(getattr(bms, "current", 0) or 0) # mA
|
|
volt_v = None
|
|
try:
|
|
pv = getattr(msg, "power_v", None)
|
|
if pv:
|
|
volt_v = round(float(pv), 1)
|
|
except Exception:
|
|
volt_v = None
|
|
temp_c = None
|
|
try:
|
|
ntc = []
|
|
for attr in ("bq_ntc", "mcu_ntc"):
|
|
nt = getattr(bms, attr, None)
|
|
if nt is not None:
|
|
vals = [int(x) for x in nt] if hasattr(nt, "__iter__") else [int(nt)]
|
|
ntc.extend(v for v in vals if -40 <= v <= 150)
|
|
if ntc:
|
|
temp_c = max(ntc)
|
|
except Exception:
|
|
temp_c = None
|
|
try:
|
|
vh, vl = getattr(bms, "version_high", None), getattr(bms, "version_low", None)
|
|
if vh is not None:
|
|
self._fw["bms"] = f"{int(vh)}.{int(vl or 0)}"
|
|
except Exception:
|
|
pass
|
|
with self._lock:
|
|
self._bms = {
|
|
"soc": max(0, min(100, soc)),
|
|
"current_a": round(cur / 1000.0, 2),
|
|
"voltage_v": volt_v,
|
|
"temp_c": temp_c,
|
|
"soh": int(getattr(bms, "soh", 0) or 0),
|
|
"cycles": int(getattr(bms, "cycle", 0) or 0),
|
|
}
|
|
self._bms_ts = time.monotonic()
|
|
temps: List[float] = []
|
|
max_dq = 0.0
|
|
for m in (getattr(msg, "motor_state", None) or []):
|
|
t = getattr(m, "temperature", None)
|
|
if t is not None:
|
|
try:
|
|
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)
|
|
except Exception:
|
|
pass
|
|
dq = getattr(m, "dq", None)
|
|
if dq is not None:
|
|
try:
|
|
max_dq = max(max_dq, abs(float(dq)))
|
|
except Exception:
|
|
pass
|
|
with self._lock:
|
|
self._low_ts = time.monotonic()
|
|
self._temps = temps
|
|
self._max_dq = max_dq
|
|
except Exception:
|
|
pass
|
|
|
|
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,
|
|
"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,
|
|
"fw": dict(self._fw),
|
|
}
|
|
|
|
def fsm_id(self) -> Optional[int]:
|
|
return None # Go2 has no loco FSM id RPC
|
|
|
|
|
|
# Go2 has no r1/g1 FSM-id scheme (SportClient modes). Placeholders — UNVERIFIED.
|
|
_FSM_STATUS = {}
|
|
# Control-panel mode labels + the switchable set.
|
|
_CONTROL_MODES = {0: "idle", 1: "stand", 2: "walk"}
|
|
_CONTROL_SWITCHABLE = ["damp", "stand", "walk"]
|
|
|
|
|
|
_control_cache: Dict[str, Any] = {"ts": 0.0, "data": None}
|
|
|
|
|
|
def read_control(cfg: Config) -> Optional[Dict[str, Any]]:
|
|
"""READ-ONLY control status from the Sanad dashboard's /api/controller/status
|
|
(no DDS, no motion). Reports the current loco mode + the switchable set.
|
|
Remote SWITCHING is a separate, motion-capable path gated by CONTROL_ENABLE."""
|
|
now = time.monotonic()
|
|
if _control_cache["data"] is not None and now - _control_cache["ts"] < 1.5:
|
|
return _control_cache["data"]
|
|
url = cfg.control_url
|
|
if not url:
|
|
port = _REMOTE_STAT.get("port")
|
|
if not port:
|
|
return None
|
|
url = f"http://127.0.0.1:{port}/api/controller/status"
|
|
try:
|
|
r = requests.get(url, timeout=2)
|
|
d = r.json() if r.ok else None
|
|
except Exception:
|
|
d = None
|
|
if not isinstance(d, dict):
|
|
_control_cache.update(ts=now, data=None)
|
|
return None
|
|
fid = d.get("fsm_id")
|
|
out = {
|
|
"fsm_id": fid,
|
|
"mode": _CONTROL_MODES.get(fid, "unknown") if fid is not None else None,
|
|
"armed": d.get("armed"),
|
|
"walk_ready": d.get("walk_ready"),
|
|
"teleop_active": d.get("teleop_active"),
|
|
"last_velocity": d.get("last_velocity"),
|
|
"sdk_available": d.get("sdk_available"),
|
|
"switchable_modes": _CONTROL_SWITCHABLE,
|
|
"remote_switch_enabled": bool(cfg.control_enable), # remote mode-switch armed?
|
|
}
|
|
_control_cache.update(ts=now, data=out)
|
|
return out
|
|
|
|
|
|
class RosbridgePosition:
|
|
def __init__(self, cfg: Config):
|
|
self.cfg = cfg
|
|
self._xy: Optional[Dict[str, float]] = None
|
|
self._lock = threading.Lock()
|
|
self._stop = False
|
|
try:
|
|
import websocket # noqa: F401
|
|
except Exception as e:
|
|
log.warning("websocket-client absent (%s) — rosbridge position disabled", e)
|
|
self._ok = False
|
|
return
|
|
self._ok = True
|
|
threading.Thread(target=self._run, daemon=True).start()
|
|
|
|
def _run(self) -> None:
|
|
import websocket
|
|
sub = json.dumps({"op": "subscribe", "topic": "/odom",
|
|
"type": "nav_msgs/Odometry", "throttle_rate": 500})
|
|
while not self._stop:
|
|
try:
|
|
ws = websocket.create_connection(self.cfg.rosbridge_url, timeout=5)
|
|
ws.send(sub)
|
|
while not self._stop:
|
|
msg = json.loads(ws.recv())
|
|
pos = (((msg.get("msg") or {}).get("pose") or {}).get("pose") or {}).get("position")
|
|
if pos:
|
|
with self._lock:
|
|
self._xy = {"x": round(float(pos["x"]), 3), "y": round(float(pos["y"]), 3)}
|
|
except Exception as e:
|
|
log.debug("rosbridge position reconnect: %s", e)
|
|
time.sleep(3)
|
|
|
|
def get(self) -> Optional[Dict[str, float]]:
|
|
with self._lock:
|
|
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: <stem>.yaml + <stem>.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"]
|
|
roots += list(cfg.extra_map_dirs) # Nav2/Pudu map dirs mounted via EXTRA_MAP_DIRS
|
|
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))
|
|
# Pudu/Nav2 converter emits a plain map + a keepout-BAKED twin (obstacles baked
|
|
# in for Foxy, which has no KeepoutFilter). The baked one is the deploy map — drop
|
|
# the redundant plain twin so the fleet server gets one canonical map, not two.
|
|
baked = {m.stem[: -len("_keepout_baked")] for m in out if m.stem.endswith("_keepout_baked")}
|
|
out = [m for m in out if m.stem not in baked]
|
|
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 "[<project>-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._exclude = None
|
|
if cfg.project_log_exclude:
|
|
try:
|
|
import re
|
|
self._exclude = re.compile(cfg.project_log_exclude)
|
|
except Exception:
|
|
self._exclude = None
|
|
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
|
|
self.label = label
|
|
size = p.stat().st_size
|
|
self._pos = size # new lines ship from here on
|
|
self._pending: List[str] = []
|
|
# backfill: the last N RELEVANT lines (post-filter) from the last ~1 MB,
|
|
# so the noise (access-log spam) doesn't eat the history window
|
|
if self._backfill and size:
|
|
try:
|
|
take = min(size, 8 * 1024 * 1024)
|
|
with p.open("rb") as f:
|
|
f.seek(size - take)
|
|
tail = f.read(take)
|
|
raw = tail.decode("utf-8", "replace").splitlines()
|
|
if take < size and raw:
|
|
raw = raw[1:] # first line is a partial record — drop it
|
|
for ln in raw:
|
|
ln = ln.strip()
|
|
if ln.startswith("{"):
|
|
try:
|
|
ln = (json.loads(ln).get("log") or "").rstrip()
|
|
except Exception:
|
|
pass
|
|
if not ln:
|
|
continue
|
|
if self._exclude is not None and self._exclude.search(ln):
|
|
continue
|
|
self._pending.append(f"[{label}] {ln}")
|
|
self._pending = self._pending[-self._backfill:]
|
|
except Exception:
|
|
self._pending = []
|
|
self.active = True
|
|
|
|
def poll(self) -> List[str]:
|
|
"""New lines since last poll (docker json-log unwrapped), labeled.
|
|
The startup backfill (last N relevant lines) is returned on first call."""
|
|
if not self.active or self._cur is None:
|
|
return []
|
|
pend, self._pending = self._pending, []
|
|
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 not ln:
|
|
continue
|
|
if self._exclude is not None and self._exclude.search(ln):
|
|
continue # access-log noise — not in the project's log panel
|
|
out.append(f"[{self.label}] {ln}")
|
|
except Exception as e:
|
|
log.debug("project-log poll failed: %s", e)
|
|
return pend + out[-100:] # backfill first, then ≤100 new/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
|
|
|
|
|
|
# --------------------------------------------------------------------------- #
|
|
# remote dashboard — discover the Sanad web UI and register its URL for the
|
|
# fleet dashboard to embed ("full dashboard through the fleet API"). No changes
|
|
# to the Sanad app: we only probe its port and POST the URL to /{sn}/remote.
|
|
# --------------------------------------------------------------------------- #
|
|
_REMOTE_STAT: Dict[str, Any] = {"url": None, "port": None, "kind": None, "ok": None,
|
|
"ssh": None, "ssh_ok": None}
|
|
|
|
|
|
def _primary_ip() -> str:
|
|
import socket
|
|
try:
|
|
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
|
s.connect(("1.1.1.1", 80))
|
|
ip = s.getsockname()[0]
|
|
s.close()
|
|
return ip
|
|
except Exception:
|
|
return "127.0.0.1"
|
|
|
|
|
|
def discover_dashboard(cfg: Config) -> Optional[Dict[str, Any]]:
|
|
"""Find the Sanad dashboard: probe each candidate port on localhost; the one
|
|
that returns 200 with a 'Sanad'/dashboard page wins. Returns {url, port}."""
|
|
if cfg.remote_url: # explicit URL (e.g. a tunnel) overrides
|
|
return {"url": cfg.remote_url, "port": None}
|
|
host = cfg.remote_host or _primary_ip() # the LAN IP the fleet can reach
|
|
for tok in cfg.remote_ports.split(","):
|
|
tok = tok.strip()
|
|
if not tok.isdigit():
|
|
continue
|
|
port = int(tok)
|
|
try:
|
|
r = requests.get(f"http://127.0.0.1:{port}/", timeout=2)
|
|
except requests.RequestException:
|
|
continue
|
|
if r.status_code == 200 and ("sanad" in r.text.lower() or "dashboard" in r.text.lower()):
|
|
return {"url": f"http://{host}:{port}", "port": port}
|
|
return None
|
|
|
|
|
|
def _post_remote(cfg: Config, session: requests.Session, body: Dict[str, Any]) -> bool:
|
|
try:
|
|
r = session.post(cfg.remote_url_ep(), json=body, headers=cfg.auth_headers(),
|
|
timeout=cfg.http_timeout, verify=cfg.verify_tls)
|
|
if not r.ok:
|
|
log.warning("remote(%s) register failed: HTTP %s %s",
|
|
body.get("kind"), r.status_code, r.text[:120])
|
|
return bool(r.ok)
|
|
except requests.RequestException as e:
|
|
log.debug("remote(%s) register failed: %s", body.get("kind"), e)
|
|
return False
|
|
|
|
|
|
def register_remote(cfg: Config, session: requests.Session) -> None:
|
|
if not cfg.remote_enable:
|
|
return
|
|
host = cfg.remote_host or _primary_ip()
|
|
# 1) the Sanad dashboard (kind=web) — the exact UI, for the fleet to embed
|
|
d = discover_dashboard(cfg)
|
|
if d:
|
|
ok = _post_remote(cfg, session, {
|
|
"sn": cfg.sn, "name": cfg.name, "kind": cfg.remote_kind,
|
|
"url": d["url"], "label": f"{cfg.name} — Sanad Dashboard",
|
|
"port": d["port"], "ts": int(time.time())})
|
|
_REMOTE_STAT.update(url=d["url"], port=d["port"], kind=cfg.remote_kind, ok=ok)
|
|
if ok:
|
|
log.info("remote dashboard registered: %s -> HTTP 200", d["url"])
|
|
else:
|
|
_REMOTE_STAT.update(url=None, port=None, ok=None)
|
|
# 2) SSH access (kind=ssh) — "ssh unitree@<ip>"
|
|
if cfg.ssh_enable:
|
|
cmd = f"ssh {cfg.ssh_user}@{host}"
|
|
if cfg.ssh_port != 22:
|
|
cmd += f" -p {cfg.ssh_port}"
|
|
ok = _post_remote(cfg, session, {
|
|
"sn": cfg.sn, "name": cfg.name, "kind": "ssh",
|
|
"url": f"ssh://{cfg.ssh_user}@{host}:{cfg.ssh_port}", # URL-valid form
|
|
"command": cmd, # "ssh unitree@<ip>"
|
|
"host": host, "port": cfg.ssh_port, "user": cfg.ssh_user,
|
|
"label": f"{cfg.name} — SSH", "ts": int(time.time())})
|
|
_REMOTE_STAT.update(ssh=cmd, ssh_ok=ok)
|
|
if ok:
|
|
log.info("remote SSH registered: %s -> HTTP 200", cmd)
|
|
|
|
|
|
def remote_loop(cfg: Config, session: requests.Session) -> None:
|
|
while True:
|
|
try:
|
|
register_remote(cfg, session)
|
|
except Exception as e:
|
|
log.debug("remote loop: %s", e)
|
|
time.sleep(cfg.remote_interval)
|
|
|
|
|
|
_ALERT_SEEN: set = set()
|
|
|
|
|
|
def _post_alert(cfg: Config, session: requests.Session, text: str) -> bool:
|
|
"""POST a single alert string to /{sn}/alert; updates the alert status."""
|
|
try:
|
|
r = session.post(cfg.alert_url(),
|
|
json={"sn": cfg.sn, "name": cfg.name,
|
|
"alert": text, "message": text, "ts": int(time.time())},
|
|
headers=cfg.auth_headers(),
|
|
timeout=cfg.http_timeout, verify=cfg.verify_tls)
|
|
_ALERTS_STAT.update(last=text, last_time=_now_str(), ok=bool(r.ok))
|
|
if r.ok:
|
|
_ALERTS_STAT["sent"] += 1
|
|
log.info("alert sent: %s -> HTTP %s", text[:120], r.status_code)
|
|
return bool(r.ok)
|
|
except requests.RequestException as e:
|
|
_ALERTS_STAT.update(last=text, last_time=_now_str(), ok=False)
|
|
log.debug("alert send failed (%s): %s", text[:60], e)
|
|
return False
|
|
|
|
|
|
def send_alerts(cfg: Config, session: requests.Session, faults: List[str]) -> None:
|
|
"""POST each NEW fault (rising edge) to /{sn}/alert. Faults are strings.
|
|
Includes LOW_BATTERY (<= LOW_SOC, default 50%), MOTOR_OVERTEMP, COMMS_STALE."""
|
|
global _ALERT_SEEN
|
|
current = set(faults)
|
|
new = current - _ALERT_SEEN
|
|
_ALERT_SEEN = current
|
|
for f in sorted(new):
|
|
_post_alert(cfg, session, f)
|
|
|
|
|
|
class LogAlertScanner:
|
|
"""Scans the robot's project logs (sanadr1) for error/billing patterns and
|
|
fires an alert on each NEW signature (deduped with a cooldown). Catches the
|
|
Gemini 'credits depleted' billing error and any ERROR/Traceback the app logs."""
|
|
|
|
def __init__(self, cfg: Config):
|
|
import re
|
|
self._path: Optional[Path] = None
|
|
self._pos = 0
|
|
self._cooldown = cfg.alert_log_cooldown
|
|
self._seen: Dict[str, float] = {}
|
|
self._patterns: List[Any] = []
|
|
self._pending: List[Any] = []
|
|
for entry in cfg.alert_log_patterns.split(";;"):
|
|
if "=" in entry:
|
|
code, rx = entry.split("=", 1)
|
|
try:
|
|
# case-SENSITIVE (uppercase log levels); use (?i) inline for text
|
|
self._patterns.append((code.strip(), re.compile(rx)))
|
|
except re.error as e:
|
|
log.warning("bad alert pattern %s: %s", code, e)
|
|
if _PROJECT_TAIL is not None and _PROJECT_TAIL.active and _PROJECT_TAIL._cur:
|
|
self._path = _PROJECT_TAIL._cur
|
|
try:
|
|
size = self._path.stat().st_size
|
|
self._pos = size # ongoing reads start at EOF
|
|
# STARTUP BACKFILL: scan a large tail once (access-logs bury real
|
|
# errors) so a currently-active error (e.g. Gemini billing) alerts
|
|
# right away. Only the matching lines are kept — cheap.
|
|
back = min(size, cfg.alert_backfill_bytes)
|
|
with self._path.open("rb") as f:
|
|
f.seek(size - back)
|
|
raw = f.read(back).decode("utf-8", "replace").splitlines()
|
|
if back < size and raw:
|
|
raw = raw[1:] # first line is partial
|
|
matches = []
|
|
for ln in raw:
|
|
ln = ln.strip()
|
|
if ln.startswith("{"):
|
|
try:
|
|
ln = (json.loads(ln).get("log") or "").rstrip()
|
|
except Exception:
|
|
pass
|
|
if not ln:
|
|
continue
|
|
for code, rx in self._patterns:
|
|
if rx.search(ln):
|
|
matches.append((code, ln))
|
|
break
|
|
# keep the most-recent unique signature per (code+line), cap 20
|
|
seen = set()
|
|
uniq = []
|
|
for code, ln in reversed(matches):
|
|
sig = code + ":" + re.sub(r"\d+", "#", ln)[:120]
|
|
if sig in seen:
|
|
continue
|
|
seen.add(sig)
|
|
uniq.append((code, ln))
|
|
self._pending = list(reversed(uniq))[-20:]
|
|
except Exception:
|
|
self._pos = 0
|
|
if self._path and self._patterns:
|
|
log.info("log-alert scan on %s (%d patterns, %d backfilled)",
|
|
self._path.name, len(self._patterns), len(self._pending))
|
|
|
|
def scan(self, cfg: Config, session: requests.Session) -> None:
|
|
if not self._path or not self._patterns:
|
|
return
|
|
import re
|
|
now0 = time.monotonic()
|
|
if self._pending: # flush startup backfill first
|
|
pend, self._pending = self._pending, []
|
|
for code, ln in pend:
|
|
sig = code + ":" + re.sub(r"\d+", "#", ln)[:120]
|
|
self._seen[sig] = now0
|
|
_post_alert(cfg, session, f"{code}: {ln[:220]}")
|
|
try:
|
|
st = self._path.stat()
|
|
if st.st_size < self._pos: # rotated
|
|
self._pos = 0
|
|
if st.st_size <= self._pos:
|
|
return
|
|
with self._path.open("rb") as f:
|
|
f.seek(self._pos)
|
|
chunk = f.read(min(st.st_size - self._pos, 512 * 1024))
|
|
self._pos = f.tell()
|
|
except Exception as e:
|
|
log.debug("alert scan read failed: %s", e)
|
|
return
|
|
now = time.monotonic()
|
|
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 not ln:
|
|
continue
|
|
for code, rx in self._patterns:
|
|
if rx.search(ln):
|
|
sig = code + ":" + re.sub(r"\d+", "#", ln)[:120] # dedup key
|
|
if now - self._seen.get(sig, -1e9) > self._cooldown:
|
|
self._seen[sig] = now
|
|
_post_alert(cfg, session, f"{code}: {ln[:220]}")
|
|
break
|
|
|
|
|
|
_LOG_ALERTS: Optional[LogAlertScanner] = None
|
|
|
|
|
|
def alert_scan_loop(cfg: Config, session: requests.Session) -> None:
|
|
while True:
|
|
time.sleep(cfg.alert_scan_interval)
|
|
try:
|
|
if _LOG_ALERTS is not None:
|
|
_LOG_ALERTS.scan(cfg, session)
|
|
except Exception as e:
|
|
log.debug("alert scan loop: %s", e)
|
|
|
|
|
|
# --------------------------------------------------------------------------- #
|
|
# telemetry assembly
|
|
# --------------------------------------------------------------------------- #
|
|
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")
|
|
if bms and bms.get("soc", 100) <= cfg.low_soc:
|
|
faults.append(f"LOW_BATTERY: battery {bms['soc']}% (warning)")
|
|
temps = snap.get("temps") or []
|
|
if temps and max(temps) >= cfg.motor_temp_max:
|
|
faults.append(f"MOTOR_OVERTEMP: motor temp {max(temps):.0f}C (warning)")
|
|
if snap.get("low_age") is not None and snap["low_age"] > 3.0:
|
|
faults.append(f"COMMS_STALE: no rt/lowstate for {snap['low_age']:.0f}s (critical)")
|
|
return faults
|
|
|
|
|
|
def derive_status(cfg: Config, snap: Dict[str, Any], fsm: Optional[int]) -> str:
|
|
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
|
|
if not alive and bms is None:
|
|
return "offline"
|
|
if charging:
|
|
return "charging"
|
|
if snap.get("max_dq", 0.0) > 0.15:
|
|
return "moving"
|
|
return base or "idle"
|
|
|
|
|
|
def build_telemetry(cfg: Config, mac: str, reader: Optional[DDSReader],
|
|
pos: Optional[RosbridgePosition],
|
|
sim: Optional[Dict[str, Any]] = None) -> Dict[str, Any]:
|
|
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},
|
|
"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, "xy": None}
|
|
fsm = reader.fsm_id() if (reader and cfg.read_fsm) else None
|
|
|
|
bms = snap.get("bms")
|
|
battery = bms["soc"] if bms else None
|
|
charging = bool(bms and bms.get("current_a", 0.0) > 0.05)
|
|
status = derive_status(cfg, snap, fsm)
|
|
faults = derive_faults(cfg, snap)
|
|
|
|
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")}
|
|
|
|
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 = snap.get("xy")
|
|
if position is None and pos is not None:
|
|
position = pos.get()
|
|
|
|
return {
|
|
"sn": cfg.sn,
|
|
"name": cfg.name, # friendly display name (e.g. go2_77)
|
|
"mac": mac,
|
|
"brand": cfg.brand,
|
|
"type": cfg.robot_type, # humanoid
|
|
"model": cfg.model, # g1
|
|
"software": read_software(cfg), # ros/os/kernel/arch/python/agent
|
|
"firmware": {**read_firmware_static(), **(snap.get("fw") or {})},
|
|
# board/l4t/kernel + live robot/bms fw versions
|
|
"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 odom/localization source
|
|
"control": read_control(cfg), # loco mode (zero_torque/damp/lock/running) + switchable set
|
|
"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
|
|
"remote": (dict(_REMOTE_STAT) if _REMOTE_STAT.get("url") else None),
|
|
# Sanad dashboard URL registered for the fleet UI
|
|
"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()),
|
|
}
|
|
|
|
|
|
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(),
|
|
timeout=cfg.http_timeout, verify=cfg.verify_tls)
|
|
except requests.RequestException as e:
|
|
log.error("telemetry POST failed (transport): %s", e)
|
|
return False
|
|
if not r.ok:
|
|
log.error("telemetry POST failed: HTTP %s %s", r.status_code, r.text[:200])
|
|
return False
|
|
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"]), 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": 200 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 main(argv: Optional[List[str]] = None) -> int:
|
|
ap = argparse.ArgumentParser(description="Go2 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("--map-only", action="store_true", help="upload discovered maps once (no DDS, no telemetry), 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")
|
|
# 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, _LOG_ALERTS
|
|
_PROJECT_TAIL = ProjectLogTail(cfg)
|
|
_LOG_ALERTS = LogAlertScanner(cfg) # error/billing alerts from the project logs
|
|
mac = read_mac(cfg.mac_interface)
|
|
log.info("sanad_api_go2 — 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 "")
|
|
|
|
session = requests.Session()
|
|
|
|
if args.map_only:
|
|
# map upload only — search all map roots (incl. EXTRA_MAP_DIRS) and ship,
|
|
# without opening DDS or posting telemetry (won't disturb a live feed).
|
|
map_sync_once(cfg, session, force=args.force, dry_run=args.dry_run)
|
|
return 0
|
|
|
|
reader = None
|
|
pos = None
|
|
if not args.simulate:
|
|
reader = DDSReader(cfg)
|
|
if cfg.position_source == "rosbridge":
|
|
pos = RosbridgePosition(cfg)
|
|
time.sleep(1.0)
|
|
|
|
tick = 0
|
|
|
|
def one_telemetry() -> None:
|
|
nonlocal tick
|
|
sim = _sim_state(tick) if args.simulate else None
|
|
payload = build_telemetry(cfg, mac, reader, pos, sim=sim)
|
|
if args.dry_run:
|
|
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:
|
|
# 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 cfg.remote_enable and not args.dry_run:
|
|
register_remote(cfg, session)
|
|
elif cfg.remote_enable:
|
|
d = discover_dashboard(cfg)
|
|
if d:
|
|
_REMOTE_STAT.update(url=d["url"], port=d["port"], kind=cfg.remote_kind, ok=None)
|
|
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 + log shipping + remote registration in bg threads
|
|
threading.Thread(target=map_loop, args=(cfg, session), daemon=True).start()
|
|
threading.Thread(target=logs_loop, args=(cfg, session), daemon=True).start()
|
|
threading.Thread(target=alert_scan_loop, args=(cfg, session), daemon=True).start()
|
|
if cfg.remote_enable:
|
|
threading.Thread(target=remote_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()
|
|
except Exception as e:
|
|
log.exception("telemetry tick failed: %s", e)
|
|
try:
|
|
time.sleep(cfg.poll_interval)
|
|
except KeyboardInterrupt:
|
|
log.info("stopped")
|
|
return 0
|
|
|
|
|
|
if __name__ == "__main__":
|
|
sys.exit(main())
|