@foxglove/ros1
Version:
Standalone TypeScript implementation of the ROS 1 (Robot Operating System) protocol with a pluggable transport layer
581 lines • 28 kB
JavaScript
"use strict";
Object.defineProperty(exports, "__esModule", { value: true });
exports.RosNode = void 0;
const rosmsg_1 = require("@foxglove/rosmsg");
const rosmsg_serialization_1 = require("@foxglove/rosmsg-serialization");
const xmlrpc_1 = require("@foxglove/xmlrpc");
const eventemitter3_1 = require("eventemitter3");
const Publication_1 = require("./Publication");
const RosFollower_1 = require("./RosFollower");
const RosFollowerClient_1 = require("./RosFollowerClient");
const RosMasterClient_1 = require("./RosMasterClient");
const RosParamClient_1 = require("./RosParamClient");
const Subscription_1 = require("./Subscription");
const TcpConnection_1 = require("./TcpConnection");
const TcpPublisher_1 = require("./TcpPublisher");
const backoff_1 = require("./backoff");
const difference_1 = require("./difference");
const objectTests_1 = require("./objectTests");
const OK = 1;
class RosNode extends eventemitter3_1.EventEmitter {
constructor(options) {
super();
this.subscriptions = new Map();
this.publications = new Map();
this.parameters = new Map();
this._running = true;
this._connectionIdCounter = 0;
this._newConnectionId = () => {
return this._connectionIdCounter++;
};
this._getPublication = (topic) => {
return this.publications.get(topic);
};
this._handleParamUpdate = (key, value, callerId) => {
const prevValue = this.parameters.get(key);
this.parameters.set(key, value);
this.emit("paramUpdate", { key, value, prevValue, callerId });
};
this._handlePublisherUpdate = (topic, publishers, callerId) => {
const sub = this.subscriptions.get(topic);
if (sub == undefined) {
return;
}
const prevPublishers = Array.from(sub.publishers().values()).map((v) => v.publisherXmlRpcUrl());
const removed = (0, difference_1.difference)(prevPublishers, publishers);
const added = (0, difference_1.difference)(publishers, prevPublishers);
// Remove all publishers that have disappeared
for (const removePub of removed) {
for (const [connectionId, pub] of sub.publishers().entries()) {
if (pub.publisherXmlRpcUrl() === removePub) {
this._log?.info?.(`publisher ${removePub} for ${sub.name} went offline, disconnecting`);
sub.removePublisher(connectionId);
break;
}
}
}
// Add any new publishers that have appeared
for (const addPub of added) {
this._log?.info?.(`publisher ${addPub} for ${sub.name} came online, connecting`);
this._subscribeToPublisher(addPub, sub).catch((err) => {
// This should never be called, this._subscribeToPublisher() is not expected to throw
this._log?.warn?.(`subscribe to publisher unexpectedly failed: ${err}`);
});
}
this.emit("publisherUpdate", { topic, publishers, prevPublishers, callerId });
};
this._handleTcpClientConnection = (topic, connectionId, destinationCallerId, client) => {
const publication = this.publications.get(topic);
if (publication == undefined) {
this._log?.warn?.(`${client.toString()} connected to non-published topic ${topic}`);
this.emit("error", new Error(`${client.toString()} connected to non-published topic ${topic}`));
client.close();
return;
}
this._log?.info?.(`adding subscriber ${client.toString()} (${destinationCallerId}) to topic ${topic}, connectionId ${connectionId}`);
publication.addSubscriber(connectionId, destinationCallerId, client);
};
this._handleTcpPublisherError = (err) => {
this.emit("error", err);
};
this.name = options.name;
this.hostname = options.hostname;
this.pid = options.pid;
this.rosMasterClient = new RosMasterClient_1.RosMasterClient(options.rosMasterUri);
this.rosParamClient = new RosParamClient_1.RosParamClient(options.rosMasterUri);
this.rosFollower = new RosFollower_1.RosFollower(this, options.httpServer);
this._tcpSocketCreate = options.tcpSocketCreate;
if (options.tcpServer != undefined) {
this._tcpPublisher = new TcpPublisher_1.TcpPublisher({
server: options.tcpServer,
nodeName: this.name,
getConnectionId: this._newConnectionId,
getPublication: this._getPublication,
log: options.log,
});
this._tcpPublisher.on("connection", this._handleTcpClientConnection);
this._tcpPublisher.on("error", this._handleTcpPublisherError);
}
this._log = options.log;
this.rosFollower.on("paramUpdate", this._handleParamUpdate);
this.rosFollower.on("publisherUpdate", this._handlePublisherUpdate);
}
async start(port) {
return await this.rosFollower
.start(this.hostname, port)
// eslint-disable-next-line @typescript-eslint/restrict-template-expressions
.then(() => this._log?.debug?.(`rosfollower listening at ${this.rosFollower.url()}`));
}
shutdown(_msg) {
this._log?.debug?.("shutting down");
this._running = false;
this._tcpPublisher?.close();
this.rosFollower.close();
if (this.parameters.size > 0) {
this.unsubscribeAllParams().catch((unk) => {
const err = unk instanceof Error ? unk : new Error(unk);
this._log?.warn?.(err.message, "shutdown");
this.emit("error", err);
});
}
for (const subTopic of Array.from(this.subscriptions.keys())) {
this.unsubscribe(subTopic);
}
for (const pubTopic of Array.from(this.publications.keys())) {
this.unadvertise(pubTopic);
}
this.subscriptions.clear();
this.publications.clear();
this.parameters.clear();
}
subscribe(options) {
const { topic, dataType } = options;
const md5sum = options.md5sum ?? "*";
const tcpNoDelay = options.tcpNoDelay ?? false;
// Check if we are already subscribed
let subscription = this.subscriptions.get(topic);
if (subscription != undefined) {
this._log?.debug?.(`reusing existing subscribtion to ${topic} (${dataType})`);
return subscription;
}
subscription = new Subscription_1.Subscription({ name: topic, md5sum, dataType, tcpNoDelay });
this.subscriptions.set(topic, subscription);
this._log?.debug?.(`subscribing to ${topic} (${dataType})`);
// Asynchronously register this subscription with rosmaster and connect to
// each publisher
this._registerSubscriberAndConnect(subscription).catch((err) => {
// This should never be called, this._registerSubscriberAndConnect() is not expected to throw
this._log?.warn?.(`subscribe registration and connection unexpectedly failed: ${err}`, "subscribe");
});
return subscription;
}
async advertise(options) {
const { topic, dataType } = options;
const addr = await this.tcpServerAddress();
if (addr == undefined) {
throw new Error(`Cannot publish ${topic} without a listening tcpServer`);
}
// Check if we are already publishing
let publication = this.publications.get(topic);
if (publication != undefined) {
this._log?.debug?.(`reusing existing publication for ${topic} (${dataType})`);
return publication;
}
const messageDefinition = options.messageDefinition ?? (0, rosmsg_1.parse)(options.messageDefinitionText ?? "");
const canonicalMsgDefText = (0, rosmsg_1.stringify)(messageDefinition);
const messageDefinitionText = options.messageDefinitionText ?? canonicalMsgDefText;
const md5sum = options.md5sum ?? (0, rosmsg_1.md5)(messageDefinition);
const messageWriter = new rosmsg_serialization_1.MessageWriter(messageDefinition);
publication = new Publication_1.Publication(topic, md5sum, dataType, options.latching ?? false, messageDefinition, messageDefinitionText, messageWriter);
this.publications.set(topic, publication);
this._log?.debug?.(`publishing ${topic} (${dataType})`);
// Register with with rosmaster as a publisher for the requested topic. If
// this request fails, an exception is thrown
const subscribers = await this._registerPublisher(publication);
this._log?.info?.(`registered as a publisher for ${topic}, ${subscribers.length} current subscriber(s)`);
return publication;
}
async publish(topic, message) {
if (this._tcpPublisher == undefined) {
throw new Error(`Cannot publish without a tcpServer`);
}
const publication = this.publications.get(topic);
if (publication == undefined) {
throw new Error(`Cannot publish to unadvertised topic "${topic}"`);
}
await this._tcpPublisher.publish(publication, message);
}
isSubscribedTo(topic) {
return this._running && this.subscriptions.has(topic);
}
isAdvertising(topic) {
return this._running && this.publications.has(topic);
}
unsubscribe(topic) {
const subscription = this.subscriptions.get(topic);
if (subscription == null) {
return false;
}
this._unregisterSubscriber(topic).catch((err) => {
// This should never happen
this._log?.warn?.(`unregisterSubscriber failed for ${topic}: ${err}`);
});
subscription.close();
this.subscriptions.delete(topic);
return true;
}
unadvertise(topic) {
const publication = this.publications.get(topic);
if (publication == null) {
return false;
}
this._unregisterPublisher(topic).catch((err) => {
// This should never happen
this._log?.warn?.(`_unregisterPublisher failed for ${topic}: ${err}`);
});
publication.close();
this.publications.delete(topic);
return true;
}
async getParamNames() {
const [status, msg, names] = await this.rosParamClient.getParamNames(this.name);
if (status !== OK) {
throw new Error(`getParamNames returned failure (status=${status}): ${msg}`);
}
if (!Array.isArray(names)) {
throw new Error(`getParamNames returned unrecognized data (${msg})`);
}
return names;
}
async setParameter(key, value) {
const [status, msg] = await this.rosParamClient.setParam(this.name, key, value);
if (status !== OK) {
throw new Error(`setParam returned failure (status=${status}): ${msg}`);
}
// Also do a local update because ROS param server won't notify us if
// we initiated the parameter update.
this._handleParamUpdate(key, value, this.name);
}
async subscribeParam(key) {
const callerApi = this._callerApi();
const [status, msg, value] = await this.rosParamClient.subscribeParam(this.name, callerApi, key);
if (status !== OK) {
throw new Error(`subscribeParam returned failure (status=${status}): ${msg}`);
}
// rosparam server returns an empty object ({}) if the parameter has not been set yet
const adjustedValue = (0, objectTests_1.isEmptyPlainObject)(value) ? undefined : value;
this.parameters.set(key, adjustedValue);
this._log?.debug?.(
// workaround for https://github.com/typescript-eslint/typescript-eslint/issues/10632
`subscribed ${callerApi} to param "${key}" (${String(adjustedValue)})`);
return adjustedValue;
}
async unsubscribeParam(key) {
const callerApi = this._callerApi();
const [status, msg, value] = await this.rosParamClient.unsubscribeParam(this.name, callerApi, key);
if (status !== OK) {
throw new Error(`unsubscribeParam returned failure (status=${status}): ${msg}`);
}
this.parameters.delete(key);
const didUnsubscribe = value === 1;
this._log?.debug?.(
// eslint-disable-next-line @typescript-eslint/restrict-template-expressions
`unsubscribed ${callerApi} from param "${key}" (didUnsubscribe=${didUnsubscribe})`);
return didUnsubscribe;
}
async subscribeAllParams() {
let keys = await this.getParamNames();
const curKeys = Array.from(this.parameters.keys());
const callerApi = this._callerApi();
// Remove any local parameters the rosparam server didn't return
const removedKeys = (0, difference_1.difference)(curKeys, keys);
if (removedKeys.length > 0) {
this._log?.debug?.(`removing missing parameters ${JSON.stringify(removedKeys)}`);
for (const key of removedKeys) {
this.parameters.delete(key);
}
}
// Check if there are any parameters we don't already have locally
keys = (0, difference_1.difference)(keys, curKeys);
if (keys.length === 0) {
return this.parameters;
}
const res = await this.rosParamClient.subscribeParams(this.name, callerApi, keys);
if (res.length !== keys.length) {
throw new Error(`subscribeAllParams returned ${res.length} entries, expected ${keys.length}: ${JSON.stringify(res)}`);
}
// Update the local map of all subscribed parameters
for (let i = 0; i < keys.length; i++) {
const key = keys[i];
const entry = res[i];
if (entry instanceof xmlrpc_1.XmlRpcFault) {
// eslint-disable-next-line @typescript-eslint/no-base-to-string, @typescript-eslint/restrict-template-expressions
this._log?.warn?.(`subscribeAllParams faulted on "${key}" (${entry})`);
// eslint-disable-next-line @typescript-eslint/no-base-to-string, @typescript-eslint/restrict-template-expressions
this.emit("error", new Error(`subscribeAllParams faulted on "${key}" (${entry})`));
continue;
}
const [status, msg, value] = entry;
if (status !== OK) {
this._log?.warn?.(`subscribeAllParams not ok for "${key}" (status=${status}): ${msg}`);
this.emit("error", new Error(`subscribeAllParams not ok for "${key}" (status=${status}): ${msg}`));
continue;
}
// rosparam server returns an empty object ({}) if the parameter has not been set yet
const adjustedValue = (0, objectTests_1.isEmptyPlainObject)(value) ? undefined : value;
this.parameters.set(key, adjustedValue);
}
// eslint-disable-next-line @typescript-eslint/restrict-template-expressions
this._log?.debug?.(`subscribed ${callerApi} to parameters (${keys})`);
return this.parameters;
}
async unsubscribeAllParams() {
const keys = Array.from(this.parameters.keys());
const callerApi = this._callerApi();
const res = await this.rosParamClient.unsubscribeParams(this.name, callerApi, keys);
if (res.length !== keys.length) {
throw new Error(`unsubscribeAllParams returned unrecognized data: ${JSON.stringify(res)}`);
}
for (let i = 0; i < keys.length; i++) {
const key = keys[i];
const [status, msg] = res[i];
if (status !== OK) {
this._log?.warn?.(`unsubscribeAllParams failed for "${key}" (status=${status}): ${msg}`);
this.emit("error", new Error(`unsubscribeAllParams failed for "${key}" (status=${status}): ${msg}`));
continue;
}
}
this._log?.debug?.(`unsubscribed ${callerApi} from all parameters (${String(keys)})`);
this.parameters.clear();
}
async getPublishedTopics(subgraph) {
const [status, msg, topicsAndTypes] = await this.rosMasterClient.getPublishedTopics(this.name, subgraph);
if (status !== OK) {
throw new Error(`getPublishedTopics returned failure (status=${status}): ${msg}`);
}
return topicsAndTypes;
}
async getSystemState() {
const [status, msg, systemState] = await this.rosMasterClient.getSystemState(this.name);
if (status !== OK) {
throw new Error(`getPublishedTopics returned failure (status=${status}): ${msg}`);
}
if (!Array.isArray(systemState) || systemState.length !== 3) {
throw new Error(`getPublishedTopics returned unrecognized data (${msg})`);
}
const [pubs, subs, srvs] = systemState;
const createMap = (entries) => new Map(entries.map(([topic, nodes]) => [topic, new Set(nodes)]));
return {
publishers: createMap(pubs),
subscribers: createMap(subs),
services: createMap(srvs),
};
}
async tcpServerAddress() {
return await this._tcpPublisher?.address();
}
receivedBytes() {
let bytes = 0;
for (const sub of this.subscriptions.values()) {
bytes += sub.receivedBytes();
}
return bytes;
}
static async RequestTopic(name, topic, apiClient) {
let res;
try {
res = await apiClient.requestTopic(name, topic, [["TCPROS"]]);
}
catch (err) {
// eslint-disable-next-line @typescript-eslint/restrict-template-expressions
throw new Error(`requestTopic("${topic}") from ${apiClient.url()} failed. err=${err}`);
}
const [status, msg, protocol] = res;
if (status !== OK) {
throw new Error(`requestTopic("${topic}") from ${apiClient.url()} failed. status=${status}, msg=${msg}`);
}
if (!Array.isArray(protocol) || protocol.length < 3 || protocol[0] !== "TCPROS") {
throw new Error(`TCP not supported by ${apiClient.url()} for topic "${topic}"`);
}
return { port: protocol[2], address: protocol[1] };
}
_callerApi() {
if (this._localApiUrl != undefined) {
return this._localApiUrl;
}
this._localApiUrl = this.rosFollower.url();
if (this._localApiUrl == undefined) {
throw new Error("Local XMLRPC server was not started");
}
return this._localApiUrl;
}
async _registerSubscriber(subscription) {
if (!this._running) {
return [];
}
const callerApi = this._callerApi();
// Register with rosmaster as a subscriber to this topic
const [status, msg, publishers] = await this.rosMasterClient.registerSubscriber(this.name, subscription.name, subscription.dataType, callerApi);
if (status !== OK) {
throw new Error(`registerSubscriber() failed. status=${status}, msg="${msg}"`);
}
if (!Array.isArray(publishers)) {
throw new Error(
// eslint-disable-next-line @typescript-eslint/no-base-to-string, @typescript-eslint/restrict-template-expressions
`registerSubscriber() did not receive a list of publishers. value=${publishers}`);
}
this._log?.debug?.(`registered subscriber to ${subscription.name} (${subscription.dataType})`);
return publishers;
}
async _registerPublisher(publication) {
if (!this._running) {
return [];
}
const callerApi = this._callerApi();
const [status, msg, subscribers] = await this.rosMasterClient.registerPublisher(this.name, publication.name, publication.dataType, callerApi);
if (status !== OK) {
throw new Error(`registerPublisher() failed. status=${status}, msg="${msg}"`);
}
this._log?.debug?.(`registered publisher for ${publication.name} (${publication.dataType})`);
if (!Array.isArray(subscribers)) {
throw new Error(
// eslint-disable-next-line @typescript-eslint/no-base-to-string
`registerPublisher() did not receive a list of subscribers. value=${String(subscribers)}`);
}
return subscribers;
}
async _unregisterSubscriber(topic) {
try {
const callerApi = this._callerApi();
// Unregister with rosmaster as a subscriber to this topic
const [status, msg] = await this.rosMasterClient.unregisterSubscriber(this.name, topic, callerApi);
if (status !== OK) {
throw new Error(`unregisterSubscriber() failed. status=${status}, msg="${msg}"`);
}
this._log?.debug?.(`unregistered subscriber to ${topic}`);
}
catch (unk) {
// Warn and carry on, the rosmaster graph will be out of sync but there's
// not much we can do (it may already be offline)
const err = unk instanceof Error ? unk : new Error(unk);
this._log?.warn?.(err.message, "unregisterSubscriber");
this.emit("error", err);
}
}
async _unregisterPublisher(topic) {
try {
const callerApi = this._callerApi();
// Unregister with rosmaster as a publisher of this topic
const [status, msg] = await this.rosMasterClient.unregisterPublisher(this.name, topic, callerApi);
if (status !== OK) {
throw new Error(`unregisterPublisher() failed. status=${status}, msg="${msg}"`);
}
this._log?.debug?.(`unregistered publisher for ${topic}`);
}
catch (unk) {
// Warn and carry on, the rosmaster graph will be out of sync but there's
// not much we can do (it may already be offline)
const err = unk instanceof Error ? unk : new Error(unk);
this._log?.warn?.(err.message, "unregisterPublisher");
this.emit("error", err);
}
}
async _registerSubscriberAndConnect(subscription) {
// Register with rosmaster as a subscriber to the requested topic. Continue
// retrying until the XML-RPC call to roscore succeeds, or we are no longer
// subscribed
const publishers = await (0, backoff_1.retryForever)(async () => {
if (!this.isSubscribedTo(subscription.name)) {
return [];
}
return await this._registerSubscriber(subscription);
});
// Register with each publisher. Any failures communicating with individual node XML-RPC servers
// or TCP sockets will be caught and retried
await Promise.allSettled(publishers.map(async (pubUrl) => {
await this._subscribeToPublisher(pubUrl, subscription);
}));
}
async _subscribeToPublisher(pubUrl, subscription) {
const topic = subscription.name;
const dataType = subscription.dataType;
const md5sum = subscription.md5sum;
const tcpNoDelay = subscription.tcpNoDelay;
if (!this.isSubscribedTo(topic)) {
return;
}
let connection;
let address;
let port;
try {
// Create an XMLRPC client to talk to this publisher
const rosFollowerClient = new RosFollowerClient_1.RosFollowerClient(pubUrl);
// Call requestTopic on this publisher to register ourselves as a subscriber
const socketInfo = await RosNode.RequestTopic(this.name, topic, rosFollowerClient);
({ address, port } = socketInfo);
const uri = TcpConnection_1.TcpConnection.Uri(address, port);
this._log?.debug?.(`registered with ${pubUrl} as a subscriber to ${topic}, connecting to ${uri}`);
if (!this.isSubscribedTo(topic)) {
return;
}
// Create a TCP socket connecting to this publisher
const socket = await this._tcpSocketCreate({ host: address, port });
connection = new TcpConnection_1.TcpConnection(socket, address, port, new Map([
["topic", topic],
// eslint-disable-next-line @typescript-eslint/no-unnecessary-condition
["md5sum", md5sum ?? "*"],
["callerid", this.name],
["type", dataType],
["tcp_nodelay", tcpNoDelay ? "1" : "0"],
]), this._log);
if (!this.isSubscribedTo(topic)) {
socket.close().catch((err) => {
this._log?.warn?.(`closing connection to ${address}:${port} for topic "${topic}" failed: ${err}`);
});
return;
}
// Hold a reference to this publisher
const connectionId = this._newConnectionId();
subscription.addPublisher(connectionId, rosFollowerClient, connection);
}
catch (err) {
// Consider tracking failed RosFollower connections (node XML-RPC servers) and entering a
// retry loop
this._log?.warn?.(`subscribing to ${topic} at ${pubUrl} failed (${err}), this connection will be dropped`);
this.emit("error", new Error(`Subscribing to ${topic} at ${pubUrl} failed (${err}), this connection will be dropped`));
return;
}
// Asynchronously initiate the socket connection. This will enter a retry loop on failure
connection.connect().catch((err) => {
// This should never happen, connect() is assumed not to throw
this._log?.warn?.(`connecting to ${address}:${port} for topic "${topic}" unexpectedly failed: ${err}`);
});
}
static GetRosHostname(getEnvVar, getHostname, getNetworkInterfaces) {
// Prefer ROS_HOSTNAME, then ROS_IP env vars
let hostname = getEnvVar("ROS_HOSTNAME") ?? getEnvVar("ROS_IP");
if (hostname != undefined && hostname.length > 0) {
return hostname;
}
// Try to get the operating system hostname
hostname = getHostname();
if (hostname != undefined && hostname.length > 0) {
return hostname;
}
// Fall back to iterating network interfaces looking for an IP address
let bestAddr;
const ifaces = getNetworkInterfaces();
for (const iface of ifaces) {
if (
// eslint-disable-next-line @typescript-eslint/no-unnecessary-condition
(iface.family !== "IPv4" && iface.family !== "IPv6") ||
iface.internal ||
iface.address.length === 0) {
continue;
}
if (bestAddr == undefined) {
// Use the first non-internal interface we find
bestAddr = iface;
}
else if (RosNode.IsPrivateIP(bestAddr.address) && !RosNode.IsPrivateIP(iface.address)) {
// Prefer public IPs over private
bestAddr = iface;
}
else if (bestAddr.family !== "IPv6" && iface.family === "IPv6") {
// Prefer IPv6
bestAddr = iface;
}
}
if (bestAddr != undefined) {
return bestAddr.address;
}
// Last resort, return IPv4 loopback
return "127.0.0.1";
}
static IsPrivateIP(ip) {
// Logic based on isPrivateIP() in ros_comm network.cpp
return ip.startsWith("192.168") || ip.startsWith("10.") || ip.startsWith("169.254");
}
}
exports.RosNode = RosNode;
//# sourceMappingURL=RosNode.js.map