agenticros 0.3.3 → 0.3.5

package/runtime/packages/core/src/external-capability.ts

@@ -0,0 +1,219 @@
+/**
+ * External ROS-node capability executor — Phase 1 completion.
+ *
+ * Dispatches `implementation.kind: "external_ros_node"` capabilities via
+ * the existing RosTransport action / service / topic APIs. The manifest
+ * `launch` field is metadata only (operator-owned bringup) — the gateway
+ * never shells out to `ros2 launch` in v1.
+ */
+
+import type { Capability, CapabilityImplementation } from "./capabilities.js";
+import type { RosTransport } from "./transport/transport.js";
+import { toNamespacedTopic } from "./topic-utils.js";
+
+export interface ExecuteExternalOptions {
+  /** Robot namespace for topic/action/service prefixing. */
+  namespace?: string;
+  /** Optional timeout for subscribe_once (ms). */
+  timeoutMs?: number;
+}
+
+export interface ExecuteExternalResult {
+  text: string;
+  outputs?: Record<string, unknown>;
+  isError?: boolean;
+}
+
+function isExternalImpl(
+  impl: CapabilityImplementation | undefined,
+): impl is Extract<CapabilityImplementation, { kind: "external_ros_node" }> {
+  return impl?.kind === "external_ros_node";
+}
+
+function resolveName(name: string, namespace?: string): string {
+  const trimmed = name.trim();
+  if (!trimmed) return trimmed;
+  if (!namespace?.trim()) return trimmed.startsWith("/") ? trimmed : `/${trimmed}`;
+  return toNamespacedTopic(namespace, trimmed);
+}
+
+/**
+ * Map navigate_to-style inputs into a Nav2 NavigateToPose goal when the
+ * action type looks like NavigateToPose and inputs have x/y.
+ */
+export function buildExternalGoal(
+  impl: Extract<CapabilityImplementation, { kind: "external_ros_node" }>,
+  inputs: Record<string, unknown>,
+): Record<string, unknown> {
+  // Explicit goal / request / message wins.
+  if (inputs.goal && typeof inputs.goal === "object") {
+    return inputs.goal as Record<string, unknown>;
+  }
+  if (inputs.request && typeof inputs.request === "object") {
+    return inputs.request as Record<string, unknown>;
+  }
+  if (inputs.message && typeof inputs.message === "object") {
+    return inputs.message as Record<string, unknown>;
+  }
+
+  const msgType = (impl.msg_type ?? "").toLowerCase();
+  if (msgType.includes("navigatetopose") || impl.action?.includes("navigate_to_pose")) {
+    const x = Number(inputs.x ?? 0) || 0;
+    const y = Number(inputs.y ?? 0) || 0;
+    const yaw = Number(inputs.yaw ?? 0) || 0;
+    const qz = Math.sin(yaw / 2);
+    const qw = Math.cos(yaw / 2);
+    const frame = typeof inputs.frame_id === "string" ? inputs.frame_id : "map";
+    return {
+      pose: {
+        header: { frame_id: frame },
+        pose: {
+          position: { x, y, z: 0 },
+          orientation: { x: 0, y: 0, z: qz, w: qw },
+        },
+      },
+    };
+  }
+
+  // Passthrough remaining inputs (minus robot_id).
+  const { robot_id: _rid, ...rest } = inputs;
+  return rest;
+}
+
+/**
+ * Execute an external_ros_node capability against a connected transport.
+ */
+export async function executeExternalCapability(
+  capability: Capability,
+  inputs: Record<string, unknown>,
+  transport: RosTransport,
+  options: ExecuteExternalOptions = {},
+): Promise<ExecuteExternalResult> {
+  const impl = capability.implementation;
+  if (!isExternalImpl(impl)) {
+    return {
+      text: `Capability "${capability.id}" is not an external_ros_node implementation.`,
+      isError: true,
+    };
+  }
+
+  const ns = options.namespace;
+
+  try {
+    if (impl.action) {
+      const action = resolveName(impl.action, ns);
+      const actionType = impl.msg_type ?? "";
+      if (!actionType) {
+        return {
+          text: `Capability "${capability.id}" external action requires implementation.msg_type.`,
+          isError: true,
+        };
+      }
+      const args = buildExternalGoal(impl, inputs);
+      const result = await transport.sendActionGoal({
+        action,
+        actionType,
+        args,
+      });
+      const outputs = {
+        success: result.result,
+        ...(result.values ?? {}),
+        launch_hint: impl.launch ?? null,
+        package_hint: impl.package ?? null,
+      };
+      return {
+        text: JSON.stringify(outputs),
+        outputs,
+        isError: !result.result,
+      };
+    }
+
+    if (impl.service) {
+      const service = resolveName(impl.service, ns);
+      const serviceType = impl.msg_type ?? "";
+      if (!serviceType) {
+        return {
+          text: `Capability "${capability.id}" external service requires implementation.msg_type.`,
+          isError: true,
+        };
+      }
+      const args = buildExternalGoal(impl, inputs);
+      const result = await transport.callService({
+        service,
+        type: serviceType,
+        args,
+      });
+      const outputs = {
+        success: result.result,
+        ...(result.values ?? {}),
+        launch_hint: impl.launch ?? null,
+      };
+      return {
+        text: JSON.stringify(outputs),
+        outputs,
+        isError: !result.result,
+      };
+    }
+
+    if (impl.topic) {
+      const topic = resolveName(impl.topic, ns);
+      const msgType = impl.msg_type ?? "";
+      const mode = typeof inputs.mode === "string" ? inputs.mode : "publish";
+      if (mode === "subscribe" || mode === "subscribe_once") {
+        const timeout =
+          options.timeoutMs ?? (typeof inputs.timeout === "number" ? inputs.timeout : 5000);
+        const msg = await new Promise<Record<string, unknown>>((resolve, reject) => {
+          let settled = false;
+          const timer = setTimeout(() => {
+            if (settled) return;
+            settled = true;
+            try {
+              sub.unsubscribe();
+            } catch {
+              /* ignore */
+            }
+            reject(new Error(`Timed out waiting for message on ${topic} after ${timeout}ms`));
+          }, timeout);
+          const sub = transport.subscribe(
+            { topic, type: msgType || "std_msgs/msg/String" },
+            (message) => {
+              if (settled) return;
+              settled = true;
+              clearTimeout(timer);
+              try {
+                sub.unsubscribe();
+              } catch {
+                /* ignore */
+              }
+              resolve(message as Record<string, unknown>);
+            },
+          );
+        });
+        const outputs = { message: msg, topic };
+        return { text: JSON.stringify(outputs), outputs };
+      }
+      if (!msgType) {
+        return {
+          text: `Capability "${capability.id}" external topic publish requires implementation.msg_type.`,
+          isError: true,
+        };
+      }
+      const message = buildExternalGoal(impl, inputs);
+      await transport.publish({ topic, type: msgType, msg: message });
+      const outputs = { published: true, topic, launch_hint: impl.launch ?? null };
+      return { text: JSON.stringify(outputs), outputs };
+    }
+
+    return {
+      text:
+        `Capability "${capability.id}" external_ros_node has no action, service, or topic. ` +
+        `Bringup hint: ${impl.launch ?? impl.package ?? "(none)"}.`,
+      isError: true,
+    };
+  } catch (err) {
+    return {
+      text: err instanceof Error ? err.message : String(err),
+      isError: true,
+    };
+  }
+}

