agenticros 0.0.1 → 0.1.1

package/runtime/packages/agenticros/src/context/robot-context.ts

@@ -0,0 +1,373 @@
+import type { OpenClawPluginApi } from "../plugin-api.js";
+import type { AgenticROSConfig, MemoryRecord } from "@agenticros/core";
+import type { TopicInfo, ServiceInfo, ActionInfo } from "@agenticros/core";
+import { resolveMemoryNamespace } from "@agenticros/core";
+import { getTransport } from "../service.js";
+import { getLoadedSkillIds } from "../skill-loader.js";
+import { getMemory } from "../memory.js";
+
+/** Cached discovery results with TTL. */
+interface DiscoveryCache {
+  topics: TopicInfo[];
+  services: ServiceInfo[];
+  actions: ActionInfo[];
+  timestamp: number;
+}
+
+const CACHE_TTL_MS = 60_000; // 60s
+let cache: DiscoveryCache | null = null;
+
+/** Clear the discovery cache so the next agent start re-discovers capabilities. */
+export function clearDiscoveryCache(): void {
+  cache = null;
+}
+
+/**
+ * Register the before_agent_start hook to inject robot capabilities
+ * into the AI agent's system context.
+ */
+export function registerRobotContext(api: OpenClawPluginApi, config: AgenticROSConfig): void {
+  const robotName = config.robot.name;
+  const robotNamespace = config.robot.namespace;
+
+  // Reactive re-discovery: clear cache on transport reconnect
+  try {
+    const transport = getTransport();
+    transport.onConnection((status: string) => {
+      if (status === "connected") {
+        cache = null; // Force re-discovery on next agent start
+        api.logger.info("Transport reconnected — capability cache cleared");
+      }
+    });
+  } catch {
+    // Transport not initialized yet — will be set up by the service.
+    // The onConnection handler will be registered when the hook fires.
+  }
+
+  api.on("before_agent_start", async (_event, _ctx) => {
+    const capabilities = await discoverCapabilities(api, robotNamespace);
+    const cameraTopicHint =
+      (config.robot?.cameraTopic ?? "").trim() || "/camera/camera/color/image_raw/compressed";
+    const memorySection = await buildMemorySection(config);
+    const context =
+      buildRobotContext(config, robotName, robotNamespace, capabilities, cameraTopicHint) +
+      memorySection;
+    return { prependContext: context };
+  });
+}
+
+/**
+ * Build a "Memory" section that tells the LLM how to use memory tools and
+ * what's already stored. Returns "" when memory is disabled or unavailable,
+ * so non-memory users see no change in their context.
+ */
+async function buildMemorySection(config: AgenticROSConfig): Promise<string> {
+  if (!config.memory?.enabled) return "";
+  const memory = getMemory();
+  if (!memory) {
+    // Provider isn't ready yet (async init still pending). Skip silently;
+    // the section will appear on the next session.
+    return "";
+  }
+  const namespace = resolveMemoryNamespace(config);
+  let recent: MemoryRecord[] = [];
+  try {
+    recent = await memory.recent(namespace, 10);
+  } catch {
+    recent = [];
+  }
+  const recentBlock =
+    recent.length === 0
+      ? "_No memories saved yet for this robot._"
+      : recent
+          .map((r, i) => `${i + 1}. ${r.content.replace(/\s+/g, " ").trim()}`)
+          .join("\n");
+  return `\n\n### Memory (cross-session, ${memory.backend} backend)
+You have a shared long-term memory store. It is shared across **all** AgenticROS adapters talking to this robot — facts written from Claude Desktop, Claude Code, Gemini CLI, and this OpenClaw chat all live in the same store and surface here.
+
+**Always call \`memory_recall\` BEFORE answering** when the user asks a personal-context question, including:
+- "What do I have for X?", "What's my Y?", "Where is the Z?"
+- "What did I tell you about ...?", "Do you remember ...?", "What's my preference for ...?"
+- Anything that depends on facts the user previously shared about themselves, their robot setup, their home, or their preferences.
+
+**Call \`memory_remember\`** when the user explicitly says "remember that ...", "note that ...", "from now on ...", or shares a durable personal fact (preferences, names, places, routines, robot hardware). Do **not** auto-store conversation transcripts.
+
+**Recently remembered (newest first):**
+${recentBlock}
+
+If a question seems answerable from this list, answer directly. If you need more (e.g. semantic match, full search), call \`memory_recall\` with a focused query.`;
+}
+
+/**
+ * Discover live capabilities from the transport layer, with caching.
+ * Falls back to empty lists if discovery fails.
+ */
+async function discoverCapabilities(
+  api: OpenClawPluginApi,
+  namespace: string,
+): Promise<DiscoveryCache> {
+  // Return cached results if still fresh
+  if (cache && Date.now() - cache.timestamp < CACHE_TTL_MS) {
+    return cache;
+  }
+
+  try {
+    const transport = getTransport();
+
+    const [topics, services, actions] = await Promise.all([
+      transport.listTopics(),
+      transport.listServices(),
+      transport.listActions(),
+    ]);
+
+    // Filter by namespace if configured; always include camera topics (they often live under /camera/, not robot namespace)
+    const filterByNs = (name: string) => {
+      if (!namespace) return true;
+      const normalized = name.replace(/^\/+/, "");
+      if (normalized.startsWith(namespace)) return true;
+      if (normalized.startsWith("camera/") || normalized.includes("/camera/")) return true;
+      return false;
+    };
+
+    cache = {
+      topics: topics.filter((t: TopicInfo) => filterByNs(t.name)),
+      services: services.filter((s: ServiceInfo) => filterByNs(s.name)),
+      actions: actions.filter((a: ActionInfo) => filterByNs(a.name)),
+      timestamp: Date.now(),
+    };
+
+    api.logger.info(
+      `Discovered ${cache.topics.length} topics, ${cache.services.length} services, ${cache.actions.length} actions`,
+    );
+
+    return cache;
+  } catch (err) {
+    api.logger.warn(`Capability discovery failed, using defaults: ${err}`);
+    return {
+      topics: [],
+      services: [],
+      actions: [],
+      timestamp: 0,
+    };
+  }
+}
+
+/**
+ * Build the robot context string that gets injected into the agent's system prompt.
+ */
+function buildRobotContext(
+  config: AgenticROSConfig,
+  name: string,
+  namespace: string,
+  capabilities: DiscoveryCache,
+  cameraTopicHint: string,
+): string {
+  const { topics, services, actions } = capabilities;
+
+  // If discovery returned results, use them
+  if (topics.length > 0 || services.length > 0 || actions.length > 0) {
+    return buildDynamicContext(config, name, namespace, topics, services, actions);
+  }
+
+  // Fall back to hardcoded defaults if discovery failed
+  return buildFallbackContext(config, name, namespace, cameraTopicHint);
+}
+
+function transportMode(config: AgenticROSConfig): "local" | "rosbridge" | "webrtc" | "zenoh" {
+  const m = config.transport?.mode;
+  if (m === "local" || m === "rosbridge" || m === "webrtc" || m === "zenoh") return m;
+  return "local";
+}
+
+/** How AgenticROS reaches ROS 2 — matches plugin transport.mode (injected into the model). */
+function buildUserInterfaceBlurb(config: AgenticROSConfig): string {
+  const mode = transportMode(config);
+  const rosbridgeUrl = (config.rosbridge?.url ?? "ws://localhost:9090").trim() || "ws://localhost:9090";
+  const zenohEp = (config.zenoh?.routerEndpoint ?? "ws://localhost:10000").trim() || "ws://localhost:10000";
+  const domainId = config.local?.domainId ?? 0;
+
+  let connectionLine: string;
+  let pairingLine: string;
+  switch (mode) {
+    case "local":
+      connectionLine = `It connects to ROS 2 on the **same machine** as this gateway using **local DDS** (direct participant via rclnodejs, ROS_DOMAIN_ID **${domainId}** by default). No rosbridge WebSocket and no Zenoh router are used in this mode.`;
+      pairingLine = `If the user says "nodes status shows zero" or "how do I pair the robot": explain that the robot stack is reached through **local DDS** on this host. There is **no** OpenClaw device pairing step for ROS. Ensure ROS 2 nodes and OpenClaw share the **same domain ID** and that the AgenticROS transport is connected—\`openclaw nodes status\` counts **OpenClaw mobile/desktop nodes**, not your ROS graph.`;
+      break;
+    case "zenoh":
+      connectionLine = `It connects to ROS 2 through **Zenoh** (zenoh-ts), typically at **${zenohEp}** (WebSocket to zenoh-plugin-remote-api — use \`ws://\`, not raw \`tcp/\` for the plugin).`;
+      pairingLine = `If the user says "nodes status shows zero" or "how do I pair the robot": explain that the robot is reached via the **Zenoh router** endpoint above. No OpenClaw node pairing is required for AgenticROS. If the plugin is loaded and Zenoh/your bridge see ROS traffic, the robot is connected—\`openclaw nodes status\` is irrelevant for AgenticROS.`;
+      break;
+    case "webrtc":
+      connectionLine = `It connects to the robot over **WebRTC** (signaling and robot id configured in the plugin). There is typically **no** rosbridge on this path.`;
+      pairingLine = `If the user says "nodes status shows zero" or "how do I pair the robot": explain that **OpenClaw device pairing** is unrelated; WebRTC robot connectivity uses the plugin's signaling/robot configuration.`;
+      break;
+    default:
+      connectionLine = `It connects to ROS 2 via **rosbridge** (WebSocket to \`rosbridge_server\`, e.g. **${rosbridgeUrl}**).`;
+      pairingLine = `If the user says "nodes status shows zero" or "how do I pair the robot": explain that the robot is already connected through the AgenticROS plugin's **rosbridge** URL (e.g. **${rosbridgeUrl}**). No pairing step is required. If the plugin is loaded and rosbridge is reachable, the robot is connected—\`openclaw nodes status\` is irrelevant for AgenticROS.`;
+  }
+
+  return `
+### User-facing interface (tell users this when they ask)
+- **There is no separate robot GUI, dashboard, or URL.** The interface is this chat.
+- The user controls the robot by typing here (e.g. "move forward 1 meter", "what do you see?", "check the battery"). You execute commands with the ros2_* tools and reply in chat.
+- For telemetry: use \`ros2_subscribe_once\` or \`ros2_camera_snapshot\` and describe or show the result in your reply. There is no separate feed URL—you are the feed. If they want a "controller app," this chat is it.
+
+### OpenClaw "nodes" — do not confuse with AgenticROS
+- AgenticROS is an **OpenClaw plugin** that runs inside this gateway. ${connectionLine} There is **no separate "AgenticROS agent" or OpenClaw "node"** to pair for ROS control.
+- **Never tell users** to run \`openclaw node pair\`, \`openclaw nodes status\`, QR codes, or auth tokens **for ROS / robot control**. Those apply to OpenClaw's **companion device pairing**, not to AgenticROS talking to ROS 2.
+- ${pairingLine}
+`.trim();
+}
+
+/** For camera / image tips — all transports AgenticROS supports. */
+function imageTransportHint(config: AgenticROSConfig): string {
+  const mode = transportMode(config);
+  if (mode === "local") {
+    return "AgenticROS supports `sensor_msgs/msg/Image` and `sensor_msgs/msg/CompressedImage` over **local DDS** (this mode), and the same types over Zenoh or rosbridge when those transports are selected.";
+  }
+  return "AgenticROS supports `sensor_msgs/msg/Image` and `sensor_msgs/msg/CompressedImage` over **local DDS**, **Zenoh**, and **rosbridge**.";
+}
+
+function buildDynamicContext(
+  config: AgenticROSConfig,
+  name: string,
+  namespace: string,
+  topics: TopicInfo[],
+  services: ServiceInfo[],
+  actions: ActionInfo[],
+): string {
+  let context = `## Robot: ${name}\n\n`;
+  context += `You are connected to a ROS2 robot named "${name}". You can control it using the ros2_* tools.\n\n`;
+  context += `**Topics below** come from a **short live sample** when the session started. They are not guaranteed complete or up to date. If the user needs certainty—or says a topic is missing—call \`ros2_list_topics\` and answer from that result only.\n\n`;
+  if (namespace) {
+    context += `**Velocity commands:** Use \`ros2_publish\` with topic \`/cmd_vel\`; the plugin sends them to \`/${namespace}/cmd_vel\`.\n\n`;
+  }
+  context += `${buildUserInterfaceBlurb(config)}\n\n`;
+
+  // Cap injected lists to avoid huge context (rate limits / token burn)
+  const MAX_TOPICS = 25;
+  const MAX_SERVICES = 15;
+  const MAX_ACTIONS = 15;
+
+  if (topics.length > 0) {
+    context += "### Available Topics\n";
+    const showTopics = topics.slice(0, MAX_TOPICS);
+    for (const t of showTopics) {
+      context += `- \`${t.name}\` (${t.type})\n`;
+    }
+    if (topics.length > MAX_TOPICS) {
+      context += `- … and ${topics.length - MAX_TOPICS} more (use \`ros2_list_topics\` if needed)\n`;
+    }
+    context += "\n";
+  }
+
+  if (services.length > 0) {
+    context += "### Available Services\n";
+    const showServices = services.slice(0, MAX_SERVICES);
+    for (const s of showServices) {
+      context += `- \`${s.name}\` (${s.type})\n`;
+    }
+    if (services.length > MAX_SERVICES) {
+      context += `- … and ${services.length - MAX_SERVICES} more\n`;
+    }
+    context += "\n";
+  }
+
+  if (actions.length > 0) {
+    context += "### Available Actions\n";
+    const showActions = actions.slice(0, MAX_ACTIONS);
+    for (const a of showActions) {
+      context += `- \`${a.name}\` (${a.type})\n`;
+    }
+    if (actions.length > MAX_ACTIONS) {
+      context += `- … and ${actions.length - MAX_ACTIONS} more\n`;
+    }
+    context += "\n";
+  }
+
+  const skillIds = getLoadedSkillIds();
+  if (skillIds.length > 0) {
+    context += "### Available skills\n";
+    for (const id of skillIds) {
+      if (id === "followme") {
+        context += `- **followme**: Use the **\`follow_robot\`** tool with action \`start\`, \`stop\`, or \`status\` to control person-following. There is no separate follow-robot HTTP service or port—everything runs inside this gateway via ROS2 (Zenoh, local DDS, or rosbridge depending on transport). For \"follow me\" or \"start following\", call \`follow_robot\` with action \`start\`; to stop, use action \`stop\`. Optional: \`follow_me_see\` (what the tracker sees), \`ollama_status\` (if using Ollama).\n`;
+      } else {
+        context += `- **${id}**: Loaded; use the tools provided by this skill as documented in the skill.\n`;
+      }
+    }
+    context += "\n";
+  }
+
+  context += `### Safety Limits
+- Maximum linear velocity: 1.0 m/s
+- Maximum angular velocity: 1.5 rad/s
+- All velocity commands are validated before execution
+
+### Camera / "What does the robot see?"
+- When the user asks what the robot sees (or for a photo, camera view, or snapshot), **always call \`ros2_camera_snapshot\`** (or \`ros2_subscribe_once\` on a camera topic). Prefer a topic from the list above that contains **color** and **compressed** (e.g. \`/camera/camera/color/image_raw/compressed\`) for RGB. Do not assume the transport cannot decode images—${imageTransportHint(config)} If the tool returns an error, report it; otherwise show or describe the image. **Do not paste \`data:\` URLs or raw base64** in your reply—the tool returns a proper image block for the UI; describe what you see in prose.
+
+### Tips
+- Use \`ros2_list_topics\` to discover all available topics
+- Use \`ros2_subscribe_once\` to read the current value of any topic
+- Use \`ros2_camera_snapshot\` to see what the robot sees
+- The user can say /estop at any time to immediately stop the robot`;
+
+  return context;
+}
+
+function buildFallbackContext(
+  config: AgenticROSConfig,
+  name: string,
+  namespace: string,
+  cameraTopicHint: string,
+): string {
+  const skillIds = getLoadedSkillIds();
+  const skillsSection =
+    skillIds.length > 0
+      ? "### Available skills\n" +
+        skillIds
+          .map((id) =>
+            id === "followme"
+              ? "- **followme**: Use the **`follow_robot`** tool with action `start`, `stop`, or `status` to control person-following. There is no separate follow-robot HTTP service or port—everything runs inside this gateway via ROS2 (Zenoh, local DDS, or rosbridge depending on transport). For \"follow me\" or \"start following\", call `follow_robot` with action `start`; to stop, use action `stop`. Optional: `follow_me_see`, `ollama_status`."
+              : `- **${id}**: Loaded; use the tools provided by this skill as documented in the skill.`,
+          )
+          .join("\n") +
+        "\n\n"
+      : "";
+
+  const nsLine = namespace
+    ? `Configured **robot.namespace** is \`${namespace}\` (used for cmd_vel-style namespacing in tools — not proof those topics exist).\n\n`
+    : "";
+
+  return `
+## Robot: ${name}
+
+You are connected to a ROS2 robot named "${name}". You can control it using the ros2_* tools.
+
+${buildUserInterfaceBlurb(config)}
+
+### Topic discovery (read carefully)
+**No live topic list was available when this session started** (transport still connecting, Zenoh sampling saw no keys yet, or discovery failed). There is **no** "### Available Topics" section below because nothing was observed on the bus.
+
+- **You must call \`ros2_list_topics\`** and treat its return value as the **only** authoritative list. **Do not** tell the user that topics such as \`odom\`, \`scan\`, \`battery_state\`, or \`cmd_vel\` exist unless they appear in that tool output (or you successfully subscribe and get data).
+- If the user asks what topics exist, **run the tool first**, then summarize **only** what it returned. If the list is empty, say so plainly and suggest checking the robot stack, Zenoh bridge, and gateway logs.
+- For camera snapshots, after listing topics pick a **CompressedImage** (or Image) topic from the tool result; if none exist, say there is no camera topic. A common **default in plugin config** (not verified live) is \`${cameraTopicHint}\` — still confirm with \`ros2_list_topics\` before relying on it.
+
+${nsLine}${skillsSection}### Safety Limits
+- Maximum linear velocity: 1.0 m/s
+- Maximum angular velocity: 1.5 rad/s
+- All velocity commands are validated before execution
+
+### Camera / "What does the robot see?"
+- When the user asks what the robot sees (or for a photo, camera view, or snapshot), **always call \`ros2_camera_snapshot\`** (or \`ros2_subscribe_once\` on a camera topic). Do not assume the transport cannot decode images—${imageTransportHint(config)} If the tool returns an error, report it; otherwise show or describe the image. **Do not paste \`data:\` URLs or raw base64** in your reply—the tool returns a proper image block for the UI; describe what you see in prose.
+
+### Distance / "How far am I?"
+- When the user asks how far they are from the robot (or depth / distance in meters), **call \`ros2_depth_distance\`** only on a depth Image topic that **\`ros2_list_topics\`** (or prior tool output) shows exists. Report the tool result or **quote the exact error text** if it fails (do not claim a generic "decode" failure without the tool message). If the result is valid, give **distance_m** as the measured answer (nearer-surface percentile; **median_m** is also returned and often reflects background if the person only fills part of the depth patch).
+
+### Tips
+- Use \`ros2_list_topics\` to discover all available topics
+- Use \`ros2_subscribe_once\` to read the current value of any topic
+- Use \`ros2_camera_snapshot\` to see what the robot sees
+- The user can say /estop at any time to immediately stop the robot
+`.trim();
+}

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

