diff --git a/package-lock.json b/package-lock.json index e1c2cbaac..7e107de87 100644 --- a/package-lock.json +++ b/package-lock.json @@ -18,6 +18,7 @@ "devDependencies": { "@testing-library/react": "^14.2.1", "@types/node": "^20.11.19", + "@types/ws": "^8.5.10", "eslint": "^8.56.0", "globals": "^14.0.0", "jsdoc": "^4.0.2", @@ -1286,6 +1287,15 @@ "integrity": "sha512-WZLiwShhwLRmeV6zH+GkbOFT6Z6VklCItrDioxUnv+u4Ll+8vKeFySoFyK/0ctcRpOmwAicELfmys1sDc/Rw+A==", "dev": true }, + "node_modules/@types/ws": { + "version": "8.5.10", + "resolved": "https://registry.npmjs.org/@types/ws/-/ws-8.5.10.tgz", + "integrity": "sha512-vmQSUcfalpIq0R9q7uTo2lXs6eGIpt9wtnLdMv9LVpIjCA/+ufZRozlVoVelIYixx1ugCBKDhn89vnsEGOCx9A==", + "dev": true, + "dependencies": { + "@types/node": "*" + } + }, "node_modules/@ungap/structured-clone": { "version": "1.2.0", "resolved": "https://registry.npmjs.org/@ungap/structured-clone/-/structured-clone-1.2.0.tgz", diff --git a/package.json b/package.json index 8fae2bd9f..a847844c0 100644 --- a/package.json +++ b/package.json @@ -19,6 +19,7 @@ "devDependencies": { "@testing-library/react": "^14.2.1", "@types/node": "^20.11.19", + "@types/ws": "^8.5.10", "eslint": "^8.56.0", "globals": "^14.0.0", "jsdoc": "^4.0.2", diff --git a/src/actionlib/ActionClient.js b/src/actionlib/ActionClient.js index 7baeb6090..154c4042c 100644 --- a/src/actionlib/ActionClient.js +++ b/src/actionlib/ActionClient.js @@ -19,6 +19,9 @@ import { EventEmitter } from 'eventemitter3'; * */ export default class ActionClient extends EventEmitter { + goals = {}; + /** flag to check if a status has been received */ + receivedStatus = false /** * @param {Object} options * @param {Ros} options.ros - The ROSLIB.Ros connection handle. @@ -31,7 +34,6 @@ export default class ActionClient extends EventEmitter { */ constructor(options) { super(); - var that = this; this.ros = options.ros; this.serverName = options.serverName; this.actionName = options.actionName; @@ -39,10 +41,6 @@ export default class ActionClient extends EventEmitter { this.omitFeedback = options.omitFeedback; this.omitStatus = options.omitStatus; this.omitResult = options.omitResult; - this.goals = {}; - - // flag to check if a status has been received - var receivedStatus = false; // create the topics associated with actionlib this.feedbackListener = new Topic({ @@ -81,10 +79,10 @@ export default class ActionClient extends EventEmitter { // subscribe to the status topic if (!this.omitStatus) { - this.statusListener.subscribe(function (statusMessage) { - receivedStatus = true; - statusMessage.status_list.forEach(function (status) { - var goal = that.goals[status.goal_id.id]; + this.statusListener.subscribe((statusMessage) => { + this.receivedStatus = true; + statusMessage.status_list.forEach((status) => { + var goal = this.goals[status.goal_id.id]; if (goal) { goal.emit('status', status); } @@ -94,8 +92,8 @@ export default class ActionClient extends EventEmitter { // subscribe the the feedback topic if (!this.omitFeedback) { - this.feedbackListener.subscribe(function (feedbackMessage) { - var goal = that.goals[feedbackMessage.status.goal_id.id]; + this.feedbackListener.subscribe((feedbackMessage) => { + var goal = this.goals[feedbackMessage.status.goal_id.id]; if (goal) { goal.emit('status', feedbackMessage.status); goal.emit('feedback', feedbackMessage.feedback); @@ -105,8 +103,8 @@ export default class ActionClient extends EventEmitter { // subscribe to the result topic if (!this.omitResult) { - this.resultListener.subscribe(function (resultMessage) { - var goal = that.goals[resultMessage.status.goal_id.id]; + this.resultListener.subscribe((resultMessage) => { + var goal = this.goals[resultMessage.status.goal_id.id]; if (goal) { goal.emit('status', resultMessage.status); @@ -117,9 +115,9 @@ export default class ActionClient extends EventEmitter { // If timeout specified, emit a 'timeout' event if the action server does not respond if (this.timeout) { - setTimeout(function () { - if (!receivedStatus) { - that.emit('timeout'); + setTimeout(() => { + if (!this.receivedStatus) { + this.emit('timeout'); } }, this.timeout); } diff --git a/src/actionlib/ActionListener.js b/src/actionlib/ActionListener.js index 71b8e928f..301148524 100644 --- a/src/actionlib/ActionListener.js +++ b/src/actionlib/ActionListener.js @@ -27,7 +27,6 @@ export default class ActionListener extends EventEmitter { */ constructor(options) { super(); - var that = this; this.ros = options.ros; this.serverName = options.serverName; this.actionName = options.actionName; @@ -57,25 +56,25 @@ export default class ActionListener extends EventEmitter { messageType: this.actionName + 'Result' }); - goalListener.subscribe(function (goalMessage) { - that.emit('goal', goalMessage); + goalListener.subscribe((goalMessage) => { + this.emit('goal', goalMessage); }); - statusListener.subscribe(function (statusMessage) { - statusMessage.status_list.forEach(function (status) { - that.emit('status', status); + statusListener.subscribe((statusMessage) => { + statusMessage.status_list.forEach((status) => { + this.emit('status', status); }); }); - feedbackListener.subscribe(function (feedbackMessage) { - that.emit('status', feedbackMessage.status); - that.emit('feedback', feedbackMessage.feedback); + feedbackListener.subscribe((feedbackMessage) => { + this.emit('status', feedbackMessage.status); + this.emit('feedback', feedbackMessage.feedback); }); // subscribe to the result topic - resultListener.subscribe(function (resultMessage) { - that.emit('status', resultMessage.status); - that.emit('result', resultMessage.result); + resultListener.subscribe((resultMessage) => { + this.emit('status', resultMessage.status); + this.emit('result', resultMessage.result); }); } } diff --git a/src/actionlib/Goal.js b/src/actionlib/Goal.js index b7f0100a4..999e2a5ef 100644 --- a/src/actionlib/Goal.js +++ b/src/actionlib/Goal.js @@ -14,6 +14,12 @@ import ActionClient from './ActionClient.js'; * * 'timeout' - If a timeout occurred while sending a goal. */ export default class Goal extends EventEmitter { + isFinished = false; + status = undefined; + result = undefined; + feedback = undefined; + // Create a random ID + goalID = 'goal_' + Math.random() + '_' + new Date().getTime(); /** * @param {Object} options * @param {ActionClient} options.actionClient - The ROSLIB.ActionClient to use with this goal. @@ -21,19 +27,8 @@ export default class Goal extends EventEmitter { */ constructor(options) { super(); - var that = this; this.actionClient = options.actionClient; - this.goalMessage = options.goalMessage; - this.isFinished = false; - this.status = undefined; - this.result = undefined; - this.feedback = undefined; - // Used to create random IDs - var date = new Date(); - - // Create a random ID - this.goalID = 'goal_' + Math.random() + '_' + date.getTime(); // Fill in the goal message this.goalMessage = { goal_id: { @@ -43,20 +38,20 @@ export default class Goal extends EventEmitter { }, id: this.goalID }, - goal: this.goalMessage + goal: options.goalMessage }; - this.on('status', function (status) { - that.status = status; + this.on('status', (status) => { + this.status = status; }); - this.on('result', function (result) { - that.isFinished = true; - that.result = result; + this.on('result', (result) => { + this.isFinished = true; + this.result = result; }); - this.on('feedback', function (feedback) { - that.feedback = feedback; + this.on('feedback', (feedback) => { + this.feedback = feedback; }); // Add the goal @@ -68,12 +63,11 @@ export default class Goal extends EventEmitter { * @param {number} [timeout] - A timeout length for the goal's result. */ send(timeout) { - var that = this; - that.actionClient.goalTopic.publish(that.goalMessage); + this.actionClient.goalTopic.publish(this.goalMessage); if (timeout) { - setTimeout(function () { - if (!that.isFinished) { - that.emit('timeout'); + setTimeout(() => { + if (!this.isFinished) { + this.emit('timeout'); } }, timeout); } diff --git a/src/actionlib/SimpleActionServer.js b/src/actionlib/SimpleActionServer.js index 3816cf0d6..0f6201c0e 100644 --- a/src/actionlib/SimpleActionServer.js +++ b/src/actionlib/SimpleActionServer.js @@ -15,6 +15,11 @@ import { EventEmitter } from 'eventemitter3'; * * 'cancel' - Action client has canceled the request. */ export default class SimpleActionServer extends EventEmitter { + // needed for handling preemption prompted by a new goal being received + /** @type {{goal_id: {id: any, stamp: any}, goal: any} | null} */ + currentGoal = null; // currently tracked goal + /** @type {{goal_id: {id: any, stamp: any}, goal: any} | null} */ + nextGoal = null; // the one this'll be preempting /** * @param {Object} options * @param {Ros} options.ros - The ROSLIB.Ros connection handle. @@ -23,7 +28,6 @@ export default class SimpleActionServer extends EventEmitter { */ constructor(options) { super(); - var that = this; this.ros = options.ros; this.serverName = options.serverName; this.actionName = options.actionName; @@ -73,19 +77,15 @@ export default class SimpleActionServer extends EventEmitter { status_list: [] }; - // needed for handling preemption prompted by a new goal being received - this.currentGoal = null; // currently tracked goal - this.nextGoal = null; // the one that'll be preempting - - goalListener.subscribe(function (goalMessage) { - if (that.currentGoal) { - that.nextGoal = goalMessage; + goalListener.subscribe((goalMessage) => { + if (this.currentGoal) { + this.nextGoal = goalMessage; // needs to happen AFTER rest is set up - that.emit('cancel'); + this.emit('cancel'); } else { - that.statusMessage.status_list = [{ goal_id: goalMessage.goal_id, status: 1 }]; - that.currentGoal = goalMessage; - that.emit('goal', goalMessage.goal); + this.statusMessage.status_list = [{ goal_id: goalMessage.goal_id, status: 1 }]; + this.currentGoal = goalMessage; + this.emit('goal', goalMessage.goal); } }); @@ -107,56 +107,56 @@ export default class SimpleActionServer extends EventEmitter { // not sure if the callbacks can ever wind up with a scenario // where we've been preempted by a next goal, it hasn't finished // processing, and then we get a cancel message - cancelListener.subscribe(function (cancelMessage) { + cancelListener.subscribe((cancelMessage) => { // cancel ALL goals if both empty if ( cancelMessage.stamp.secs === 0 && cancelMessage.stamp.secs === 0 && cancelMessage.id === '' ) { - that.nextGoal = null; - if (that.currentGoal) { - that.emit('cancel'); + this.nextGoal = null; + if (this.currentGoal) { + this.emit('cancel'); } } else { // treat id and stamp independently if ( - that.currentGoal && - cancelMessage.id === that.currentGoal.goal_id.id + this.currentGoal && + cancelMessage.id === this.currentGoal.goal_id.id ) { - that.emit('cancel'); + this.emit('cancel'); } else if ( - that.nextGoal && - cancelMessage.id === that.nextGoal.goal_id.id + this.nextGoal && + cancelMessage.id === this.nextGoal.goal_id.id ) { - that.nextGoal = null; + this.nextGoal = null; } if ( - that.nextGoal && - isEarlier(that.nextGoal.goal_id.stamp, cancelMessage.stamp) + this.nextGoal && + isEarlier(this.nextGoal.goal_id.stamp, cancelMessage.stamp) ) { - that.nextGoal = null; + this.nextGoal = null; } if ( - that.currentGoal && - isEarlier(that.currentGoal.goal_id.stamp, cancelMessage.stamp) + this.currentGoal && + isEarlier(this.currentGoal.goal_id.stamp, cancelMessage.stamp) ) { - that.emit('cancel'); + this.emit('cancel'); } } }); // publish status at pseudo-fixed rate; required for clients to know they've connected - var statusInterval = setInterval(function () { + setInterval(() => { var currentTime = new Date(); var secs = Math.floor(currentTime.getTime() / 1000); var nsecs = Math.round( 1000000000 * (currentTime.getTime() / 1000 - secs) ); - that.statusMessage.header.stamp.secs = secs; - that.statusMessage.header.stamp.nsecs = nsecs; - statusPublisher.publish(that.statusMessage); + this.statusMessage.header.stamp.secs = secs; + this.statusMessage.header.stamp.nsecs = nsecs; + statusPublisher.publish(this.statusMessage); }, 500); // publish every 500ms } /** @@ -165,19 +165,21 @@ export default class SimpleActionServer extends EventEmitter { * @param {Object} result - The result to return to the client. */ setSucceeded(result) { - var resultMessage = { - status: { goal_id: this.currentGoal.goal_id, status: 3 }, - result: result - }; - this.resultPublisher.publish(resultMessage); - - this.statusMessage.status_list = []; - if (this.nextGoal) { - this.currentGoal = this.nextGoal; - this.nextGoal = null; - this.emit('goal', this.currentGoal.goal); - } else { - this.currentGoal = null; + if (this.currentGoal !== null) { + var resultMessage = { + status: { goal_id: this.currentGoal.goal_id, status: 3 }, + result: result + }; + this.resultPublisher.publish(resultMessage); + + this.statusMessage.status_list = []; + if (this.nextGoal) { + this.currentGoal = this.nextGoal; + this.nextGoal = null; + this.emit('goal', this.currentGoal.goal); + } else { + this.currentGoal = null; + } } } /** @@ -186,19 +188,21 @@ export default class SimpleActionServer extends EventEmitter { * @param {Object} result - The result to return to the client. */ setAborted(result) { - var resultMessage = { - status: { goal_id: this.currentGoal.goal_id, status: 4 }, - result: result - }; - this.resultPublisher.publish(resultMessage); - - this.statusMessage.status_list = []; - if (this.nextGoal) { - this.currentGoal = this.nextGoal; - this.nextGoal = null; - this.emit('goal', this.currentGoal.goal); - } else { - this.currentGoal = null; + if (this.currentGoal !== null) { + var resultMessage = { + status: { goal_id: this.currentGoal.goal_id, status: 4 }, + result: result + }; + this.resultPublisher.publish(resultMessage); + + this.statusMessage.status_list = []; + if (this.nextGoal) { + this.currentGoal = this.nextGoal; + this.nextGoal = null; + this.emit('goal', this.currentGoal.goal); + } else { + this.currentGoal = null; + } } } /** @@ -207,28 +211,32 @@ export default class SimpleActionServer extends EventEmitter { * @param {Object} feedback - The feedback to send to the client. */ sendFeedback(feedback) { - var feedbackMessage = { - status: { goal_id: this.currentGoal.goal_id, status: 1 }, - feedback: feedback - }; - this.feedbackPublisher.publish(feedbackMessage); + if (this.currentGoal !== null) { + var feedbackMessage = { + status: { goal_id: this.currentGoal.goal_id, status: 1 }, + feedback: feedback + }; + this.feedbackPublisher.publish(feedbackMessage); + } } /** * Handle case where client requests preemption. */ setPreempted() { - this.statusMessage.status_list = []; - var resultMessage = { - status: { goal_id: this.currentGoal.goal_id, status: 2 } - }; - this.resultPublisher.publish(resultMessage); - - if (this.nextGoal) { - this.currentGoal = this.nextGoal; - this.nextGoal = null; - this.emit('goal', this.currentGoal.goal); - } else { - this.currentGoal = null; + if (this.currentGoal !== null) { + this.statusMessage.status_list = []; + var resultMessage = { + status: { goal_id: this.currentGoal.goal_id, status: 2 } + }; + this.resultPublisher.publish(resultMessage); + + if (this.nextGoal) { + this.currentGoal = this.nextGoal; + this.nextGoal = null; + this.emit('goal', this.currentGoal.goal); + } else { + this.currentGoal = null; + } } } } diff --git a/src/core/Action.js b/src/core/Action.js index 3c5d5ffb5..74bae80dc 100644 --- a/src/core/Action.js +++ b/src/core/Action.js @@ -11,6 +11,26 @@ import Ros from '../core/Ros.js'; * @template TGoal, TFeedback, TResult */ export default class Action extends EventEmitter { + isAdvertised = false; + /** + * @callback advertiseActionCallback + * @param {TGoal} goal - The action goal. + * @param {string} id - The ID of the action goal to execute. + */ + /** + * @private + * @type {advertiseActionCallback | null} + */ + _actionCallback = null; + /** + * @callback advertiseCancelCallback + * @param {string} id - The ID of the action goal to cancel. + */ + /** + * @private + * @type {advertiseCancelCallback | null} + */ + _cancelCallback = null; /** * @param {Object} options * @param {Ros} options.ros - The ROSLIB.Ros connection handle. @@ -22,10 +42,6 @@ export default class Action extends EventEmitter { this.ros = options.ros; this.name = options.name; this.actionType = options.actionType; - this.isAdvertised = false; - - this._actionCallback = null; - this._cancelCallback = null; } /** @@ -105,15 +121,6 @@ export default class Action extends EventEmitter { this.ros.callOnConnection(call); } - /** - * @callback advertiseActionCallback - * @param {TGoal} goal - The action goal. - * @param {string} id - The ID of the action goal to execute. - */ - /** - * @callback advertiseCancelCallback - * @param {string} id - The ID of the action goal to cancel. - */ /** * Advertise the action. This turns the Action object from a client * into a server. The callback will be called with every goal sent to this action. diff --git a/src/core/Ros.js b/src/core/Ros.js index 155d32add..e7fa5155a 100644 --- a/src/core/Ros.js +++ b/src/core/Ros.js @@ -12,7 +12,6 @@ import TFClient from '../tf/TFClient.js'; import ActionClient from '../actionlib/ActionClient.js'; import SimpleActionServer from '../actionlib/SimpleActionServer.js'; import { EventEmitter } from 'eventemitter3'; -import { WebSocket } from 'ws'; /** * Manages connection to the server and all interactions with ROS. @@ -25,6 +24,11 @@ import { WebSocket } from 'ws'; * * <serviceID> - A service response came from rosbridge with the given ID. */ export default class Ros extends EventEmitter { + /** @type {WebSocket | import("ws").WebSocket | null} */ + socket = null; + idCounter = 0; + isConnected = false; + groovyCompatibility = true; /** * @param {Object} [options] * @param {string} [options.url] - The WebSocket URL for rosbridge. Can be specified later with `connect`. @@ -35,21 +39,9 @@ export default class Ros extends EventEmitter { constructor(options) { super(); options = options || {}; - var that = this; - this.socket = null; - this.idCounter = 0; - this.isConnected = false; this.transportLibrary = options.transportLibrary || 'websocket'; this.transportOptions = options.transportOptions || {}; - this._sendFunc = function (msg) { - that.sendEncodedMessage(msg); - }; - - if (typeof options.groovyCompatibility === 'undefined') { - this.groovyCompatibility = true; - } else { - this.groovyCompatibility = options.groovyCompatibility; - } + this.groovyCompatibility = options.groovyCompatibility ?? true; // begin by checking if a URL was given if (options.url) { @@ -71,9 +63,18 @@ export default class Ros extends EventEmitter { } else if (this.transportLibrary === 'websocket') { if (!this.socket || this.socket.readyState === WebSocket.CLOSED) { // Detect if in browser vs in NodeJS - var sock = typeof window !== 'undefined' ? new window.WebSocket(url) : new WebSocket(url); - sock.binaryType = 'arraybuffer'; - this.socket = Object.assign(sock, socketAdapter(this)); + if (typeof window !== 'undefined') { + const sock = new WebSocket(url); + sock.binaryType = 'arraybuffer'; + this.socket = Object.assign(sock, socketAdapter(this)); + } else { + // if in Node.js, import ws to replace browser WebSocket API + import('ws').then((ws) => { + const sock = new ws.WebSocket(url); + sock.binaryType = 'arraybuffer' + this.socket = Object.assign(sock, socketAdapter(this)); + }) + } } } else { throw 'Unknown transportLibrary: ' + this.transportLibrary.toString(); @@ -121,10 +122,14 @@ export default class Ros extends EventEmitter { sendEncodedMessage(messageEncoded) { if (!this.isConnected) { this.once('connection', () => { - this.socket.send(messageEncoded); + if (this.socket !== null) { + this.socket.send(messageEncoded); + } }); } else { - this.socket.send(messageEncoded); + if (this.socket !== null) { + this.socket.send(messageEncoded); + } } } /** @@ -135,9 +140,9 @@ export default class Ros extends EventEmitter { */ callOnConnection(message) { if (this.transportOptions.encoder) { - this.transportOptions.encoder(message, this._sendFunc); + this.transportOptions.encoder(message, this.sendEncodedMessage); } else { - this._sendFunc(JSON.stringify(message)); + this.sendEncodedMessage(JSON.stringify(message)); } } /** @@ -705,9 +710,7 @@ export default class Ros extends EventEmitter { * @param {Object[]} defs - Array of type_def dictionary. */ decodeTypeDefs(defs) { - var that = this; - - var decodeTypeDefsRec = function (theType, hints) { + var decodeTypeDefsRec = (theType, hints) => { // calls itself recursively to resolve type definition using hints. var typeDefDict = {}; for (var i = 0; i < theType.fieldnames.length; i++) { @@ -738,7 +741,7 @@ export default class Ros extends EventEmitter { typeDefDict[fieldName] = [subResult]; } } else { - that.emit( + this.emit( 'error', 'Cannot find ' + fieldType + ' in decodeTypeDefs' ); diff --git a/src/core/Topic.js b/src/core/Topic.js index 990d7931b..6cbe8dc8b 100644 --- a/src/core/Topic.js +++ b/src/core/Topic.js @@ -15,6 +15,11 @@ import Ros from './Ros.js'; * @template T */ export default class Topic extends EventEmitter { + /** @type {boolean | undefined} */ + waitForReconnect = undefined; + /** @type {(() => void) | undefined} */ + reconnectFunc = undefined; + isAdvertised = false; /** * @param {Object} options * @param {Ros} options.ros - The ROSLIB.Ros connection handle. @@ -32,7 +37,6 @@ export default class Topic extends EventEmitter { this.ros = options.ros; this.name = options.name; this.messageType = options.messageType; - this.isAdvertised = false; this.compression = options.compression || 'none'; this.throttle_rate = options.throttle_rate || 0; this.latch = options.latch || false; @@ -42,8 +46,6 @@ export default class Topic extends EventEmitter { options.reconnect_on_close !== undefined ? options.reconnect_on_close : true; - this.waitForReconnect = undefined; - this.reconnectFunc = undefined; // Check for valid compression types if ( @@ -67,31 +69,30 @@ export default class Topic extends EventEmitter { this.throttle_rate = 0; } - var that = this; if (this.reconnect_on_close) { - this.callForSubscribeAndAdvertise = function (message) { - that.ros.callOnConnection(message); + this.callForSubscribeAndAdvertise = (message) => { + this.ros.callOnConnection(message); - that.waitForReconnect = false; - that.reconnectFunc = function () { - if (!that.waitForReconnect) { - that.waitForReconnect = true; - that.ros.callOnConnection(message); - that.ros.once('connection', function () { - that.waitForReconnect = false; + this.waitForReconnect = false; + this.reconnectFunc = () => { + if (!this.waitForReconnect) { + this.waitForReconnect = true; + this.ros.callOnConnection(message); + this.ros.once('connection', () => { + this.waitForReconnect = false; }); } }; - that.ros.on('close', that.reconnectFunc); + this.ros.on('close', this.reconnectFunc); }; } else { this.callForSubscribeAndAdvertise = this.ros.callOnConnection; } - - this._messageCallback = function (data) { - that.emit('message', data); - }; } + + _messageCallback = (data) => { + this.emit('message', data); + }; /** * @callback subscribeCallback * @param {T} message - The published message. @@ -177,9 +178,8 @@ export default class Topic extends EventEmitter { this.isAdvertised = true; if (!this.reconnect_on_close) { - var that = this; - this.ros.on('close', function () { - that.isAdvertised = false; + this.ros.on('close', () => { + this.isAdvertised = false; }); } } diff --git a/src/tf/TFClient.js b/src/tf/TFClient.js index b4d9e61e9..e2b691c56 100644 --- a/src/tf/TFClient.js +++ b/src/tf/TFClient.js @@ -18,6 +18,15 @@ import { EventEmitter } from 'eventemitter3'; * A TF Client that listens to TFs from tf2_web_republisher. */ export default class TFClient extends EventEmitter { + /** @type {Goal|false} */ + currentGoal = false; + /** @type {Topic|false} */ + currentTopic = false; + frameInfos = {}; + republisherUpdateRequested = false; + /** @type {((tf: any) => any) | undefined} */ + _subscribeCB = undefined; + _isDisposed = false; /** * @param {Object} options * @param {Ros} options.ros - The ROSLIB.Ros connection handle. @@ -49,15 +58,6 @@ export default class TFClient extends EventEmitter { this.serverName = options.serverName || '/tf2_web_republisher'; this.repubServiceName = options.repubServiceName || '/republish_tfs'; - /** @type {Goal|false} */ - this.currentGoal = false; - /** @type {Topic|false} */ - this.currentTopic = false; - this.frameInfos = {}; - this.republisherUpdateRequested = false; - this._subscribeCB = undefined; - this._isDisposed = false; - // Create an Action Client this.actionClient = new ActionClient({ ros: options.ros, @@ -81,19 +81,18 @@ export default class TFClient extends EventEmitter { * @param {Object} tf - The TF message from the server. */ processTFArray(tf) { - var that = this; - tf.transforms.forEach(function (transform) { + tf.transforms.forEach((transform) => { var frameID = transform.child_frame_id; if (frameID[0] === '/') { frameID = frameID.substring(1); } - var info = that.frameInfos[frameID]; + var info = this.frameInfos[frameID]; if (info) { info.transform = new Transform({ translation: transform.transform.translation, rotation: transform.transform.rotation }); - info.cbs.forEach(function (cb) { + info.cbs.forEach((cb) => { cb(info.transform); }); } diff --git a/src/urdf/UrdfBox.js b/src/urdf/UrdfBox.js index 671dc7d0c..437c45399 100644 --- a/src/urdf/UrdfBox.js +++ b/src/urdf/UrdfBox.js @@ -11,21 +11,25 @@ import * as UrdfTypes from './UrdfTypes.js'; * A Box element in a URDF. */ export default class UrdfBox { + /** @type {Vector3 | null} */ + dimension; /** * @param {Object} options * @param {Element} options.xml - The XML element to parse. */ constructor(options) { - this.dimension = null; this.type = UrdfTypes.URDF_BOX; // Parse the xml string - // @ts-expect-error -- possibly null - var xyz = options.xml.getAttribute('size').split(' '); - this.dimension = new Vector3({ - x: parseFloat(xyz[0]), - y: parseFloat(xyz[1]), - z: parseFloat(xyz[2]) - }); + var xyz = options.xml.getAttribute('size')?.split(' '); + if (xyz) { + this.dimension = new Vector3({ + x: parseFloat(xyz[0]), + y: parseFloat(xyz[1]), + z: parseFloat(xyz[2]) + }); + } else { + this.dimension = null; + } } } diff --git a/src/urdf/UrdfColor.js b/src/urdf/UrdfColor.js index fba17762f..be5ec2821 100644 --- a/src/urdf/UrdfColor.js +++ b/src/urdf/UrdfColor.js @@ -14,11 +14,12 @@ export default class UrdfColor { */ constructor(options) { // Parse the xml string - // @ts-expect-error -- possibly null - var rgba = options.xml.getAttribute('rgba').split(' '); - this.r = parseFloat(rgba[0]); - this.g = parseFloat(rgba[1]); - this.b = parseFloat(rgba[2]); - this.a = parseFloat(rgba[3]); + var rgba = options.xml.getAttribute('rgba')?.split(' '); + if (rgba) { + this.r = parseFloat(rgba[0]); + this.g = parseFloat(rgba[1]); + this.b = parseFloat(rgba[2]); + this.a = parseFloat(rgba[3]); + } } } diff --git a/src/urdf/UrdfMaterial.js b/src/urdf/UrdfMaterial.js index 6279e4487..c3fbd3142 100644 --- a/src/urdf/UrdfMaterial.js +++ b/src/urdf/UrdfMaterial.js @@ -10,13 +10,15 @@ import UrdfColor from './UrdfColor.js'; * A Material element in a URDF. */ export default class UrdfMaterial { + /** @type {string | null} */ + textureFilename = null; + /** @type {UrdfColor | null} */ + color = null; /** * @param {Object} options * @param {Element} options.xml - The XML element to parse. */ constructor(options) { - this.textureFilename = null; - this.color = null; this.name = options.xml.getAttribute('name'); diff --git a/src/urdf/UrdfMesh.js b/src/urdf/UrdfMesh.js index f0ee4ec3f..d4429ec66 100644 --- a/src/urdf/UrdfMesh.js +++ b/src/urdf/UrdfMesh.js @@ -11,13 +11,13 @@ import * as UrdfTypes from './UrdfTypes.js'; * A Mesh element in a URDF. */ export default class UrdfMesh { + /** @type {Vector3 | null} */ + scale = null; /** * @param {Object} options * @param {Element} options.xml - The XML element to parse. */ constructor(options) { - this.scale = null; - this.type = UrdfTypes.URDF_MESH; this.filename = options.xml.getAttribute('filename'); diff --git a/src/urdf/UrdfModel.js b/src/urdf/UrdfModel.js index 7c76db752..6a279e887 100644 --- a/src/urdf/UrdfModel.js +++ b/src/urdf/UrdfModel.js @@ -16,6 +16,9 @@ var XPATH_FIRST_ORDERED_NODE_TYPE = 9; * A URDF Model can be used to parse a given URDF into the appropriate elements. */ export default class UrdfModel { + materials = {}; + links = {}; + joints = {}; /** * @param {Object} options * @param {Element} [options.xml] - The XML element to parse. @@ -24,9 +27,6 @@ export default class UrdfModel { constructor(options) { var xmlDoc = options.xml; var string = options.string; - this.materials = {}; - this.links = {}; - this.joints = {}; // Check if we are using a string or an XML element if (string) { diff --git a/src/urdf/UrdfVisual.js b/src/urdf/UrdfVisual.js index 61a56607d..2832a72fb 100644 --- a/src/urdf/UrdfVisual.js +++ b/src/urdf/UrdfVisual.js @@ -18,16 +18,18 @@ import UrdfSphere from './UrdfSphere.js'; * A Visual element in a URDF. */ export default class UrdfVisual { + /** @type {Pose | null} */ + origin = null; + /** @type {UrdfMesh | UrdfSphere | UrdfBox | UrdfCylinder | null} */ + geometry = null; + /** @type {UrdfMaterial | null} */ + material = null; /** * @param {Object} options * @param {Element} options.xml - The XML element to parse. */ constructor(options) { var xml = options.xml; - this.origin = null; - this.geometry = null; - this.material = null; - this.name = options.xml.getAttribute('name'); // Origin