@foxglove/ros1
Version:
Standalone TypeScript implementation of the ROS 1 (Robot Operating System) protocol with a pluggable transport layer
170 lines • 7.58 kB
JavaScript
"use strict";
var __importDefault = (this && this.__importDefault) || function (mod) {
return (mod && mod.__esModule) ? mod : { "default": mod };
};
Object.defineProperty(exports, "__esModule", { value: true });
exports.RosFollower = void 0;
const xmlrpc_1 = require("@foxglove/xmlrpc");
const eventemitter3_1 = require("eventemitter3");
const ipaddr_js_1 = __importDefault(require("ipaddr.js"));
function CheckArguments(args, expected) {
if (args.length !== expected.length) {
return new Error(`Expected ${expected.length} arguments, got ${args.length}`);
}
for (let i = 0; i < args.length; i++) {
if (expected[i] !== "*" && typeof args[i] !== expected[i]) {
return new Error(`Expected "${expected[i]}" for arg ${i}, got "${typeof args[i]}"`);
}
}
return undefined;
}
function TcpRequested(protocols) {
for (const proto of protocols) {
if (Array.isArray(proto) && proto.length > 0) {
if (proto[0] === "TCPROS") {
return true;
}
}
}
return false;
}
class RosFollower extends eventemitter3_1.EventEmitter {
constructor(rosNode, httpServer) {
super();
this.getBusStats = async (_, args) => {
const err = CheckArguments(args, ["string"]);
if (err != null) {
throw err;
}
const publications = this._rosNode.publications.values();
const subscriptions = this._rosNode.subscriptions.values();
const publishStats = Array.from(publications, (pub, __) => pub.getStats());
const subscribeStats = Array.from(subscriptions, (sub, __) => sub.getStats());
const serviceStats = [];
return [1, "", [publishStats, subscribeStats, serviceStats]];
};
this.getBusInfo = async (_, args) => {
const err = CheckArguments(args, ["string"]);
if (err != null) {
throw err;
}
return [1, "", ""];
};
this.shutdown = async (_, args) => {
if (args.length !== 1 && args.length !== 2) {
throw new Error(`Expected 1-2 arguments, got ${args.length}`);
}
for (let i = 0; i < args.length; i++) {
if (typeof args[i] !== "string") {
throw new Error(`Expected "string" for arg ${i}, got "${typeof args[i]}"`);
}
}
const msg = args[1];
this._rosNode.shutdown(msg);
return [1, "", 0];
};
this.getPid = async (_, args) => {
const err = CheckArguments(args, ["string"]);
if (err != null) {
throw err;
}
return [1, "", this._rosNode.pid];
};
this.getSubscriptions = async (_, args) => {
const err = CheckArguments(args, ["string"]);
if (err != null) {
throw err;
}
const subs = [];
this._rosNode.subscriptions.forEach((sub) => subs.push([sub.name, sub.dataType]));
return [1, "subscriptions", subs];
};
this.getPublications = async (_, args) => {
const err = CheckArguments(args, ["string"]);
if (err != null) {
throw err;
}
const pubs = [];
this._rosNode.publications.forEach((pub) => pubs.push([pub.name, pub.dataType]));
return [1, "publications", pubs];
};
this.paramUpdate = async (_, args) => {
const err = CheckArguments(args, ["string", "string", "*"]);
if (err != null) {
throw err;
}
const [callerId, paramKey, paramValue] = args;
// Normalize the parameter key since rosparam server may append "/"
const normalizedKey = paramKey.endsWith("/") ? paramKey.slice(0, -1) : paramKey;
this.emit("paramUpdate", normalizedKey, paramValue, callerId);
return [1, "", 0];
};
this.publisherUpdate = async (_, args) => {
const err = CheckArguments(args, ["string", "string", "*"]);
if (err != null) {
throw err;
}
const [callerId, topic, publishers] = args;
if (!Array.isArray(publishers)) {
throw new Error(`invalid publishers list`);
}
this.emit("publisherUpdate", topic, publishers, callerId);
return [1, "", 0];
};
this.requestTopic = async (_, args, req) => {
const err = CheckArguments(args, ["string", "string", "*"]);
if (err != null) {
throw err;
}
const topic = args[1];
if (!this._rosNode.publications.has(topic)) {
return [0, `topic "${topic} is not advertised by node ${this._rosNode.name}"`, []];
}
const protocols = args[2];
if (!Array.isArray(protocols) || !TcpRequested(protocols)) {
return [0, "unsupported protocol", []];
}
const addr = await this._rosNode.tcpServerAddress();
if (addr == undefined) {
return [0, "cannot receive incoming connections", []];
}
// ROS subscriber clients use the address and port response arguments to determine where to
// establish a connection to receive messages. ROS clients may be on the local machine or on
// remote hosts. To better support subscribers on local or remote hosts, we attempt to use the
// socket localAddress of the xmlrpc request if it present. Since the xmlrpc server and tcp
// socket publishers are launched within the same node, echoing the localAddress back as the
// tpcros destination address increases the chance the client will be able to establish a
// connection to the publishing node since it has already made a successful xmlrpc request to
// the same address.
//
// Note: We still use `addr.port` because we need the _port_ of the TCP server not the HTTP
// xmlrpc server.
const socketLocalAddress = ipaddr_js_1.default.process(req?.socket?.localAddress ?? addr.address);
return [1, "", ["TCPROS", socketLocalAddress.toString(), addr.port]];
};
this._rosNode = rosNode;
this._server = new xmlrpc_1.XmlRpcServer(httpServer);
}
async start(hostname, port) {
await this._server.listen(port, undefined, 10);
// eslint-disable-next-line @typescript-eslint/restrict-template-expressions
this._url = `http://${hostname}:${this._server.port()}/`;
this._server.setHandler("getBusStats", this.getBusStats);
this._server.setHandler("getBusInfo", this.getBusInfo);
this._server.setHandler("shutdown", this.shutdown);
this._server.setHandler("getPid", this.getPid);
this._server.setHandler("getSubscriptions", this.getSubscriptions);
this._server.setHandler("getPublications", this.getPublications);
this._server.setHandler("paramUpdate", this.paramUpdate);
this._server.setHandler("publisherUpdate", this.publisherUpdate);
this._server.setHandler("requestTopic", this.requestTopic);
}
close() {
this._server.close();
}
url() {
return this._url;
}
}
exports.RosFollower = RosFollower;
//# sourceMappingURL=RosFollower.js.map