agenticros 0.0.1 → 0.1.1

package/runtime/scripts/smoke_test_nemoclaw.sh

@@ -0,0 +1,123 @@
+#!/usr/bin/env bash
+#
+# Smoke-test the hybrid NemoClaw + AgenticROS bridge.
+#
+# Checks, in order:
+#   1. NemoClaw sandbox container is up.
+#   2. agenticros_rosbridge policy is loaded into the live gateway bundle
+#      with the SSRF allowed_ips block (so host.docker.internal is reachable).
+#   3. rosbridge_server is bound on the host (0.0.0.0:9090).
+#   4. The plugin's actual TCP connection through the OPA proxy is ALLOWED
+#      (not denied by engine:ssrf or engine:opa).
+#   5. A real WebSocket request to /rosapi/topics through the proxy returns
+#      a list of ROS 2 topics (i.e. the gateway can both reach rosbridge and
+#      speak its protocol).
+#   6. The NemoClaw dashboard HTTP endpoint serves a non-empty body.
+#
+# Exit code: 0 on all-pass, 1 on any failure. Prints a one-line PASS/FAIL per
+# check + a final summary.
+#
+# Usage:   ./scripts/smoke_test_nemoclaw.sh
+
+set -uo pipefail
+
+GREEN=$'\033[32m'; RED=$'\033[31m'; YELLOW=$'\033[33m'; RESET=$'\033[0m'
+PASS=0; FAIL=0
+pass() { echo "${GREEN}PASS${RESET} $*"; PASS=$((PASS+1)); }
+fail() { echo "${RED}FAIL${RESET} $*"; FAIL=$((FAIL+1)); }
+warn() { echo "${YELLOW}WARN${RESET} $*"; }
+
+CONTAINER=$(docker ps --format '{{.Names}}' 2>/dev/null | grep '^openshell-nemo-' | head -1 || true)
+if [[ -z "${CONTAINER}" ]]; then
+  fail "No openshell-nemo-* container running — start NemoClaw with 'nemoclaw nemo start'"
+  exit 1
+fi
+pass "sandbox container: ${CONTAINER}"
+
+# ----- 2. agenticros_rosbridge policy loaded with allowed_ips ---------------
+OPENSHELL_BIN="${OPENSHELL_BIN:-$(command -v openshell || echo "$HOME/.local/bin/openshell")}"
+if [[ ! -x "${OPENSHELL_BIN}" ]]; then
+  warn "openshell CLI not found at ${OPENSHELL_BIN} — skipping policy bundle check"
+else
+  POLICY_BUNDLE=$("${OPENSHELL_BIN}" policy get --full nemo 2>/dev/null || true)
+  if grep -q 'agenticros_rosbridge:' <<<"${POLICY_BUNDLE}"; then
+    if grep -q 'allowed_ips:' <<<"${POLICY_BUNDLE}"; then
+      pass "agenticros_rosbridge policy is loaded with allowed_ips (SSRF guard satisfied)"
+    else
+      fail "agenticros_rosbridge policy is loaded but missing allowed_ips — SSRF guard will deny"
+    fi
+  else
+    fail "agenticros_rosbridge policy is NOT loaded — run: nemoclaw nemo policy-add --yes --from-file scripts/agenticros-rosbridge.policy.yaml"
+  fi
+fi
+
+# ----- 3. rosbridge_server bound on host:9090 -------------------------------
+if ss -ltn 2>/dev/null | awk '{print $4}' | grep -qE '(:9090|:::9090|0\.0\.0\.0:9090)$'; then
+  pass "rosbridge_server is listening on host :9090"
+else
+  fail "nothing is listening on host :9090 — start the host stack: ./scripts/run_nemoclaw_host_stack.sh humble"
+fi
+
+# ----- 4. sandbox proxy decision for the plugin's binary --------------------
+LOG_FILE=$(docker exec "${CONTAINER}" sh -c 'ls -t /var/log/openshell.*.log 2>/dev/null | head -1' 2>/dev/null | tr -d '\r')
+if [[ -n "${LOG_FILE}" ]]; then
+  LAST_DECISION=$(docker exec "${CONTAINER}" sh -c "grep -E 'host.docker.internal:9090|host.openshell.internal:9090' '${LOG_FILE}' | tail -1" 2>/dev/null)
+  if [[ -z "${LAST_DECISION}" ]]; then
+    warn "no recent proxy decisions for host.docker.internal:9090 in ${LOG_FILE} (gateway hasn't tried yet?)"
+  elif grep -qE 'ALLOWED.*policy:agenticros_rosbridge' <<<"${LAST_DECISION}"; then
+    pass "last proxy decision: ALLOWED via agenticros_rosbridge"
+  elif grep -q 'engine:ssrf' <<<"${LAST_DECISION}"; then
+    fail "last proxy decision: DENIED by SSRF guard — add allowed_ips to the policy. Saw: ${LAST_DECISION##*OCSF }"
+  elif grep -q 'engine:opa' <<<"${LAST_DECISION}"; then
+    fail "last proxy decision: DENIED by OPA — endpoint missing from agenticros_rosbridge. Saw: ${LAST_DECISION##*OCSF }"
+  else
+    warn "unrecognised proxy decision: ${LAST_DECISION##*OCSF }"
+  fi
+fi
+
+# ----- 5. live WebSocket roundtrip to /rosapi/topics through the proxy ------
+GW_PID=$(docker exec "${CONTAINER}" pgrep -f openclaw-gateway 2>/dev/null | head -1 | tr -d '\r')
+WS_PATH=$(docker exec "${CONTAINER}" sh -c 'ls -d /sandbox/agenticros/node_modules/.pnpm/ws@*/node_modules/ws 2>/dev/null | head -1' | tr -d '\r')
+if [[ -z "${GW_PID}" ]] || [[ -z "${WS_PATH}" ]]; then
+  warn "skip WebSocket roundtrip — gateway pid=${GW_PID:-?} ws=${WS_PATH:-?}"
+else
+  TOPICS_JSON=$(docker exec "${CONTAINER}" sh -c "nsenter --target ${GW_PID} --net -- node --input-type=commonjs -e '
+const http=require(\"http\");
+const WebSocket=require(\"${WS_PATH}\");
+const agent=new http.Agent({keepAlive:false});
+agent.createConnection=(opts,cb)=>{
+  const req=http.request({host:\"10.200.0.1\",port:3128,method:\"CONNECT\",path:opts.host+\":\"+opts.port,headers:{Host:opts.host+\":\"+opts.port}});
+  req.on(\"connect\",(res,sock)=>{ if(res.statusCode!==200){cb(new Error(\"proxy \"+res.statusCode));return;} cb(null,sock); });
+  req.on(\"error\",cb); req.end();
+};
+const ws=new WebSocket(\"ws://host.docker.internal:9090\",{agent});
+ws.on(\"open\",()=>ws.send(JSON.stringify({op:\"call_service\",service:\"/rosapi/topics\",id:\"smoke\"})));
+ws.on(\"message\",m=>{const o=JSON.parse(m.toString()); console.log(JSON.stringify({count:(o.values&&o.values.topics||[]).length, sample:(o.values&&o.values.topics||[]).slice(0,3)})); ws.close(); process.exit(0);});
+ws.on(\"error\",e=>{console.error(\"ERR\",e.message); process.exit(1);});
+setTimeout(()=>{console.error(\"timeout\"); process.exit(2);},6000);
+' 2>&1" 2>/dev/null | tail -1)
+  if grep -qE '^\{"count":[0-9]+' <<<"${TOPICS_JSON}"; then
+    pass "WebSocket roundtrip: $(jq -r '"\(.count) topics, sample=\(.sample|join(", "))"' <<<"${TOPICS_JSON}" 2>/dev/null || echo "${TOPICS_JSON}")"
+  else
+    fail "WebSocket roundtrip failed: ${TOPICS_JSON:-<no output>}"
+  fi
+fi
+
+# ----- 6. NemoClaw dashboard reachable + non-empty --------------------------
+DASH_URL=$(nemoclaw nemo dashboard-url --quiet 2>/dev/null | head -1 || true)
+DASH_BASE=${DASH_URL%%#*}
+if [[ -z "${DASH_BASE}" ]]; then
+  warn "dashboard URL not reported by nemoclaw (sandbox not Ready?)"
+else
+  DASH_BYTES=$(curl -fsS "${DASH_BASE}" 2>/dev/null | wc -c | tr -d ' ')
+  if [[ -n "${DASH_BYTES}" ]] && [[ "${DASH_BYTES}" -gt 200 ]]; then
+    pass "dashboard at ${DASH_BASE} returns ${DASH_BYTES}-byte body"
+    echo "    URL with token: ${DASH_URL}"
+  else
+    fail "dashboard at ${DASH_BASE} returned ${DASH_BYTES:-0} bytes — gateway crashed?"
+  fi
+fi
+
+echo
+echo "Summary: ${GREEN}${PASS} passed${RESET}, ${RED}${FAIL} failed${RESET}"
+[[ ${FAIL} -eq 0 ]]

package/runtime/scripts/start_demo.sh

@@ -0,0 +1,55 @@
+#!/bin/bash
+# start_demo.sh - Prepare the AgenticROS Claude Code demo (local DDS transport).
+#
+# What this does:
+#   1. Sources ROS2 + the agenticros workspace
+#   2. Launches the RealSense camera in the background (logs to /tmp)
+#   3. Builds the @agenticros/claude-code MCP server
+#   4. Starts the robot's motor controller via `robotics start motors`
+#
+# After this finishes, launch Claude Code from the repo root. The MCP server
+# is auto-started by .mcp.json over stdio — nothing else to run on this host.
+#
+# Usage: ./scripts/start_demo.sh [jazzy|humble]
+
+set -e
+
+ROS_DISTRO="${1:-jazzy}"
+REPO_ROOT=$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)
+CAMERA_LOG=/tmp/agenticros-camera.log
+CAMERA_PID_FILE=/tmp/agenticros-camera.pid
+source "$REPO_ROOT/scripts/lib/agenticros-banner.sh"
+
+agenticros_banner
+echo "Starting AgenticROS Claude Code demo"
+echo ""
+
+echo "==> Sourcing ROS2 ($ROS_DISTRO) and agenticros workspace"
+source "/opt/ros/$ROS_DISTRO/setup.bash"
+if [[ -f "$REPO_ROOT/ros2_ws/install/setup.bash" ]]; then
+    source "$REPO_ROOT/ros2_ws/install/setup.bash"
+else
+    echo "   (ros2_ws is not built — run: cd ros2_ws && colcon build --symlink-install)"
+fi
+
+echo "==> Starting RealSense camera (logs: $CAMERA_LOG)"
+if [[ -f "$CAMERA_PID_FILE" ]] && kill -0 "$(cat "$CAMERA_PID_FILE")" 2>/dev/null; then
+    echo "   Already running (pid $(cat "$CAMERA_PID_FILE")) — skipping"
+elif pgrep -f "realsense2_camera_node" >/dev/null; then
+    echo "   Detected an existing realsense2_camera_node — skipping"
+else
+    nohup ros2 launch realsense2_camera rs_launch.py >"$CAMERA_LOG" 2>&1 &
+    echo $! >"$CAMERA_PID_FILE"
+    echo "   Started (pid $(cat "$CAMERA_PID_FILE"))"
+fi
+
+echo "==> Building @agenticros/claude-code MCP server"
+cd "$REPO_ROOT"
+pnpm --filter @agenticros/claude-code build
+
+echo "==> Starting motor controller (robotics start motors)"
+robotics start motors
+
+echo ""
+echo "Demo ready. Launch Claude Code from $REPO_ROOT — .mcp.json starts the MCP server."
+echo "Stop the camera with: kill \$(cat $CAMERA_PID_FILE)"

package/runtime/scripts/sync-skill-tools.mjs

@@ -0,0 +1,335 @@
+#!/usr/bin/env node
+/**
+ * sync-skill-tools.mjs
+ *
+ * Discover AgenticROS skill tool names and merge them into the agenticros plugin's
+ * `contracts.tools` allowlist in `packages/agenticros/openclaw.plugin.json`.
+ *
+ * OpenClaw 2026+ enforces `contracts.tools` as a strict allowlist: any tool that a
+ * plugin registers at runtime but that is not declared in the manifest is rejected
+ * with `plugin must declare contracts.tools for: <name>` and silently dropped.
+ * Since AgenticROS skills register their own tools dynamically (e.g. `follow_robot`),
+ * the manifest must list every skill tool that should be exposed.
+ *
+ * How it works
+ * ------------
+ * 1. Resolve the OpenClaw config (OPENCLAW_CONFIG or ~/.openclaw/openclaw.json).
+ * 2. Read `plugins.entries.agenticros.config.skillPaths` and `.skillPackages`.
+ * 3. For each entry, locate the skill's built entry (dist/index.js), import it,
+ *    and call `registerSkill(api, config, context)` with a stub `api` whose
+ *    `registerTool` is a spy that just records `tool.name`.
+ * 4. Merge the discovered tool names with the static core tool list and write
+ *    them back into `packages/agenticros/openclaw.plugin.json`.
+ *
+ * Usage
+ * -----
+ *   node scripts/sync-skill-tools.mjs            # write the manifest
+ *   node scripts/sync-skill-tools.mjs --dry-run  # print what would change
+ *   node scripts/sync-skill-tools.mjs --verbose  # show per-skill discovery details
+ *
+ * Run this whenever you add, remove, or rebuild a skill in `skillPaths` /
+ * `skillPackages`. After it succeeds, refresh the OpenClaw plugin registry
+ * (`openclaw plugins registry --refresh`) and restart the gateway so OpenClaw
+ * re-reads the updated `contracts.tools`.
+ */
+
+import { readFileSync, writeFileSync, existsSync } from "node:fs";
+import { join, resolve, dirname, isAbsolute } from "node:path";
+import { homedir } from "node:os";
+import { pathToFileURL, fileURLToPath } from "node:url";
+import { createRequire } from "node:module";
+
+const __filename = fileURLToPath(import.meta.url);
+const __dirname = dirname(__filename);
+const REPO_ROOT = resolve(__dirname, "..");
+const MANIFEST_PATH = join(REPO_ROOT, "packages/agenticros/openclaw.plugin.json");
+
+const args = new Set(process.argv.slice(2));
+const DRY_RUN = args.has("--dry-run") || args.has("-n");
+const VERBOSE = args.has("--verbose") || args.has("-v");
+
+const CORE_TOOLS = [
+  "ros2_publish",
+  "ros2_subscribe_once",
+  "ros2_service_call",
+  "ros2_action_goal",
+  "ros2_param_get",
+  "ros2_param_set",
+  "ros2_list_topics",
+  "ros2_camera_snapshot",
+  "ros2_depth_distance",
+  // Memory tools (registered conditionally at runtime when memory.enabled=true).
+  // Listed here so they remain in the allowlist after sync-skill-tools rewrites it.
+  "memory_remember",
+  "memory_recall",
+  "memory_forget",
+  "memory_status",
+];
+
+function log(...m) {
+  console.log("[sync-skill-tools]", ...m);
+}
+function vlog(...m) {
+  if (VERBOSE) console.log("[sync-skill-tools]", ...m);
+}
+function warn(...m) {
+  console.warn("[sync-skill-tools] WARN:", ...m);
+}
+function fail(msg) {
+  console.error("[sync-skill-tools] ERROR:", msg);
+  process.exit(1);
+}
+
+function getOpenClawConfigPath() {
+  const env = process.env.OPENCLAW_CONFIG;
+  if (env && env.length > 0) return resolve(env);
+  return join(homedir(), ".openclaw", "openclaw.json");
+}
+
+function readOpenClawConfig() {
+  const p = getOpenClawConfigPath();
+  if (!existsSync(p)) {
+    warn(`OpenClaw config not found: ${p} — assuming no skills configured.`);
+    return null;
+  }
+  try {
+    return JSON.parse(readFileSync(p, "utf8"));
+  } catch (e) {
+    fail(`Cannot parse ${p}: ${e?.message ?? e}`);
+  }
+}
+
+function getSkillEntries(cfg) {
+  const skillPaths = cfg?.plugins?.entries?.agenticros?.config?.skillPaths ?? [];
+  const skillPackages = cfg?.plugins?.entries?.agenticros?.config?.skillPackages ?? [];
+  return { skillPaths, skillPackages };
+}
+
+function findSkillEntryFromPath(dirPath) {
+  if (!existsSync(dirPath)) {
+    warn(`skillPaths entry does not exist: ${dirPath}`);
+    return null;
+  }
+  const pkgPath = join(dirPath, "package.json");
+  if (!existsSync(pkgPath)) {
+    warn(`skillPaths entry missing package.json: ${dirPath}`);
+    return null;
+  }
+  let pkg;
+  try {
+    pkg = JSON.parse(readFileSync(pkgPath, "utf8"));
+  } catch (e) {
+    warn(`Invalid package.json in ${dirPath}: ${e?.message ?? e}`);
+    return null;
+  }
+  if (!pkg.agenticrosSkill) {
+    warn(`Not an AgenticROS skill (missing agenticrosSkill flag): ${dirPath}`);
+    return null;
+  }
+  const main = pkg.main ?? "index.js";
+  const entry = join(dirPath, main);
+  if (!existsSync(entry)) {
+    warn(`Skill entry not built (run pnpm build in the skill repo): ${entry}`);
+    return null;
+  }
+  return { entry, packageName: pkg.name ?? dirPath, packageJson: pkg };
+}
+
+function resolvePackageEntry(packageName) {
+  if (!packageName || packageName.trim() === "") return null;
+  if (packageName.includes(" ")) {
+    warn(`Invalid skillPackages entry (contains spaces): "${packageName}"`);
+    return null;
+  }
+  try {
+    const req = createRequire(join(REPO_ROOT, "package.json"));
+    const entry = req.resolve(packageName);
+    return entry;
+  } catch {
+    warn(`Skill package not resolvable from repo root: ${packageName}`);
+    return null;
+  }
+}
+
+/**
+ * Create a stub OpenClawPluginApi that captures registerTool calls.
+ * Other methods are no-ops so that registerSkill can complete without crashing.
+ */
+function createSpyApi() {
+  const tools = [];
+  const noop = () => {};
+  const noopLog = {
+    info: (m) => vlog("  skill.logger.info:", m),
+    warn: (m) => vlog("  skill.logger.warn:", m),
+    error: (m) => vlog("  skill.logger.error:", m),
+  };
+  return {
+    tools,
+    api: {
+      pluginConfig: {},
+      logger: noopLog,
+      registerTool(tool) {
+        if (tool && typeof tool.name === "string" && tool.name.length > 0) {
+          tools.push(tool.name);
+        }
+      },
+      registerService: noop,
+      registerCommand: noop,
+      registerHttpRoute: noop,
+      on: noop,
+    },
+  };
+}
+
+function createStubContext() {
+  return {
+    getTransport() {
+      throw new Error("stub: getTransport not available during contract discovery");
+    },
+    getDepthDistance() {
+      return Promise.resolve({ valid: false, distance_m: 0 });
+    },
+    getDepthSectors() {
+      return Promise.resolve({ valid: false, left_m: 0, center_m: 0, right_m: 0 });
+    },
+    logger: {
+      info: () => {},
+      warn: () => {},
+      error: () => {},
+    },
+  };
+}
+
+async function discoverSkillTools(entryPath, packageName) {
+  const url = pathToFileURL(entryPath).href;
+  let mod;
+  try {
+    mod = await import(url);
+  } catch (e) {
+    warn(`Failed to import ${packageName} (${entryPath}): ${e?.message ?? e}`);
+    return [];
+  }
+  const registerSkill =
+    mod.registerSkill ?? mod.default?.registerSkill ?? mod.default;
+  if (typeof registerSkill !== "function") {
+    warn(`${packageName}: no registerSkill export — skipping discovery`);
+    return [];
+  }
+  const { tools, api } = createSpyApi();
+  const ctx = createStubContext();
+  // Provide a config skeleton that matches AgenticROSConfig defaults loosely
+  // enough that registration doesn't crash on access paths like config.skills.x.
+  const stubConfig = {
+    transport: { mode: "rosbridge" },
+    robot: { name: "Robot", namespace: "", cameraTopic: "" },
+    teleop: { cmdVelTopic: "", cameraTopic: "", speedDefault: 0.3, cameraPollMs: 150 },
+    safety: { maxLinearVelocity: 1, maxAngularVelocity: 1.5 },
+    skills: {},
+    skillPaths: [],
+    skillPackages: [],
+  };
+  try {
+    await Promise.resolve(registerSkill(api, stubConfig, ctx));
+  } catch (e) {
+    warn(
+      `${packageName} registerSkill threw during discovery — captured ${tools.length} tools before error: ${e?.message ?? e}`,
+    );
+  }
+  return tools;
+}
+
+function uniqueOrdered(...lists) {
+  const seen = new Set();
+  const out = [];
+  for (const list of lists) {
+    for (const name of list) {
+      if (typeof name !== "string") continue;
+      const trimmed = name.trim();
+      if (!trimmed || seen.has(trimmed)) continue;
+      seen.add(trimmed);
+      out.push(trimmed);
+    }
+  }
+  return out;
+}
+
+function arraysEqual(a, b) {
+  if (a.length !== b.length) return false;
+  for (let i = 0; i < a.length; i++) if (a[i] !== b[i]) return false;
+  return true;
+}
+
+async function main() {
+  if (!existsSync(MANIFEST_PATH)) fail(`Manifest not found: ${MANIFEST_PATH}`);
+  const manifestRaw = readFileSync(MANIFEST_PATH, "utf8");
+  let manifest;
+  try {
+    manifest = JSON.parse(manifestRaw);
+  } catch (e) {
+    fail(`Cannot parse manifest: ${e?.message ?? e}`);
+  }
+
+  const cfg = readOpenClawConfig();
+  const { skillPaths, skillPackages } = cfg ? getSkillEntries(cfg) : { skillPaths: [], skillPackages: [] };
+
+  log(`Resolved OpenClaw config: ${getOpenClawConfigPath()}`);
+  log(`skillPaths: ${skillPaths.length === 0 ? "(none)" : skillPaths.join(", ")}`);
+  log(`skillPackages: ${skillPackages.length === 0 ? "(none)" : skillPackages.join(", ")}`);
+
+  const discovered = new Map(); // skillId/packageName -> tools[]
+
+  for (const pkg of skillPackages) {
+    const entry = resolvePackageEntry(pkg);
+    if (!entry) continue;
+    vlog(`Resolving package "${pkg}" → ${entry}`);
+    const tools = await discoverSkillTools(entry, pkg);
+    if (tools.length > 0) discovered.set(pkg, tools);
+    else vlog(`  no tools discovered for ${pkg}`);
+  }
+
+  for (const dir of skillPaths) {
+    const absDir = isAbsolute(dir) ? dir : resolve(REPO_ROOT, dir);
+    const resolved = findSkillEntryFromPath(absDir);
+    if (!resolved) continue;
+    vlog(`Resolving path "${absDir}" → ${resolved.entry} (${resolved.packageName})`);
+    const tools = await discoverSkillTools(resolved.entry, resolved.packageName);
+    if (tools.length > 0) discovered.set(resolved.packageName, tools);
+    else vlog(`  no tools discovered for ${resolved.packageName}`);
+  }
+
+  if (discovered.size === 0) {
+    log("No skill tools discovered. contracts.tools will contain only core tools.");
+  } else {
+    for (const [name, tools] of discovered.entries()) {
+      log(`Discovered ${tools.length} tool(s) from ${name}: ${tools.join(", ")}`);
+    }
+  }
+
+  const skillTools = [...discovered.values()].flat();
+  const merged = uniqueOrdered(CORE_TOOLS, skillTools);
+  const previous = Array.isArray(manifest?.contracts?.tools)
+    ? manifest.contracts.tools.slice()
+    : [];
+
+  if (arraysEqual(previous, merged)) {
+    log("Manifest is already up to date.");
+    return;
+  }
+
+  log(`Updating contracts.tools:\n  before: [${previous.join(", ")}]\n  after:  [${merged.join(", ")}]`);
+
+  if (DRY_RUN) {
+    log("Dry run — manifest not modified.");
+    return;
+  }
+
+  manifest.contracts = manifest.contracts ?? {};
+  manifest.contracts.tools = merged;
+  writeFileSync(MANIFEST_PATH, JSON.stringify(manifest, null, 2) + "\n", "utf8");
+  log(`Wrote ${MANIFEST_PATH}`);
+  log("Next steps:");
+  log("  1. openclaw plugins registry --refresh");
+  log("  2. Restart the OpenClaw gateway so the new contracts take effect.");
+}
+
+main().catch((e) => fail(e?.stack ?? String(e)));

package/runtime/scripts/test-follow-me-sim.mjs

@@ -0,0 +1,135 @@
+#!/usr/bin/env node
+/**
+ * Drive ros2_follow_me_start (mode=depth) against the AMR sim.
+ *
+ * Sequence:
+ *   1. start follow-me in depth mode
+ *   2. poll status every 1.5s for 15s, capturing detection events
+ *   3. stop follow-me
+ *   4. send a zero cmd_vel just in case
+ *
+ * Reports: did the depth loop see a target, what distance/lateral did it pick,
+ * how many cmd_vel commands were issued.
+ */
+
+import { spawn } from "node:child_process";
+import { fileURLToPath } from "node:url";
+import { dirname, join, resolve } from "node:path";
+
+const __dirname = dirname(fileURLToPath(import.meta.url));
+const repoRoot = resolve(__dirname, "..");
+const serverDist = join(repoRoot, "packages/agenticros-claude-code/dist/index.js");
+
+const child = spawn(process.execPath, [serverDist], {
+  stdio: ["pipe", "pipe", "pipe"],
+  env: { ...process.env },
+});
+
+child.stderr.on("data", (d) => {
+  process.stderr.write(`[mcp-stderr] ${d}`);
+});
+
+let nextId = 1;
+const pending = new Map();
+let buf = "";
+
+child.stdout.on("data", (chunk) => {
+  buf += chunk.toString();
+  let nl;
+  while ((nl = buf.indexOf("\n")) !== -1) {
+    const line = buf.slice(0, nl).trim();
+    buf = buf.slice(nl + 1);
+    if (!line) continue;
+    let msg;
+    try {
+      msg = JSON.parse(line);
+    } catch {
+      continue;
+    }
+    if (msg.id !== undefined && pending.has(msg.id)) {
+      const { resolve, reject } = pending.get(msg.id);
+      pending.delete(msg.id);
+      if (msg.error) reject(new Error(msg.error.message));
+      else resolve(msg.result);
+    }
+  }
+});
+
+function rpc(method, params = {}, timeoutMs = 15000) {
+  const id = nextId++;
+  return new Promise((resolveOuter, reject) => {
+    const t = setTimeout(() => {
+      pending.delete(id);
+      reject(new Error(`Timeout: ${method}`));
+    }, timeoutMs);
+    pending.set(id, {
+      resolve: (v) => {
+        clearTimeout(t);
+        resolveOuter(v);
+      },
+      reject: (e) => {
+        clearTimeout(t);
+        reject(e);
+      },
+    });
+    child.stdin.write(JSON.stringify({ jsonrpc: "2.0", id, method, params }) + "\n");
+  });
+}
+
+function pickText(result) {
+  return result?.content?.map((c) => c.text ?? "").join("\n") ?? "";
+}
+
+async function main() {
+  console.log("=== follow_me (depth) E2E ===");
+
+  await rpc("initialize", {
+    protocolVersion: "2024-11-05",
+    capabilities: { tools: {} },
+    clientInfo: { name: "fm-e2e", version: "0.0.1" },
+  });
+  await rpc("notifications/initialized", {}).catch(() => {});
+
+  console.log("\n-- start follow_me mode=depth --");
+  const start = await rpc("tools/call", {
+    name: "ros2_follow_me_start",
+    arguments: { mode: "depth", targetDistance: 1.5 },
+  });
+  console.log(pickText(start));
+
+  console.log("\n-- poll status for 15s --");
+  const polls = [];
+  for (let i = 0; i < 10; i++) {
+    await new Promise((r) => setTimeout(r, 1500));
+    const s = await rpc("tools/call", { name: "ros2_follow_me_status", arguments: {} });
+    const text = pickText(s);
+    polls.push({ t: (i + 1) * 1.5, text });
+    console.log(`t+${((i + 1) * 1.5).toFixed(1)}s :: ${text.replace(/\s+/g, " ").slice(0, 200)}`);
+  }
+
+  console.log("\n-- stop follow_me --");
+  const stop = await rpc("tools/call", {
+    name: "ros2_follow_me_stop",
+    arguments: {},
+  });
+  console.log(pickText(stop));
+
+  console.log("\n-- safety zero-twist --");
+  await rpc("tools/call", {
+    name: "ros2_publish",
+    arguments: {
+      topic: "/cmd_vel",
+      type: "geometry_msgs/msg/Twist",
+      message: { linear: { x: 0, y: 0, z: 0 }, angular: { x: 0, y: 0, z: 0 } },
+    },
+  });
+
+  child.kill("SIGTERM");
+  console.log("\n=== Done ===");
+}
+
+main().catch((e) => {
+  console.error("Failed:", e);
+  child.kill("SIGKILL");
+  process.exit(1);
+});