agenticros 0.0.1 → 0.1.1

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

@@ -0,0 +1,139 @@
+/**
+ * Gemini chat loop: generateContent with ROS2 tools, handle function calls, return final text.
+ */
+
+import type { Content } from "@google/genai";
+import {
+  createPartFromBase64,
+  createPartFromFunctionResponse,
+  createPartFromText,
+  createUserContent,
+} from "@google/genai";
+import { GoogleGenAI } from "@google/genai";
+import type { AgenticROSConfig } from "@agenticros/core";
+import { buildGeminiTools } from "./tools.js";
+import { executeTool } from "./tools.js";
+
+/** Override with env GEMINI_MODEL (e.g. gemini-2.5-flash, gemini-2.0-flash). */
+const DEFAULT_MODEL = "gemini-2.5-flash";
+const MAX_TURNS = 20;
+
+function resolveModel(explicit?: string): string {
+  const fromEnv = process.env.GEMINI_MODEL?.trim();
+  return explicit ?? (fromEnv && fromEnv.length > 0 ? fromEnv : DEFAULT_MODEL);
+}
+
+export interface ChatOptions {
+  apiKey?: string;
+  model?: string;
+  systemInstruction?: string;
+}
+
+/**
+ * Run a single user message through Gemini with ROS2 tools. Repeats until the model
+ * returns a final text response (no more function calls) or MAX_TURNS is reached.
+ */
+export async function chatWithRobot(
+  userMessage: string,
+  config: AgenticROSConfig,
+  options: ChatOptions = {},
+): Promise<string> {
+  const apiKey = options.apiKey ?? process.env.GEMINI_API_KEY ?? process.env.GOOGLE_API_KEY;
+  if (!apiKey || apiKey.trim().length === 0) {
+    throw new Error("Gemini API key required. Set GEMINI_API_KEY or GOOGLE_API_KEY.");
+  }
+
+  const ai = new GoogleGenAI({ apiKey });
+  const model = resolveModel(options.model);
+
+  const tools = await buildGeminiTools(config);
+  const generateConfig = {
+    tools,
+    systemInstruction: options.systemInstruction ?? "You are a helpful assistant controlling a ROS2 robot. Use the provided tools to list topics, publish commands, read sensor data, call services, send action goals, get/set parameters, capture camera images, and read depth distance. Be concise and safe with velocity commands.",
+  };
+
+  let contents: Content[] = [createUserContent(userMessage)];
+  let turns = 0;
+  let lastToolOutputs: string[] = [];
+
+  while (turns < MAX_TURNS) {
+    turns++;
+    let response;
+    try {
+      response = await ai.models.generateContent({
+        model,
+        contents,
+        config: generateConfig,
+      });
+    } catch (err) {
+      const message = err instanceof Error ? err.message : String(err);
+      if (
+        (message.includes("429") || message.includes("RESOURCE_EXHAUSTED") || message.toLowerCase().includes("quota")) &&
+        lastToolOutputs.length > 0
+      ) {
+        return (
+          `${lastToolOutputs.join("\n")}\n\n` +
+          "(Gemini quota hit while composing final response. Returning latest tool output directly.)"
+        );
+      }
+      throw err;
+    }
+
+    const functionCalls = response.functionCalls;
+    if (!functionCalls || functionCalls.length === 0) {
+      const text = response.text?.trim();
+      return text ?? "(No text response from model.)";
+    }
+
+    // Build model turn (so we can send it back in history). Use candidate content if available.
+    const modelContent: Content = response.candidates?.[0]?.content ?? {
+      role: "model",
+      parts: functionCalls.map((fc) => ({ functionCall: { name: fc.name, args: fc.args ?? {}, id: fc.id } })),
+    };
+
+    // Execute each function call and build function response parts.
+    const responseParts = [];
+    lastToolOutputs = [];
+    const additionalUserContents: Content[] = [];
+    for (const fc of functionCalls) {
+      const name = fc.name ?? "unknown";
+      const args = (fc.args ?? {}) as Record<string, unknown>;
+      const id = fc.id ?? `call_${turns}_${name}`;
+      if (process.stderr?.write) {
+        process.stderr.write(`[AgenticROS] Tool: ${name}(${JSON.stringify(args)})\n`);
+      }
+      let output: string;
+      let parts: import("@google/genai").FunctionResponsePart[] | undefined;
+      try {
+        const result = await executeTool(name, args, config);
+        output = result.output;
+        parts = result.parts;
+        if (result.inlineImage) {
+          // Models that reject multimodal function responses can still reason over
+          // image bytes when provided in a regular user multimodal turn.
+          additionalUserContents.push(
+            createUserContent([
+              createPartFromText(`Image returned by tool ${name}. Use it to answer the user request.`),
+              createPartFromBase64(result.inlineImage.data, result.inlineImage.mimeType),
+            ]),
+          );
+        }
+      } catch (err) {
+        output = err instanceof Error ? err.message : String(err);
+      }
+      const part = createPartFromFunctionResponse(id, name, { output }, parts);
+      responseParts.push(part);
+      lastToolOutputs.push(output);
+    }
+
+    // Next turn: previous conversation + model's function-call turn + our function responses as user content.
+    contents = [
+      ...contents,
+      modelContent,
+      createUserContent(responseParts),
+      ...additionalUserContents,
+    ];
+  }
+
+  return "(Max turns reached; model did not return a final text response.)";
+}

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

