From 52cbf426a7c138e3262478a66ccfab03b24e1577 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Sun, 10 Mar 2024 14:05:42 -0600 Subject: [PATCH 1/4] add TF2Client Signed-off-by: Paul Gesel --- src/tf/TF2Client.js | 191 ++++++++++++++++++++++++++++++++++++++++++++ src/tf/index.js | 1 + 2 files changed, 192 insertions(+) create mode 100644 src/tf/TF2Client.js diff --git a/src/tf/TF2Client.js b/src/tf/TF2Client.js new file mode 100644 index 000000000..b8457c583 --- /dev/null +++ b/src/tf/TF2Client.js @@ -0,0 +1,191 @@ +/** + * @fileOverview + * @author David Gossow - dgossow@willowgarage.com + */ + +import Action from '../core/Action.js'; +// import Goal from '../actionlib/Goal.js'; +// import Topic from '../core/Topic.js'; + +import Transform from '../math/Transform.js'; + +import Ros from '../core/Ros.js'; +import {EventEmitter} from 'eventemitter3'; + +/** + * A TF Client that listens to TFs from tf2_web_republisher. + */ +export default class TF2Client extends EventEmitter { + /** + * @param {Object} options + * @param {Ros} options.ros - The ROSLIB.Ros connection handle. + * @param {string} [options.fixedFrame=base_link] - The fixed frame. + * @param {number} [options.angularThres=2.0] - The angular threshold for the TF republisher. + * @param {number} [options.transThres=0.01] - The translation threshold for the TF republisher. + * @param {number} [options.rate=10.0] - The rate for the TF republisher. + * @param {number} [options.updateDelay=50] - The time (in ms) to wait after a new subscription + * to update the TF republisher's list of TFs. + * @param {number} [options.topicTimeout=2.0] - The timeout parameter for the TF republisher. + * @param {string} [options.serverName="/tf2_web_republisher"] - The name of the tf2_web_republisher server. + * @param {string} [options.repubServiceName="/republish_tfs"] - The name of the republish_tfs service (non groovy compatibility mode only). + */ + constructor(options) { + super(); + this.ros = options.ros; + this.fixedFrame = options.fixedFrame || 'base_link'; + this.angularThres = options.angularThres || 2.0; + this.transThres = options.transThres || 0.01; + this.rate = options.rate || 10.0; + this.updateDelay = options.updateDelay || 50; + var seconds = options.topicTimeout || 2.0; + var secs = Math.floor(seconds); + var nsecs = Math.floor((seconds - secs) * 1000000000); + this.topicTimeout = { + secs: secs, + nsecs: nsecs + }; + this.serverName = options.serverName || '/tf2_web_republisher'; + // this.repubServiceName = options.repubServiceName || '/republish_tfs'; + + // this.currentGoal = {}; + this.goal_id = ''; + this.frameInfos = {}; + this.republisherUpdateRequested = false; + this._subscribeCB = undefined; + this._isDisposed = false; + + // Create an Action Client + this.actionClient = new Action({ + ros: options.ros, + name: this.serverName, + actionType: 'tf2_web_republisher_msgs/TFSubscription', + }); + + } + + /** + * Process the incoming TF message and send them out using the callback + * functions. + * + * @param {Object} tf - The TF message from the server. + */ + processTFArray(tf) { + var that = this; + tf.transforms.forEach(function (transform) { + var frameID = transform.child_frame_id; + if (frameID[0] === '/') { + frameID = frameID.substring(1); + } + var info = that.frameInfos[frameID]; + if (info) { + info.transform = new Transform({ + translation: transform.transform.translation, + rotation: transform.transform.rotation + }); + info.cbs.forEach(function (cb) { + cb(info.transform); + }); + } + }, this); + } + + /** + * Create and send a new goal (or service request) to the tf2_web_republisher + * based on the current list of TFs. + */ + updateGoal() { + var goalMessage = { + source_frames: Object.keys(this.frameInfos), + target_frame: this.fixedFrame, + angular_thres: this.angularThres, + trans_thres: this.transThres, + rate: this.rate + }; + + if (this.goal_id !== '') { + this.actionClient.cancelGoal(this.goal_id); + } + this.currentGoal = goalMessage; + + // this.currentGoal.on('feedback', this.processTFArray.bind(this)); + const id = this.actionClient.sendGoal(goalMessage, + (result) => { + console.log('Result for action goal on :'); + }, + (feedback) => { + this.processTFArray(feedback) + console.log('Feedback for action on : '); + }, + ); + if (typeof id === 'string') { + this.goal_id = id; + } + + this.republisherUpdateRequested = false; + } + + /** + * @callback subscribeCallback + * @param {Transform} callback.transform - The transform data. + */ + /** + * Subscribe to the given TF frame. + * + * @param {string} frameID - The TF frame to subscribe to. + * @param {subscribeCallback} callback - Function with the following params: + */ + subscribe(frameID, callback) { + // remove leading slash, if it's there + if (frameID[0] === '/') { + frameID = frameID.substring(1); + } + // if there is no callback registered for the given frame, create empty callback list + if (!this.frameInfos[frameID]) { + this.frameInfos[frameID] = { + cbs: [] + }; + if (!this.republisherUpdateRequested) { + setTimeout(this.updateGoal.bind(this), this.updateDelay); + this.republisherUpdateRequested = true; + } + } + + // if we already have a transform, callback immediately + else if (this.frameInfos[frameID].transform) { + callback(this.frameInfos[frameID].transform); + } + this.frameInfos[frameID].cbs.push(callback); + } + + /** + * Unsubscribe from the given TF frame. + * + * @param {string} frameID - The TF frame to unsubscribe from. + * @param {function} callback - The callback function to remove. + */ + unsubscribe(frameID, callback) { + // remove leading slash, if it's there + if (frameID[0] === '/') { + frameID = frameID.substring(1); + } + var info = this.frameInfos[frameID]; + for (var cbs = (info && info.cbs) || [], idx = cbs.length; idx--;) { + if (cbs[idx] === callback) { + cbs.splice(idx, 1); + } + } + if (!callback || cbs.length === 0) { + delete this.frameInfos[frameID]; + } + } + + /** + * Unsubscribe and unadvertise all topics associated with this TFClient. + */ + dispose() { + this._isDisposed = true; + // if (this.currentTopic) { + // this.currentTopic.unsubscribe(this._subscribeCB); + // } + } +} diff --git a/src/tf/index.js b/src/tf/index.js index f7d988004..305bb9950 100644 --- a/src/tf/index.js +++ b/src/tf/index.js @@ -1 +1,2 @@ export { default as TFClient } from './TFClient.js'; +export { default as TF2Client } from './TF2Client.js'; From 5ed01e8f1fdf546f1cb544c1071c66c94d1973b8 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 29 May 2024 15:26:07 -0600 Subject: [PATCH 2/4] delete commented code Signed-off-by: Paul Gesel --- src/tf/TF2Client.js | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/src/tf/TF2Client.js b/src/tf/TF2Client.js index b8457c583..9a0dded0b 100644 --- a/src/tf/TF2Client.js +++ b/src/tf/TF2Client.js @@ -4,9 +4,6 @@ */ import Action from '../core/Action.js'; -// import Goal from '../actionlib/Goal.js'; -// import Topic from '../core/Topic.js'; - import Transform from '../math/Transform.js'; import Ros from '../core/Ros.js'; @@ -39,15 +36,12 @@ export default class TF2Client extends EventEmitter { this.updateDelay = options.updateDelay || 50; var seconds = options.topicTimeout || 2.0; var secs = Math.floor(seconds); - var nsecs = Math.floor((seconds - secs) * 1000000000); + var nsecs = Math.floor((seconds - secs) * 1E9); this.topicTimeout = { secs: secs, nsecs: nsecs }; this.serverName = options.serverName || '/tf2_web_republisher'; - // this.repubServiceName = options.repubServiceName || '/republish_tfs'; - - // this.currentGoal = {}; this.goal_id = ''; this.frameInfos = {}; this.republisherUpdateRequested = false; @@ -184,8 +178,5 @@ export default class TF2Client extends EventEmitter { */ dispose() { this._isDisposed = true; - // if (this.currentTopic) { - // this.currentTopic.unsubscribe(this._subscribeCB); - // } } } From 110a32221abde1e86fc6da5553ae6b447763aab1 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Thu, 30 May 2024 09:38:47 -0600 Subject: [PATCH 3/4] Update src/tf/TF2Client.js Co-authored-by: Ezra Brooks --- src/tf/TF2Client.js | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/tf/TF2Client.js b/src/tf/TF2Client.js index 9a0dded0b..fbef9f16b 100644 --- a/src/tf/TF2Client.js +++ b/src/tf/TF2Client.js @@ -1,7 +1,3 @@ -/** - * @fileOverview - * @author David Gossow - dgossow@willowgarage.com - */ import Action from '../core/Action.js'; import Transform from '../math/Transform.js'; From aafbb9949d49ab9b3822416e8ced2850d2f7cfa7 Mon Sep 17 00:00:00 2001 From: parallels Date: Thu, 30 May 2024 12:45:55 -0600 Subject: [PATCH 4/4] fixed comments, updated code --- src/tf/{TF2Client.js => ROS2TFClient.js} | 21 +++++++++------------ src/tf/index.js | 2 +- 2 files changed, 10 insertions(+), 13 deletions(-) rename src/tf/{TF2Client.js => ROS2TFClient.js} (90%) diff --git a/src/tf/TF2Client.js b/src/tf/ROS2TFClient.js similarity index 90% rename from src/tf/TF2Client.js rename to src/tf/ROS2TFClient.js index fbef9f16b..451870e07 100644 --- a/src/tf/TF2Client.js +++ b/src/tf/ROS2TFClient.js @@ -8,7 +8,7 @@ import {EventEmitter} from 'eventemitter3'; /** * A TF Client that listens to TFs from tf2_web_republisher. */ -export default class TF2Client extends EventEmitter { +export default class ROS2TFClient extends EventEmitter { /** * @param {Object} options * @param {Ros} options.ros - The ROSLIB.Ros connection handle. @@ -30,9 +30,9 @@ export default class TF2Client extends EventEmitter { this.transThres = options.transThres || 0.01; this.rate = options.rate || 10.0; this.updateDelay = options.updateDelay || 50; - var seconds = options.topicTimeout || 2.0; - var secs = Math.floor(seconds); - var nsecs = Math.floor((seconds - secs) * 1E9); + const seconds = options.topicTimeout || 2.0; + const secs = Math.floor(seconds); + const nsecs = Math.floor((seconds - secs) * 1E9); this.topicTimeout = { secs: secs, nsecs: nsecs @@ -60,13 +60,13 @@ export default class TF2Client extends EventEmitter { * @param {Object} tf - The TF message from the server. */ processTFArray(tf) { - var that = this; + let that = this; tf.transforms.forEach(function (transform) { - var frameID = transform.child_frame_id; + let frameID = transform.child_frame_id; if (frameID[0] === '/') { frameID = frameID.substring(1); } - var info = that.frameInfos[frameID]; + const info = that.frameInfos[frameID]; if (info) { info.transform = new Transform({ translation: transform.transform.translation, @@ -84,7 +84,7 @@ export default class TF2Client extends EventEmitter { * based on the current list of TFs. */ updateGoal() { - var goalMessage = { + const goalMessage = { source_frames: Object.keys(this.frameInfos), target_frame: this.fixedFrame, angular_thres: this.angularThres, @@ -97,14 +97,11 @@ export default class TF2Client extends EventEmitter { } this.currentGoal = goalMessage; - // this.currentGoal.on('feedback', this.processTFArray.bind(this)); const id = this.actionClient.sendGoal(goalMessage, (result) => { - console.log('Result for action goal on :'); }, (feedback) => { this.processTFArray(feedback) - console.log('Feedback for action on : '); }, ); if (typeof id === 'string') { @@ -158,7 +155,7 @@ export default class TF2Client extends EventEmitter { if (frameID[0] === '/') { frameID = frameID.substring(1); } - var info = this.frameInfos[frameID]; + const info = this.frameInfos[frameID]; for (var cbs = (info && info.cbs) || [], idx = cbs.length; idx--;) { if (cbs[idx] === callback) { cbs.splice(idx, 1); diff --git a/src/tf/index.js b/src/tf/index.js index 305bb9950..5efc488c9 100644 --- a/src/tf/index.js +++ b/src/tf/index.js @@ -1,2 +1,2 @@ export { default as TFClient } from './TFClient.js'; -export { default as TF2Client } from './TF2Client.js'; +export { default as ROS2TFClient } from './ROS2TFClient.js';