agenticros 0.1.0 → 0.1.2

package/runtime/scripts/sim/run_sim.sh

@@ -0,0 +1,169 @@
+#!/usr/bin/env bash
+# scripts/sim/run_sim.sh
+#
+# Worker script invoked by `agenticros up sim-amr` (and later `sim-arm`). Sources
+# ROS 2 + the AgenticROS overlay, runs `ros2 launch agenticros_sim ...`, writes
+# its PID into /tmp/agenticros-sim.pid so `agenticros down` can stop it, and
+# tees output to /tmp/agenticros-sim.log so `agenticros logs sim` works.
+#
+# Usage:
+#   run_sim.sh --robot amr [--namespace sim_robot] [--rviz] [--no-gui]
+#
+# Flags:
+#   --robot amr|arm        Which sim launch to start (default: amr).
+#   --namespace <ns>       Robot namespace; exported as AGENTICROS_ROBOT_NAMESPACE.
+#   --rviz                 Bring up RViz alongside Gazebo.
+#   --no-gui               Run gz-sim headless (CI / docker).
+#   --ros-distro <distro>  Override ROS 2 distro (auto-detect by default).
+#   --colcon-ws <path>     Override the colcon workspace (default: <repo>/ros2_ws).
+#   --help                 Print this help and exit.
+#
+# Exit codes:
+#   0   sim exited cleanly
+#   1   missing dependency (gz, ros2, ros_gz_*, our launch file)
+#   2   bad CLI usage
+#   3   sim crashed at runtime — check /tmp/agenticros-sim.log
+
+# Strict mode, but `set -u` clashes with ROS setup scripts that reference
+# AMENT_TRACE_SETUP_FILES etc. We toggle nounset off around each `source`.
+set -eo pipefail
+shopt -s expand_aliases || true
+
+# ---------- args ----------
+ROBOT="amr"
+NAMESPACE=""
+USE_RVIZ="false"
+GUI="true"
+ROS_DISTRO_OVERRIDE=""
+COLCON_WS=""
+
+while [[ $# -gt 0 ]]; do
+  case "$1" in
+    --robot)        ROBOT="$2"; shift 2 ;;
+    --namespace)    NAMESPACE="$2"; shift 2 ;;
+    --rviz)         USE_RVIZ="true"; shift ;;
+    --no-gui)       GUI="false"; shift ;;
+    --ros-distro)   ROS_DISTRO_OVERRIDE="$2"; shift 2 ;;
+    --colcon-ws)    COLCON_WS="$2"; shift 2 ;;
+    -h|--help)
+      sed -n '2,24p' "${BASH_SOURCE[0]}" | sed 's/^# \{0,1\}//'
+      exit 0 ;;
+    *) echo "Unknown arg: $1" >&2; exit 2 ;;
+  esac
+done
+
+if [[ "$ROBOT" != "amr" && "$ROBOT" != "arm" ]]; then
+  echo "--robot must be 'amr' or 'arm' (got '$ROBOT')" >&2
+  exit 2
+fi
+
+# ---------- paths ----------
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+REPO_ROOT="$(cd "$SCRIPT_DIR/../.." && pwd)"
+COLCON_WS="${COLCON_WS:-$REPO_ROOT/ros2_ws}"
+
+LOG_FILE="/tmp/agenticros-sim.log"
+PID_FILE="/tmp/agenticros-sim.pid"
+
+log() { printf "\033[36m[run_sim]\033[0m %s\n" "$*"; }
+err() { printf "\033[31m[run_sim]\033[0m %s\n" "$*" >&2; }
+
+# ---------- ROS 2 distro detection ----------
+if [[ -n "$ROS_DISTRO_OVERRIDE" ]]; then
+  ROS_DISTRO="$ROS_DISTRO_OVERRIDE"
+elif [[ -n "${ROS_DISTRO:-}" ]]; then
+  : # already sourced
+else
+  for d in humble jazzy iron rolling; do
+    if [[ -f "/opt/ros/$d/setup.bash" ]]; then ROS_DISTRO="$d"; break; fi
+  done
+fi
+if [[ -z "${ROS_DISTRO:-}" ]] || [[ ! -f "/opt/ros/$ROS_DISTRO/setup.bash" ]]; then
+  err "ROS 2 not found under /opt/ros/. Install Humble or Jazzy first."
+  exit 1
+fi
+
+log "ROS_DISTRO=$ROS_DISTRO"
+log "COLCON_WS=$COLCON_WS"
+log "robot=$ROBOT  namespace=${NAMESPACE:-<none>}  rviz=$USE_RVIZ  gui=$GUI"
+
+# shellcheck disable=SC1090
+source "/opt/ros/$ROS_DISTRO/setup.bash"
+
+# ---------- Build agenticros_sim if not yet built ----------
+if [[ ! -f "$COLCON_WS/install/agenticros_sim/share/agenticros_sim/launch/sim_amr.launch.py" ]]; then
+  log "agenticros_sim not built in $COLCON_WS — building now..."
+  (cd "$COLCON_WS" && colcon build --symlink-install --packages-select agenticros_sim agenticros_msgs)
+fi
+
+# shellcheck disable=SC1090
+source "$COLCON_WS/install/setup.bash"
+
+# ---------- Dependency checks ----------
+for bin in gz ros2 rviz2; do
+  if ! command -v "$bin" >/dev/null; then
+    if [[ "$bin" == "rviz2" ]] && [[ "$USE_RVIZ" != "true" ]]; then
+      continue
+    fi
+    err "Required binary '$bin' not found on PATH."
+    exit 1
+  fi
+done
+
+# ---------- Wire up environment ----------
+if [[ -n "$NAMESPACE" ]]; then
+  export AGENTICROS_ROBOT_NAMESPACE="$NAMESPACE"
+fi
+
+# Jetson rendering fix. On Tegra boards Mesa is picked first and tries to load
+# nvidia-drm_dri.so which doesn't exist, so the gz GUI viewport comes up solid
+# white. We:
+#   1. Point libglvnd at the NVIDIA EGL vendor (works for some Jetson L4T images)
+#   2. As a fallback, allow AGENTICROS_GZ_SOFTWARE_RENDER=1 to force llvmpipe -
+#      slow (~5 fps) but actually renders the world so the demo is viewable.
+# Honor AGENTICROS_GZ_NO_TWEAKS=1 to skip both (useful on x86/laptops).
+if [[ "$GUI" == "true" ]] && [[ -z "${AGENTICROS_GZ_NO_TWEAKS:-}" ]]; then
+  if [[ -f /usr/lib/aarch64-linux-gnu/tegra-egl/libEGL_nvidia.so.0 ]]; then
+    log "Jetson detected: forcing NVIDIA EGL/GL vendor"
+    export __GLX_VENDOR_LIBRARY_NAME="${__GLX_VENDOR_LIBRARY_NAME:-nvidia}"
+    export __EGL_VENDOR_LIBRARY_FILENAMES="${__EGL_VENDOR_LIBRARY_FILENAMES:-/usr/share/glvnd/egl_vendor.d/10_nvidia.json}"
+    export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu/tegra-egl:/usr/lib/aarch64-linux-gnu/tegra:${LD_LIBRARY_PATH:-}"
+  fi
+  if [[ -n "${AGENTICROS_GZ_SOFTWARE_RENDER:-}" ]]; then
+    log "AGENTICROS_GZ_SOFTWARE_RENDER set - forcing Mesa llvmpipe software renderer"
+    export LIBGL_ALWAYS_SOFTWARE=1
+    export GALLIUM_DRIVER=llvmpipe
+    export MESA_GL_VERSION_OVERRIDE=4.5
+    export OGRE_RTT_MODE=Copy
+  fi
+fi
+
+LAUNCH_ARGS=(
+  "use_rviz:=$USE_RVIZ"
+  "gui:=$GUI"
+)
+
+# ---------- Pick launch file ----------
+case "$ROBOT" in
+  amr) LAUNCH_FILE="sim_amr.launch.py" ;;
+  arm) LAUNCH_FILE="sim_arm.launch.py" ;;  # Phase 3
+esac
+
+if ! ros2 launch --help >/dev/null 2>&1; then
+  err "ros2 launch not available — is ROS sourced properly?"
+  exit 1
+fi
+
+log "Logging to $LOG_FILE"
+log "ros2 launch agenticros_sim $LAUNCH_FILE ${LAUNCH_ARGS[*]}"
+log "Press Ctrl+C to stop (or run \`agenticros down\` from another terminal)."
+
+# Run in foreground so Ctrl+C in the parent shell still works, but also tee to
+# the log so `agenticros logs sim -f` can follow concurrently. The PID we record
+# is this script's own PID so `agenticros down` SIGTERMs the whole subtree.
+echo "$$" > "$PID_FILE"
+
+# Use `exec` so the ros2 process replaces our shell — that way signals from the
+# CLI (or Ctrl+C) hit ros2 directly instead of bash.
+exec 1> >(tee -a "$LOG_FILE") 2>&1
+exec ros2 launch agenticros_sim "$LAUNCH_FILE" "${LAUNCH_ARGS[@]}"

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);
+});

