agenticros 0.1.18 → 0.1.19

package/runtime/packages/core/src/robots.ts

@@ -0,0 +1,282 @@
+/**
+ * Multi-robot support — Phase 1.d of the AgenticROS strategy.
+ *
+ * For most deployments today the gateway talks to a single robot, so
+ * config.robot (a single object) is all that's used. Phase 1.d adds a
+ * `robots` array to the config so a single gateway / chat session can
+ * address multiple robots by id.
+ *
+ * This module is the source of truth for that resolution: from the raw
+ * AgenticROSConfig, what robots does the agent see, and which one is the
+ * active default? Adapters call into it from:
+ *   - `ros2_list_robots` (this phase) — list everything the gateway knows about.
+ *   - `ros2_list_capabilities(robot_id?)` (next iteration) — scope to one robot.
+ *   - `run_mission` (next iteration) — `mission.robot_id` field.
+ *   - per-tool `robot_id` (next iteration) — `ros2_publish`, `ros2_subscribe_once`,
+ *     `ros2_camera_snapshot`, etc.
+ *
+ * Backwards compatibility: when `robots` is empty (i.e. nothing in
+ * config.robots), this module synthesises a one-entry list from the
+ * legacy `config.robot` object so old configs and the single-robot
+ * mental model keep working unchanged. The synthesised entry is marked
+ * default so it's selected by `getActiveRobotId()`.
+ */
+
+import type { AgenticROSConfig } from "./config.js";
+import { getTransportConfig } from "./config.js";
+import type { TransportConfig } from "./transport/types.js";
+
+/** Sensor/hardware tags on a robot (Phase 1.e). */
+export interface RobotSensors {
+  /** Intel RealSense depth+RGB camera. */
+  has_realsense: boolean;
+  /** 2D or 3D LiDAR sensor. */
+  has_lidar: boolean;
+  /** Robotic manipulator (arm) attached. */
+  has_arm: boolean;
+}
+
+/** A resolved robot — what the agent + adapters operate on. */
+export interface ResolvedRobot {
+  /** Stable, human-readable identifier. Falls back to namespace when not set. */
+  id: string;
+  /** Display name used by the agent in chat replies. */
+  name: string;
+  /** ROS2 topic namespace prefix (e.g. "robot3946b404..." → /robot3946.../cmd_vel). */
+  namespace: string;
+  /** Default camera topic for `ros2_camera_snapshot`. Empty string when unset. */
+  cameraTopic: string;
+  /**
+   * Phase 1.e robot kind ("amr" | "arm" | "drone" | "rover" | …).
+   * Free-form string. Defaults to "amr" for back-compat (the legacy
+   * fallback synthesises this too).
+   */
+  kind: string;
+  /** Phase 1.e sensor/hardware tags. Defaults to all-false. */
+  sensors: RobotSensors;
+  /**
+   * Phase 1.e optional per-robot capability allowlist. When `undefined`
+   * the robot inherits the global capability registry (the common
+   * case); when set it's the exact list `ros2_find_robots_for` will
+   * filter against for this robot.
+   */
+  capabilities?: string[];
+  /**
+   * Source tag — handy for diagnostics ("Why is this robot in the list?").
+   *   - "config":  from config.robots[]
+   *   - "legacy":  synthesised from the legacy single config.robot object
+   */
+  source: "config" | "legacy";
+}
+
+/**
+ * Read every robot the config knows about, in declaration order, with
+ * the legacy single-robot fallback applied when config.robots is empty.
+ *
+ * Never throws — an empty list is returned when neither config.robots
+ * nor config.robot.namespace is meaningfully set (and the synthesised
+ * fallback uses "default" as the id in that degenerate case).
+ */
+/** All-false default for {@link RobotSensors}. */
+const DEFAULT_SENSORS: RobotSensors = {
+  has_realsense: false,
+  has_lidar: false,
+  has_arm: false,
+};
+
+export function listRobots(config: AgenticROSConfig): ResolvedRobot[] {
+  const explicit = Array.isArray(config.robots) ? config.robots : [];
+  if (explicit.length > 0) {
+    return explicit.map((r) => ({
+      id: String(r.id),
+      name: r.name ?? "Robot",
+      namespace: r.namespace ?? "",
+      cameraTopic: r.cameraTopic ?? "",
+      kind: r.kind ?? "amr",
+      sensors: { ...DEFAULT_SENSORS, ...(r.sensors ?? {}) },
+      capabilities: r.capabilities,
+      source: "config" as const,
+    }));
+  }
+
+  // Legacy fallback — synthesise one entry from config.robot. The
+  // legacy schema doesn't carry kind/sensors/capabilities, so we
+  // default them: kind="amr" (every existing real-robot deployment we
+  // ship is an AMR with RealSense) and sensors all-false (which is
+  // overly conservative but won't surface a false positive in
+  // ros2_find_robots_for). Users on multi-robot deployments will have
+  // promoted into config.robots[] anyway, where the fields are
+  // explicit.
+  const legacy = config.robot ?? { name: "Robot", namespace: "", cameraTopic: "" };
+  const id = (legacy.namespace?.trim() || "default");
+  return [
+    {
+      id,
+      name: legacy.name ?? "Robot",
+      namespace: legacy.namespace ?? "",
+      cameraTopic: legacy.cameraTopic ?? "",
+      kind: "amr",
+      sensors: { ...DEFAULT_SENSORS },
+      source: "legacy",
+    },
+  ];
+}
+
+/**
+ * Pick the active robot's id according to these rules, in order:
+ *   1. `override` argument (when truthy) — usually a per-tool-call robot_id.
+ *   2. An entry in config.robots with `default: true`.
+ *   3. The first entry in the resolved list (which includes the legacy
+ *      fallback when nothing else is configured).
+ *
+ * Throws when no robots exist at all — that should be impossible because
+ * the legacy fallback always produces at least one — but the guard is
+ * there for robustness against a corrupted config.
+ */
+export function getActiveRobotId(config: AgenticROSConfig, override?: string): string {
+  if (override && override.trim().length > 0) return override.trim();
+  const explicit = Array.isArray(config.robots) ? config.robots : [];
+  const flagged = explicit.find((r) => r.default === true);
+  if (flagged) return flagged.id;
+  const robots = listRobots(config);
+  if (robots.length === 0) {
+    throw new Error("No robots configured (config.robots is empty and config.robot has no namespace).");
+  }
+  return robots[0].id;
+}
+
+/**
+ * Convenience wrapper for tool handlers: extract an optional `robot_id`
+ * (a string) from a tool-args record and call `resolveRobot`. Returns
+ * the active robot when `robot_id` is missing or not a string.
+ *
+ * Throws — with the known robot ids listed in the error — when
+ * `robot_id` is set to an unknown value. Adapters surface that as a
+ * tool error so the agent self-corrects via `ros2_list_robots`.
+ */
+export function resolveRobotFromArgs(
+  config: AgenticROSConfig,
+  args: Record<string, unknown>,
+): ResolvedRobot {
+  const raw = args["robot_id"];
+  const robotId = typeof raw === "string" ? raw : undefined;
+  return resolveRobot(config, robotId);
+}
+
+/**
+ * Resolve a robot to its full record by id. When `robotId` is omitted or
+ * empty, the active robot is returned. When `robotId` is provided but
+ * doesn't match any configured robot, throws with the available ids in
+ * the message so the agent can correct itself.
+ */
+export function resolveRobot(config: AgenticROSConfig, robotId?: string): ResolvedRobot {
+  const robots = listRobots(config);
+  if (!robotId || robotId.trim().length === 0) {
+    const activeId = getActiveRobotId(config);
+    const found = robots.find((r) => r.id === activeId);
+    if (!found) {
+      throw new Error(`Active robot id "${activeId}" is not in the resolved list. This is a bug.`);
+    }
+    return found;
+  }
+  const trimmed = robotId.trim();
+  const found = robots.find((r) => r.id === trimmed);
+  if (!found) {
+    const known = robots.map((r) => r.id).join(", ");
+    throw new Error(`Unknown robot_id "${trimmed}". Known ids: ${known || "(none)"}. Use ros2_list_robots to discover.`);
+  }
+  return found;
+}
+
+/**
+ * Phase 1.d-resolve helper: compute the effective `TransportConfig` for
+ * a given robot, honouring an optional per-robot override in
+ * `config.robots[i].transport`.
+ *
+ * Precedence (most specific wins):
+ *   1. The robot's `transport.<mode>` sub-section (e.g. the override's
+ *      `zenoh: { routerEndpoint: ... }`).
+ *   2. The top-level `config.<mode>` section (e.g. the global
+ *      `config.zenoh`).
+ *
+ * When the robot has no `transport` override at all, this returns the
+ * global transport from `getTransportConfig(config)` — so single-robot
+ * deployments behave exactly as before.
+ *
+ * Why this lives here (and not next to `createTransport`):
+ *   - The resolver owns "which robot are we talking about?"; the
+ *     transport factory owns "given a TransportConfig, build an
+ *     instance". Mixing them couples a multi-robot decision into the
+ *     low-level transport layer, which is wrong: adapters that don't
+ *     care about per-robot transports keep using `createTransport` and
+ *     never see this helper.
+ *   - Callers that DO want per-robot pools call this to materialise the
+ *     right `TransportConfig` per id, then hand it to `createTransport`.
+ *
+ * Throws when `robotId` is given but doesn't match any configured robot
+ * (delegated to `resolveRobot`'s error path).
+ */
+export function getTransportConfigForRobot(
+  config: AgenticROSConfig,
+  robotId?: string,
+): TransportConfig {
+  // No explicit robots[] AND no override possible — fast path.
+  const explicit = Array.isArray(config.robots) ? config.robots : [];
+  if (explicit.length === 0) return getTransportConfig(config);
+
+  // Resolve the id to the *raw* entry (not the synthesised ResolvedRobot,
+  // because that strips the override). `resolveRobot` validates the id
+  // and throws on unknown — we reuse it for that side effect.
+  const robot = resolveRobot(config, robotId);
+  const raw = explicit.find((r) => r.id === robot.id);
+  if (!raw || !raw.transport) return getTransportConfig(config);
+
+  // Build a synthetic config whose top-level transport mirrors the
+  // override, then run it through the standard `getTransportConfig`
+  // path. This keeps the "what shape does TransportConfig take?" logic
+  // in one place — if a new transport mode is added, only
+  // getTransportConfig needs to change.
+  const override = raw.transport;
+  const synthetic: AgenticROSConfig = {
+    ...config,
+    transport: { mode: override.mode },
+    rosbridge:
+      override.mode === "rosbridge" && override.rosbridge
+        ? { ...config.rosbridge, ...override.rosbridge }
+        : config.rosbridge,
+    local:
+      override.mode === "local" && override.local
+        ? { ...config.local, ...override.local }
+        : config.local,
+    zenoh:
+      override.mode === "zenoh" && override.zenoh
+        ? { ...config.zenoh, ...override.zenoh }
+        : config.zenoh,
+    webrtc:
+      override.mode === "webrtc" && override.webrtc
+        ? { ...config.webrtc, ...override.webrtc }
+        : config.webrtc,
+  };
+  return getTransportConfig(synthetic);
+}
+
+/**
+ * True when the named robot (or active robot when `robotId` is omitted)
+ * has a per-robot transport override. Adapters use this as a cheap
+ * branch to decide between "reuse the global transport" (fast path) and
+ * "build a per-robot one via `getTransportConfigForRobot`" (pool path).
+ */
+export function hasRobotTransportOverride(
+  config: AgenticROSConfig,
+  robotId?: string,
+): boolean {
+  const explicit = Array.isArray(config.robots) ? config.robots : [];
+  if (explicit.length === 0) return false;
+  try {
+    const robot = resolveRobot(config, robotId);
+    const raw = explicit.find((r) => r.id === robot.id);
+    return Boolean(raw?.transport);
+  } catch {
+    return false;
+  }
+}

