agenticros 0.0.1 → 0.1.1

package/runtime/packages/core/src/transport/local/transport.ts

@@ -0,0 +1,406 @@
+import { createRequire } from "node:module";
+import type { RosTransport } from "../transport.js";
+import type {
+  ConnectionStatus,
+  ConnectionHandler,
+  Subscription,
+  PublishOptions,
+  SubscribeOptions,
+  ServiceCallOptions,
+  ServiceCallResult,
+  ActionGoalOptions,
+  ActionResult,
+  TopicInfo,
+  ServiceInfo,
+  ActionInfo,
+  MessageHandler,
+} from "../types.js";
+import { EntityCache } from "./entities.js";
+import { toRosMessage, fromRosMessage, loadMessageClass, clearTypeCache } from "./conversion.js";
+
+const require = createRequire(import.meta.url);
+
+export interface LocalTransportOptions {
+  domainId?: number;
+}
+
+/** Internal ROS2 topics/services to filter from introspection results. */
+const INTERNAL_TOPIC_PREFIXES = ["/rosout", "/parameter_events", "/agenticros/"];
+const INTERNAL_SERVICE_SUFFIXES = [
+  "/describe_parameters",
+  "/get_parameter_types",
+  "/get_parameters",
+  "/list_parameters",
+  "/set_parameters",
+  "/set_parameters_atomically",
+];
+
+/**
+ * Mode A transport: direct local DDS communication via rclnodejs.
+ *
+ * When OpenClaw runs on the robot itself, this transport talks to ROS2
+ * directly via the local DDS bus — no network intermediary needed.
+ */
+export class LocalTransport implements RosTransport {
+  private domainId: number;
+  private status: ConnectionStatus = "disconnected";
+  private connectionHandlers = new Set<ConnectionHandler>();
+  /** rclnodejs module — loaded dynamically at runtime (optional dep, no types). */
+  private rclnodejs: any = null;
+  private node: any = null;
+  private entityCache: EntityCache | null = null;
+  private activeGoals = new Map<string, any>();
+
+  /** Singleton guard — rclnodejs.init() must only be called once per process. */
+  private static rclInitialized = false;
+
+  constructor(options?: LocalTransportOptions) {
+    this.domainId = options?.domainId ?? 0;
+  }
+
+  // --- Connection lifecycle ---
+
+  async connect(): Promise<void> {
+    if (this.status === "connected") return;
+    this.setStatus("connecting");
+
+    try {
+      this.rclnodejs = require("rclnodejs");
+
+      // Set domain ID before init if non-default
+      if (this.domainId !== 0) {
+        process.env.ROS_DOMAIN_ID = String(this.domainId);
+      }
+
+      // Global init — only once per process
+      if (!LocalTransport.rclInitialized) {
+        await this.rclnodejs!.init();
+        LocalTransport.rclInitialized = true;
+      }
+
+      // Use a process-unique node name. If two AgenticROS adapters (e.g. the
+      // OpenClaw gateway plugin + the Claude Code MCP server) share the host,
+      // a fixed name collides on the DDS bus: rcl logs "Publisher already
+      // registered for provided node name" and DDS discovery on the duplicate
+      // becomes slow/lossy — listTopics() then returns 0 right after connect.
+      const nodeName = `agenticros_local_${process.pid}`;
+      this.node = this.rclnodejs!.createNode(nodeName);
+      this.rclnodejs!.spin(this.node);
+      this.entityCache = new EntityCache();
+
+      // Give DDS discovery a brief moment to receive announcements from peers
+      // before we report "connected". Without this the very first listTopics()
+      // can race the executor and return empty even though peers exist.
+      await new Promise<void>((resolve) => setTimeout(resolve, 750));
+
+      this.setStatus("connected");
+    } catch (err) {
+      this.setStatus("disconnected");
+      throw err;
+    }
+  }
+
+  async disconnect(): Promise<void> {
+    if (this.status === "disconnected") return;
+
+    // Cancel any active action goals
+    for (const [action] of this.activeGoals) {
+      try {
+        await this.cancelActionGoal(action);
+      } catch {
+        // Best-effort
+      }
+    }
+    this.activeGoals.clear();
+
+    if (this.entityCache && this.node) {
+      this.entityCache.destroyAll(this.node);
+      this.entityCache = null;
+    }
+
+    if (this.node) {
+      this.node.destroy();
+      this.node = null;
+    }
+
+    if (this.rclnodejs && LocalTransport.rclInitialized) {
+      try {
+        this.rclnodejs.shutdown();
+      } catch {
+        // May already be shut down
+      }
+      LocalTransport.rclInitialized = false;
+    }
+
+    clearTypeCache();
+    this.rclnodejs = null;
+    this.setStatus("disconnected");
+  }
+
+  getStatus(): ConnectionStatus {
+    return this.status;
+  }
+
+  onConnection(handler: ConnectionHandler): () => void {
+    this.connectionHandlers.add(handler);
+    return () => {
+      this.connectionHandlers.delete(handler);
+    };
+  }
+
+  // --- Topics ---
+
+  publish(options: PublishOptions): void {
+    this.ensureConnected();
+    const publisher = this.entityCache!.getPublisher(this.node, options.topic, options.type);
+    const rosMsg = toRosMessage(options.type, options.msg);
+    publisher.publish(rosMsg);
+  }
+
+  subscribe(options: SubscribeOptions, handler: MessageHandler): Subscription {
+    this.ensureConnected();
+    const type = options.type ?? this.resolveTopicType(options.topic);
+    if (!type) {
+      throw new Error(
+        `Cannot subscribe to ${options.topic}: type is required when it cannot be inferred`,
+      );
+    }
+    return this.entityCache!.addSubscription(this.node, options.topic, type, handler);
+  }
+
+  // --- Services ---
+
+  async callService(options: ServiceCallOptions): Promise<ServiceCallResult> {
+    this.ensureConnected();
+
+    const type = options.type ?? this.resolveServiceType(options.service);
+    if (!type) {
+      throw new Error(
+        `Cannot call service ${options.service}: type is required when it cannot be inferred`,
+      );
+    }
+
+    const client = this.entityCache!.getServiceClient(this.node, options.service, type);
+
+    // Wait for service to become available (5s timeout)
+    const available = await client.waitForService(5000);
+    if (!available) {
+      throw new Error(`Service ${options.service} not available after 5 seconds`);
+    }
+
+    // Create request from args
+    const ServiceClass = loadMessageClass(type);
+    const request = new ServiceClass.Request();
+    if (options.args) {
+      for (const [key, value] of Object.entries(options.args)) {
+        request[key] = value;
+      }
+    }
+
+    // Send request with Promise wrapping + timeout
+    const response = await this.sendServiceRequest(client, request, 30_000);
+
+    return {
+      result: true,
+      values: fromRosMessage(response),
+    };
+  }
+
+  // --- Actions ---
+
+  async sendActionGoal(options: ActionGoalOptions): Promise<ActionResult> {
+    this.ensureConnected();
+
+    const ActionClass = loadMessageClass(options.actionType);
+    const actionClient = new (this.rclnodejs!.ActionClient as any)(
+      this.node,
+      ActionClass,
+      options.action,
+    );
+
+    // Wait for action server (5s)
+    const available = await actionClient.waitForServer(5000);
+    if (!available) {
+      actionClient.destroy();
+      throw new Error(`Action server ${options.action} not available after 5 seconds`);
+    }
+
+    // Build goal message
+    const goal = new ActionClass.Goal();
+    if (options.args) {
+      for (const [key, value] of Object.entries(options.args)) {
+        goal[key] = value;
+      }
+    }
+
+    try {
+      const result = await new Promise<Record<string, unknown>>((resolve, reject) => {
+        const timer = setTimeout(() => {
+          this.activeGoals.delete(options.action);
+          reject(new Error(`Action ${options.action} timed out after 120 seconds`));
+        }, 120_000);
+
+        actionClient.sendGoal(
+          goal,
+          (goalHandle: any) => {
+            // Goal response callback — store for cancellation
+            this.activeGoals.set(options.action, goalHandle);
+          },
+          (feedback: any) => {
+            // Feedback callback
+            if (options.onFeedback) {
+              options.onFeedback(fromRosMessage(feedback));
+            }
+          },
+          (resultResponse: any) => {
+            // Result callback
+            clearTimeout(timer);
+            this.activeGoals.delete(options.action);
+            resolve(fromRosMessage(resultResponse));
+          },
+        );
+      });
+
+      return { result: true, values: result };
+    } finally {
+      actionClient.destroy();
+    }
+  }
+
+  async cancelActionGoal(action: string): Promise<void> {
+    const goalHandle = this.activeGoals.get(action);
+    if (goalHandle && typeof goalHandle.cancelGoal === "function") {
+      await goalHandle.cancelGoal();
+      this.activeGoals.delete(action);
+    }
+  }
+
+  // --- Introspection ---
+
+  async listTopics(): Promise<TopicInfo[]> {
+    this.ensureConnected();
+
+    // DDS discovery is asynchronous. On a freshly-connected node,
+    // getTopicNamesAndTypes() trickles in results as peers respond - the
+    // first call may return 0, the second 2, the third all of them.
+    // Poll until the count is stable across two consecutive 300ms reads (or
+    // time-bounded to ~3s) so we don't return a partial view.
+    const externalFilter = (t: { name: string; types: string[] }) =>
+      !INTERNAL_TOPIC_PREFIXES.some((prefix) => t.name.startsWith(prefix));
+
+    const deadline = Date.now() + 3000;
+    let raw: Array<{ name: string; types: string[] }> = [];
+    let prevCount = -1;
+    let stableHits = 0;
+    while (Date.now() < deadline) {
+      raw = this.node.getTopicNamesAndTypes();
+      const externalCount = raw.filter(externalFilter).length;
+      if (externalCount === prevCount && externalCount > 0) {
+        stableHits++;
+        if (stableHits >= 1) break; // one stable confirmation is enough
+      } else {
+        stableHits = 0;
+        prevCount = externalCount;
+      }
+      await new Promise<void>((resolve) => setTimeout(resolve, 300));
+    }
+
+    return raw.filter(externalFilter).map((t) => ({ name: t.name, type: t.types[0] ?? "" }));
+  }
+
+  async listServices(): Promise<ServiceInfo[]> {
+    this.ensureConnected();
+
+    const namesAndTypes: Array<{ name: string; types: string[] }> =
+      this.node.getServiceNamesAndTypes();
+
+    return namesAndTypes
+      .filter((s) => !INTERNAL_SERVICE_SUFFIXES.some((suffix) => s.name.endsWith(suffix)))
+      .filter((s) => !INTERNAL_TOPIC_PREFIXES.some((prefix) => s.name.startsWith(prefix)))
+      .map((s) => ({ name: s.name, type: s.types[0] ?? "" }));
+  }
+
+  async listActions(): Promise<ActionInfo[]> {
+    // Same feedback-topic heuristic as rosbridge/adapter.ts
+    const topics = await this.listTopics();
+    const actions: ActionInfo[] = [];
+    const feedbackSuffix = "/_action/feedback";
+
+    for (const topic of topics) {
+      if (topic.name.endsWith(feedbackSuffix)) {
+        const actionName = topic.name.slice(0, -feedbackSuffix.length);
+        let actionType = topic.type;
+        if (actionType.endsWith("_FeedbackMessage")) {
+          actionType = actionType.slice(0, -"_FeedbackMessage".length);
+        }
+        actions.push({ name: actionName, type: actionType });
+      }
+    }
+
+    return actions;
+  }
+
+  // --- Private helpers ---
+
+  private setStatus(status: ConnectionStatus): void {
+    this.status = status;
+    for (const handler of this.connectionHandlers) {
+      handler(status);
+    }
+  }
+
+  private ensureConnected(): void {
+    if (this.status !== "connected" || !this.node || !this.entityCache) {
+      throw new Error("LocalTransport is not connected");
+    }
+  }
+
+  /**
+   * Resolve a topic's type from the node's graph introspection.
+   * Returns undefined if the topic is not yet known.
+   */
+  private resolveTopicType(topic: string): string | undefined {
+    if (!this.node) return undefined;
+    const namesAndTypes: Array<{ name: string; types: string[] }> =
+      this.node.getTopicNamesAndTypes();
+    const entry = namesAndTypes.find((t) => t.name === topic);
+    return entry?.types[0];
+  }
+
+  /**
+   * Resolve a service's type from the node's graph introspection.
+   * Returns undefined if the service is not yet known.
+   */
+  private resolveServiceType(service: string): string | undefined {
+    if (!this.node) return undefined;
+    const namesAndTypes: Array<{ name: string; types: string[] }> =
+      this.node.getServiceNamesAndTypes();
+    const entry = namesAndTypes.find((s) => s.name === service);
+    return entry?.types[0];
+  }
+
+  /**
+   * Wrap rclnodejs callback-based sendRequest in a Promise with timeout.
+   */
+  private sendServiceRequest(client: any, request: any, timeoutMs: number): Promise<any> {
+    return new Promise((resolve, reject) => {
+      const timer = setTimeout(() => {
+        reject(new Error(`Service call timed out after ${timeoutMs}ms`));
+      }, timeoutMs);
+
+      try {
+        client.sendRequest(request, (response: any) => {
+          clearTimeout(timer);
+          if (response) {
+            resolve(response);
+          } else {
+            reject(new Error("Service returned no response"));
+          }
+        });
+      } catch (err) {
+        clearTimeout(timer);
+        reject(err);
+      }
+    });
+  }
+}