@@ -0,0 +1,92 @@
+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";
+
+/** Same as Claude Code MCP: optional `robot.namespace` override from the environment. */
+function applyMcpEnvOverrides(config: AgenticROSConfig): AgenticROSConfig {
+  const ns = process.env.AGENTICROS_ROBOT_NAMESPACE?.trim();
+  if (!ns) return config;
+  return {
+    ...config,
+    robot: {
+      ...config.robot,
+      namespace: ns,
+    },
+  };
+}
+
+/**
+ * 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) {
+    return path.resolve(env);
+  }
+  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
+    ? path.resolve(openclawEnv)
+    : path.join(os.homedir(), ".openclaw", "openclaw.json");
+  try {
+    const raw = fs.readFileSync(openclawPath, "utf8");
+    const parsed = JSON.parse(raw) as Record<string, unknown>;
+    const entries = parsed?.plugins && typeof (parsed.plugins as Record<string, unknown>).entries === "object"
+      ? (parsed.plugins as Record<string, unknown>).entries as Record<string, unknown>
+      : null;
+    const agenticros = entries?.agenticros && typeof entries.agenticros === "object"
+      ? entries.agenticros as Record<string, unknown>
+      : null;
+    const config = agenticros?.config;
+    if (config !== null && typeof config === "object") {
+      return config as Record<string, unknown>;
+    }
+  } catch {
+    // ignore
+  }
+  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 {
+  const primaryPath = getConfigPath();
+  try {
+    const raw = fs.readFileSync(primaryPath, "utf8");
+    const parsed = JSON.parse(raw) as unknown;
+    if (parsed !== null && typeof parsed === "object") {
+      const cfg = parseConfig(parsed as Record<string, unknown>);
+      if (process.stderr && typeof process.stderr.write === "function") {
+        process.stderr.write(`[AgenticROS] Config from ${primaryPath}\n`);
+      }
+      return applyMcpEnvOverrides(cfg);
+    }
+  } catch (err) {
+    const nodeErr = err as NodeJS.ErrnoException;
+    if (nodeErr.code === "ENOENT") {
+      const openclawConfig = tryOpenClawConfig();
+      if (openclawConfig) {
+        return applyMcpEnvOverrides(parseConfig(openclawConfig));
+      }
+      throw new Error(
+        `AgenticROS config not found at ${primaryPath}. Create it or set AGENTICROS_CONFIG_PATH. ` +
+        "See README for an example config.",
+      );
+    }
+    throw err;
+  }
+  return applyMcpEnvOverrides(parseConfig({}));
+}

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

@@ -0,0 +1,173 @@
+/**
+ * Sample distance (meters) from a ROS2 depth image topic.
+ * Same logic as OpenClaw and Claude Code adapters.
+ */
+
+import type { RosTransport } from "@agenticros/core";
+import {
+  ROS_MSG_IMAGE,
+  coerceRosImageDataToBuffer,
+  normalizeDepthImageEncoding,
+  rosBoolField,
+  rosNumericField,
+  rosStringField,
+} from "@agenticros/ros-camera";
+
+function depthImageDataBytes(data: unknown): Uint8Array {
+  try {
+    return new Uint8Array(coerceRosImageDataToBuffer(data));
+  } catch (e) {
+    const hint = e instanceof Error ? e.message : String(e);
+    const kind =
+      data == null
+        ? "null"
+        : `${typeof data}${typeof data === "object" && data !== null ? ` (${(data as object).constructor?.name ?? "Object"})` : ""}`;
+    throw new Error(`Depth image bytes: ${hint} (data field: ${kind})`);
+  }
+}
+
+function bytesPerPixelForDepthEncoding(encoding: string): number {
+  return normalizeDepthImageEncoding(encoding) === "32FC1" ? 4 : 2;
+}
+
+const DEPTH_SAMPLE_MAX_M = 40;
+const DEPTH_REPORT_PERCENTILE = 12;
+
+function sanitizeDepthSamplesMeters(values: number[]): number[] {
+  return values.filter((v) => Number.isFinite(v) && v > 0 && v <= DEPTH_SAMPLE_MAX_M);
+}
+
+function percentileLowerSorted(sortedAsc: number[], p: number): number {
+  const n = sortedAsc.length;
+  if (n === 0) return NaN;
+  if (n === 1) return sortedAsc[0]!;
+  const pp = Math.max(0, Math.min(100, p));
+  const idx = Math.min(n - 1, Math.floor((pp / 100) * n));
+  return sortedAsc[idx]!;
+}
+
+export function sampleDepthMeters(
+  width: number,
+  height: number,
+  step: number,
+  encoding: string,
+  data: Uint8Array,
+  centerFraction = 0.3,
+  isBigEndian = false,
+): number[] {
+  const enc = normalizeDepthImageEncoding(encoding);
+  const values: number[] = [];
+  const cx = width / 2;
+  const cy = height / 2;
+  const halfW = Math.max(1, Math.floor((width * centerFraction) / 2));
+  const halfH = Math.max(1, Math.floor((height * centerFraction) / 2));
+  const x0 = Math.max(0, Math.floor(cx - halfW));
+  const x1 = Math.min(width, Math.floor(cx + halfW));
+  const y0 = Math.max(0, Math.floor(cy - halfH));
+  const y1 = Math.min(height, Math.floor(cy + halfH));
+
+  if (enc === "16UC1") {
+    for (let y = y0; y < y1; y++) {
+      for (let x = x0; x < x1; x++) {
+        const off = y * step + x * 2;
+        if (off + 2 > data.length) continue;
+        const lo = data[off];
+        const hi = data[off + 1];
+        const v = isBigEndian ? (lo << 8) | hi : (hi << 8) | lo;
+        if (v > 0) values.push(v / 1000);
+      }
+    }
+  } else if (enc === "32FC1") {
+    for (let y = y0; y < y1; y++) {
+      for (let x = x0; x < x1; x++) {
+        const off = y * step + x * 4;
+        if (off + 4 > data.length) continue;
+        const v = new DataView(data.buffer, data.byteOffset + off, 4).getFloat32(0, !isBigEndian);
+        if (Number.isFinite(v) && v > 0) values.push(v);
+      }
+    }
+  } else {
+    throw new Error(
+      `Unsupported depth encoding: "${encoding}" (interpreted as "${enc}"). Use 16UC1/mono16 (mm) or 32FC1 (m).`,
+    );
+  }
+  return values;
+}
+
+function median(sorted: number[]): number {
+  if (sorted.length === 0) return NaN;
+  const m = sorted.length >> 1;
+  if (sorted.length % 2 === 1) return sorted[m];
+  return (sorted[m - 1] + sorted[m]) / 2;
+}
+
+export interface DepthSampleResult {
+  distance_m: number;
+  median_m: number;
+  valid: boolean;
+  topic: string;
+  encoding: string;
+  width: number;
+  height: number;
+  sample_count: number;
+  min_m: number;
+  max_m: number;
+}
+
+export async function getDepthDistance(
+  transport: RosTransport,
+  topic: string,
+  timeoutMs = 5000,
+): Promise<DepthSampleResult> {
+  const result = await new Promise<Record<string, unknown>>((resolve, reject) => {
+    const sub = transport.subscribe(
+      { topic, type: ROS_MSG_IMAGE },
+      (msg: Record<string, unknown>) => {
+        clearTimeout(timer);
+        sub.unsubscribe();
+        resolve(msg);
+      },
+    );
+    const timer = setTimeout(() => {
+      sub.unsubscribe();
+      reject(
+        new Error(
+          `Depth snapshot timeout on ${topic} (${timeoutMs}ms). No sensor_msgs/Image received—check topic and that it publishes raw depth (not CompressedImage only). With Zenoh, check the gateway log for CDR decode warnings.`,
+        ),
+      );
+    }, timeoutMs);
+  });
+
+  const encoding = normalizeDepthImageEncoding(rosStringField(result.encoding, "16UC1"));
+  const bpp = bytesPerPixelForDepthEncoding(encoding);
+  const width = rosNumericField(result.width, "width");
+  const height = rosNumericField(result.height, "height");
+  const step =
+    result.step != null && result.step !== ""
+      ? rosNumericField(result.step, "step")
+      : width * bpp;
+  const isBigEndian = rosBoolField(result.is_bigendian);
+  const data = depthImageDataBytes(result.data);
+
+  const values = sanitizeDepthSamplesMeters(
+    sampleDepthMeters(width, height, step, encoding, data, 0.3, isBigEndian),
+  );
+  const sorted = values.slice().sort((a, b) => a - b);
+  const distance_m = percentileLowerSorted(sorted, DEPTH_REPORT_PERCENTILE);
+  const median_m = median(sorted);
+  const min_m = sorted.length ? sorted[0] : NaN;
+  const max_m = sorted.length ? sorted[sorted.length - 1] : NaN;
+
+  return {
+    distance_m: Math.round(distance_m * 1000) / 1000,
+    median_m: Math.round(median_m * 1000) / 1000,
+    valid: sorted.length > 0 && Number.isFinite(distance_m),
+    topic,
+    encoding,
+    width,
+    height,
+    sample_count: sorted.length,
+    min_m: Math.round(min_m * 1000) / 1000,
+    max_m: Math.round(max_m * 1000) / 1000,
+  };
+}

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

