agenticros 0.0.1 → 0.1.1

package/runtime/packages/agenticros/src/teleop/routes.ts

@@ -0,0 +1,650 @@
+import type { OpenClawPluginApi, HttpRouteRequest, HttpRouteResponse } from "../plugin-api.js";
+import type { AgenticROSConfig, ConnectionStatus } from "@agenticros/core";
+import {
+  toNamespacedTopicFull,
+  toTeleopCameraTopicShort,
+  resolveCameraSubscribeTopic,
+  applyCmdVelTwistSignConvention,
+} from "@agenticros/core";
+import { getTransport, getTransportOrNull, getTransportMode, tryReconnectFromFile } from "../service.js";
+import { readAgenticROSConfigFromFile } from "../config-file.js";
+import { getTeleopPageHtml } from "./page.js";
+import { ROS_MSG_COMPRESSED_IMAGE, bufferAndMimeFromCompressedImageMessage } from "@agenticros/ros-camera";
+
+const TWIST_TYPE = "geometry_msgs/msg/Twist";
+
+/** Image/CompressedImage type names (for filtering topics). */
+const IMAGE_TYPE_PATTERN = /Image|CompressedImage/i;
+/** When type is unknown (e.g. Zenoh), treat topic as camera if name looks like one. */
+const CAMERA_TOPIC_NAME_PATTERN = /camera|image|compressed/i;
+
+/**
+ * Preset topics when Zenoh discovery returns nothing (remote-api). Values are full ROS names like `ros2 topic list`.
+ */
+const COMMON_CAMERA_TOPIC_PRESETS: ReadonlyArray<string> = [
+  "/camera/camera/color/image_raw/compressed",
+  "/camera/camera/color/image_raw/compressed/zstd",
+  "/camera/image_raw/compressed",
+  "/image_raw/compressed",
+  "/camera/camera/infra1/image_rect_raw/compressed",
+  "/camera/camera/infra2/image_rect_raw/compressed",
+  "/zed/zed_node/rgb/image_rect_color/compressed",
+  "/usb_cam/image_raw/compressed",
+];
+
+function parseUrl(req: HttpRouteRequest): { pathname: string; searchParams: URLSearchParams } {
+  const base = "http://localhost";
+  const url = new URL(req.url ?? "", base);
+  return { pathname: url.pathname, searchParams: url.searchParams };
+}
+
+/** Machine-readable reason for teleop/camera JSON errors (client UI / automation). */
+export type TeleopCameraErrorCode =
+  | "unsupported_image_type"
+  | "transport_unready"
+  | "transport_not_connected"
+  | "worker"
+  | "timeout"
+  | "unknown";
+
+function teleopCameraErrorBody(code: TeleopCameraErrorCode, error: string): string {
+  return JSON.stringify({ code, error });
+}
+
+function readRequestBody(stream: NodeJS.ReadableStream): Promise<string> {
+  return new Promise((resolve, reject) => {
+    const chunks: Buffer[] = [];
+    stream.on("data", (chunk: Buffer | string) => {
+      chunks.push(Buffer.isBuffer(chunk) ? chunk : Buffer.from(chunk));
+    });
+    stream.on("end", () => resolve(Buffer.concat(chunks).toString("utf8")));
+    stream.on("error", reject);
+  });
+}
+
+function getDefaultCameraTopic(config: AgenticROSConfig): string {
+  const t = (config.teleop?.cameraTopic ?? "").trim();
+  if (t) return toTeleopCameraTopicShort(config, t);
+  const r = (config.robot?.cameraTopic ?? "").trim();
+  if (r) return toTeleopCameraTopicShort(config, r);
+  return "/camera/camera/color/image_raw/compressed";
+}
+
+/** Resolve cmd_vel topic from config (teleop override or robot namespace). Used by estop and teleop. */
+export function getCmdVelTopic(config: AgenticROSConfig): string {
+  const t = (config.teleop?.cmdVelTopic ?? "").trim();
+  if (t) return t;
+  return toNamespacedTopicFull(config, "/cmd_vel");
+}
+
+function clampTwist(
+  config: AgenticROSConfig,
+  linearX: number,
+  linearY: number,
+  linearZ: number,
+  angularX: number,
+  angularY: number,
+  angularZ: number,
+): { linear: { x: number; y: number; z: number }; angular: { x: number; y: number; z: number } } {
+  const maxLin = config.safety?.maxLinearVelocity ?? 1.0;
+  const maxAng = config.safety?.maxAngularVelocity ?? 1.5;
+
+  const linMag = Math.sqrt(linearX * linearX + linearY * linearY + linearZ * linearZ);
+  const scaleLin = linMag > maxLin && linMag > 0 ? maxLin / linMag : 1;
+  const angMag = Math.abs(angularZ);
+  const scaleAng = angMag > maxAng && angMag > 0 ? maxAng / angMag : 1;
+
+  return {
+    linear: {
+      x: linearX * scaleLin,
+      y: linearY * scaleLin,
+      z: linearZ * scaleLin,
+    },
+    angular: {
+      x: angularX * scaleAng,
+      y: angularY * scaleAng,
+      z: Math.max(-maxAng, Math.min(maxAng, angularZ)),
+    },
+  };
+}
+
+/** Shared latest-frame cache: one subscription per topic, serve from cache so polling doesn't timeout. */
+type CameraCacheState = {
+  topic: string;
+  sub: { unsubscribe(): void } | null;
+  cache: Buffer | null;
+  mime: string;
+  firstFrameResolve: ((r: { buf: Buffer; mime: string }) => void) | null;
+  firstFrameReject: ((e: Error) => void) | null;
+  firstFrameTimeout: ReturnType<typeof setTimeout> | null;
+};
+let cameraCacheState: CameraCacheState | null = null;
+/** Throttle camera timeout logs to avoid flooding (log at most once per 15s per topic). */
+let lastCameraTimeoutLog: { topic: string; at: number } = { topic: "", at: 0 };
+const CAMERA_TIMEOUT_LOG_INTERVAL_MS = 15_000;
+
+/**
+ * Register Phase 3 teleop HTTP routes (sources, camera, twist, index page).
+ * Only registers when api.registerHttpRoute is available.
+ */
+export function registerTeleopRoutes(api: OpenClawPluginApi, config: AgenticROSConfig): void {
+  const register = api.registerHttpRoute;
+  if (typeof register !== "function") {
+    api.logger.info("AgenticROS teleop: registerHttpRoute not available, skipping routes");
+    return;
+  }
+  const route = (opts: { path: string; method?: string; handler: (req: HttpRouteRequest, res: HttpRouteResponse) => void | Promise<void> }) =>
+    register({ ...opts, requireAuth: false, auth: "plugin" });
+
+  /** Use config from file so namespace/camera/topics apply without gateway restart; fallback to initial config. */
+  function getCurrentConfig(): AgenticROSConfig {
+    try {
+      return readAgenticROSConfigFromFile();
+    } catch {
+      return config;
+    }
+  }
+
+  const pingHandler = (_req: HttpRouteRequest, res: HttpRouteResponse) => {
+    res.setHeader("Content-Type", "application/json");
+    res.statusCode = 200;
+    res.end(JSON.stringify({ ok: true, agenticros: "teleop" }));
+  };
+
+  const statusHandler = (_req: HttpRouteRequest, res: HttpRouteResponse) => {
+    const t = getTransportOrNull();
+    const mode = getTransportMode();
+    const connected = t ? t.getStatus() === "connected" : false;
+    res.setHeader("Content-Type", "application/json");
+    res.statusCode = 200;
+    res.end(JSON.stringify({ mode: mode ?? "none", connected }));
+  };
+
+  const reconnectHandler = async (_req: HttpRouteRequest, res: HttpRouteResponse) => {
+    try {
+      const result = await tryReconnectFromFile(api);
+      res.setHeader("Content-Type", "application/json");
+      res.statusCode = 200;
+      res.end(JSON.stringify(result));
+    } catch (e) {
+      res.setHeader("Content-Type", "application/json");
+      res.statusCode = 500;
+      res.end(JSON.stringify({ ok: false, error: e instanceof Error ? e.message : String(e) }));
+    }
+  };
+
+  for (const base of ["/agenticros", "/api/agenticros", "/plugins/agenticros"]) {
+    route({ path: `${base}/teleop/ping`, method: "GET", handler: pingHandler });
+    route({ path: `${base}/teleop/status`, method: "GET", handler: statusHandler });
+    route({ path: `${base}/teleop/reconnect`, method: "GET", handler: reconnectHandler });
+    route({ path: `${base}/teleop/reconnect`, method: "POST", handler: reconnectHandler });
+  }
+
+  /** Config default + preset paths + Zenoh discovery (deduped). Dropdown uses short `/camera/...` paths; server namespaces on subscribe. */
+  function buildTeleopCameraSources(
+    cfg: AgenticROSConfig,
+    defaultTopic: string,
+    discovered: Array<{ topic: string; label?: string }>,
+  ): Array<{ topic: string; label?: string }> {
+    const seen = new Set<string>();
+    const out: Array<{ topic: string; label?: string }> = [];
+    const add = (topic: string) => {
+      const short = toTeleopCameraTopicShort(cfg, topic);
+      if (!short || short === "/" || seen.has(short)) return;
+      seen.add(short);
+      out.push({ topic: short, label: short });
+    };
+    add(defaultTopic);
+    for (const preset of COMMON_CAMERA_TOPIC_PRESETS) add(preset);
+    for (const d of discovered) add(d.topic);
+    return out;
+  }
+
+  // GET .../teleop/sources — JSON list of camera topics. Always returns at least one option (default camera) when discovery is empty or transport fails.
+  const sourcesHandler = async (_req: HttpRouteRequest, res: HttpRouteResponse) => {
+    try {
+      const currentConfig = getCurrentConfig();
+      const defaultTopic = getDefaultCameraTopic(currentConfig);
+      let list: Array<{ topic: string; label?: string }>;
+      const explicit = currentConfig.teleop?.cameraTopics ?? [];
+      if (explicit.length > 0) {
+        list = explicit.map((o) => {
+          const topic = toTeleopCameraTopicShort(currentConfig, o.topic.trim());
+          const label = (o.label ?? "").trim() || topic;
+          return { topic, label };
+        });
+      } else {
+        try {
+          const transport = getTransport();
+          const topics = await transport.listTopics();
+          const imageTopics = topics.filter((t) => {
+            if (t.type && IMAGE_TYPE_PATTERN.test(t.type)) return true;
+            if (!t.type || t.type === "unknown") {
+              return CAMERA_TOPIC_NAME_PATTERN.test(t.name);
+            }
+            return false;
+          });
+          const compressedOnly = imageTopics.filter((t) => !/\/zstd\/?$/i.test(t.name));
+          const toList = compressedOnly.length > 0 ? compressedOnly : imageTopics;
+          toList.sort((a, b) => {
+            const aCompressed = /compressed/i.test(a.name) ? 1 : 0;
+            const bCompressed = /compressed/i.test(b.name) ? 1 : 0;
+            return bCompressed - aCompressed;
+          });
+          const discovered = toList.map((t) => {
+            const name = t.name.startsWith("/") ? t.name : `/${t.name}`;
+            return { topic: name, label: name };
+          });
+          list = buildTeleopCameraSources(currentConfig, defaultTopic, discovered);
+        } catch (e) {
+          api.logger.warn("Teleop sources (discovery failed, using presets): " + (e instanceof Error ? e.message : String(e)));
+          list = buildTeleopCameraSources(currentConfig, defaultTopic, []);
+        }
+      }
+      res.setHeader("Content-Type", "application/json");
+      res.statusCode = 200;
+      res.end(JSON.stringify(list));
+    } catch (e) {
+      api.logger.warn("Teleop sources error: " + (e instanceof Error ? e.message : String(e)));
+      res.setHeader("Content-Type", "application/json");
+      res.statusCode = 200;
+      try {
+        const cfg = getCurrentConfig();
+        const dt = getDefaultCameraTopic(cfg);
+        res.end(JSON.stringify(buildTeleopCameraSources(cfg, dt, [])));
+      } catch {
+        res.end(JSON.stringify(buildTeleopCameraSources(config, "/camera/camera/color/image_raw/compressed", [])));
+      }
+    }
+  };
+
+  for (const base of ["/agenticros", "/api/agenticros", "/plugins/agenticros"]) {
+    route({ path: `${base}/teleop/sources`, method: "GET", handler: sourcesHandler });
+  }
+
+  // GET .../teleop/camera?topic=...&type=compressed|image
+  const cameraHandler = async (req: HttpRouteRequest, res: HttpRouteResponse) => {
+      const currentConfig = getCurrentConfig();
+      const { searchParams } = parseUrl(req);
+      let topic = searchParams.get("topic")?.trim();
+      if (topic) topic = topic.replace(/\?.*$/, "").replace(/#.*$/, "").trim();
+      if (!topic) topic = getDefaultCameraTopic(currentConfig);
+      if (topic && /\/zstd\/?$/i.test(topic)) {
+        topic = topic.replace(/\/zstd\/?$/i, "/compressed");
+      }
+      if (topic && /image_raw$/i.test(topic) && !/compressed|zstd/i.test(topic)) {
+        topic = topic.replace(/\/?$/, "") + "/compressed";
+      }
+      const typeParam = (searchParams.get("type") ?? "compressed").toLowerCase();
+      const useImage = typeParam === "image";
+      // Camera topics are often absolute `/camera/...` on the DDS graph while cmd_vel is `/<robot.namespace>/cmd_vel`.
+      // Full namespacing every path breaks those feeds (Zenoh key mismatch → timeout → 503).
+      const resolvedTopic = resolveCameraSubscribeTopic(currentConfig, topic);
+      if (cameraCacheState?.topic !== resolvedTopic) {
+        api.logger.info(`Teleop camera: topic=${topic} resolvedTopic=${resolvedTopic} namespace=${(currentConfig.robot?.namespace ?? "").trim() || "(none)"}`);
+      }
+
+      if (useImage) {
+        res.setHeader("Content-Type", "application/json");
+        res.setHeader("Cache-Control", "no-store");
+        res.setHeader("Pragma", "no-cache");
+        res.statusCode = 501;
+        res.end(
+          teleopCameraErrorBody(
+            "unsupported_image_type",
+            "Raw Image topics not supported; use a CompressedImage topic (e.g. .../image_raw/compressed)",
+          ),
+        );
+        return;
+      }
+
+      try {
+        if (cameraCacheState?.topic !== resolvedTopic) {
+          if (cameraCacheState?.sub) {
+            cameraCacheState.sub.unsubscribe();
+            if (cameraCacheState.firstFrameTimeout) clearTimeout(cameraCacheState.firstFrameTimeout);
+          }
+          cameraCacheState = {
+            topic: resolvedTopic,
+            sub: null,
+            cache: null,
+            mime: "image/jpeg",
+            firstFrameResolve: null,
+            firstFrameReject: null,
+            firstFrameTimeout: null,
+          };
+        }
+        const state = cameraCacheState;
+
+        if (state.cache && state.cache.length > 0) {
+          res.setHeader("Content-Type", state.mime);
+          res.setHeader("Cache-Control", "no-store, no-cache, must-revalidate");
+          res.setHeader("Pragma", "no-cache");
+          res.statusCode = 200;
+          res.end(state.cache);
+          return;
+        }
+
+        if (!state.sub) {
+          const transport = getTransportOrNull();
+          if (!transport) {
+            res.setHeader("Content-Type", "application/json");
+            res.setHeader("Cache-Control", "no-store");
+            res.setHeader("Pragma", "no-cache");
+            res.statusCode = 503;
+            res.end(
+              teleopCameraErrorBody(
+                "transport_unready",
+                "ROS2 transport not ready (no session on this process). Start Zenoh/rosbridge, then Reconnect — or use a single gateway worker so status and camera hit the same process.",
+              ),
+            );
+            return;
+          }
+          const camConn: ConnectionStatus = transport.getStatus();
+          if (camConn !== "connected") {
+            res.setHeader("Content-Type", "application/json");
+            res.setHeader("Cache-Control", "no-store");
+            res.setHeader("Pragma", "no-cache");
+            res.statusCode = 503;
+            res.end(
+              teleopCameraErrorBody(
+                "transport_not_connected",
+                `ROS2 transport not connected (status: ${camConn}). Wait for the session or fix Zenoh/rosbridge; see gateway logs.`,
+              ),
+            );
+            return;
+          }
+          const firstFramePromise = new Promise<{ buf: Buffer; mime: string }>((resolve, reject) => {
+            state.firstFrameResolve = resolve;
+            state.firstFrameReject = reject;
+            state.firstFrameTimeout = setTimeout(() => {
+              if (state.firstFrameReject) {
+                state.firstFrameReject(new Error("timeout"));
+                state.firstFrameResolve = null;
+                state.firstFrameReject = null;
+                state.firstFrameTimeout = null;
+              }
+            }, 8000);
+          });
+          const handler = (msg: Record<string, unknown>) => {
+            const out = bufferAndMimeFromCompressedImageMessage(msg);
+            if (!out || out.buf.length === 0) return;
+            state.mime = out.mime;
+            state.cache = out.buf;
+            if (state.firstFrameTimeout) {
+              clearTimeout(state.firstFrameTimeout);
+              state.firstFrameTimeout = null;
+            }
+            if (state.firstFrameResolve) {
+              state.firstFrameResolve({ buf: out.buf, mime: state.mime });
+              state.firstFrameResolve = null;
+              state.firstFrameReject = null;
+            }
+          };
+          if (typeof (transport as { subscribeAsync?: unknown }).subscribeAsync === "function") {
+            state.sub = await (transport as { subscribeAsync(opts: { topic: string; type: string }, h: (msg: Record<string, unknown>) => void): Promise<{ unsubscribe(): void }> }).subscribeAsync(
+              { topic: resolvedTopic, type: ROS_MSG_COMPRESSED_IMAGE },
+              handler,
+            );
+          } else {
+            state.sub = transport.subscribe(
+              { topic: resolvedTopic, type: ROS_MSG_COMPRESSED_IMAGE },
+              handler,
+            );
+          }
+          const { buf, mime } = await firstFramePromise;
+          res.setHeader("Content-Type", mime);
+          res.setHeader("Cache-Control", "no-store, no-cache, must-revalidate");
+          res.setHeader("Pragma", "no-cache");
+          res.statusCode = 200;
+          res.end(buf);
+          return;
+        }
+
+        const waitForFirst = new Promise<{ buf: Buffer; mime: string }>((resolve, reject) => {
+          const deadline = Date.now() + 5000;
+          const check = () => {
+            if (state.cache && state.cache.length > 0) {
+              resolve({ buf: state.cache, mime: state.mime });
+              return;
+            }
+            if (Date.now() >= deadline) {
+              reject(new Error("timeout"));
+              return;
+            }
+            setTimeout(check, 30);
+          };
+          check();
+        });
+        const { buf, mime } = await waitForFirst;
+        res.setHeader("Content-Type", mime);
+        res.setHeader("Cache-Control", "no-store, no-cache, must-revalidate");
+        res.setHeader("Pragma", "no-cache");
+        res.statusCode = 200;
+        res.end(buf);
+      } catch (e) {
+        const raw = e instanceof Error ? e.message : String(e);
+        const isTimeout = /timeout/i.test(raw);
+        const now = Date.now();
+        if (!isTimeout || resolvedTopic !== lastCameraTimeoutLog.topic || now - lastCameraTimeoutLog.at >= CAMERA_TIMEOUT_LOG_INTERVAL_MS) {
+          api.logger.warn("Teleop camera error: " + raw);
+          if (isTimeout) lastCameraTimeoutLog = { topic: resolvedTopic, at: now };
+        }
+        res.setHeader("Content-Type", "application/json");
+        res.setHeader("Cache-Control", "no-store");
+        res.setHeader("Pragma", "no-cache");
+        res.statusCode = 503;
+        const { code, userMsg }: { code: TeleopCameraErrorCode; userMsg: string } = (() => {
+          if (/session|transport|not initialized|undefined/i.test(raw)) {
+            return {
+              code: "worker" as const,
+              userMsg:
+                "Camera unavailable. Transport may be on another gateway worker — try opening teleop on the gateway directly or run the gateway with a single worker.",
+            };
+          }
+          if (isTimeout) {
+            return {
+              code: "timeout" as const,
+              userMsg: `No CompressedImage frames on "${resolvedTopic}" before timeout. Confirm the topic exists and publishes sensor_msgs/msg/CompressedImage (e.g. ros2 topic echo); match robot namespace; check Zenoh/rosbridge and gateway logs.`,
+            };
+          }
+          return { code: "unknown" as const, userMsg: raw };
+        })();
+        res.end(teleopCameraErrorBody(code, userMsg));
+      }
+  };
+
+  for (const base of ["/agenticros", "/api/agenticros", "/plugins/agenticros"]) {
+    route({ path: `${base}/teleop/camera`, method: "GET", handler: cameraHandler });
+  }
+
+  // Shared: publish twist and send JSON response
+  async function publishTwistAndRespond(
+    res: HttpRouteResponse,
+    lx: number,
+    ly: number,
+    lz: number,
+    ax: number,
+    ay: number,
+    az: number,
+  ): Promise<void> {
+    const currentConfig = getCurrentConfig();
+    const clamped = clampTwist(currentConfig, lx, ly, lz, ax, ay, az);
+    const topic = getCmdVelTopic(currentConfig);
+    const twistMsg = applyCmdVelTwistSignConvention(topic, TWIST_TYPE, {
+      linear: clamped.linear,
+      angular: clamped.angular,
+    });
+    const lin = twistMsg["linear"] as Record<string, unknown> | undefined;
+    const ang = twistMsg["angular"] as Record<string, unknown> | undefined;
+    api.logger.info(`Teleop twist: publishing linear.x=${lin?.["x"]} angular.z=${ang?.["z"]} to topic=${topic}`);
+    try {
+      const transport = getTransport();
+      const status = transport.getStatus?.();
+      if (status && status !== "connected") {
+        res.setHeader("Content-Type", "application/json");
+        res.statusCode = 503;
+        res.end(JSON.stringify({ error: `ROS2 transport not connected (status: ${status}). Check Zenoh router and gateway logs.` }));
+        return;
+      }
+      const publishResult = transport.publish({
+        topic,
+        type: TWIST_TYPE,
+        msg: twistMsg,
+      });
+      await Promise.resolve(publishResult);
+      res.setHeader("Content-Type", "application/json");
+      res.statusCode = 200;
+      res.end(JSON.stringify({ ok: true, topic }));
+    } catch (e) {
+      const msg = e instanceof Error ? e.message : String(e);
+      api.logger.warn("Teleop twist error: " + msg);
+      res.setHeader("Content-Type", "application/json");
+      const isUnavailable = /not initialized|not connected/i.test(msg);
+      res.statusCode = isUnavailable ? 503 : 500;
+      res.end(JSON.stringify({ error: isUnavailable ? "ROS2 transport not ready. Start the gateway service and ensure Zenoh/rosbridge is connected." : "Failed to publish twist" }));
+    }
+  }
+
+  const twistPostHandler = async (req: HttpRouteRequest, res: HttpRouteResponse) => {
+    let body: Record<string, unknown> = {};
+    try {
+      if (typeof req.readJsonBody === "function") {
+        body = (await req.readJsonBody()) ?? {};
+      }
+      if (Object.keys(body).length === 0) {
+        const raw = (req as { body?: unknown }).body;
+        if (typeof raw === "object" && raw !== null) {
+          body = raw as Record<string, unknown>;
+        } else if (raw && typeof (raw as Promise<unknown>).then === "function") {
+          const parsed = await (raw as Promise<Record<string, unknown>>);
+          if (parsed && typeof parsed === "object") body = parsed;
+        }
+      }
+      if (Object.keys(body).length === 0 && typeof (req as { on?: (e: string, cb: (c?: Buffer) => void) => void }).on === "function") {
+        const rawBody = await readRequestBody(req as unknown as NodeJS.ReadableStream);
+        if (rawBody.trim()) {
+          try {
+            body = JSON.parse(rawBody) as Record<string, unknown>;
+          } catch {
+            try {
+              for (const part of rawBody.split("&")) {
+                const eq = part.indexOf("=");
+                const k = eq >= 0 ? decodeURIComponent(part.slice(0, eq).replace(/\+/g, " ")) : decodeURIComponent(part.replace(/\+/g, " "));
+                const v = eq >= 0 ? decodeURIComponent(part.slice(eq + 1).replace(/\+/g, " ")) : "";
+                if (k) body[k] = v;
+              }
+            } catch {
+              // leave body empty
+            }
+          }
+        }
+      }
+    } catch {
+      res.setHeader("Content-Type", "application/json");
+      res.statusCode = 400;
+      res.end(JSON.stringify({ error: "Invalid JSON body" }));
+      return;
+    }
+    let lx = Number(body.linear_x ?? (body as Record<string, unknown>).linearX ?? 0);
+    let ly = Number(body.linear_y ?? (body as Record<string, unknown>).linearY ?? 0);
+    let lz = Number(body.linear_z ?? (body as Record<string, unknown>).linearZ ?? 0);
+    let ax = Number(body.angular_x ?? (body as Record<string, unknown>).angularX ?? 0);
+    let ay = Number(body.angular_y ?? (body as Record<string, unknown>).angularY ?? 0);
+    let az = Number(body.angular_z ?? (body as Record<string, unknown>).angularZ ?? 0);
+    const bodyAllZero = lx === 0 && ly === 0 && lz === 0 && ax === 0 && ay === 0 && az === 0;
+    if (bodyAllZero) {
+      const fromQuery = getTwistParamsFromQuery(req);
+      if (fromQuery.source !== "none") {
+        lx = fromQuery.lx;
+        ly = fromQuery.ly;
+        lz = fromQuery.lz;
+        ax = fromQuery.ax;
+        ay = fromQuery.ay;
+        az = fromQuery.az;
+        api.logger.info(`Teleop twist POST: linear_x=${lx} linear_y=${ly} angular_z=${az} (from ${fromQuery.source}, body was empty/zeros)`);
+      } else {
+        api.logger.info(`Teleop twist POST: linear_x=0 linear_y=0 angular_z=0 (body empty/zeros, no query — open via proxy so query is forwarded)`);
+      }
+    } else {
+      api.logger.info(`Teleop twist POST: linear_x=${lx} linear_y=${ly} angular_z=${az}`);
+    }
+    await publishTwistAndRespond(res, lx, ly, lz, ax, ay, az);
+  };
+
+  /** Get a header value (case-insensitive). */
+  function getHeader(req: HttpRouteRequest, name: string): string | undefined {
+    const h = (req as { headers?: Record<string, string | string[] | undefined> }).headers;
+    if (!h) return undefined;
+    const lower = name.toLowerCase();
+    for (const [k, v] of Object.entries(h)) {
+      if (k.toLowerCase() === lower) {
+        return typeof v === "string" ? v : Array.isArray(v) ? (v[0] as string) : undefined;
+      }
+    }
+    return undefined;
+  }
+
+  /** Parse twist params from URL query string or X-AgenticROS-Query (used by GET and as POST fallback when body is zeros). */
+  function getTwistParamsFromQuery(req: HttpRouteRequest): { lx: number; ly: number; lz: number; ax: number; ay: number; az: number; source: string } {
+    const rawUrl = (req as { url?: string; originalUrl?: string }).url ?? (req as { originalUrl?: string }).originalUrl ?? "";
+    const forwardedQuery = getHeader(req, "x-agenticros-query");
+    const { searchParams } = parseUrl(req);
+    const q = (req as { query?: Record<string, string | string[] | undefined> }).query;
+    const getParam = (name: string, alt: string): string | undefined => {
+      const fromUrl = searchParams.get(name) ?? searchParams.get(alt);
+      if (fromUrl != null) return fromUrl;
+      if (q) {
+        const v = q[name] ?? q[alt];
+        return Array.isArray(v) ? v[0] : v;
+      }
+      const queryString = rawUrl.includes("?") ? rawUrl.slice(rawUrl.indexOf("?") + 1) : forwardedQuery;
+      if (queryString) {
+        try {
+          const params = new URLSearchParams(queryString);
+          return params.get(name) ?? params.get(alt) ?? undefined;
+        } catch {
+          // ignore
+        }
+      }
+      return undefined;
+    };
+    const lx = Number(getParam("linear_x", "linearX") ?? 0);
+    const ly = Number(getParam("linear_y", "linearY") ?? 0);
+    const lz = Number(getParam("linear_z", "linearZ") ?? 0);
+    const ax = Number(getParam("angular_x", "angularX") ?? 0);
+    const ay = Number(getParam("angular_y", "angularY") ?? 0);
+    const az = Number(getParam("angular_z", "angularZ") ?? 0);
+    const source = rawUrl.includes("?") ? "url" : forwardedQuery ? "header" : "none";
+    return { lx, ly, lz, ax, ay, az, source };
+  }
+
+  const twistGetHandler = async (req: HttpRouteRequest, res: HttpRouteResponse) => {
+    const { lx, ly, lz, ax, ay, az, source } = getTwistParamsFromQuery(req);
+    const allZero = lx === 0 && ly === 0 && lz === 0 && ax === 0 && ay === 0 && az === 0;
+    api.logger.info(`Teleop twist GET: linear_x=${lx} linear_y=${ly} angular_z=${az} (source: ${source})${allZero && source === "none" ? " — params missing: open teleop via proxy (http://127.0.0.1:18790/plugins/agenticros/) so twist query is forwarded" : ""}`);
+    await publishTwistAndRespond(res, lx, ly, lz, ax, ay, az);
+  };
+
+  for (const base of ["/agenticros", "/api/agenticros", "/plugins/agenticros"]) {
+    route({ path: `${base}/teleop/twist`, method: "POST", handler: twistPostHandler });
+    route({ path: `${base}/teleop/twist`, method: "GET", handler: twistGetHandler });
+  }
+
+  // GET .../teleop/ and .../teleop/index.html
+  const servePage = (_req: HttpRouteRequest, res: HttpRouteResponse) => {
+    const html = getTeleopPageHtml(getCurrentConfig());
+    res.setHeader("Content-Type", "text/html; charset=utf-8");
+    res.setHeader("Cache-Control", "no-store, no-cache, must-revalidate");
+    res.setHeader("Pragma", "no-cache");
+    res.statusCode = 200;
+    res.end(html);
+  };
+
+  for (const base of ["/agenticros", "/api/agenticros", "/plugins/agenticros"]) {
+    route({ path: `${base}/teleop/`, method: "GET", handler: servePage });
+    route({ path: `${base}/teleop/index.html`, method: "GET", handler: servePage });
+  }
+
+  api.logger.info("AgenticROS teleop routes registered (GET /agenticros/teleop/, /ping, /sources, /camera; GET/POST /twist)");
+}

package/runtime/packages/agenticros/src/tools/index.ts

@@ -0,0 +1,26 @@
+import type { OpenClawPluginApi } from "../plugin-api.js";
+import type { AgenticROSConfig } from "@agenticros/core";
+import { registerPublishTool } from "./ros2-publish.js";
+import { registerSubscribeTool } from "./ros2-subscribe.js";
+import { registerServiceTool } from "./ros2-service.js";
+import { registerActionTool } from "./ros2-action.js";
+import { registerParamTools } from "./ros2-param.js";
+import { registerIntrospectTool } from "./ros2-introspect.js";
+import { registerCameraTool } from "./ros2-camera.js";
+import { registerDepthDistanceTool } from "./ros2-depth-distance.js";
+
+/**
+ * Register core ROS2 tools with the OpenClaw AI agent.
+ * Optional skills (e.g. Follow Me) register their own tools via the skill loader.
+ * Memory tools register asynchronously from index.ts after initMemory resolves.
+ */
+export function registerTools(api: OpenClawPluginApi, config: AgenticROSConfig): void {
+  registerPublishTool(api, config);
+  registerSubscribeTool(api, config);
+  registerServiceTool(api, config);
+  registerActionTool(api, config);
+  registerParamTools(api, config);
+  registerIntrospectTool(api);
+  registerCameraTool(api, config);
+  registerDepthDistanceTool(api, config);
+}