agenticros 0.3.4 → 0.4.0

package/runtime/packages/agenticros-claude-code/src/tools.ts

@@ -7,7 +7,6 @@ import type {
   Capability,
   Mission,
   MissionToolDispatcher,
-  CapabilityToolBindings,
   ResolvedRobot,
 } from "@agenticros/core";
 import {
@@ -15,6 +14,7 @@ import {
   toNamespacedTopic,
   toNamespacedTopicFull,
   listAllCapabilities,
+  listCapabilitiesWithDiscoverable,
   runMission,
   listRobots,
   getActiveRobotId,
@@ -26,6 +26,10 @@ import {
   createMemoryTranscriptSink,
   missionTranscriptNamespace,
   compileGoalToMission,
+  buildMissionBindings,
+  isExternalToolName,
+  capabilityIdFromExternalTool,
+  executeExternalCapability,
 } from "@agenticros/core";
 import { getMissionRegistry } from "./mission-registry.js";
 import {
@@ -73,9 +77,12 @@ export const NO_TRANSPORT_TOOL_NAMES = new Set<string>([
   // tool falls back to a transport-driven discovery only when the caller
   // passes online=true. See the handler for the precise branch.
   "ros2_find_robots_for",
-  // mission_cancel only mutates the in-process MissionRegistry — it
-  // never publishes to ROS, so it must work even when zenohd is down.
+  // mission_cancel / mission_pause / mission_resume only mutate the
+  // in-process MissionRegistry — they never publish to ROS, so they
+  // must work even when zenohd is down.
   "mission_cancel",
+  "mission_pause",
+  "mission_resume",
   // run_mission's *outer* call doesn't need transport — its handler
   // first compiles the goal (Phase 1.g, no transport) or validates
   // the mission shape, then per-step calls go through the transport
@@ -464,6 +471,40 @@ export const TOOLS: McpTool[] = [
       required: ["mission_id"],
     },
   },
+  {
+    name: "mission_pause",
+    description:
+      "Pause a mission that's currently running in this MCP server. Pass the mission_id returned by run_mission. The runner waits at the next step boundary until mission_resume (or mission_cancel). Idempotent if already paused.",
+    inputSchema: {
+      type: "object",
+      properties: {
+        mission_id: {
+          type: "string",
+          description: "The mission_id echoed by run_mission. Required.",
+        },
+        reason: {
+          type: "string",
+          description: "Optional free-text reason — surfaced in the paused transcript entry.",
+        },
+      },
+      required: ["mission_id"],
+    },
+  },
+  {
+    name: "mission_resume",
+    description:
+      "Resume a mission previously paused with mission_pause. Pass the mission_id returned by run_mission. Idempotent if the mission is not paused.",
+    inputSchema: {
+      type: "object",
+      properties: {
+        mission_id: {
+          type: "string",
+          description: "The mission_id echoed by run_mission. Required.",
+        },
+      },
+      required: ["mission_id"],
+    },
+  },
 ];
 
 export type ToolContent = { type: "text"; text: string } | { type: "image"; data: string; mimeType: string };
@@ -509,16 +550,18 @@ async function publishFollowMeCmd(
  * Format the capability list for tool responses — shared by claude-code,
  * OpenClaw, and Gemini so every adapter returns the same JSON shape.
  */
-function formatCapabilitiesResponse(caps: Capability[]): string {
-  const byKind = {
-    intrinsic: caps.filter((c) => c.source?.kind === "builtin").length,
-    skill: caps.filter((c) => c.source?.kind === "skill").length,
-  };
+function formatCapabilitiesResponse(
+  caps: Array<Capability & { discoverable?: boolean; installed?: boolean; install_ref?: string }>,
+): string {
+  const intrinsic = caps.filter((c) => c.source?.kind === "builtin").length;
+  const skill = caps.filter((c) => c.installed !== false && c.source?.kind === "skill").length;
+  const discoverable = caps.filter((c) => c.discoverable === true).length;
   return JSON.stringify({
     success: true,
     total: caps.length,
-    intrinsic_count: byKind.intrinsic,
-    skill_count: byKind.skill,
+    intrinsic_count: intrinsic,
+    skill_count: skill,
+    discoverable_count: discoverable,
     capabilities: caps,
   });
 }
@@ -573,95 +616,6 @@ function formatFindRobotsForResponse(result: FindRobotsForResult): string {
   });
 }
 
-/**
- * Capability → MCP tool dispatch table used by `run_mission`.
- *
- * Every capability returned by `ros2_list_capabilities` that the mission
- * runner can actually execute must appear here. If you add a new
- * intrinsic capability to BUILTIN_CAPABILITIES, mirror it here. For new
- * skill-declared capabilities, this is where you map them to the
- * corresponding MCP tool. Steps that name a capability without an entry
- * fail with an actionable "no mission-runner tool binding" error.
- */
-const MISSION_BINDINGS: CapabilityToolBindings = {
-  drive_base: {
-    tool: "ros2_publish",
-    buildArgs: (inputs) => {
-      const lx = Number(inputs.linear_x ?? 0) || 0;
-      const az = Number(inputs.angular_z ?? 0) || 0;
-      return {
-        topic: "/cmd_vel",
-        type: "geometry_msgs/msg/Twist",
-        message: {
-          linear: { x: lx, y: 0, z: 0 },
-          angular: { x: 0, y: 0, z: az },
-        },
-      };
-    },
-  },
-  take_snapshot: {
-    tool: "ros2_camera_snapshot",
-    buildArgs: (inputs) => {
-      const out: Record<string, unknown> = {};
-      if (typeof inputs.topic === "string") out.topic = inputs.topic;
-      if (typeof inputs.message_type === "string") out.message_type = inputs.message_type;
-      if (typeof inputs.timeout === "number") out.timeout = inputs.timeout;
-      return out;
-    },
-  },
-  measure_depth: {
-    tool: "ros2_depth_distance",
-    buildArgs: (inputs) => {
-      const out: Record<string, unknown> = {};
-      if (typeof inputs.topic === "string") out.topic = inputs.topic;
-      if (typeof inputs.timeout === "number") out.timeout = inputs.timeout;
-      return out;
-    },
-  },
-  list_topics: {
-    tool: "ros2_list_topics",
-    buildArgs: () => ({}),
-  },
-  publish_topic: {
-    tool: "ros2_publish",
-    buildArgs: (inputs) => ({
-      topic: String(inputs.topic ?? ""),
-      type: String(inputs.type ?? inputs.msg_type ?? ""),
-      message: inputs.message ?? inputs.msg ?? {},
-    }),
-  },
-  subscribe_once: {
-    tool: "ros2_subscribe_once",
-    buildArgs: (inputs) => {
-      const out: Record<string, unknown> = { topic: String(inputs.topic ?? "") };
-      if (typeof inputs.type === "string") out.type = inputs.type;
-      if (typeof inputs.timeout === "number") out.timeout = inputs.timeout;
-      return out;
-    },
-  },
-  follow_person: {
-    tool: "ros2_follow_me_start",
-    buildArgs: (inputs) => {
-      const out: Record<string, unknown> = {};
-      if (typeof inputs.target_distance === "number") out.target_distance = inputs.target_distance;
-      if (typeof inputs.mode === "string") out.mode = inputs.mode;
-      return out;
-    },
-  },
-  find_object: {
-    tool: "ros2_find_object",
-    buildArgs: (inputs) => {
-      const target = String(inputs.target ?? "");
-      const out: Record<string, unknown> = { target };
-      if (typeof inputs.angular_speed === "number") out.angular_speed = inputs.angular_speed;
-      if (typeof inputs.clockwise === "boolean") out.clockwise = inputs.clockwise;
-      if (typeof inputs.timeout_seconds === "number") out.timeout_seconds = inputs.timeout_seconds;
-      if (typeof inputs.min_confidence === "number") out.min_confidence = inputs.min_confidence;
-      return out;
-    },
-  },
-};
-
 /**
  * Phase 1.f — `mission_cancel` handler. Lives at module scope so the
  * outer `handleToolCall` can invoke it WITHOUT first resolving a
@@ -701,6 +655,70 @@ function handleMissionCancel(
   };
 }
 
+function handleMissionPause(
+  args: Record<string, unknown>,
+): { content: ToolContent[]; isError?: boolean } {
+  const missionId = typeof args["mission_id"] === "string" ? args["mission_id"].trim() : "";
+  if (!missionId) {
+    return {
+      content: [
+        {
+          type: "text",
+          text: "mission_pause requires 'mission_id' (a non-empty string returned by run_mission).",
+        },
+      ],
+      isError: true,
+    };
+  }
+  const reason = typeof args["reason"] === "string" ? args["reason"] : undefined;
+  const outcome = getMissionRegistry().pause(missionId, reason);
+  return {
+    content: [
+      {
+        type: "text",
+        text: JSON.stringify({
+          success: true,
+          mission_id: missionId,
+          found: outcome.found,
+          already_paused: outcome.alreadyPaused,
+          reason: reason ?? null,
+        }),
+      },
+    ],
+  };
+}
+
+function handleMissionResume(
+  args: Record<string, unknown>,
+): { content: ToolContent[]; isError?: boolean } {
+  const missionId = typeof args["mission_id"] === "string" ? args["mission_id"].trim() : "";
+  if (!missionId) {
+    return {
+      content: [
+        {
+          type: "text",
+          text: "mission_resume requires 'mission_id' (a non-empty string returned by run_mission).",
+        },
+      ],
+      isError: true,
+    };
+  }
+  const outcome = getMissionRegistry().resume(missionId);
+  return {
+    content: [
+      {
+        type: "text",
+        text: JSON.stringify({
+          success: true,
+          mission_id: missionId,
+          found: outcome.found,
+          was_paused: outcome.wasPaused,
+        }),
+      },
+    ],
+  };
+}
+
 /**
  * Phase 1.c + 1.f + 1.g — `run_mission` handler. Lives at module scope
  * so the outer `handleToolCall` can short-circuit BEFORE
@@ -778,6 +796,30 @@ async function handleRunMission(
     }
   }
   const dispatcher: MissionToolDispatcher = async (toolName, toolArgs) => {
+    if (isExternalToolName(toolName)) {
+      const capId = capabilityIdFromExternalTool(toolName);
+      const cap = caps.find((c) => c.id === capId);
+      if (!cap) {
+        return {
+          text: `Unknown external capability "${capId}".`,
+          isError: true,
+        };
+      }
+      let robot;
+      try {
+        robot = resolveRobotFromArgs(config, toolArgs);
+      } catch (err) {
+        return {
+          text: err instanceof Error ? err.message : String(err),
+          isError: true,
+        };
+      }
+      const transport = await getTransportForRobot(config, robot);
+      const ext = await executeExternalCapability(cap, toolArgs, transport, {
+        namespace: robot.namespace,
+      });
+      return { text: ext.text, outputs: ext.outputs, isError: ext.isError };
+    }
     const res = await handleToolCall(toolName, toolArgs, config);
     const text = res.content
       .map((c) => (c.type === "text" ? c.text : `[image: ${c.mimeType}]`))
@@ -796,7 +838,7 @@ async function handleRunMission(
 
   let result;
   try {
-    result = await runMission(mission, caps, MISSION_BINDINGS, dispatcher, {
+    result = await runMission(mission, caps, buildMissionBindings(caps), dispatcher, {
       mission_id: missionId,
       cancellation: regEntry.cancellation,
       transcript,
@@ -852,7 +894,7 @@ export async function handleToolCall(
     } catch (err) {
       return robotResolveError(err);
     }
-    const caps = listAllCapabilities(config);
+    const caps = await listCapabilitiesWithDiscoverable(config);
     return { content: [{ type: "text", text: formatCapabilitiesResponse(caps) }] };
   }
   // ros2_list_robots reads the multi-robot section of the config (with
@@ -915,15 +957,17 @@ export async function handleToolCall(
       };
     }
   }
-  // mission_cancel + run_mission must NOT trigger a transport connect
-  // at this outer layer: mission_cancel only mutates the in-process
-  // MissionRegistry, and run_mission's handler does its OWN per-step
-  // dispatch (each step's inner `handleToolCall` connects as needed).
-  // Routing the outer call through `getTransportForRobot()` would
-  // hang or fail when an agent only wanted to compile / cancel.
+  // mission_cancel / pause / resume + run_mission must NOT trigger a
+  // transport connect at this outer layer.
   if (name === "mission_cancel") {
     return handleMissionCancel(args);
   }
+  if (name === "mission_pause") {
+    return handleMissionPause(args);
+  }
+  if (name === "mission_resume") {
+    return handleMissionResume(args);
+  }
   if (name === "run_mission") {
     return handleRunMission(args, config);
   }

package/runtime/packages/agenticros-gemini/package.json

@@ -18,6 +18,8 @@
     "@agenticros/core": "workspace:*",
     "@agenticros/ros-camera": "workspace:*",
     "@google/genai": "^1.0.0",
+    "onnxruntime-node": "^1.20.0",
+    "sharp": "^0.33.0",
     "zod": "^3.24.0"
   },
   "devDependencies": {

package/runtime/packages/agenticros-gemini/src/config.ts

@@ -2,7 +2,7 @@ import fs from "node:fs";
 import path from "node:path";
 import os from "node:os";
 import type { AgenticROSConfig } from "@agenticros/core";
-import { parseConfig } from "@agenticros/core";
+import { parseConfig, withResolvedSkillRefs, applyCachedSkillRefs } from "@agenticros/core";
 
 /** Same as Claude Code MCP: optional `robot.namespace` override from the environment. */
 function applyMcpEnvOverrides(config: AgenticROSConfig): AgenticROSConfig {
@@ -17,11 +17,6 @@ function applyMcpEnvOverrides(config: AgenticROSConfig): AgenticROSConfig {
   };
 }
 
-/**
- * Resolve path to AgenticROS config file.
- * Prefer AGENTICROS_CONFIG_PATH; else ~/.agenticros/config.json.
- * Optional: fallback to OpenClaw config and read plugins.entries.agenticros.config.
- */
 function getConfigPath(): string {
   const env = process.env.AGENTICROS_CONFIG_PATH;
   if (env && env.trim().length > 0) {
@@ -30,9 +25,6 @@ function getConfigPath(): string {
   return path.join(os.homedir(), ".agenticros", "config.json");
 }
 
-/**
- * Try to read config from OpenClaw file (plugins.entries.agenticros.config).
- */
 function tryOpenClawConfig(): Record<string, unknown> | null {
   const openclawEnv = process.env.OPENCLAW_CONFIG;
   const openclawPath = openclawEnv && openclawEnv.trim().length > 0
@@ -57,12 +49,7 @@ function tryOpenClawConfig(): Record<string, unknown> | null {
   return null;
 }
 
-/**
- * Load and parse AgenticROS config.
- * 1) AGENTICROS_CONFIG_PATH or ~/.agenticros/config.json (full JSON object = config).
- * 2) If that file does not exist, try OpenClaw config and read plugins.entries.agenticros.config.
- */
-export function loadConfig(): AgenticROSConfig {
+function loadConfigSync(): AgenticROSConfig {
   const primaryPath = getConfigPath();
   try {
     const raw = fs.readFileSync(primaryPath, "utf8");
@@ -72,14 +59,14 @@ export function loadConfig(): AgenticROSConfig {
       if (process.stderr && typeof process.stderr.write === "function") {
         process.stderr.write(`[AgenticROS] Config from ${primaryPath}\n`);
       }
-      return applyMcpEnvOverrides(cfg);
+      return applyMcpEnvOverrides(applyCachedSkillRefs(cfg));
     }
   } catch (err) {
     const nodeErr = err as NodeJS.ErrnoException;
     if (nodeErr.code === "ENOENT") {
       const openclawConfig = tryOpenClawConfig();
       if (openclawConfig) {
-        return applyMcpEnvOverrides(parseConfig(openclawConfig));
+        return applyMcpEnvOverrides(applyCachedSkillRefs(parseConfig(openclawConfig)));
       }
       throw new Error(
         `AgenticROS config not found at ${primaryPath}. Create it or set AGENTICROS_CONFIG_PATH. ` +
@@ -88,5 +75,27 @@ export function loadConfig(): AgenticROSConfig {
     }
     throw err;
   }
-  return applyMcpEnvOverrides(parseConfig({}));
+  return applyMcpEnvOverrides(applyCachedSkillRefs(parseConfig({})));
+}
+
+export function loadConfig(): AgenticROSConfig {
+  return loadConfigSync();
+}
+
+export async function loadConfigAsync(): Promise<AgenticROSConfig> {
+  const base = loadConfigSync();
+  if (!(base.skillRefs?.length)) return base;
+  const { config, errors } = await withResolvedSkillRefs(base, {
+    onLog: (msg) => {
+      if (process.stderr && typeof process.stderr.write === "function") {
+        process.stderr.write(`[AgenticROS] ${msg}\n`);
+      }
+    },
+  });
+  for (const e of errors) {
+    if (process.stderr && typeof process.stderr.write === "function") {
+      process.stderr.write(`[AgenticROS] skillRef ${e.ref}: ${e.error}\n`);
+    }
+  }
+  return applyMcpEnvOverrides(config);
 }

package/runtime/packages/agenticros-gemini/src/find-object/coco-classes.ts

@@ -0,0 +1,38 @@
+/**
+ * COCO 80-class names (index = YOLOv8 class ID). Lower-cased for lookup.
+ */
+
+export const COCO_CLASSES = [
+  "person", "bicycle", "car", "motorcycle", "airplane", "bus", "train", "truck",
+  "boat", "traffic light", "fire hydrant", "stop sign", "parking meter", "bench",
+  "bird", "cat", "dog", "horse", "sheep", "cow", "elephant", "bear", "zebra",
+  "giraffe", "backpack", "umbrella", "handbag", "tie", "suitcase", "frisbee",
+  "skis", "snowboard", "sports ball", "kite", "baseball bat", "baseball glove",
+  "skateboard", "surfboard", "tennis racket", "bottle", "wine glass", "cup",
+  "fork", "knife", "spoon", "bowl", "banana", "apple", "sandwich", "orange",
+  "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", "chair", "couch",
+  "potted plant", "bed", "dining table", "toilet", "tv", "laptop", "mouse",
+  "remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink",
+  "refrigerator", "book", "clock", "vase", "scissors", "teddy bear", "hair drier",
+  "toothbrush",
+] as const;
+
+const ALIASES: Record<string, string> = {
+  phone: "cell phone",
+  cellphone: "cell phone",
+  mobile: "cell phone",
+  smartphone: "cell phone",
+  tv: "tv",
+  television: "tv",
+  monitor: "tv",
+  sofa: "couch",
+  plant: "potted plant",
+  bike: "bicycle",
+};
+
+export function resolveCocoClassId(name: string): number | null {
+  const key = name.trim().toLowerCase();
+  const canonical = ALIASES[key] ?? key;
+  const idx = COCO_CLASSES.indexOf(canonical as (typeof COCO_CLASSES)[number]);
+  return idx >= 0 ? idx : null;
+}

package/runtime/packages/agenticros-gemini/src/find-object/find-object.ts

@@ -0,0 +1,191 @@
+/**
+ * Rotate the robot in place until a target COCO class is detected in the
+ * camera feed, then stop. Used by the ros2_find_object MCP tool.
+ */
+
+import type { AgenticROSConfig, ResolvedRobot, RosTransport } from "@agenticros/core";
+import { resolveCameraSubscribeTopic, toNamespacedTopic } from "@agenticros/core";
+import {
+  ROS_MSG_COMPRESSED_IMAGE,
+  cameraSnapshotFromPlainMessage,
+} from "@agenticros/ros-camera";
+import { PersonDetector } from "../follow-me/detector.js";
+import { resolveCocoClassId, COCO_CLASSES } from "./coco-classes.js";
+
+const DEFAULT_COLOR_TOPIC = "/camera/camera/color/image_raw/compressed";
+const DEFAULT_ANGULAR_SPEED = 0.3; // rad/s
+const DEFAULT_TIMEOUT_SEC = 30;
+const DEFAULT_MIN_CONFIDENCE = 0.5;
+const POLL_INTERVAL_MS = 500;
+const SNAPSHOT_TIMEOUT_MS = 3000;
+
+export interface FindObjectOptions {
+  target: string;
+  angularSpeed?: number;
+  timeoutSeconds?: number;
+  minConfidence?: number;
+  clockwise?: boolean;
+}
+
+export interface FindObjectResult {
+  found: boolean;
+  target: string;
+  classId: number;
+  elapsedSeconds: number;
+  rotationDirection: "clockwise" | "counterclockwise";
+  angularSpeed: number;
+  detection?: {
+    confidence: number;
+    cx: number;
+    cy: number;
+    width: number;
+    height: number;
+    imageWidth: number;
+    imageHeight: number;
+    horizontalOffset: number;
+  };
+  error?: string;
+}
+
+export async function findObject(
+  robot: ResolvedRobot,
+  config: AgenticROSConfig,
+  transport: RosTransport,
+  opts: FindObjectOptions,
+): Promise<FindObjectResult> {
+  const classId = resolveCocoClassId(opts.target);
+  if (classId === null) {
+    return {
+      found: false,
+      target: opts.target,
+      classId: -1,
+      elapsedSeconds: 0,
+      rotationDirection: "clockwise",
+      angularSpeed: 0,
+      error:
+        `Unknown target "${opts.target}". Must be a COCO class name (e.g., "cell phone", "chair", "bottle"). ` +
+        `Supported: ${COCO_CLASSES.join(", ")}.`,
+    };
+  }
+
+  const safety = config.safety ?? {};
+  const maxAngular = safety.maxAngularVelocity ?? 1.5;
+  const requestedSpeed = Math.max(0.05, Math.min(maxAngular, opts.angularSpeed ?? DEFAULT_ANGULAR_SPEED));
+  const clockwise = opts.clockwise ?? true;
+  const angularZ = clockwise ? -requestedSpeed : requestedSpeed;
+  const timeoutMs = Math.max(1000, (opts.timeoutSeconds ?? DEFAULT_TIMEOUT_SEC) * 1000);
+  const minConfidence = opts.minConfidence ?? DEFAULT_MIN_CONFIDENCE;
+
+  const detector = new PersonDetector({ scoreThreshold: minConfidence });
+  await detector.load();
+
+  const cmdVelTopic = resolveCmdVelTopic(config, robot);
+  const colorTopic = resolveCameraSubscribeTopic(
+    robot.namespace,
+    robot.cameraTopic.trim() || DEFAULT_COLOR_TOPIC,
+  );
+
+  const startedAt = Date.now();
+  let rotating = false;
+  let result: FindObjectResult["detection"] | undefined;
+
+  const publishTwist = async (linearX: number, angZ: number) => {
+    try {
+      await transport.publish({
+        topic: cmdVelTopic,
+        type: "geometry_msgs/msg/Twist",
+        msg: { linear: { x: linearX, y: 0, z: 0 }, angular: { x: 0, y: 0, z: angZ } },
+      });
+    } catch {
+      // best-effort; loop will retry
+    }
+  };
+
+  try {
+    await publishTwist(0, angularZ);
+    rotating = true;
+
+    const deadline = startedAt + timeoutMs;
+    while (Date.now() < deadline && !result) {
+      // Keep the rotation alive in case the robot times out cmd_vel commands.
+      await publishTwist(0, angularZ);
+
+      const frame = await snapshotOnce(transport, colorTopic).catch(() => null);
+      if (frame) {
+        const det = await detector.detectClass(frame.buffer, classId);
+        if (det.detections.length > 0) {
+          const best = det.detections.reduce((a, b) => (a.confidence > b.confidence ? a : b));
+          result = {
+            confidence: best.confidence,
+            cx: best.cx,
+            cy: best.cy,
+            width: best.width,
+            height: best.height,
+            imageWidth: det.width,
+            imageHeight: det.height,
+            horizontalOffset: (best.cx - det.width / 2) / (det.width / 2),
+          };
+          break;
+        }
+      }
+
+      const remaining = deadline - Date.now();
+      if (remaining <= 0) break;
+      await sleep(Math.min(POLL_INTERVAL_MS, remaining));
+    }
+  } finally {
+    if (rotating) await publishTwist(0, 0);
+    await detector.dispose().catch(() => {});
+  }
+
+  const elapsedSeconds = (Date.now() - startedAt) / 1000;
+  return {
+    found: !!result,
+    target: opts.target,
+    classId,
+    elapsedSeconds,
+    rotationDirection: clockwise ? "clockwise" : "counterclockwise",
+    angularSpeed: requestedSpeed,
+    detection: result,
+  };
+}
+
+async function snapshotOnce(
+  transport: RosTransport,
+  topic: string,
+): Promise<{ buffer: Buffer } | null> {
+  return new Promise((resolve) => {
+    const sub = transport.subscribe(
+      { topic, type: ROS_MSG_COMPRESSED_IMAGE },
+      (msg: Record<string, unknown>) => {
+        clearTimeout(timer);
+        sub.unsubscribe();
+        try {
+          const payload = cameraSnapshotFromPlainMessage("CompressedImage", msg);
+          resolve({ buffer: Buffer.from(payload.dataBase64, "base64") });
+        } catch {
+          resolve(null);
+        }
+      },
+    );
+    const timer = setTimeout(() => {
+      sub.unsubscribe();
+      resolve(null);
+    }, SNAPSHOT_TIMEOUT_MS);
+  });
+}
+
+function sleep(ms: number): Promise<void> {
+  return new Promise((resolve) => setTimeout(resolve, ms));
+}
+
+function resolveCmdVelTopic(config: AgenticROSConfig, robot: ResolvedRobot): string {
+  const raw = (config.teleop?.cmdVelTopic ?? "").trim() || "/cmd_vel";
+  const namespaced = toNamespacedTopic(robot.namespace, raw);
+  const match = namespaced.match(/^\/([^/]+)\/cmd_vel$/i);
+  const segment = match?.[1] ?? "";
+  if (match && !segment.toLowerCase().startsWith("robot")) {
+    return `/robot${segment.replace(/-/g, "")}/cmd_vel`;
+  }
+  return namespaced;
+}