diff --git a/src/tf/ROS2TFClient.js b/src/tf/ROS2TFClient.js new file mode 100644 index 000000000..451870e07 --- /dev/null +++ b/src/tf/ROS2TFClient.js @@ -0,0 +1,175 @@ + +import Action from '../core/Action.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 ROS2TFClient 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; + const seconds = options.topicTimeout || 2.0; + const secs = Math.floor(seconds); + const nsecs = Math.floor((seconds - secs) * 1E9); + this.topicTimeout = { + secs: secs, + nsecs: nsecs + }; + this.serverName = options.serverName || '/tf2_web_republisher'; + 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) { + let that = this; + tf.transforms.forEach(function (transform) { + let frameID = transform.child_frame_id; + if (frameID[0] === '/') { + frameID = frameID.substring(1); + } + const 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() { + const 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; + + const id = this.actionClient.sendGoal(goalMessage, + (result) => { + }, + (feedback) => { + this.processTFArray(feedback) + }, + ); + 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); + } + const 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; + } +} diff --git a/src/tf/index.js b/src/tf/index.js index f7d988004..5efc488c9 100644 --- a/src/tf/index.js +++ b/src/tf/index.js @@ -1 +1,2 @@ export { default as TFClient } from './TFClient.js'; +export { default as ROS2TFClient } from './ROS2TFClient.js';