package/runtime/packages/core/src/transport/rosbridge/actions.ts

@@ -0,0 +1,81 @@
+import type { RosbridgeClient } from "./client.js";
+import type {
+  ActionResultMessage,
+  ActionFeedbackMessage,
+} from "./types.js";
+
+export interface ActionGoalOptions {
+  action: string;
+  actionType: string;
+  args?: Record<string, unknown>;
+  onFeedback?: (feedback: ActionFeedbackMessage) => void;
+  timeoutMs?: number;
+}
+
+/**
+ * Client for sending action goals and receiving feedback/results.
+ */
+export class ActionClient {
+  constructor(private client: RosbridgeClient) {}
+
+  /**
+   * Send an action goal and wait for the result.
+   *
+   * @param options - Action goal options including feedback handler
+   * @returns The action result
+   */
+  async sendGoal(options: ActionGoalOptions): Promise<ActionResultMessage> {
+    const id = this.client.nextId("action");
+    const timeoutMs = options.timeoutMs ?? 120_000; // Actions can be long-running
+
+    // Register feedback handler if provided
+    let removeFeedbackHandler: (() => void) | null = null;
+    if (options.onFeedback) {
+      const feedbackKey = `__action_feedback__${id}`;
+      removeFeedbackHandler = this.client.onMessage(feedbackKey, (msg) => {
+        options.onFeedback!(msg as unknown as ActionFeedbackMessage);
+      });
+    }
+
+    // Create promise that resolves on action_result
+    const resultPromise = new Promise<ActionResultMessage>((resolve, reject) => {
+      this.client.registerPending(
+        id,
+        (result) => resolve(result as ActionResultMessage),
+        reject,
+        timeoutMs,
+      );
+    });
+
+    // Send the goal
+    this.client.send({
+      op: "send_action_goal",
+      id,
+      action: options.action,
+      action_type: options.actionType,
+      args: options.args,
+    });
+
+    try {
+      return await resultPromise;
+    } finally {
+      // Clean up feedback handler regardless of outcome
+      if (removeFeedbackHandler) {
+        removeFeedbackHandler();
+      }
+    }
+  }
+
+  /**
+   * Cancel an in-progress action goal.
+   *
+   * @param action - The action server name
+   */
+  async cancelGoal(action: string): Promise<void> {
+    this.client.send({
+      op: "cancel_action_goal",
+      id: this.client.nextId("cancel"),
+      action,
+    });
+  }
+}