package/runtime/scripts/test-mcp-e2e.mjs

@@ -0,0 +1,184 @@
+#!/usr/bin/env node
+/**
+ * Spin up the @agenticros/claude-code MCP server as a child process and exercise
+ * a real JSON-RPC session over stdio. Used for end-to-end testing of the MCP
+ * tools against a live sim (or real robot).
+ *
+ * Usage:
+ *   node scripts/test-mcp-e2e.mjs
+ *
+ * Honours AGENTICROS_CONFIG_PATH if set; otherwise uses ~/.agenticros/config.json.
+ */
+
+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}`);
+});
+child.on("exit", (code, sig) => {
+  process.stderr.write(`[mcp] exited code=${code} sig=${sig}\n`);
+});
+
+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 (e) {
+      process.stderr.write(`[mcp] non-json line: ${line}\n`);
+      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.code}: ${msg.error.message}`));
+      else resolve(msg.result);
+    }
+  }
+});
+
+function rpc(method, params = {}, timeoutMs = 20000) {
+  const id = nextId++;
+  const payload = { jsonrpc: "2.0", id, method, params };
+  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(payload) + "\n");
+  });
+}
+
+function summarise(content, max = 4000) {
+  if (!Array.isArray(content)) return JSON.stringify(content).slice(0, max);
+  return content
+    .map((c) => {
+      if (c.type === "text") {
+        const t = c.text ?? "";
+        return t.length > max ? `${t.slice(0, max)} … (+${t.length - max} chars)` : t;
+      }
+      if (c.type === "image") return `[image base64 len=${(c.data ?? "").length} mime=${c.mimeType}]`;
+      return `[${c.type}]`;
+    })
+    .join("\n");
+}
+
+async function callTool(name, args = {}, timeoutMs = 20000) {
+  const t0 = Date.now();
+  try {
+    const result = await rpc("tools/call", { name, arguments: args }, timeoutMs);
+    const ms = Date.now() - t0;
+    const ok = result?.isError ? "ERR" : "ok ";
+    console.log(`[${ok}] (${ms.toString().padStart(5)}ms) ${name}`);
+    console.log(`        ${summarise(result?.content)}`);
+    return result;
+  } catch (e) {
+    const ms = Date.now() - t0;
+    console.log(`[ERR] (${ms.toString().padStart(5)}ms) ${name}  ${e.message}`);
+    return null;
+  }
+}
+
+async function main() {
+  console.log("=== MCP E2E harness ===");
+
+  console.log("\n-- initialize --");
+  const init = await rpc("initialize", {
+    protocolVersion: "2024-11-05",
+    capabilities: { tools: {} },
+    clientInfo: { name: "agenticros-e2e", version: "0.0.1" },
+  });
+  console.log(`server: ${init.serverInfo?.name} v${init.serverInfo?.version}`);
+  await rpc("notifications/initialized", {}).catch(() => {});
+
+  console.log("\n-- tools/list --");
+  const tl = await rpc("tools/list", {});
+  console.log(`  ${tl.tools?.length ?? 0} tools advertised`);
+  for (const t of tl.tools ?? []) {
+    console.log(`    - ${t.name}`);
+  }
+
+  console.log("\n-- ros2_list_topics --");
+  await callTool("ros2_list_topics", {}, 30000);
+
+  console.log("\n-- ros2_publish /cmd_vel (linear.x=0.2) --");
+  await callTool(
+    "ros2_publish",
+    {
+      topic: "/cmd_vel",
+      type: "geometry_msgs/msg/Twist",
+      message: { linear: { x: 0.2, y: 0, z: 0 }, angular: { x: 0, y: 0, z: 0 } },
+    },
+    10000,
+  );
+
+  console.log("\n-- ros2_subscribe_once /imu/data --");
+  await callTool(
+    "ros2_subscribe_once",
+    { topic: "/imu/data", type: "sensor_msgs/msg/Imu", timeoutMs: 5000 },
+    10000,
+  );
+
+  console.log("\n-- ros2_subscribe_once /scan --");
+  await callTool(
+    "ros2_subscribe_once",
+    { topic: "/scan", type: "sensor_msgs/msg/LaserScan", timeoutMs: 5000 },
+    10000,
+  );
+
+  console.log("\n-- ros2_camera_snapshot (RGB) --");
+  await callTool("ros2_camera_snapshot", {}, 15000);
+
+  console.log("\n-- ros2_depth_distance --");
+  await callTool("ros2_depth_distance", {}, 15000);
+
+  console.log("\n-- stop --");
+  await callTool(
+    "ros2_publish",
+    {
+      topic: "/cmd_vel",
+      type: "geometry_msgs/msg/Twist",
+      message: { linear: { x: 0, y: 0, z: 0 }, angular: { x: 0, y: 0, z: 0 } },
+    },
+    5000,
+  );
+
+  console.log("\n=== Done ===");
+  child.kill("SIGTERM");
+}
+
+main().catch((e) => {
+  console.error("Harness failed:", e);
+  child.kill("SIGKILL");
+  process.exit(1);
+});