@@ -0,0 +1,58 @@
+#!/usr/bin/env node
+/**
+ * AgenticROS Gemini CLI — chat with your ROS2 robot using Google Gemini.
+ *
+ * Usage:
+ *   GEMINI_API_KEY=xxx agenticros-gemini "What do you see?"
+ *   GEMINI_API_KEY=xxx agenticros-gemini   # read message from stdin
+ *
+ * Config: AGENTICROS_CONFIG_PATH or ~/.agenticros/config.json (same as other adapters).
+ * Optional: GEMINI_MODEL (default gemini-2.5-flash) if you hit quota on a specific model.
+ */
+
+import { loadConfig } from "./config.js";
+import { connect, disconnect } from "./transport.js";
+import { chatWithRobot } from "./chat.js";
+import { renderAgenticROSBanner } from "@agenticros/core";
+
+async function main(): Promise<void> {
+  process.stderr.write(renderAgenticROSBanner({ color: process.stderr.isTTY }) + "\n\n");
+
+  const args = process.argv.slice(2);
+  const normalizedArgs = args.length > 0 && args[0] === "--" ? args.slice(1) : args;
+  let userMessage: string;
+  if (normalizedArgs.length >= 1 && normalizedArgs[0].trim().length > 0) {
+    userMessage = normalizedArgs.join(" ").trim();
+  } else {
+    userMessage = await readStdin();
+  }
+  if (!userMessage) {
+    process.stderr.write("Usage: agenticros-gemini \"<message>\" or pipe message via stdin.\n");
+    process.stderr.write("Set GEMINI_API_KEY (or GOOGLE_API_KEY) in the environment.\n");
+    process.exit(1);
+  }
+
+  const config = loadConfig();
+  await connect(config);
+
+  try {
+    const response = await chatWithRobot(userMessage, config);
+    process.stdout.write(response + "\n");
+  } finally {
+    await disconnect();
+  }
+}
+
+function readStdin(): Promise<string> {
+  return new Promise((resolve) => {
+    const chunks: Buffer[] = [];
+    process.stdin.setEncoding("utf8");
+    process.stdin.on("data", (chunk: string | Buffer) => chunks.push(Buffer.from(chunk)));
+    process.stdin.on("end", () => resolve(Buffer.concat(chunks).toString("utf8").trim()));
+  });
+}
+
+main().catch((err) => {
+  process.stderr.write(String(err instanceof Error ? err.message : err) + "\n");
+  process.exit(1);
+});

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