package/runtime/packages/core/src/transport/rosbridge/adapter.ts

@@ -0,0 +1,157 @@
+import type { RosTransport } from "../transport.js";
+import type {
+  ConnectionStatus,
+  ConnectionHandler,
+  Subscription,
+  PublishOptions,
+  AdvertiseOptions,
+  SubscribeOptions,
+  ServiceCallOptions,
+  ServiceCallResult,
+  ActionGoalOptions,
+  ActionResult,
+  TopicInfo,
+  ServiceInfo,
+  ActionInfo,
+  MessageHandler,
+} from "../types.js";
+import { RosbridgeClient } from "./client.js";
+import { TopicPublisher, TopicSubscriber } from "./topics.js";
+import { callService } from "./services.js";
+import { ActionClient } from "./actions.js";
+import type { RosbridgeClientOptions } from "./types.js";
+
+/**
+ * RosTransport adapter that wraps the existing RosbridgeClient.
+ *
+ * This is the Mode B (Local Network) transport. It connects to a
+ * rosbridge_server running on the robot via WebSocket and translates
+ * RosTransport method calls into rosbridge protocol messages.
+ */
+export class RosbridgeTransport implements RosTransport {
+  private client: RosbridgeClient;
+  private actionClient: ActionClient;
+
+  constructor(options: RosbridgeClientOptions) {
+    this.client = new RosbridgeClient(options);
+    this.actionClient = new ActionClient(this.client);
+  }
+
+  async connect(): Promise<void> {
+    await this.client.connect();
+  }
+
+  async disconnect(): Promise<void> {
+    await this.client.disconnect();
+  }
+
+  getStatus(): ConnectionStatus {
+    return this.client.getStatus();
+  }
+
+  onConnection(handler: ConnectionHandler): () => void {
+    return this.client.onConnection(handler);
+  }
+
+  advertise(options: AdvertiseOptions): void {
+    this.client.send({
+      op: "advertise",
+      topic: options.topic,
+      type: options.type,
+    });
+  }
+
+  publish(options: PublishOptions): void {
+    const publisher = new TopicPublisher(this.client, options.topic, options.type);
+    publisher.publish(options.msg);
+  }
+
+  subscribe(options: SubscribeOptions, handler: MessageHandler): Subscription {
+    const subscriber = new TopicSubscriber(this.client, options.topic, options.type);
+    subscriber.subscribe(handler);
+    return {
+      unsubscribe() {
+        subscriber.unsubscribe();
+      },
+    };
+  }
+
+  async callService(options: ServiceCallOptions): Promise<ServiceCallResult> {
+    const response = await callService(
+      this.client,
+      options.service,
+      options.args,
+      options.type,
+    );
+    return {
+      result: response.result,
+      values: response.values,
+    };
+  }
+
+  async sendActionGoal(options: ActionGoalOptions): Promise<ActionResult> {
+    const response = await this.actionClient.sendGoal({
+      action: options.action,
+      actionType: options.actionType,
+      args: options.args,
+      onFeedback: options.onFeedback
+        ? (feedback) => options.onFeedback!(feedback.values)
+        : undefined,
+    });
+    return {
+      result: response.result,
+      values: response.values,
+    };
+  }
+
+  async cancelActionGoal(action: string): Promise<void> {
+    await this.actionClient.cancelGoal(action);
+  }
+
+  async listTopics(): Promise<TopicInfo[]> {
+    const response = await callService(
+      this.client,
+      "/rosapi/topics",
+      {},
+      "rosapi/srv/Topics",
+    );
+    const topics = (response.values?.["topics"] as string[]) ?? [];
+    const types = (response.values?.["types"] as string[]) ?? [];
+    return topics.map((name, i) => ({ name, type: types[i] ?? "" }));
+  }
+
+  async listServices(): Promise<ServiceInfo[]> {
+    const response = await callService(
+      this.client,
+      "/rosapi/services",
+      {},
+      "rosapi/srv/Services",
+    );
+    const services = (response.values?.["services"] as string[]) ?? [];
+    const types = (response.values?.["types"] as string[]) ?? [];
+    return services.map((name, i) => ({ name, type: types[i] ?? "" }));
+  }
+
+  async listActions(): Promise<ActionInfo[]> {
+    // rosapi has no built-in action listing. Heuristic: action servers expose
+    // topics matching */_action/feedback. Extract action names from that pattern.
+    const topics = await this.listTopics();
+    const actions: ActionInfo[] = [];
+    const feedbackSuffix = "/_action/feedback";
+
+    for (const topic of topics) {
+      if (topic.name.endsWith(feedbackSuffix)) {
+        const actionName = topic.name.slice(0, -feedbackSuffix.length);
+        // Feedback type is like "pkg/action/Name_FeedbackMessage"
+        // Extract base action type by stripping "_FeedbackMessage" suffix
+        let actionType = topic.type;
+        if (actionType.endsWith("_FeedbackMessage")) {
+          actionType = actionType.slice(0, -"_FeedbackMessage".length);
+        }
+        actions.push({ name: actionName, type: actionType });
+      }
+    }
+
+    return actions;
+  }
+}