@@ -0,0 +1,313 @@
+/**
+ * Sample distance (meters) from a ROS2 depth image topic (e.g. RealSense).
+ * Supports 16UC1 (depth in mm) and 32FC1 (depth in m).
+ */
+
+import type { RosTransport } from "@agenticros/core";
+import {
+  ROS_MSG_IMAGE,
+  coerceRosImageDataToBuffer,
+  normalizeDepthImageEncoding,
+  rosBoolField,
+  rosNumericField,
+  rosStringField,
+} from "@agenticros/ros-camera";
+const DEFAULT_TIMEOUT_MS = 5000;
+
+/** Drop absurd ranges; keeps sky/glitches from skewing percentiles. */
+const DEPTH_SAMPLE_MAX_M = 40;
+
+/**
+ * Reported distance uses a **lower percentile** (not median): the center ROI often mixes the
+ * person (near) with floor/wall/sky (far). Median then tracks background; ~12th percentile
+ * tracks nearer surfaces better for "how far is the person in front".
+ */
+const DEPTH_REPORT_PERCENTILE = 12;
+
+function sanitizeDepthSamplesMeters(values: number[]): number[] {
+  return values.filter((v) => Number.isFinite(v) && v > 0 && v <= DEPTH_SAMPLE_MAX_M);
+}
+
+/** Ascending `sorted`; return value near the low end at percentile `p` (0–100). */
+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]!;
+}
+
+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;
+}
+
+/**
+ * Sample center region of a depth image and return per-pixel distances in meters.
+ * - 16UC1: values in mm (RealSense typical) → divide by 1000
+ * - 32FC1: values in m → use as-is
+ * Invalid/zero pixels are skipped.
+ */
+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") {
+    // 2 bytes per pixel, row stride = step
+    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); // mm -> m
+      }
+    }
+  } 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;
+}
+
+/** Sample a vertical band (x0..x1) of the depth image; returns distances in meters. */
+function sampleBand(
+  width: number,
+  height: number,
+  step: number,
+  encoding: string,
+  data: Uint8Array,
+  x0: number,
+  x1: number,
+  heightFraction = 0.5,
+  isBigEndian = false,
+): number[] {
+  const enc = normalizeDepthImageEncoding(encoding);
+  const values: number[] = [];
+  const cy = height / 2;
+  const halfH = Math.max(1, Math.floor((height * heightFraction) / 2));
+  const y0 = Math.max(0, Math.floor(cy - halfH));
+  const y1 = Math.min(height, Math.floor(cy + halfH));
+  const ix0 = Math.max(0, Math.floor(x0));
+  const ix1 = Math.min(width, Math.floor(x1));
+
+  if (enc === "16UC1") {
+    for (let y = y0; y < y1; y++) {
+      for (let x = ix0; x < ix1; 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 = ix0; x < ix1; 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);
+      }
+    }
+  }
+  return values;
+}
+
+export interface DepthSectorsResult {
+  left_m: number;
+  center_m: number;
+  right_m: number;
+  valid: boolean;
+  topic: string;
+}
+
+/**
+ * Sample left, center, and right thirds of a depth image; return median distance per sector.
+ * Used by Follow Me to turn toward the person when not using Ollama.
+ */
+export async function getDepthSectors(
+  transport: RosTransport,
+  topic: string,
+  timeoutMs = DEFAULT_TIMEOUT_MS,
+): Promise<DepthSectorsResult> {
+  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 sectors 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 third = width / 3;
+  const leftV = sampleBand(width, height, step, encoding, data, 0, third, 0.5, isBigEndian);
+  const centerV = sampleBand(width, height, step, encoding, data, third, 2 * third, 0.5, isBigEndian);
+  const rightV = sampleBand(width, height, step, encoding, data, 2 * third, width, 0.5, isBigEndian);
+
+  const sortN = (v: number[]) => sanitizeDepthSamplesMeters(v).sort((a, b) => a - b);
+  const leftS = sortN(leftV);
+  const centerS = sortN(centerV);
+  const rightS = sortN(rightV);
+  const left_m = percentileLowerSorted(leftS, DEPTH_REPORT_PERCENTILE);
+  const center_m = percentileLowerSorted(centerS, DEPTH_REPORT_PERCENTILE);
+  const right_m = percentileLowerSorted(rightS, DEPTH_REPORT_PERCENTILE);
+
+  const round = (x: number) => (Number.isFinite(x) ? Math.round(x * 1000) / 1000 : NaN);
+  const valid =
+    leftS.length > 0 || centerS.length > 0 || rightS.length > 0;
+
+  return {
+    left_m: round(left_m),
+    center_m: round(center_m),
+    right_m: round(right_m),
+    valid,
+    topic,
+  };
+}
+
+export interface DepthSampleResult {
+  /** ~12th percentile depth in the center ROI — biased toward nearer surfaces (see module comment). */
+  distance_m: number;
+  /** Median depth in the same ROI (often background-dominated when a person only fills part of the patch). */
+  median_m: number;
+  valid: boolean;
+  topic: string;
+  encoding: string;
+  width: number;
+  height: number;
+  sample_count: number;
+  min_m: number;
+  max_m: number;
+}
+
+/**
+ * Subscribe to a depth topic, get one message, sample center region, return distance in meters
+ * (lower-tail percentile toward nearer pixels; see DEPTH_REPORT_PERCENTILE).
+ */
+export async function getDepthDistance(
+  transport: RosTransport,
+  topic: string,
+  timeoutMs = DEFAULT_TIMEOUT_MS,
+): 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,
+  };
+}