package/runtime/packages/core/src/topic-utils.ts

@@ -1,5 +1,20 @@
 import type { AgenticROSConfig } from "./config.js";
 
+/**
+ * Topic-utility module — the single source of truth for namespace
+ * prefixing across all adapters and the mission runner.
+ *
+ * Phase 1.d (multi-robot) extends every public helper to accept *either*
+ * the full AgenticROSConfig (uses config.robot.namespace, the legacy
+ * single-robot behaviour) *or* a bare namespace string. The string-arg
+ * form is what per-tool `robot_id` routing goes through: callers resolve
+ * a `ResolvedRobot` via `resolveRobot(config, robot_id)` and pass
+ * `robot.namespace` directly.
+ *
+ * Every existing call site continues to work unchanged because the
+ * config-arg form is preserved verbatim.
+ */
+
 /**
  * Normalize a ROS 2 topic name to a canonical form (leading slash, no trailing slash).
  */
@@ -16,18 +31,33 @@ function isRootLevelTopic(normalized: string): boolean {
   return withoutLeading.length > 0 && !withoutLeading.includes("/");
 }
 
+/**
+ * Resolve a namespace from either a string or an AgenticROSConfig. The
+ * config form pulls from config.robot.namespace (legacy single-robot
+ * behaviour); the string form is taken verbatim. Empty/whitespace
+ * collapses to "" so the rest of the prefix logic short-circuits.
+ */
+function resolveNamespace(target: AgenticROSConfig | string): string {
+  if (typeof target === "string") return target.trim();
+  return (target.robot?.namespace ?? "").trim();
+}
+
 /**
  * Apply robot namespace to a topic (or service/action name) when configured.
- * If config.robot.namespace is set and the name is root-level (e.g. cmd_vel, battery_state),
+ * If the namespace is set and the name is root-level (e.g. cmd_vel, battery_state),
  * returns /<namespace>/<name>. Otherwise returns the normalized name as-is.
  *
+ * The first argument may be either an `AgenticROSConfig` (legacy single-
+ * robot path) or a bare namespace string (used by per-tool `robot_id`
+ * routing — pass `resolveRobot(config, robot_id).namespace`).
+ *
  * Example: namespace "robot-uuid", topic "/cmd_vel" -> "/robot-uuid/cmd_vel"
  * Example: namespace "", topic "/cmd_vel" -> "/cmd_vel"
  * Example: namespace "robot-uuid", topic "/robot-uuid/odom" -> "/robot-uuid/odom" (unchanged)
  */