package/runtime/packages/core/src/fleet-config.ts

@@ -0,0 +1,91 @@
+/**
+ * Static fleet file — Phase 1.d hybrid discovery.
+ *
+ * Precedence: `AGENTICROS_FLEET_PATH` → `~/.agenticros/fleet.json` →
+ * `config.robots[]` (via listRobots). When a fleet file exists and
+ * parses as a non-empty robot array (or `{ robots: [...] }`), it wins.
+ */
+
+import { existsSync, readFileSync } from "node:fs";
+import { homedir } from "node:os";
+import { join } from "node:path";
+import type { AgenticROSConfig } from "./config.js";
+import { AgenticROSConfigSchema } from "./config.js";
+
+export const DEFAULT_FLEET_FILENAME = "fleet.json";
+
+/** Resolve the fleet file path (env override or default under ~/.agenticros). */
+export function resolveFleetPath(env: NodeJS.ProcessEnv = process.env): string {
+  const override = env.AGENTICROS_FLEET_PATH?.trim();
+  if (override) return override;
+  return join(homedir(), ".agenticros", DEFAULT_FLEET_FILENAME);
+}
+
+export interface FleetFileResult {
+  /** Absolute path that was checked. */
+  path: string;
+  /** True when the file exists and was used. */
+  used: boolean;
+  /** Parsed robot entries (empty when unused / invalid). */
+  robots: NonNullable<AgenticROSConfig["robots"]>;
+  /** Human-readable error when the file exists but failed to parse. */
+  error?: string;
+}
+
+/**
+ * Load `fleet.json` if present. Does not throw on missing file.
+ * Invalid JSON / schema → `used: false` with `error` set so callers can warn.
+ */
+export function loadFleetFile(path?: string): FleetFileResult {
+  const fleetPath = path ?? resolveFleetPath();
+  if (!existsSync(fleetPath)) {
+    return { path: fleetPath, used: false, robots: [] };
+  }
+  try {
+    const raw = JSON.parse(readFileSync(fleetPath, "utf8")) as unknown;
+    let robotsRaw: unknown;
+    if (Array.isArray(raw)) {
+      robotsRaw = raw;
+    } else if (raw && typeof raw === "object" && Array.isArray((raw as { robots?: unknown }).robots)) {
+      robotsRaw = (raw as { robots: unknown[] }).robots;
+    } else {
+      return {
+        path: fleetPath,
+        used: false,
+        robots: [],
+        error: "fleet.json must be an array of robots or { robots: [...] }",
+      };
+    }
+    const parsed = AgenticROSConfigSchema.pick({ robots: true }).safeParse({ robots: robotsRaw });
+    if (!parsed.success) {
+      return {
+        path: fleetPath,
+        used: false,
+        robots: [],
+        error: parsed.error.message,
+      };
+    }
+    const robots = parsed.data.robots ?? [];
+    if (robots.length === 0) {
+      return { path: fleetPath, used: false, robots: [] };
+    }
+    return { path: fleetPath, used: true, robots };
+  } catch (err) {
+    return {
+      path: fleetPath,
+      used: false,
+      robots: [],
+      error: err instanceof Error ? err.message : String(err),
+    };
+  }
+}
+
+/**
+ * Return a config view where `robots` comes from fleet.json when present.
+ * Does not mutate the input config object.
+ */
+export function applyFleetOverride(config: AgenticROSConfig, fleetPath?: string): AgenticROSConfig {
+  const fleet = loadFleetFile(fleetPath);
+  if (!fleet.used) return config;
+  return { ...config, robots: fleet.robots };
+}

