@foxglove/ros1/dist/RosMaster.js

Standalone TypeScript implementation of the ROS 1 (Robot Operating System) protocol with a pluggable transport layer

"use strict";
Object.defineProperty(exports, "__esModule", { value: true });
exports.RosMaster = void 0;
const xmlrpc_1 = require("@foxglove/xmlrpc");
const RosFollowerClient_1 = require("./RosFollowerClient");
const objectTests_1 = require("./objectTests");
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;
}
// A server implementing the <http://wiki.ros.org/ROS/Master_API> and
// <http://wiki.ros.org/ROS/Parameter%20Server%20API> APIs. This can be used as
// an alternative server implementation than roscore provided by the ros_comm
// library.
class RosMaster {
    constructor(httpServer, log) {
        this._nodes = new Map();
        this._services = new Map();
        this._topics = new Map();
        this._publications = new Map();
        this._subscriptions = new Map();
        this._parameters = new Map();
        this._paramSubscriptions = new Map();
        // <http://wiki.ros.org/ROS/Master_API> handlers
        this.registerService = async (_methodName, args) => {
            // [callerId, service, serviceApi, callerApi]
            const err = CheckArguments(args, ["string", "string", "string", "string"]);
            if (err != null) {
                throw err;
            }
            const [callerId, service, serviceApi, callerApi] = args;
            if (!this._services.has(service)) {
                this._services.set(service, new Map());
            }
            const serviceProviders = this._services.get(service);
            serviceProviders.set(callerId, serviceApi);
            this._nodes.set(callerId, callerApi);
            return [1, "", 0];
        };
        this.unregisterService = async (_methodName, args) => {
            // [callerId, service, serviceApi]
            const err = CheckArguments(args, ["string", "string", "string"]);
            if (err != null) {
                throw err;
            }
            const [callerId, service, _serviceApi] = args;
            const serviceProviders = this._services.get(service);
            if (serviceProviders == undefined) {
                return [1, "", 0];
            }
            const removed = serviceProviders.delete(callerId);
            if (serviceProviders.size === 0) {
                this._services.delete(service);
            }
            return [1, "", removed ? 1 : 0];
        };
        this.registerSubscriber = async (_methodName, args) => {
            // [callerId, topic, topicType, callerApi]
            const err = CheckArguments(args, ["string", "string", "string", "string"]);
            if (err != null) {
                throw err;
            }
            const [callerId, topic, topicType, callerApi] = args;
            const dataType = this._topics.get(topic);
            if (dataType != undefined && dataType !== topicType) {
                return [0, `topic_type "${topicType}" for topic "${topic}" does not match "${dataType}"`, []];
            }
            if (!this._subscriptions.has(topic)) {
                this._subscriptions.set(topic, new Set());
            }
            const subscribers = this._subscriptions.get(topic);
            subscribers.add(callerId);
            this._nodes.set(callerId, callerApi);
            const publishers = Array.from((this._publications.get(topic) ?? new Set()).values());
            const publisherApis = publishers.map((p) => this._nodes.get(p)).filter((a) => a != undefined);
            return [1, "", publisherApis];
        };
        this.unregisterSubscriber = async (_methodName, args) => {
            // [callerId, topic, callerApi]
            const err = CheckArguments(args, ["string", "string", "string"]);
            if (err != null) {
                throw err;
            }
            const [callerId, topic, _callerApi] = args;
            const subscribers = this._subscriptions.get(topic);
            if (subscribers == undefined) {
                return [1, "", 0];
            }
            const removed = subscribers.delete(callerId);
            if (subscribers.size === 0) {
                this._subscriptions.delete(topic);
            }
            return [1, "", removed ? 1 : 0];
        };
        this.registerPublisher = async (_methodName, args) => {
            // [callerId, topic, topicType, callerApi]
            const err = CheckArguments(args, ["string", "string", "string", "string"]);
            if (err != null) {
                throw err;
            }
            const [callerId, topic, topicType, callerApi] = args;
            const dataType = this._topics.get(topic);
            if (dataType != undefined && dataType !== topicType) {
                return [0, `topic_type "${topicType}" for topic "${topic}" does not match "${dataType}"`, []];
            }
            if (!this._publications.has(topic)) {
                this._publications.set(topic, new Set());
            }
            const publishers = this._publications.get(topic);
            publishers.add(callerId);
            this._topics.set(topic, topicType);
            this._nodes.set(callerId, callerApi);
            const subscribers = Array.from((this._subscriptions.get(topic) ?? new Set()).values());
            const subscriberApis = subscribers.map((s) => this._nodes.get(s)).filter((a) => a != undefined);
            // Inform all subscribers of the new publisher
            const publisherApis = Array.from(publishers.values())
                .sort()
                .map((p) => this._nodes.get(p))
                .filter((a) => a != undefined);
            for (const api of subscriberApis) {
                new RosFollowerClient_1.RosFollowerClient(api)
                    .publisherUpdate(callerId, topic, publisherApis)
                    .catch((apiErr) => this._log?.warn?.(`publisherUpdate call to ${api} failed: ${apiErr}`));
            }
            return [1, "", subscriberApis];
        };
        this.unregisterPublisher = async (_methodName, args) => {
            // [callerId, topic, callerApi]
            const err = CheckArguments(args, ["string", "string", "string"]);
            if (err != null) {
                throw err;
            }
            const [callerId, topic, _callerApi] = args;
            const publishers = this._publications.get(topic);
            if (publishers == undefined) {
                return [1, "", 0];
            }
            const removed = publishers.delete(callerId);
            if (publishers.size === 0) {
                this._publications.delete(topic);
            }
            return [1, "", removed ? 1 : 0];
        };
        this.lookupNode = async (_methodName, args) => {
            // [callerId, nodeName]
            const err = CheckArguments(args, ["string", "string"]);
            if (err != null) {
                throw err;
            }
            const [_callerId, nodeName] = args;
            const nodeApi = this._nodes.get(nodeName);
            if (nodeApi == undefined) {
                return [0, `node "${nodeName}" not found`, ""];
            }
            return [1, "", nodeApi];
        };
        this.getPublishedTopics = async (_methodName, args) => {
            // [callerId, subgraph]
            const err = CheckArguments(args, ["string", "string"]);
            if (err != null) {
                throw err;
            }
            // Subgraph filtering would need to be supported to become a fully compatible implementation
            const [_callerId, _subgraph] = args;
            const entries = [];
            for (const topic of this._publications.keys()) {
                const dataType = this._topics.get(topic);
                if (dataType != undefined) {
                    entries.push([topic, dataType]);
                }
            }
            return [1, "", entries];
        };
        this.getTopicTypes = async (_methodName, args) => {
            // [callerId]
            const err = CheckArguments(args, ["string"]);
            if (err != null) {
                throw err;
            }
            const entries = Array.from(this._topics.entries());
            return [1, "", entries];
        };
        this.getSystemState = async (_methodName, args) => {
            // [callerId]
            const err = CheckArguments(args, ["string"]);
            if (err != null) {
                throw err;
            }
            const publishers = Array.from(this._publications.entries()).map(([topic, nodeNames]) => [topic, Array.from(nodeNames.values()).sort()]);
            const subscribers = Array.from(this._subscriptions.entries()).map(([topic, nodeNames]) => [topic, Array.from(nodeNames.values()).sort()]);
            const services = Array.from(this._services.entries()).map(([service, nodeNamesToServiceApis]) => [
                service,
                Array.from(nodeNamesToServiceApis.keys()).sort(),
            ]);
            return [1, "", [publishers, subscribers, services]];
        };
        this.getUri = async (_methodName, args) => {
            // [callerId]
            const err = CheckArguments(args, ["string"]);
            if (err != null) {
                throw err;
            }
            const url = this._url;
            if (url == undefined) {
                return [0, "", "not running"];
            }
            return [1, "", url];
        };
        this.lookupService = async (_methodName, args) => {
            // [callerId, service]
            const err = CheckArguments(args, ["string", "string"]);
            if (err != null) {
                throw err;
            }
            const [_callerId, service] = args;
            const serviceProviders = this._services.get(service);
            if (serviceProviders == undefined || serviceProviders.size === 0) {
                return [0, `no providers for service "${service}"`, ""];
            }
            const serviceUrl = serviceProviders.values().next().value;
            return [1, "", serviceUrl];
        };
        // <http://wiki.ros.org/ROS/Parameter%20Server%20API> handlers
        this.deleteParam = async (_methodName, args) => {
            // [callerId, key]
            const err = CheckArguments(args, ["string", "string"]);
            if (err != null) {
                throw err;
            }
            const [_callerId, key] = args;
            this._parameters.delete(key);
            return [1, "", 0];
        };
        this.setParam = async (_methodName, args) => {
            // [callerId, key, value]
            const err = CheckArguments(args, ["string", "string", "*"]);
            if (err != null) {
                throw err;
            }
            const [callerId, key, value] = args;
            const allKeyValues = (0, objectTests_1.isPlainObject)(value)
                ? objectToKeyValues(key, value)
                : [[key, value]];
            for (const [curKey, curValue] of allKeyValues) {
                this._parameters.set(curKey, curValue);
                // Notify any parameter subscribers about this new value
                const subscribers = this._paramSubscriptions.get(curKey);
                if (subscribers != undefined) {
                    for (const api of subscribers.values()) {
                        new RosFollowerClient_1.RosFollowerClient(api)
                            .paramUpdate(callerId, curKey, curValue)
                            .catch((apiErr) => this._log?.warn?.(`paramUpdate call to ${api} failed: ${apiErr}`));
                    }
                }
            }
            return [1, "", 0];
        };
        this.getParam = async (_methodName, args) => {
            // [callerId, key]
            const err = CheckArguments(args, ["string", "string"]);
            if (err != null) {
                throw err;
            }
            // This endpoint needs to support namespace retrieval to fully match the rosparam server
            // behavior
            const [_callerId, key] = args;
            const value = this._parameters.get(key);
            const status = value != undefined ? 1 : 0;
            return [status, "", value ?? {}];
        };
        this.searchParam = async (_methodName, args) => {
            // [callerId, key]
            const err = CheckArguments(args, ["string", "string"]);
            if (err != null) {
                throw err;
            }
            // This endpoint would have to take into account the callerId namespace, partial matching, and
            // returning undefined keys to fully match the rosparam server behavior
            const [_callerId, key] = args;
            const value = this._parameters.get(key);
            const status = value != undefined ? 1 : 0;
            return [status, "", value ?? {}];
        };
        this.subscribeParam = async (_methodName, args) => {
            // [callerId, callerApi, key]
            const err = CheckArguments(args, ["string", "string", "string"]);
            if (err != null) {
                throw err;
            }
            const [callerId, callerApi, key] = args;
            if (!this._paramSubscriptions.has(key)) {
                this._paramSubscriptions.set(key, new Map());
            }
            const subscriptions = this._paramSubscriptions.get(key);
            subscriptions.set(callerId, callerApi);
            const value = this._parameters.get(key) ?? {};
            return [1, "", value];
        };
        this.unsubscribeParam = async (_methodName, args) => {
            // [callerId, callerApi, key]
            const err = CheckArguments(args, ["string", "string", "string"]);
            if (err != null) {
                throw err;
            }
            const [callerId, _callerApi, key] = args;
            const subscriptions = this._paramSubscriptions.get(key);
            if (subscriptions == undefined) {
                return [1, "", 0];
            }
            const removed = subscriptions.delete(callerId);
            return [1, "", removed ? 1 : 0];
        };
        this.hasParam = async (_methodName, args) => {
            // [callerId, key]
            const err = CheckArguments(args, ["string", "string"]);
            if (err != null) {
                throw err;
            }
            const [_callerId, key] = args;
            return [1, "", this._parameters.has(key)];
        };
        this.getParamNames = async (_methodName, args) => {
            // [callerId]
            const err = CheckArguments(args, ["string"]);
            if (err != null) {
                throw err;
            }
            const keys = Array.from(this._parameters.keys()).sort();
            return [1, "", keys];
        };
        this._server = new xmlrpc_1.XmlRpcServer(httpServer);
        this._log = log;
    }
    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("registerService", this.registerService);
        this._server.setHandler("unregisterService", this.unregisterService);
        this._server.setHandler("registerSubscriber", this.registerSubscriber);
        this._server.setHandler("unregisterSubscriber", this.unregisterSubscriber);
        this._server.setHandler("registerPublisher", this.registerPublisher);
        this._server.setHandler("unregisterPublisher", this.unregisterPublisher);
        this._server.setHandler("lookupNode", this.lookupNode);
        this._server.setHandler("getPublishedTopics", this.getPublishedTopics);
        this._server.setHandler("getTopicTypes", this.getTopicTypes);
        this._server.setHandler("getSystemState", this.getSystemState);
        this._server.setHandler("getUri", this.getUri);
        this._server.setHandler("lookupService", this.lookupService);
        this._server.setHandler("deleteParam", this.deleteParam);
        this._server.setHandler("setParam", this.setParam);
        this._server.setHandler("getParam", this.getParam);
        this._server.setHandler("searchParam", this.searchParam);
        this._server.setHandler("subscribeParam", this.subscribeParam);
        this._server.setHandler("unsubscribeParam", this.unsubscribeParam);
        this._server.setHandler("hasParam", this.hasParam);
        this._server.setHandler("getParamNames", this.getParamNames);
    }
    close() {
        this._server.close();
    }
    url() {
        return this._url;
    }
}
exports.RosMaster = RosMaster;
function objectToKeyValues(prefix, object) {
    let entries = [];
    for (const curKey in object) {
        const key = `${prefix}/${curKey}`;
        const value = object[curKey];
        if ((0, objectTests_1.isPlainObject)(value)) {
            entries = entries.concat(objectToKeyValues(key, value));
        }
        else {
            entries.push([key, value]);
        }
    }
    return entries;
}
//# sourceMappingURL=RosMaster.js.map