-export function toNamespacedTopic(config: AgenticROSConfig, topic: string): string {
+export function toNamespacedTopic(target: AgenticROSConfig | string, topic: string): string {
   const normalized = normalizeTopic(topic);
-  const ns = (config.robot?.namespace ?? "").trim();
+  const ns = resolveNamespace(target);
   if (!ns) return normalized;
   if (!isRootLevelTopic(normalized)) return normalized;
   const segment = normalized.replace(/^\/+/, "");
@@ -37,16 +67,19 @@ export function toNamespacedTopic(config: AgenticROSConfig, topic: string): stri
 /**
  * Apply robot namespace to any topic when configured (for transport subscribe/publish).
  * Use this when the robot publishes/subscribes all topics under a namespace (e.g. Zenoh with
- * zenoh-bridge-ros2dds or rmw_zenoh). If config.robot.namespace is set, returns /<namespace>/<topic>
+ * zenoh-bridge-ros2dds or rmw_zenoh). If a namespace is set, returns /<namespace>/<topic>
  * unless the topic already starts with /<namespace>/.
  *
+ * The first argument may be either an `AgenticROSConfig` or a bare
+ * namespace string (per-robot routing).
+ *
  * Example: namespace "robot-uuid", topic "/cmd_vel" -> "/robot-uuid/cmd_vel"
  * Example: namespace "robot-uuid", topic "/camera/camera/color/image_raw/compressed" -> "/robot-uuid/camera/camera/color/image_raw/compressed"
  * Example: namespace "robot-uuid", topic "/robot-uuid/odom" -> "/robot-uuid/odom" (unchanged)
  */
-export function toNamespacedTopicFull(config: AgenticROSConfig, topic: string): string {
+export function toNamespacedTopicFull(target: AgenticROSConfig | string, topic: string): string {
   const normalized = normalizeTopic(topic);
-  const ns = (config.robot?.namespace ?? "").trim();
+  const ns = resolveNamespace(target);
   if (!ns) return normalized;
   const withoutLeading = normalized.replace(/^\/+/, "");
   if (!withoutLeading) return normalized;
@@ -57,6 +90,8 @@ export function toNamespacedTopicFull(config: AgenticROSConfig, topic: string):
 /**
  * Canonical topic string for teleop UI and ?topic= query params: leading slash, no robot namespace prefix.
  * Subscribe/publish still uses {@link toNamespacedTopicFull} on the server so Zenoh keys match the bridge.
+ *
+ * Teleop is single-robot today, so this still takes the full config.
  */
 export function toTeleopCameraTopicShort(config: AgenticROSConfig, topic: string): string {
   const normalized = normalizeTopic(topic);
@@ -75,12 +110,15 @@ export function toTeleopCameraTopicShort(config: AgenticROSConfig, topic: string
 /**
  * ROS topic to use when subscribing to camera streams (Zenoh / DDS).
  * Unlike {@link toNamespacedTopicFull}, common sensor topics stay at the graph root (`/camera/...`, `/zed/...`)
- * even when `robot.namespace` is set for cmd_vel. If the topic already starts with `/<namespace>/`, it is left as-is.
+ * even when the namespace is set for cmd_vel. If the topic already starts with `/<namespace>/`, it is left as-is.
  * Other multi-segment paths get the namespace prefix (same as Full) for odd layouts.
+ *
+ * The first argument may be either an `AgenticROSConfig` or a bare
+ * namespace string (per-robot routing).
  */
-export function resolveCameraSubscribeTopic(config: AgenticROSConfig, topic: string): string {
+export function resolveCameraSubscribeTopic(target: AgenticROSConfig | string, topic: string): string {
   const normalized = normalizeTopic(topic);
-  const ns = (config.robot?.namespace ?? "").trim();
+  const ns = resolveNamespace(target);
   if (!ns) return normalized;
   const withoutLeading = normalized.replace(/^\/+/, "");
   if (!withoutLeading) return normalized;

package/runtime/packages/core/src/transport-pool.ts

@@ -0,0 +1,171 @@
+/**
+ * `TransportPool` — Phase 1.d shared multi-robot transport pool.
+ *
+ * Lives in `@agenticros/core` so every adapter (Claude Code, Gemini,
+ * future ones) can route per-robot transports through the same battle-
+ * tested implementation. OpenClaw's plugin uses a thinner add-on
+ * because its existing service has long-lived eager-connect / poll
+ * semantics that don't compose cleanly with a generic pool.
+ *
+ * Semantics:
+ *
+ *   - Pool key is `__global__` when the robot has no per-robot
+ *     `transport` override in config — every robot then shares one
+ *     connection (the legacy / single-transport behaviour). Opting
+ *     into `config.robots[]` is NOT a breaking change for that path.
+ *
+ *   - Pool key is `robot.id` when the robot DOES declare an override.
+ *     A distinct transport is materialised on first use and cached
+ *     for subsequent tool calls.
+ *
+ *   - Acquires are lazy — the constructor never opens a connection.
+ *     The first call to `acquire()` for a given key triggers the
+ *     create + connect dance, gated by `connectWithTimeout()`.
+ *
+ *   - Concurrent first-acquires on the same key share one in-flight
+ *     connect promise — important when an MCP server gets two
+ *     near-simultaneous tool calls right after startup.
+ *
+ *   - Cached entries that drop to a non-`connected` state are
+ *     rebuilt on the next acquire (self-heal — matches the pre-pool
+ *     `connect()` behaviour).
+ *
+ *   - `disconnectAll()` drains every entry and is safe to call on
+ *     SIGINT/SIGTERM. Errors on individual disconnects are swallowed
+ *     so one misbehaving transport can't block the others from
+ *     cleaning up.
+ *
+ * The default factory is `createTransport` from `./transport/factory`.
+ * Adapter shutdowns / tests can inject a fake factory by passing one
+ * to the constructor — see packages/core/src/__tests__/transport-pool.test.ts
+ * for the unit-test seam.
+ */
+
+import type {
+  AgenticROSConfig,
+  ResolvedRobot,
+  RosTransport,
+  TransportConfig,
+} from "./index.js";
+import {
+  createTransport,
+  getTransportConfig,
+  getTransportConfigForRobot,
+  hasRobotTransportOverride,
+  resolveRobot,
+} from "./index.js";
+
+const CONNECT_TIMEOUT_MS = 15_000;
+export const TRANSPORT_POOL_GLOBAL_KEY = "__global__";
+
+export type TransportFactory = (cfg: TransportConfig) => Promise<RosTransport>;
+
+export class TransportPool {
+  private readonly entries = new Map<string, RosTransport>();
+  /** Promises kept while a connection is in flight, so concurrent acquires share the same connect. */
+  private readonly inFlight = new Map<string, Promise<RosTransport>>();
+
+  constructor(private readonly factory: TransportFactory = createTransport) {}
+
+  /**
+   * Acquire (lazy-connect on first call) the transport for this robot.
+   * See the module docstring for the full key + caching contract.
+   */
+  async acquire(config: AgenticROSConfig, robot: ResolvedRobot): Promise<RosTransport> {
+    const key = hasRobotTransportOverride(config, robot.id)
+      ? robot.id
+      : TRANSPORT_POOL_GLOBAL_KEY;
+    return this.acquireByKey(key, () =>
+      key === TRANSPORT_POOL_GLOBAL_KEY
+        ? getTransportConfig(config)
+        : getTransportConfigForRobot(config, robot.id),
+    );
+  }
+
+  /**
+   * Pre-warm the active robot's transport at server start — used by
+   * adapters that want to pay the connect-latency tax upfront instead
+   * of on the first tool call.
+   */
+  async connectActive(config: AgenticROSConfig): Promise<void> {
+    const robot = resolveRobot(config);
+    await this.acquire(config, robot);
+  }
+
+  /**
+   * Disconnect every transport in the pool. Safe to call multiple
+   * times. Errors on individual disconnects are swallowed.
+   */
+  async disconnectAll(): Promise<void> {
+    const pending: Promise<unknown>[] = [];
+    for (const t of this.entries.values()) {
+      pending.push(
+        t.disconnect().catch(() => {
+          /* best-effort drain */
+        }),
+      );
+    }
+    this.entries.clear();
+    this.inFlight.clear();
+    await Promise.all(pending);
+  }
+
+  /** Visible for tests: how many distinct transports are alive right now. */
+  get size(): number {
+    return this.entries.size;
+  }
+
+  /** Visible for tests: list of pool keys currently cached. */
+  keys(): string[] {
+    return Array.from(this.entries.keys());
+  }
+
+  private async acquireByKey(
+    key: string,
+    resolveCfg: () => TransportConfig,
+  ): Promise<RosTransport> {
+    const cached = this.entries.get(key);
+    if (cached && cached.getStatus() === "connected") return cached;
+
+    // A second concurrent acquire on the same key joins the in-flight
+    // connect rather than starting a duplicate.
+    const flight = this.inFlight.get(key);
+    if (flight) return flight;
+
+    if (cached) {
+      // Stale entry (e.g. router restarted under us). Drop it cleanly.
+      this.entries.delete(key);
+      cached.disconnect().catch(() => {});
+    }
+
+    const connectFlight = (async (): Promise<RosTransport> => {
+      const cfg = resolveCfg();
+      const t = await this.factory(cfg);
+      await connectWithTimeout(t);
+      this.entries.set(key, t);
+      return t;
+    })();
+    this.inFlight.set(key, connectFlight);
+    try {
+      return await connectFlight;
+    } finally {
+      this.inFlight.delete(key);
+    }
+  }
+}
+
+async function connectWithTimeout(t: RosTransport): Promise<void> {
+  const timeout = new Promise<never>((_, reject) => {
+    setTimeout(
+      () =>
+        reject(
+          new Error(
+            `Transport connection timed out after ${CONNECT_TIMEOUT_MS / 1000}s. ` +
+              "Is zenohd running? (e.g. ws://localhost:10000). Check config and adapter logs.",
+          ),
+        ),
+      CONNECT_TIMEOUT_MS,
+    );
+  });
+  await Promise.race([t.connect(), timeout]);
+}