@@ -0,0 +1,32 @@
+import type { AgenticROSConfig, MemoryProvider } from "@agenticros/core";
+import { createMemory } from "@agenticros/core";
+
+let provider: MemoryProvider | null = null;
+let initialized = false;
+
+/**
+ * Lazy-initialize the memory provider from config.
+ *
+ * Gemini CLI is one-shot (each `agenticros-gemini "..."` invocation is a fresh
+ * process), so this is effectively per-invocation. We still cache so the
+ * `buildGeminiTools` and `executeTool` paths don't double-load it.
+ */
+export async function ensureMemory(
+  config: AgenticROSConfig,
+): Promise<MemoryProvider | null> {
+  if (initialized) return provider;
+  initialized = true;
+  try {
+    provider = await createMemory(config);
+  } catch (err) {
+    process.stderr?.write(
+      `[AgenticROS] memory: init failed — ${err instanceof Error ? err.message : String(err)}\n`,
+    );
+    provider = null;
+  }
+  return provider;
+}
+
+export function getMemory(): MemoryProvider | null {
+  return provider;
+}

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

@@ -0,0 +1,44 @@
+import type { AgenticROSConfig } from "@agenticros/core";
+
+/**
+ * Minimal safety check for ros2_publish (Twist velocity limits).
+ * Same logic as OpenClaw and Claude Code adapters.
+ */
+export function checkPublishSafety(
+  config: AgenticROSConfig,
+  params: Record<string, unknown>,
+): { block: boolean; blockReason?: string } {
+  const safety = config.safety ?? {};
+  const maxLinear = safety.maxLinearVelocity ?? 1.0;
+  const maxAngular = safety.maxAngularVelocity ?? 1.5;
+
+  const msg = params["message"] as Record<string, unknown> | undefined;
+  if (!msg) return { block: false };
+
+  const linear = msg["linear"] as Record<string, number> | undefined;
+  const angular = msg["angular"] as Record<string, number> | undefined;
+
+  if (linear) {
+    const speed = Math.sqrt(
+      (linear["x"] ?? 0) ** 2 + (linear["y"] ?? 0) ** 2 + (linear["z"] ?? 0) ** 2,
+    );
+    if (speed > maxLinear) {
+      return {
+        block: true,
+        blockReason: `Linear velocity ${speed.toFixed(2)} m/s exceeds safety limit of ${maxLinear} m/s`,
+      };
+    }
+  }
+
+  if (angular) {
+    const rate = Math.abs(angular["z"] ?? 0);
+    if (rate > maxAngular) {
+      return {
+        block: true,
+        blockReason: `Angular velocity ${rate.toFixed(2)} rad/s exceeds safety limit of ${maxAngular} rad/s`,
+      };
+    }
+  }
+
+  return { block: false };
+}