foxglove-ros-adapter 0.2.0

package/dist/index.cjs

@@ -0,0 +1,1340 @@
+'use strict';
+
+var rosmsg = require('@foxglove/rosmsg');
+var rosmsg2Serialization = require('@foxglove/rosmsg2-serialization');
+var zod = require('zod');
+
+// src/ros.ts
+var channelSchema = zod.z.object({
+  id: zod.z.number(),
+  topic: zod.z.string(),
+  encoding: zod.z.string(),
+  schemaName: zod.z.string(),
+  schema: zod.z.string(),
+  schemaEncoding: zod.z.string().optional()
+});
+var serviceSchemaSide = zod.z.object({
+  encoding: zod.z.string().optional(),
+  schemaName: zod.z.string().optional(),
+  schemaEncoding: zod.z.string().optional(),
+  schema: zod.z.string()
+});
+var serviceSchema = zod.z.object({
+  id: zod.z.number(),
+  name: zod.z.string(),
+  type: zod.z.string(),
+  // Legacy flat form (foxglove.websocket.v1 bridges).
+  requestSchema: zod.z.string().optional(),
+  responseSchema: zod.z.string().optional(),
+  // foxglove.sdk.v1 nested form.
+  request: serviceSchemaSide.optional(),
+  response: serviceSchemaSide.optional()
+});
+var parameterValueSchema = zod.z.object({
+  name: zod.z.string(),
+  value: zod.z.unknown(),
+  type: zod.z.string().optional()
+});
+var serverInfoMessageSchema = zod.z.object({
+  op: zod.z.literal("serverInfo"),
+  name: zod.z.string(),
+  capabilities: zod.z.array(zod.z.string()),
+  supportedEncodings: zod.z.array(zod.z.string()).optional(),
+  metadata: zod.z.record(zod.z.string(), zod.z.string()).optional(),
+  sessionId: zod.z.string().optional()
+});
+var advertiseMessageSchema = zod.z.object({
+  op: zod.z.literal("advertise"),
+  channels: zod.z.array(channelSchema)
+});
+var unadvertiseMessageSchema = zod.z.object({
+  op: zod.z.literal("unadvertise"),
+  channelIds: zod.z.array(zod.z.number())
+});
+var advertiseServicesMessageSchema = zod.z.object({
+  op: zod.z.literal("advertiseServices"),
+  services: zod.z.array(serviceSchema)
+});
+var unadvertiseServicesMessageSchema = zod.z.object({
+  op: zod.z.literal("unadvertiseServices"),
+  serviceIds: zod.z.array(zod.z.number())
+});
+var parameterValuesMessageSchema = zod.z.object({
+  op: zod.z.literal("parameterValues"),
+  id: zod.z.string().optional(),
+  parameters: zod.z.array(parameterValueSchema)
+});
+var statusMessageSchema = zod.z.object({
+  op: zod.z.literal("status"),
+  level: zod.z.number(),
+  message: zod.z.string().optional(),
+  msg: zod.z.string().optional()
+});
+var serviceCallFailureMessageSchema = zod.z.object({
+  op: zod.z.literal("serviceCallFailure"),
+  serviceId: zod.z.number(),
+  callId: zod.z.number(),
+  message: zod.z.string().optional()
+});
+var serverMessageSchema = zod.z.discriminatedUnion("op", [
+  serverInfoMessageSchema,
+  advertiseMessageSchema,
+  unadvertiseMessageSchema,
+  advertiseServicesMessageSchema,
+  unadvertiseServicesMessageSchema,
+  parameterValuesMessageSchema,
+  statusMessageSchema,
+  serviceCallFailureMessageSchema
+]);
+
+// src/protocol.ts
+var OP_MESSAGE_DATA = 1;
+var OP_SERVICE_CALL_RESPONSE = 3;
+var OP_CLIENT_MESSAGE_DATA = 1;
+var OP_CLIENT_SERVICE_CALL_REQUEST = 2;
+var FoxgloveProtocolClient = class {
+  ws = null;
+  handlers = {
+    serverInfo: /* @__PURE__ */ new Set(),
+    advertise: /* @__PURE__ */ new Set(),
+    unadvertise: /* @__PURE__ */ new Set(),
+    advertiseServices: /* @__PURE__ */ new Set(),
+    unadvertiseServices: /* @__PURE__ */ new Set(),
+    message: /* @__PURE__ */ new Set(),
+    serviceResponse: /* @__PURE__ */ new Set(),
+    serviceCallFailure: /* @__PURE__ */ new Set(),
+    parameterValues: /* @__PURE__ */ new Set(),
+    open: /* @__PURE__ */ new Set(),
+    close: /* @__PURE__ */ new Set(),
+    error: /* @__PURE__ */ new Set()
+  };
+  nextSubscriptionId = 1;
+  nextCallId = 1;
+  nextClientChannelId = 1;
+  connect(url) {
+    this.close();
+    const wsUrl = url.replace(/^http/, "ws");
+    this.ws = new WebSocket(wsUrl, ["foxglove.sdk.v1", "foxglove.websocket.v1"]);
+    this.ws.binaryType = "arraybuffer";
+    this.ws.onopen = () => this.dispatch(this.handlers.open, (h) => h(), "open");
+    this.ws.onclose = (event) => this.dispatch(this.handlers.close, (h) => h(event), "close");
+    this.ws.onerror = (event) => this.dispatch(this.handlers.error, (h) => h(event), "error");
+    this.ws.onmessage = (event) => this.handleMessage(event);
+  }
+  close() {
+    const ws = this.ws;
+    if (!ws) return;
+    this.ws = null;
+    this.dispatch(this.handlers.close, (h) => h(new CloseEvent("close")), "close");
+    ws.onopen = null;
+    ws.onclose = null;
+    ws.onerror = null;
+    ws.onmessage = null;
+    ws.close();
+  }
+  get isConnected() {
+    return this.ws?.readyState === WebSocket.OPEN;
+  }
+  // ─── Subscriptions ──────────────────────────────────────────────────────
+  subscribe(channelId) {
+    const id = this.nextSubscriptionId++;
+    this.sendJson({
+      op: "subscribe",
+      subscriptions: [{ id, channelId }]
+    });
+    return id;
+  }
+  unsubscribe(subscriptionId) {
+    this.sendJson({
+      op: "unsubscribe",
+      subscriptionIds: [subscriptionId]
+    });
+  }
+  // ─── Publishing ─────────────────────────────────────────────────────────
+  advertiseClientChannel(topic, encoding, schemaName) {
+    const id = this.nextClientChannelId++;
+    this.sendJson({
+      op: "advertise",
+      channels: [{ id, topic, encoding, schemaName }]
+    });
+    return id;
+  }
+  unadvertiseClientChannel(channelId) {
+    this.sendJson({
+      op: "unadvertise",
+      channelIds: [channelId]
+    });
+  }
+  publishMessage(channelId, data) {
+    if (this.ws?.readyState !== WebSocket.OPEN) return;
+    const msg = new Uint8Array(1 + 4 + data.byteLength);
+    const view = new DataView(msg.buffer);
+    view.setUint8(0, OP_CLIENT_MESSAGE_DATA);
+    view.setUint32(1, channelId, true);
+    msg.set(data, 5);
+    this.ws.send(msg);
+  }
+  // ─── Services ───────────────────────────────────────────────────────────
+  callService(serviceId, encoding, requestData) {
+    const callId = this.nextCallId++;
+    if (this.ws?.readyState !== WebSocket.OPEN) return callId;
+    const encodingBytes = new TextEncoder().encode(encoding);
+    const msg = new Uint8Array(
+      1 + 4 + 4 + 4 + encodingBytes.byteLength + requestData.byteLength
+    );
+    const view = new DataView(msg.buffer);
+    let offset = 0;
+    view.setUint8(offset, OP_CLIENT_SERVICE_CALL_REQUEST);
+    offset += 1;
+    view.setUint32(offset, serviceId, true);
+    offset += 4;
+    view.setUint32(offset, callId, true);
+    offset += 4;
+    view.setUint32(offset, encodingBytes.byteLength, true);
+    offset += 4;
+    msg.set(encodingBytes, offset);
+    offset += encodingBytes.byteLength;
+    msg.set(requestData, offset);
+    this.ws.send(msg);
+    return callId;
+  }
+  // ─── Parameters ─────────────────────────────────────────────────────────
+  getParameters(names, requestId) {
+    this.sendJson({
+      op: "getParameters",
+      parameterNames: names,
+      id: requestId
+    });
+  }
+  setParameters(parameters, requestId) {
+    this.sendJson({
+      op: "setParameters",
+      parameters,
+      id: requestId
+    });
+  }
+  // ─── Event system ───────────────────────────────────────────────────────
+  on(event, callback) {
+    this.handlers[event].add(callback);
+  }
+  off(event, callback) {
+    this.handlers[event].delete(callback);
+  }
+  /**
+   * Dispatch helper that stays outside TypeScript's distributed-generic
+   * limitation. Each caller passes the concrete Set for one event plus a
+   * function that invokes a single handler with the right argument shape,
+   * so the spread is in a non-generic context and the types line up.
+   */
+  dispatch(handlers, invoke, event) {
+    for (const handler of handlers) {
+      try {
+        invoke(handler);
+      } catch (e) {
+        console.warn(`Error in ${event} handler:`, e);
+      }
+    }
+  }
+  // ─── Message handling ───────────────────────────────────────────────────
+  handleMessage(event) {
+    if (typeof event.data === "string") {
+      this.handleTextMessage(event.data);
+    } else if (event.data instanceof ArrayBuffer) {
+      this.handleBinaryMessage(event.data);
+    }
+  }
+  handleTextMessage(text) {
+    let parsed;
+    try {
+      parsed = JSON.parse(text);
+    } catch {
+      console.warn("Failed to parse foxglove text message:", text.slice(0, 100));
+      return;
+    }
+    const result = serverMessageSchema.safeParse(parsed);
+    if (!result.success) {
+      return;
+    }
+    const msg = result.data;
+    switch (msg.op) {
+      case "serverInfo":
+        this.dispatch(this.handlers.serverInfo, (h) => h(msg), "serverInfo");
+        break;
+      case "advertise":
+        this.dispatch(this.handlers.advertise, (h) => h(msg.channels), "advertise");
+        break;
+      case "unadvertise":
+        this.dispatch(this.handlers.unadvertise, (h) => h(msg.channelIds), "unadvertise");
+        break;
+      case "advertiseServices":
+        this.dispatch(
+          this.handlers.advertiseServices,
+          (h) => h(msg.services),
+          "advertiseServices"
+        );
+        break;
+      case "unadvertiseServices":
+        this.dispatch(
+          this.handlers.unadvertiseServices,
+          (h) => h(msg.serviceIds),
+          "unadvertiseServices"
+        );
+        break;
+      case "parameterValues":
+        this.dispatch(
+          this.handlers.parameterValues,
+          (h) => h(msg.id ?? "", msg.parameters),
+          "parameterValues"
+        );
+        break;
+      case "status": {
+        const message = msg.message ?? msg.msg;
+        if (msg.level === 0) console.info("[foxglove]", message);
+        else console.warn("[foxglove]", message);
+        break;
+      }
+      case "serviceCallFailure":
+        this.dispatch(
+          this.handlers.serviceCallFailure,
+          (h) => h({
+            serviceId: msg.serviceId,
+            callId: msg.callId,
+            message: msg.message ?? "service call failed"
+          }),
+          "serviceCallFailure"
+        );
+        break;
+    }
+  }
+  handleBinaryMessage(data) {
+    const bytes = new Uint8Array(data);
+    if (bytes.length < 1) return;
+    const opcode = bytes[0];
+    if (opcode === void 0) return;
+    const view = new DataView(data);
+    switch (opcode) {
+      case OP_MESSAGE_DATA: {
+        if (bytes.length < 13) return;
+        const subscriptionId = view.getUint32(1, true);
+        const timestamp = view.getBigUint64(5, true);
+        const msgData = bytes.slice(13);
+        this.dispatch(
+          this.handlers.message,
+          (h) => h(subscriptionId, timestamp, msgData),
+          "message"
+        );
+        break;
+      }
+      case OP_SERVICE_CALL_RESPONSE: {
+        if (bytes.length < 13) return;
+        const serviceId = view.getUint32(1, true);
+        const callId = view.getUint32(5, true);
+        const encodingLength = view.getUint32(9, true);
+        const encoding = new TextDecoder().decode(bytes.slice(13, 13 + encodingLength));
+        const responseData = bytes.slice(13 + encodingLength);
+        this.dispatch(
+          this.handlers.serviceResponse,
+          (h) => h({ serviceId, callId, encoding, data: responseData }),
+          "serviceResponse"
+        );
+        break;
+      }
+      default:
+        console.warn(
+          `[foxglove] unhandled binary opcode 0x${opcode.toString(16)} (len=${bytes.length})`
+        );
+    }
+  }
+  sendJson(msg) {
+    if (this.ws?.readyState === WebSocket.OPEN) {
+      this.ws.send(JSON.stringify(msg));
+    }
+  }
+};
+
+// src/ros.ts
+function normalizeRosType(type, kind = "msg") {
+  const trimmed = type.replace(/^\/+/, "");
+  if (trimmed.includes(`/${kind}/`)) {
+    return trimmed;
+  }
+  const slash = trimmed.indexOf("/");
+  if (slash === -1) {
+    return trimmed;
+  }
+  return `${trimmed.slice(0, slash)}/${kind}/${trimmed.slice(slash + 1)}`;
+}
+function toFoxgloveParamName(name) {
+  return name.replaceAll(":", ".");
+}
+var Ros = class {
+  protocol = new FoxgloveProtocolClient();
+  connectionListeners = /* @__PURE__ */ new Set();
+  closeListeners = /* @__PURE__ */ new Set();
+  errorListeners = /* @__PURE__ */ new Set();
+  // Channel/service registries populated by foxglove_bridge advertisements
+  channels = /* @__PURE__ */ new Map();
+  // channelId → channel
+  channelsByTopic = /* @__PURE__ */ new Map();
+  // topicName → channel
+  services = /* @__PURE__ */ new Map();
+  // serviceId → service
+  servicesByName = /* @__PURE__ */ new Map();
+  // serviceName → service
+  // Subscription state
+  subscriptions = /* @__PURE__ */ new Map();
+  // subscriptionId → sub
+  subscriptionsByTopic = /* @__PURE__ */ new Map();
+  // topic → sub
+  pendingSubscribers = [];
+  // Publishing state
+  clientChannels = /* @__PURE__ */ new Map();
+  // topic → client channel
+  // Service call correlation
+  pendingServiceCalls = /* @__PURE__ */ new Map();
+  // callId → pending
+  // Splitting onValues/onClose lets callers distinguish "param missing"
+  // (resolve null) from "WebSocket closed before reply" (reject).
+  pendingParamRequests = /* @__PURE__ */ new Map();
+  nextParamRequestId = 1;
+  // Compiled reader/writer caches keyed by the canonical schema name plus the
+  // full schema text. The full text is the only collision-free identity:
+  // schema.length alone collides for equal-length schemas, and schemaName alone
+  // collides across distros that share a type name but evolve its fields.
+  // MessageReader / MessageWriter internally pre-compile per-type decode paths
+  // from a MessageDefinition list, so we reuse them across messages instead of
+  // re-parsing the schema on every topic/service hit.
+  readerCache = /* @__PURE__ */ new Map();
+  writerCache = /* @__PURE__ */ new Map();
+  isConnected = false;
+  constructor(options) {
+    this.setupProtocolHandlers();
+    if (options?.url) {
+      this.connect(options.url);
+    }
+  }
+  connect(url) {
+    this.protocol.connect(url);
+  }
+  close() {
+    this.protocol.close();
+    this.isConnected = false;
+  }
+  on(...args) {
+    const [event, callback] = args;
+    switch (event) {
+      case "connection":
+        this.connectionListeners.add(callback);
+        break;
+      case "close":
+        this.closeListeners.add(callback);
+        break;
+      case "error":
+        this.errorListeners.add(callback);
+        break;
+    }
+  }
+  off(...args) {
+    const [event, callback] = args;
+    switch (event) {
+      case "connection":
+        this.connectionListeners.delete(callback);
+        break;
+      case "close":
+        this.closeListeners.delete(callback);
+        break;
+      case "error":
+        this.errorListeners.delete(callback);
+        break;
+    }
+  }
+  /**
+   * roslib-compatible bulk listener removal. Tests use this to reset the
+   * module-level mock Ros between cases; production code does not call it.
+   */
+  removeAllListeners(event) {
+    if (event === void 0) {
+      this.connectionListeners.clear();
+      this.closeListeners.clear();
+      this.errorListeners.clear();
+      return;
+    }
+    switch (event) {
+      case "connection":
+        this.connectionListeners.clear();
+        break;
+      case "close":
+        this.closeListeners.clear();
+        break;
+      case "error":
+        this.errorListeners.clear();
+        break;
+    }
+  }
+  emit(event, error) {
+    switch (event) {
+      case "connection":
+        this.emitLifecycle("connection", this.connectionListeners);
+        break;
+      case "close":
+        this.emitLifecycle("close", this.closeListeners);
+        break;
+      case "error":
+        this.emitError(error ?? new Error("ros emit('error') called without an error"));
+        break;
+    }
+  }
+  emitLifecycle(event, listeners) {
+    for (const cb of listeners) {
+      try {
+        cb();
+      } catch (e) {
+        console.warn(`Error in Ros "${event}" handler:`, e);
+      }
+    }
+  }
+  emitError(error) {
+    for (const cb of this.errorListeners) {
+      try {
+        cb(error);
+      } catch (e) {
+        console.warn('Error in Ros "error" handler:', e);
+      }
+    }
+  }
+  // ─── Channel/Service Lookups ────────────────────────────────────────────
+  getChannel(topic) {
+    return this.channelsByTopic.get(topic);
+  }
+  getServiceByName(name) {
+    return this.servicesByName.get(name);
+  }
+  /**
+   * Return every advertised topic whose schema matches `messageType`. Matches
+   * roslib's `Ros#getTopicsForType(type, success, fail)` callback API so
+   * existing callers (e.g. `useImageTopics`) can use this adapter unchanged.
+   */
+  /**
+   * The roslib signature also accepts an `onError` callback. Foxglove resolves
+   * synchronously from the in-memory channel registry, so the failure path is
+   * unreachable; we drop the parameter to keep lint clean. Callers that pass
+   * one are unaffected — extra arguments to a JS function are ignored.
+   */
+  getTopicsForType(messageType, onSuccess) {
+    const canonical = normalizeRosType(messageType, "msg");
+    const topics = [];
+    for (const ch of this.channels.values()) {
+      if (normalizeRosType(ch.schemaName, "msg") === canonical) {
+        topics.push(ch.topic);
+      }
+    }
+    onSuccess(topics);
+  }
+  // ─── Topic Subscription ─────────────────────────────────────────────────
+  subscribeTopic(topic, messageType, callback) {
+    const existing = this.subscriptionsByTopic.get(topic);
+    if (existing) {
+      existing.callbacks.add(callback);
+      return;
+    }
+    const channel = this.channelsByTopic.get(topic);
+    if (channel) {
+      this.createSubscription(channel, callback);
+    } else {
+      this.pendingSubscribers.push({ topic, messageType, callback });
+    }
+  }
+  unsubscribeTopic(topic, callback) {
+    const keep = this.pendingSubscribers.filter(
+      (p) => p.topic !== topic || callback !== void 0 && p.callback !== callback
+    );
+    if (keep.length !== this.pendingSubscribers.length) {
+      this.pendingSubscribers.length = 0;
+      this.pendingSubscribers.push(...keep);
+    }
+    const sub = this.subscriptionsByTopic.get(topic);
+    if (!sub) return;
+    if (callback) {
+      sub.callbacks.delete(callback);
+      if (sub.callbacks.size > 0) return;
+    }
+    this.protocol.unsubscribe(sub.subscriptionId);
+    this.subscriptions.delete(sub.subscriptionId);
+    this.subscriptionsByTopic.delete(topic);
+  }
+  createSubscription(channel, callback) {
+    const reader = this.getReader(channel.schemaName, channel.schema);
+    const subscriptionId = this.protocol.subscribe(channel.id);
+    const sub = {
+      subscriptionId,
+      channelId: channel.id,
+      callbacks: /* @__PURE__ */ new Set([callback]),
+      reader
+    };
+    this.subscriptions.set(subscriptionId, sub);
+    this.subscriptionsByTopic.set(channel.topic, sub);
+  }
+  // ─── Topic Publishing ───────────────────────────────────────────────────
+  publishTopic(topic, messageType, message) {
+    let clientCh = this.clientChannels.get(topic);
+    if (!clientCh) {
+      const schemaName = normalizeRosType(messageType, "msg");
+      const writer = this.findWriterForSchema(schemaName);
+      const encoding = writer ? "cdr" : "json";
+      const clientChannelId = this.protocol.advertiseClientChannel(
+        topic,
+        encoding,
+        schemaName
+      );
+      clientCh = { clientChannelId, topic, schemaName, encoding, writer };
+      this.clientChannels.set(topic, clientCh);
+    }
+    if (clientCh.encoding === "cdr") {
+      if (!clientCh.writer) {
+        clientCh.writer = this.findWriterForSchema(clientCh.schemaName);
+      }
+      if (!clientCh.writer) {
+        throw new Error(
+          `Cannot publish CDR to "${topic}": schema "${clientCh.schemaName}" disappeared from advertised channels.`
+        );
+      }
+      const cdrData = clientCh.writer.writeMessage(message);
+      this.protocol.publishMessage(clientCh.clientChannelId, cdrData);
+      return;
+    }
+    const jsonData = new TextEncoder().encode(JSON.stringify(message));
+    this.protocol.publishMessage(clientCh.clientChannelId, jsonData);
+  }
+  findWriterForSchema(schemaName) {
+    const canonical = normalizeRosType(schemaName, "msg");
+    for (const channel of this.channels.values()) {
+      if (normalizeRosType(channel.schemaName, "msg") === canonical) {
+        return this.getWriter(channel.schemaName, channel.schema);
+      }
+    }
+    return null;
+  }
+  unpublishTopic(topic) {
+    const clientCh = this.clientChannels.get(topic);
+    if (clientCh) {
+      this.protocol.unadvertiseClientChannel(clientCh.clientChannelId);
+      this.clientChannels.delete(topic);
+    }
+  }
+  // ─── Service Calls ──────────────────────────────────────────────────────
+  callService(serviceName, serviceType, request) {
+    return new Promise((resolve, reject) => {
+      const svc = this.servicesByName.get(serviceName);
+      if (!svc) {
+        reject(new Error(`Service ${serviceName} not available`));
+        return;
+      }
+      const canonicalType = normalizeRosType(serviceType, "srv");
+      const requestSchema = svc.request?.schema ?? svc.requestSchema ?? "";
+      const responseSchema = svc.response?.schema ?? svc.responseSchema ?? "";
+      if (!requestSchema) {
+        reject(new Error(`No request definition for service ${serviceName}`));
+        return;
+      }
+      const requestWriter = this.getWriter(canonicalType + "_Request", requestSchema);
+      const responseReader = responseSchema ? this.getReader(canonicalType + "_Response", responseSchema) : null;
+      const requestData = requestWriter.writeMessage(request);
+      const callId = this.protocol.callService(svc.id, "cdr", requestData);
+      this.pendingServiceCalls.set(callId, { resolve, reject, responseReader });
+    });
+  }
+  // ─── Parameters ─────────────────────────────────────────────────────────
+  getParam(name) {
+    return new Promise((resolve, reject) => {
+      const wireName = toFoxgloveParamName(name);
+      const requestId = `param_get_${this.nextParamRequestId++}`;
+      this.pendingParamRequests.set(requestId, {
+        onValues: (params) => {
+          const param = params.find((p) => toFoxgloveParamName(p.name) === wireName);
+          resolve(param?.value ?? null);
+        },
+        onClose: () => reject(new Error(`getParam(${name}): WebSocket closed before response`))
+      });
+      this.protocol.getParameters([wireName], requestId);
+    });
+  }
+  setParam(name, value) {
+    return new Promise((resolve, reject) => {
+      const wireName = toFoxgloveParamName(name);
+      const requestId = `param_set_${this.nextParamRequestId++}`;
+      this.pendingParamRequests.set(requestId, {
+        onValues: () => resolve(),
+        onClose: () => reject(new Error(`setParam(${name}): WebSocket closed before response`))
+      });
+      this.protocol.setParameters([{ name: wireName, value }], requestId);
+    });
+  }
+  // ─── Internal: Protocol Event Handlers ──────────────────────────────────
+  setupProtocolHandlers() {
+    this.protocol.on("open", () => {
+      this.isConnected = true;
+      this.emit("connection");
+    });
+    this.protocol.on("close", () => {
+      this.isConnected = false;
+      this.channels.clear();
+      this.channelsByTopic.clear();
+      this.services.clear();
+      this.servicesByName.clear();
+      this.subscriptions.clear();
+      this.subscriptionsByTopic.clear();
+      this.clientChannels.clear();
+      this.readerCache.clear();
+      this.writerCache.clear();
+      const closeError = new Error("WebSocket closed before response received");
+      for (const pending of this.pendingServiceCalls.values()) {
+        try {
+          pending.reject(closeError);
+        } catch (e) {
+          console.warn("Error rejecting pending service call on close:", e);
+        }
+      }
+      this.pendingServiceCalls.clear();
+      for (const pending of this.pendingParamRequests.values()) {
+        try {
+          pending.onClose();
+        } catch (e) {
+          console.warn("Error completing pending param request on close:", e);
+        }
+      }
+      this.pendingParamRequests.clear();
+      this.pendingSubscribers.length = 0;
+      this.emit("close");
+    });
+    this.protocol.on("error", () => {
+      this.emit("error", new Error("Foxglove WebSocket error"));
+    });
+    this.protocol.on("advertise", (channels) => {
+      for (const ch of channels) {
+        this.channels.set(ch.id, ch);
+        this.channelsByTopic.set(ch.topic, ch);
+      }
+      this.processPendingSubscribers();
+    });
+    this.protocol.on("unadvertise", (channelIds) => {
+      for (const id of channelIds) {
+        const ch = this.channels.get(id);
+        if (ch) {
+          this.channelsByTopic.delete(ch.topic);
+          this.channels.delete(id);
+        }
+      }
+    });
+    this.protocol.on("advertiseServices", (svcs) => {
+      for (const svc of svcs) {
+        this.services.set(svc.id, svc);
+        this.servicesByName.set(svc.name, svc);
+      }
+    });
+    this.protocol.on("unadvertiseServices", (serviceIds) => {
+      for (const id of serviceIds) {
+        const svc = this.services.get(id);
+        if (svc) {
+          this.servicesByName.delete(svc.name);
+          this.services.delete(id);
+        }
+      }
+    });
+    this.protocol.on(
+      "message",
+      (subscriptionId, _timestamp, data) => {
+        const sub = this.subscriptions.get(subscriptionId);
+        if (!sub) return;
+        try {
+          const msg = sub.reader.readMessage(data);
+          for (const cb of sub.callbacks) {
+            cb(msg);
+          }
+        } catch (e) {
+          console.warn("CDR deserialization error:", e);
+        }
+      }
+    );
+    this.protocol.on("serviceResponse", (response) => {
+      const pending = this.pendingServiceCalls.get(response.callId);
+      if (!pending) return;
+      this.pendingServiceCalls.delete(response.callId);
+      try {
+        if (pending.responseReader) {
+          const result = pending.responseReader.readMessage(
+            response.data
+          );
+          pending.resolve(result);
+          return;
+        }
+        pending.resolve({});
+      } catch (e) {
+        pending.reject(e instanceof Error ? e : new Error(String(e)));
+      }
+    });
+    this.protocol.on("serviceCallFailure", (failure) => {
+      const pending = this.pendingServiceCalls.get(failure.callId);
+      if (!pending) return;
+      this.pendingServiceCalls.delete(failure.callId);
+      pending.reject(new Error(failure.message));
+    });
+    this.protocol.on("parameterValues", (id, params) => {
+      const pending = this.pendingParamRequests.get(id);
+      if (pending) {
+        this.pendingParamRequests.delete(id);
+        pending.onValues(params);
+      }
+    });
+  }
+  processPendingSubscribers() {
+    const remaining = [];
+    for (const pending of this.pendingSubscribers) {
+      const channel = this.channelsByTopic.get(pending.topic);
+      if (channel) {
+        const existing = this.subscriptionsByTopic.get(pending.topic);
+        if (existing) {
+          existing.callbacks.add(pending.callback);
+        } else {
+          this.createSubscription(channel, pending.callback);
+        }
+      } else {
+        remaining.push(pending);
+      }
+    }
+    this.pendingSubscribers.length = 0;
+    this.pendingSubscribers.push(...remaining);
+  }
+  getReader(schemaName, schema) {
+    const key = `${normalizeRosType(schemaName, "msg")} ${schema}`;
+    let reader = this.readerCache.get(key);
+    if (!reader) {
+      reader = new rosmsg2Serialization.MessageReader(rosmsg.parse(schema, { ros2: true }));
+      this.readerCache.set(key, reader);
+    }
+    return reader;
+  }
+  getWriter(schemaName, schema) {
+    const key = `${normalizeRosType(schemaName, "msg")} ${schema}`;
+    let writer = this.writerCache.get(key);
+    if (!writer) {
+      writer = new rosmsg2Serialization.MessageWriter(rosmsg.parse(schema, { ros2: true }));
+      this.writerCache.set(key, writer);
+    }
+    return writer;
+  }
+};
+
+// src/topic.ts
+var Topic = class {
+  ros;
+  name;
+  messageType;
+  messageSchema;
+  /**
+   * Throttle window in ms. `0` disables throttling — in that case `subscribe`
+   * skips the throttling code path entirely and registers an unwrapped callback
+   * so the per-message hot path has no throttle-related work.
+   */
+  throttleMs;
+  /**
+   * Original user callback → wrapped MessageCallback registered with Ros.
+   * Keyed as `object` (all JS functions are objects) so `Topic<T>` stays
+   * variance-friendly for callers that pass a `Topic<Foo>` into a slot typed
+   * as `Topic<unknown>` — otherwise T ends up invariant through this field
+   * and blocks assignments that are safe at runtime.
+   */
+  wrappedCallbacks = /* @__PURE__ */ new Map();
+  constructor(options) {
+    this.ros = options.ros;
+    this.name = options.name;
+    this.messageType = options.messageType;
+    this.messageSchema = options.messageSchema;
+    const rate = options.throttle_rate ?? 0;
+    this.throttleMs = Number.isFinite(rate) && rate > 0 ? rate : 0;
+  }
+  /**
+   * advertise() is a no-op: the foxglove adapter advertises a client channel
+   * lazily on the first publish() and reuses it afterwards, so explicit
+   * registration has no effect on the wire.
+   */
+  advertise() {
+  }
+  /**
+   * Release the client channel allocated by the lazy advertise on first publish().
+   * Callers that publish once then tear down (e.g. joint-control-spinners' StoreJointState
+   * burst) rely on this to drop the channel registration rather than leaking it for the
+   * life of the Ros connection.
+   */
+  unadvertise() {
+    this.ros.unpublishTopic(this.name);
+  }
+  subscribe(callback) {
+    if (this.wrappedCallbacks.has(callback)) {
+      return;
+    }
+    const messageSchema = this.messageSchema;
+    const deliver = messageSchema ? (msg) => callback(messageSchema.parse(msg)) : callback;
+    let wrappedCb;
+    if (this.throttleMs === 0) {
+      wrappedCb = deliver;
+    } else {
+      const windowMs = this.throttleMs;
+      let lastFiredAt = -Infinity;
+      wrappedCb = (msg) => {
+        const now = performance.now();
+        if (now - lastFiredAt < windowMs) return;
+        lastFiredAt = now;
+        deliver(msg);
+      };
+    }
+    this.wrappedCallbacks.set(callback, wrappedCb);
+    this.ros.subscribeTopic(this.name, this.messageType, wrappedCb);
+  }
+  unsubscribe(callback) {
+    if (callback) {
+      const wrapped = this.wrappedCallbacks.get(callback);
+      if (wrapped) {
+        this.ros.unsubscribeTopic(this.name, wrapped);
+        this.wrappedCallbacks.delete(callback);
+      }
+      return;
+    }
+    for (const wrapped of this.wrappedCallbacks.values()) {
+      this.ros.unsubscribeTopic(this.name, wrapped);
+    }
+    this.wrappedCallbacks.clear();
+  }
+  publish(message) {
+    this.ros.publishTopic(this.name, this.messageType, message);
+  }
+};
+
+// src/service.ts
+var Service = class {
+  ros;
+  name;
+  serviceType;
+  responseSchema;
+  constructor(options) {
+    this.ros = options.ros;
+    this.name = options.name;
+    this.serviceType = options.serviceType;
+    this.responseSchema = options.responseSchema;
+  }
+  /**
+   * Call the service (roslib-compatible callback API).
+   */
+  callService(request, onSuccess, onError) {
+    const responseSchema = this.responseSchema;
+    const deliver = onSuccess && responseSchema ? (response) => onSuccess(responseSchema.parse(response)) : onSuccess;
+    this.ros.callService(this.name, this.serviceType, request).then((response) => deliver?.(response)).catch((err) => {
+      const msg = err instanceof Error ? err.message : String(err);
+      onError?.(msg);
+    });
+  }
+};
+
+// src/param.ts
+var Param = class {
+  ros;
+  name;
+  constructor(options) {
+    this.ros = options.ros;
+    this.name = options.name;
+  }
+  get(callback) {
+    this.ros.getParam(this.name).then(callback).catch((err) => {
+      console.warn(`Param.get("${this.name}") failed:`, err);
+      callback(null);
+    });
+  }
+  set(value, onSuccess, onError) {
+    this.ros.setParam(this.name, value).then(() => onSuccess?.()).catch((err) => {
+      const msg = err instanceof Error ? err.message : String(err);
+      onError?.(msg);
+    });
+  }
+};
+
+// src/types.ts
+var Vector3 = class {
+  x;
+  y;
+  z;
+  constructor(options) {
+    this.x = options?.x ?? 0;
+    this.y = options?.y ?? 0;
+    this.z = options?.z ?? 0;
+  }
+};
+var Quaternion = class {
+  x;
+  y;
+  z;
+  w;
+  constructor(options) {
+    this.x = options?.x ?? 0;
+    this.y = options?.y ?? 0;
+    this.z = options?.z ?? 0;
+    this.w = options?.w ?? 1;
+  }
+};
+var Transform = class _Transform {
+  translation;
+  rotation;
+  constructor(options) {
+    this.translation = options?.translation ?? new Vector3();
+    this.rotation = options?.rotation ?? new Quaternion();
+  }
+  static identity() {
+    return new _Transform();
+  }
+  /**
+   * Invert a rigid transform: if `this` is a→b, the result is b→a. For a unit
+   * quaternion the inverse rotation is its conjugate, and the inverse
+   * translation is that conjugate applied to the negated translation.
+   */
+  inverse() {
+    const r = this.rotation;
+    const invRotation = new Quaternion({ x: -r.x, y: -r.y, z: -r.z, w: r.w });
+    const rotated = rotateVectorByQuaternion(this.translation, invRotation);
+    return new _Transform({
+      translation: new Vector3({ x: -rotated.x, y: -rotated.y, z: -rotated.z }),
+      rotation: invRotation
+    });
+  }
+  /**
+   * Compose two rigid transforms: `this` (a→b) composed with `other` (b→c)
+   * yields the transform from a→c.
+   */
+  multiply(other) {
+    const rotated = rotateVectorByQuaternion(other.translation, this.rotation);
+    const translation = new Vector3({
+      x: this.translation.x + rotated.x,
+      y: this.translation.y + rotated.y,
+      z: this.translation.z + rotated.z
+    });
+    const rotation = multiplyQuaternions(this.rotation, other.rotation);
+    return new _Transform({ translation, rotation });
+  }
+};
+function multiplyQuaternions(a, b) {
+  return new Quaternion({
+    x: a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
+    y: a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,
+    z: a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w,
+    w: a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z
+  });
+}
+function rotateVectorByQuaternion(v, q) {
+  const { x, y, z: z3 } = v;
+  const qx = q.x;
+  const qy = q.y;
+  const qz = q.z;
+  const qw = q.w;
+  const tx = 2 * (qy * z3 - qz * y);
+  const ty = 2 * (qz * x - qx * z3);
+  const tz = 2 * (qx * y - qy * x);
+  return new Vector3({
+    x: x + qw * tx + (qy * tz - qz * ty),
+    y: y + qw * ty + (qz * tx - qx * tz),
+    z: z3 + qw * tz + (qx * ty - qy * tx)
+  });
+}
+
+// src/tf-client.ts
+var tfMessageSchema = zod.z.object({
+  transforms: zod.z.array(
+    zod.z.object({
+      header: zod.z.object({
+        stamp: zod.z.object({ sec: zod.z.number(), nanosec: zod.z.number() }),
+        frame_id: zod.z.string()
+      }),
+      child_frame_id: zod.z.string(),
+      transform: zod.z.object({
+        translation: zod.z.object({ x: zod.z.number(), y: zod.z.number(), z: zod.z.number() }),
+        rotation: zod.z.object({
+          x: zod.z.number(),
+          y: zod.z.number(),
+          z: zod.z.number(),
+          w: zod.z.number()
+        })
+      })
+    })
+  )
+});
+function normalizeFrameId(frame) {
+  return frame.replace(/^\/+/, "");
+}
+function normalizeQuaternion(q) {
+  const norm = Math.hypot(q.x, q.y, q.z, q.w);
+  if (norm === 0) {
+    return new Quaternion({ x: 0, y: 0, z: 0, w: 1 });
+  }
+  return new Quaternion({ x: q.x / norm, y: q.y / norm, z: q.z / norm, w: q.w / norm });
+}
+function throttleLeadingEdge(fn, windowMs) {
+  let lastFiredAt = -Infinity;
+  return (msg) => {
+    const now = performance.now();
+    if (now - lastFiredAt < windowMs) return;
+    lastFiredAt = now;
+    fn(msg);
+  };
+}
+var ROS2TFClient = class {
+  ros;
+  fixedFrame;
+  frameCallbacks = /* @__PURE__ */ new Map();
+  /** child_frame_id → {parent, local transform}. One entry per child. */
+  directTransforms = /* @__PURE__ */ new Map();
+  // Every frame id ever seen as a parent or child, for enumeration. Grows
+  // monotonically for the life of the client (only dispose() clears it), which
+  // assumes frame ids are stable for a connection — true for a fixed robot, but
+  // a source that mints transient frame ids would accumulate stale entries.
+  knownFrames = /* @__PURE__ */ new Set();
+  framesListeners = /* @__PURE__ */ new Set();
+  // Children for which a conflicting parent has already been reported, so the
+  // multi-authority warning fires once per child rather than every tick.
+  multiParentWarned = /* @__PURE__ */ new Set();
+  tfSub = null;
+  tfStaticSub = null;
+  onReconnect;
+  throttleMs;
+  parseWarned = false;
+  disposed = false;
+  constructor(options) {
+    this.ros = options.ros;
+    this.fixedFrame = normalizeFrameId(options.fixedFrame ?? "world");
+    const rate = options.rate ?? 0;
+    this.throttleMs = Number.isFinite(rate) && rate > 0 ? 1e3 / rate : 0;
+    this.onReconnect = () => this.subscribeTF();
+    this.ros.on("connection", this.onReconnect);
+    this.subscribeTF();
+  }
+  subscribe(frameId, callback) {
+    if (this.warnIfDisposed("subscribe")) return;
+    const key = normalizeFrameId(frameId);
+    let callbacks = this.frameCallbacks.get(key);
+    if (!callbacks) {
+      callbacks = /* @__PURE__ */ new Set();
+      this.frameCallbacks.set(key, callbacks);
+    }
+    callbacks.add(callback);
+    const resolved = this.resolveTransform(key);
+    if (resolved) {
+      callback(resolved);
+    }
+  }
+  unsubscribe(frameId, callback) {
+    const key = normalizeFrameId(frameId);
+    if (callback) {
+      const callbacks = this.frameCallbacks.get(key);
+      callbacks?.delete(callback);
+      if (callbacks?.size === 0) {
+        this.frameCallbacks.delete(key);
+      }
+    } else {
+      this.frameCallbacks.delete(key);
+    }
+  }
+  /**
+   * Sorted list of every frame seen so far on /tf or /tf_static (as a parent or
+   * child). MoveIt Pro extension used to drive the TF visualization frame list.
+   */
+  getFrameIds() {
+    return [...this.knownFrames].sort((a, b) => a.localeCompare(b));
+  }
+  /**
+   * Register a listener fired whenever new frames appear. Invoked immediately
+   * with the current frame list so callers don't miss frames seen before they
+   * subscribed. MoveIt Pro extension, not part of roslib's ROS2TFClient.
+   */
+  addFramesListener(callback) {
+    if (this.warnIfDisposed("addFramesListener")) return;
+    this.framesListeners.add(callback);
+    callback(this.getFrameIds());
+  }
+  removeFramesListener(callback) {
+    this.framesListeners.delete(callback);
+  }
+  dispose() {
+    this.disposed = true;
+    this.ros.off("connection", this.onReconnect);
+    if (this.tfSub) {
+      this.ros.unsubscribeTopic("/tf", this.tfSub);
+      this.tfSub = null;
+    }
+    if (this.tfStaticSub) {
+      this.ros.unsubscribeTopic("/tf_static", this.tfStaticSub);
+      this.tfStaticSub = null;
+    }
+    this.frameCallbacks.clear();
+    this.directTransforms.clear();
+    this.knownFrames.clear();
+    this.framesListeners.clear();
+    this.multiParentWarned.clear();
+  }
+  /** Warn once if a mutating method is called on a disposed client. */
+  warnIfDisposed(method) {
+    if (this.disposed) {
+      console.warn(`ROS2TFClient.${method}() called after dispose(); ignoring.`);
+    }
+    return this.disposed;
+  }
+  subscribeTF() {
+    if (this.tfSub) this.ros.unsubscribeTopic("/tf", this.tfSub);
+    if (this.tfStaticSub) this.ros.unsubscribeTopic("/tf_static", this.tfStaticSub);
+    const dispatch = (msg) => this.dispatchTFMessage(msg);
+    this.tfSub = this.throttleMs > 0 ? throttleLeadingEdge(dispatch, this.throttleMs) : dispatch;
+    this.ros.subscribeTopic("/tf", "tf2_msgs/msg/TFMessage", this.tfSub);
+    this.tfStaticSub = (msg) => this.dispatchTFMessage(msg);
+    this.ros.subscribeTopic("/tf_static", "tf2_msgs/msg/TFMessage", this.tfStaticSub);
+  }
+  dispatchTFMessage(msg) {
+    const parsed = tfMessageSchema.safeParse(msg);
+    if (parsed.success) {
+      this.handleTFMessage(parsed.data);
+    } else if (!this.parseWarned) {
+      this.parseWarned = true;
+      console.warn("ROS2TFClient: ignoring /tf message that failed schema validation.");
+    }
+  }
+  /**
+   * Warn once per child when it is published with a parent different from the
+   * one already stored. A frame with two live parents is a malformed tree (tf2
+   * surfaces this as a multiple-authority warning). We keep last-writer-wins so
+   * resolution still works, but a silent pick can resolve through a different
+   * parent than tf2/RViz would, so make the misconfiguration visible.
+   */
+  warnIfReparented(childFrame, parentFrame) {
+    const existing = this.directTransforms.get(childFrame);
+    if (!existing || existing.parentFrame === parentFrame || this.multiParentWarned.has(childFrame)) {
+      return;
+    }
+    this.multiParentWarned.add(childFrame);
+    console.warn(
+      `TF: frame "${childFrame}" is published with multiple parents ("${existing.parentFrame}" and "${parentFrame}"); using the most recent. This is a malformed TF tree and may render differently from RViz.`
+    );
+  }
+  handleTFMessage(msg) {
+    let anyChanged = false;
+    const framesBefore = this.knownFrames.size;
+    for (const tfStamped of msg.transforms) {
+      const childFrame = normalizeFrameId(tfStamped.child_frame_id);
+      const parentFrame = normalizeFrameId(tfStamped.header.frame_id);
+      const t = tfStamped.transform;
+      this.knownFrames.add(childFrame);
+      this.knownFrames.add(parentFrame);
+      const transform = new Transform({
+        translation: new Vector3({
+          x: t.translation.x,
+          y: t.translation.y,
+          z: t.translation.z
+        }),
+        // Normalize at the ingest boundary so downstream inverse/compose can
+        // assume unit quaternions (see normalizeQuaternion).
+        rotation: normalizeQuaternion(t.rotation)
+      });
+      this.warnIfReparented(childFrame, parentFrame);
+      this.directTransforms.set(childFrame, { parentFrame, transform });
+      anyChanged = true;
+    }
+    if (this.knownFrames.size !== framesBefore) {
+      const frames = this.getFrameIds();
+      for (const listener of this.framesListeners) {
+        listener(frames);
+      }
+    }
+    if (!anyChanged) return;
+    for (const [frameId, callbacks] of this.frameCallbacks) {
+      if (callbacks.size === 0) continue;
+      const resolved = this.resolveTransform(frameId);
+      if (resolved) {
+        for (const cb of callbacks) {
+          cb(resolved);
+        }
+      }
+    }
+  }
+  /**
+   * Resolve the transform from `fixedFrame` to `frameId`, i.e. the pose of
+   * `frameId` expressed in `fixedFrame`.
+   *
+   * tf2 (and RViz) can relate any two connected frames, not just a frame and
+   * its descendant: it walks both frames up to their lowest common ancestor and
+   * inverts one side. We do the same. Walking only child→parent from `frameId`
+   * to `fixedFrame` (the previous approach) silently failed for any frame that
+   * is an ancestor of — or in a sibling branch to — `fixedFrame`, which is the
+   * common case for mobile-base trees where the reference frame sits below
+   * `map`/`odom`.
+   *
+   * Returns null if the two frames are not connected (yet) or the tree is
+   * malformed (cycle).
+   *
+   * Time semantics: this composes each edge's most recent value. Unlike tf2 it
+   * does not interpolate edges to a common timestamp, so during fast motion with
+   * edges published at different rates the result can briefly lead or lag tf2's.
+   * For a visualization layer this is an accepted simplification.
+   */
+  resolveTransform(frameId) {
+    if (frameId === this.fixedFrame) {
+      return Transform.identity();
+    }
+    const direct = this.directTransforms.get(frameId);
+    if (direct !== void 0) {
+      if (direct.parentFrame === this.fixedFrame) {
+        return direct.transform;
+      }
+    }
+    const targetAncestors = this.ancestorTransforms(frameId);
+    if (!targetAncestors) return null;
+    let current = this.fixedFrame;
+    let tFixed = Transform.identity();
+    const visited = /* @__PURE__ */ new Set();
+    for (; ; ) {
+      const tToTarget = targetAncestors.get(current);
+      if (tToTarget) {
+        return tFixed.inverse().multiply(tToTarget);
+      }
+      if (visited.has(current)) return null;
+      visited.add(current);
+      const direct2 = this.directTransforms.get(current);
+      if (!direct2) return null;
+      tFixed = direct2.transform.multiply(tFixed);
+      current = direct2.parentFrame;
+    }
+  }
+  /**
+   * Walk from `frameId` up to its tree root, returning a map from every
+   * ancestor frame (including `frameId`) to the transform expressing `frameId`
+   * in that ancestor's frame. Returns null on a cycle.
+   */
+  ancestorTransforms(frameId) {
+    const ancestors = /* @__PURE__ */ new Map();
+    let current = frameId;
+    let accumulated = Transform.identity();
+    ancestors.set(current, accumulated);
+    const visited = /* @__PURE__ */ new Set();
+    for (; ; ) {
+      if (visited.has(current)) return null;
+      visited.add(current);
+      const direct = this.directTransforms.get(current);
+      if (!direct) return ancestors;
+      accumulated = direct.transform.multiply(accumulated);
+      current = direct.parentFrame;
+      ancestors.set(current, accumulated);
+    }
+  }
+};
+
+exports.Param = Param;
+exports.Quaternion = Quaternion;
+exports.ROS2TFClient = ROS2TFClient;
+exports.Ros = Ros;
+exports.Service = Service;
+exports.Topic = Topic;
+exports.Transform = Transform;
+exports.Vector3 = Vector3;
+//# sourceMappingURL=index.cjs.map
+//# sourceMappingURL=index.cjs.map