#!/usr/bin/env python3 """Regenerate the r1 and go2 fleet agents from the canonical g1 agent. g1 (agents/g1/sanad_api_g1.py) is the SOURCE OF TRUTH for all shared features (telemetry, map sync, logs, alerts, project-logs, remote, control, software, firmware, timing). r1 is a near-exact subset of g1 (same unitree_hg DDS; only naming + FSM ids differ). go2 differs in the DDS family (unitree_go: battery is nested in LowState.bms_state), so its DDSReader is swapped wholesale. Run this after ANY change to g1 to keep the three agents consistent: python3 tools/gen_agents.py """ import re import sys from pathlib import Path ROOT = Path(__file__).resolve().parent.parent G1 = (ROOT / "agents/g1/sanad_api_g1.py").read_text() # ─────────────────────────── r1 (unitree_hg subset) ─────────────────────────── def make_r1(src: str) -> str: reps = [ ("sanad_api_g1", "sanad_api_r1"), ("— G1 fleet agent", "— R1 fleet agent"), ("The single G1 agent", "The single R1 agent"), ("Unitree G1, unitree_hg DDS", "Unitree R1 EDU, unitree_hg DDS — same family as the G1"), ('description="G1 fleet agent: telemetry + map sync"', 'description="R1 fleet agent: telemetry + map sync"'), ('_env("SN", "g1_0000")', '_env("SN", "r1_0000")'), ('"ROBOT_MODEL", "g1"', '"ROBOT_MODEL", "r1"'), ('"G1_READ_FSM"', '"R1_READ_FSM"'), ('_env("G1_POSITION_SOURCE", "odom")', '_env("R1_POSITION_SOURCE", "none")'), ('"fsm": 200 if moving else 4', '"fsm": 811 if moving else 4'), ("(G1 ids: 200 walk-ready / 4 stand / 2 squat / 702 lie2stand)", "(R1 ids: 0 zero-torque / 1 damping / 4 standing / 811 gait-running)"), ("e.g. g1_58", "e.g. r1_82"), ("DDS_INTERFACE (eth0)", "DDS_INTERFACE (eth10)"), # FSM + control-mode maps (R1 ids) ('# G1 FSM ids (differ from the R1\'s): 200 balance/walk-ready, 4 StandUp, 2 Squat, 702 Lie2Stand.\n' '_FSM_STATUS = {200: "ready", 4: "standing", 2: "squat", 702: "lie2stand"}\n' '# Control-panel mode labels + the switchable set (fsm_id -> friendly mode).\n' '_CONTROL_MODES = {200: "running", 4: "lock", 2: "squat", 702: "lie2stand", 0: "zero_torque", 1: "damp"}', '# R1 FSM ids (official R1 sport doc): 0 ZeroTorque, 1 Damping, 4 Locked-Standing, 811 Gait-Running.\n' '_FSM_STATUS = {0: "zero_torque", 1: "damping", 4: "locked_standing", 811: "running"}\n' '# Control-panel mode labels + the switchable set (fsm_id -> friendly mode).\n' '_CONTROL_MODES = {0: "zero_torque", 1: "damp", 4: "lock", 811: "running"}'), ] out = src for a, b in reps: if a not in out: sys.exit(f"[r1] anchor missing: {a!r}") out = out.replace(a, b) return out # ─────────────────────────── go2 (unitree_go DDS) ───────────────────────────── GO2_DDS_READER = '''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_CONTROL = ('# Go2 has no r1/g1 FSM-id scheme (SportClient modes). Placeholders — UNVERIFIED.\n' '_FSM_STATUS = {}\n' '# Control-panel mode labels + the switchable set.\n' '_CONTROL_MODES = {0: "idle", 1: "stand", 2: "walk"}\n' '_CONTROL_SWITCHABLE = ["damp", "stand", "walk"]') def make_go2(src: str) -> str: reps = [ ("sanad_api_g1", "sanad_api_go2"), ("— G1 fleet agent", "— Go2 fleet agent"), ("The single G1 agent", "The single Go2 agent"), ("Unitree G1, unitree_hg DDS", "Unitree Go2, unitree_go DDS (⚠ UNVERIFIED on hardware)"), ('description="G1 fleet agent: telemetry + map sync"', 'description="Go2 fleet agent: telemetry + map sync"'), ('_env("SN", "g1_0000")', '_env("SN", "go2_0000")'), ('"ROBOT_MODEL", "g1"', '"ROBOT_MODEL", "go2"'), ('_env("ROBOT_TYPE", "humanoid")', '_env("ROBOT_TYPE", "dog")'), ('"G1_READ_FSM"', '"GO2_READ_FSM"'), ('_env("G1_POSITION_SOURCE", "odom")', '_env("GO2_POSITION_SOURCE", "none")'), ("e.g. g1_58", "e.g. go2_77"), ("DDS_INTERFACE (eth0)", "DDS_INTERFACE (eth0)"), ] out = src for a, b in reps: if a not in out: sys.exit(f"[go2] anchor missing: {a!r}") out = out.replace(a, b) # swap DDSReader class (from 'class DDSReader:' up to the FSM/control block) start = out.index("class DDSReader:") ctrl_start = out.index('# G1 FSM ids (differ from the R1', start) out = out[:start] + GO2_DDS_READER + out[ctrl_start:] # swap the FSM/control-modes block ctrl_a = out.index('# G1 FSM ids (differ from the R1') ctrl_b = out.index('_CONTROL_SWITCHABLE = ["zero_torque", "damp", "lock", "running"]') ctrl_b = out.index("\n", ctrl_b) out = out[:ctrl_a] + GO2_CONTROL + out[ctrl_b:] return out def main() -> int: r1 = make_r1(G1) go2 = make_go2(G1) (ROOT / "agents/r1/sanad_api_r1.py").write_text(r1) (ROOT / "agents/go2/sanad_api_go2.py").write_text(go2) print(f"generated r1 ({len(r1.splitlines())} lines)") print(f"generated go2 ({len(go2.splitlines())} lines)") return 0 if __name__ == "__main__": sys.exit(main())