package/runtime/packages/core/src/heartbeat.ts

@@ -0,0 +1,167 @@
+/**
+ * RobotInfo heartbeat helpers — Phase 1.d completion.
+ *
+ * `agenticros_discovery` publishes `<ns>/agenticros/robot_info` at 1 Hz.
+ * Consumers treat a robot as online when the last stamp is within
+ * {@link DEFAULT_HEARTBEAT_STALENESS_MS} (5 s per strategy memo).
+ *
+ * Topic-scan path: when adapters only have `listTopics()`, presence of
+ * `/<ns>/agenticros/robot_info` on the graph is a weak online signal
+ * (same as cmd_vel). Prefer {@link mergeRobotHeartbeats} when message
+ * payloads with stamps are available.
+ */
+
+import { effectiveCmdVelNamespace } from "./discovery.js";
+import type { TopicInfo } from "./transport/types.js";
+import type { RobotSensors } from "./robots.js";
+
+/** Default staleness window — strategy §4(d): 1 Hz + 5 s. */
+export const DEFAULT_HEARTBEAT_STALENESS_MS = 5_000;
+
+/** Parsed robot_info payload (subset of agenticros_msgs/RobotInfo). */
+export interface RobotHeartbeat {
+  /** Stable id (falls back to namespace). */
+  id: string;
+  name: string;
+  kind: string;
+  /** ROS namespace without leading slash. */
+  robot_namespace: string;
+  capability_ids: string[];
+  sensors: RobotSensors;
+  /** Epoch ms of the heartbeat stamp (0 when unknown). */
+  stamp_ms: number;
+  /** Topic the heartbeat was observed on. */
+  topic: string;
+}
+
+export interface HeartbeatOnlineOptions {
+  /** Wall clock now (ms). Defaults to Date.now(). */
+  nowMs?: number;
+  /** Staleness window in ms. Defaults to 5000. */
+  stalenessMs?: number;
+}
+
+/**
+ * Extract namespace from `/<ns>/agenticros/robot_info` (or without leading slash).
+ * Returns "" for unnamespaced `/agenticros/robot_info`.
+ */
+export function namespaceFromRobotInfoTopic(topic: string): string {
+  const m = topic.match(/^\/?([^/]+)\/agenticros\/robot_info$/);
+  if (m) return m[1]!;
+  if (topic === "/agenticros/robot_info" || topic === "agenticros/robot_info") return "";
+  return "";
+}
+
+/** True when the topic name is a robot_info heartbeat topic. */
+export function isRobotInfoTopic(topic: string): boolean {
+  return /(?:^|\/)agenticros\/robot_info$/.test(topic);
+}
+
+/**
+ * Detect namespaces that advertise `agenticros/robot_info` on the topic graph
+ * (no stamp available — treat as online for discovery fallback).
+ */
+export function detectHeartbeatNamespacesFromTopics(topics: TopicInfo[]): string[] {
+  const out = new Set<string>();
+  for (const t of topics) {
+    if (!isRobotInfoTopic(t.name)) continue;
+    out.add(namespaceFromRobotInfoTopic(t.name));
+  }
+  return [...out];
+}
+
+function rosTimeToMs(stamp: unknown): number {
+  if (!stamp || typeof stamp !== "object") return 0;
+  const s = stamp as Record<string, unknown>;
+  const sec = Number(s.sec ?? s.secs ?? 0) || 0;
+  const nanosec = Number(s.nanosec ?? s.nsecs ?? 0) || 0;
+  return sec * 1000 + Math.floor(nanosec / 1e6);
+}
+
+/**
+ * Parse a plain RobotInfo-like message into {@link RobotHeartbeat}.
+ * Tolerates missing optional fields.
+ */
+export function parseRobotInfoMessage(
+  topic: string,
+  msg: Record<string, unknown>,
+): RobotHeartbeat {
+  const nsFromTopic = namespaceFromRobotInfoTopic(topic);
+  const robot_namespace = String(msg.robot_namespace ?? msg.namespace ?? nsFromTopic ?? "");
+  const id = String(msg.id ?? (robot_namespace || "default"));
+  const capability_ids = Array.isArray(msg.capability_ids)
+    ? msg.capability_ids.map((c) => String(c))
+    : Array.isArray(msg.capabilities)
+      ? (msg.capabilities as unknown[]).map((c) => String(c))
+      : [];
+  return {
+    id,
+    name: String(msg.name ?? "Robot"),
+    kind: String(msg.kind ?? "amr"),
+    robot_namespace,
+    capability_ids,
+    sensors: {
+      has_realsense: Boolean(msg.has_realsense),
+      has_lidar: Boolean(msg.has_lidar),
+      has_arm: Boolean(msg.has_arm),
+    },
+    stamp_ms: rosTimeToMs(msg.stamp),
+    topic,
+  };
+}
+
+/** True when the heartbeat stamp is within the staleness window. */
+export function isHeartbeatFresh(
+  heartbeat: RobotHeartbeat,
+  options: HeartbeatOnlineOptions = {},
+): boolean {
+  const now = options.nowMs ?? Date.now();
+  const window = options.stalenessMs ?? DEFAULT_HEARTBEAT_STALENESS_MS;
+  if (!heartbeat.stamp_ms) {
+    // No stamp — treat as fresh if we just received the message (caller
+    // should only pass live subscribe results here).
+    return true;
+  }
+  return now - heartbeat.stamp_ms <= window;
+}
+
+/**
+ * Merge heartbeats into an online-id set keyed by configured robot id
+ * when `configuredIdByNamespace` is provided, else by heartbeat id /
+ * effective namespace.
+ */
+export function onlineIdsFromHeartbeats(
+  heartbeats: readonly RobotHeartbeat[],
+  options: HeartbeatOnlineOptions & {
+    /** Map effective cmd_vel namespace → configured robot id. */
+    configuredIdByNamespace?: ReadonlyMap<string, string>;
+  } = {},
+): Set<string> {
+  const online = new Set<string>();
+  for (const hb of heartbeats) {
+    if (!isHeartbeatFresh(hb, options)) continue;
+    const eff = effectiveCmdVelNamespace(hb.robot_namespace);
+    const cfgId = options.configuredIdByNamespace?.get(eff);
+    online.add(cfgId ?? hb.id);
+  }
+  return online;
+}
+
+/**
+ * Build a map of effective-namespace → latest fresh heartbeat.
+ */
+export function mergeRobotHeartbeats(
+  heartbeats: readonly RobotHeartbeat[],
+  options: HeartbeatOnlineOptions = {},
+): Map<string, RobotHeartbeat> {
+  const byNs = new Map<string, RobotHeartbeat>();
+  for (const hb of heartbeats) {
+    if (!isHeartbeatFresh(hb, options)) continue;
+    const key = effectiveCmdVelNamespace(hb.robot_namespace);
+    const prev = byNs.get(key);
+    if (!prev || hb.stamp_ms >= prev.stamp_ms) {
+      byNs.set(key, hb);
+    }
+  }
+  return byNs;
+}

package/runtime/packages/core/src/index.ts

@@ -6,6 +6,7 @@ export type { AgenticROSConfig, RobotTransportOverride } from "./config.js";
 export {
   AgenticROSConfigSchema,
   parseConfig,
+  prepareConfigForPersistence,
   getTransportConfig,
 } from "./config.js";
 
@@ -65,6 +66,24 @@ export type {
   CapabilitySource,
 } from "./capabilities.js";
 
+export {
+  CapabilitySchema,
+  CapabilityFieldSchema,
+  CapabilityImplementationSchema,
+  parseCapability,
+  safeParseCapability,
+} from "./capability-schema.js";
+export type { ParsedCapability } from "./capability-schema.js";
+
+export {
+  buildExternalGoal,
+  executeExternalCapability,
+} from "./external-capability.js";
+export type {
+  ExecuteExternalOptions,
+  ExecuteExternalResult,
+} from "./external-capability.js";
+
 export { runMission } from "./mission.js";
 export type {
   Mission,
@@ -75,11 +94,27 @@ export type {
   CapabilityToolBinding,
   CapabilityToolBindings,
   MissionCancellationToken,
+  MissionControlToken,
   MissionTranscriptEntry,
   MissionTranscriptSink,
   RunMissionOptions,
 } from "./mission.js";
 
+export {
+  BUILTIN_MISSION_BINDINGS,
+  EXTERNAL_TOOL_PREFIX,
+  buildMissionBindings,
+  capabilityIdFromExternalTool,
+  defaultToolForCapability,
+  externalToolName,
+  isExternalToolName,
+  passthroughBuildArgs,
+} from "./mission-bindings.js";
+export type {
+  BuildMissionBindingsOptions,
+  CapabilityWithTool,
+} from "./mission-bindings.js";
+
 export {
   MissionRegistry,
   generateMissionId,
@@ -109,6 +144,26 @@ export {
 } from "./discovery.js";
 export type { DetectedRobot, RobotDiscoveryResult } from "./discovery.js";
 
+export {
+  DEFAULT_HEARTBEAT_STALENESS_MS,
+  detectHeartbeatNamespacesFromTopics,
+  isHeartbeatFresh,
+  isRobotInfoTopic,
+  mergeRobotHeartbeats,
+  namespaceFromRobotInfoTopic,
+  onlineIdsFromHeartbeats,
+  parseRobotInfoMessage,
+} from "./heartbeat.js";
+export type { HeartbeatOnlineOptions, RobotHeartbeat } from "./heartbeat.js";
+
+export {
+  DEFAULT_FLEET_FILENAME,
+  applyFleetOverride,
+  loadFleetFile,
+  resolveFleetPath,
+} from "./fleet-config.js";
+export type { FleetFileResult } from "./fleet-config.js";
+
 export { findRobotsFor } from "./find-robots-for.js";
 export type {
   FindRobotsForQuery,