Source: tf/TFClient.js

  1. /**
  2.  * @fileOverview
  3.  * @author David Gossow - dgossow@willowgarage.com
  4.  */
  6. var ActionClient = require('../actionlib/ActionClient');
  7. var Goal = require('../actionlib/Goal');
  9. var Service = require('../core/Service.js');
  10. var ServiceRequest = require('../core/ServiceRequest.js');
  11. var Topic = require('../core/Topic.js');
  13. var Transform = require('../math/Transform');
  15. /**
  16.  * A TF Client that listens to TFs from tf2_web_republisher.
  17.  *
  18.  * @constructor
  19.  * @param {Object} options
  20.  * @param {Ros} options.ros - The ROSLIB.Ros connection handle.
  21.  * @param {string} [options.fixedFrame=base_link] - The fixed frame.
  22.  * @param {number} [options.angularThres=2.0] - The angular threshold for the TF republisher.
  23.  * @param {number} [options.transThres=0.01] - The translation threshold for the TF republisher.
  24.  * @param {number} [options.rate=10.0] - The rate for the TF republisher.
  25.  * @param {number} [options.updateDelay=50] - The time (in ms) to wait after a new subscription
  26.  *     to update the TF republisher's list of TFs.
  27.  * @param {number} [options.topicTimeout=2.0] - The timeout parameter for the TF republisher.
  28.  * @param {string} [options.serverName=/tf2_web_republisher] - The name of the tf2_web_republisher server.
  29.  * @param {string} [options.repubServiceName=/republish_tfs] - The name of the republish_tfs service (non groovy compatibility mode only).
  30.  */
  31. function TFClient(options) {
  32.   options = options || {};
  33.   this.ros = options.ros;
  34.   this.fixedFrame = options.fixedFrame || 'base_link';
  35.   this.angularThres = options.angularThres || 2.0;
  36.   this.transThres = options.transThres || 0.01;
  37.   this.rate = options.rate || 10.0;
  38.   this.updateDelay = options.updateDelay || 50;
  39.   var seconds = options.topicTimeout || 2.0;
  40.   var secs = Math.floor(seconds);
  41.   var nsecs = Math.floor((seconds - secs) * 1000000000);
  42.   this.topicTimeout = {
  43.     secs: secs,
  44.     nsecs: nsecs
  45.   };
  46.   this.serverName = options.serverName || '/tf2_web_republisher';
  47.   this.repubServiceName = options.repubServiceName || '/republish_tfs';
  49.   this.currentGoal = false;
  50.   this.currentTopic = false;
  51.   this.frameInfos = {};
  52.   this.republisherUpdateRequested = false;
  53.   this._subscribeCB = null;
  54.   this._isDisposed = false;
  56.   // Create an Action Client
  57.   this.actionClient = new ActionClient({
  58.     ros : options.ros,
  59.     serverName : this.serverName,
  60.     actionName : 'tf2_web_republisher/TFSubscriptionAction',
  61.     omitStatus : true,
  62.     omitResult : true
  63.   });
  65.   // Create a Service Client
  66.   this.serviceClient = new Service({
  67.     ros: options.ros,
  68.     name: this.repubServiceName,
  69.     serviceType: 'tf2_web_republisher/RepublishTFs'
  70.   });
  71. }
  73. /**
  74.  * Process the incoming TF message and send them out using the callback
  75.  * functions.
  76.  *
  77.  * @param {Object} tf - The TF message from the server.
  78.  */
  79. TFClient.prototype.processTFArray = function(tf) {
  80.   var that = this;
  81.   tf.transforms.forEach(function(transform) {
  82.     var frameID = transform.child_frame_id;
  83.     if (frameID[0] === '/')
  84.     {
  85.       frameID = frameID.substring(1);
  86.     }
  87.     var info = this.frameInfos[frameID];
  88.     if (info) {
  89.       info.transform = new Transform({
  90.         translation : transform.transform.translation,
  91.         rotation : transform.transform.rotation
  92.       });
  93.       info.cbs.forEach(function(cb) {
  94.         cb(info.transform);
  95.       });
  96.     }
  97.   }, this);
  98. };
  100. /**
  101.  * Create and send a new goal (or service request) to the tf2_web_republisher
  102.  * based on the current list of TFs.
  103.  */
  104. TFClient.prototype.updateGoal = function() {
  105.   var goalMessage = {
  106.     source_frames : Object.keys(this.frameInfos),
  107.     target_frame : this.fixedFrame,
  108.     angular_thres : this.angularThres,
  109.     trans_thres : this.transThres,
  110.     rate : this.rate
  111.   };
  113.   // if we're running in groovy compatibility mode (the default)
  114.   // then use the action interface to tf2_web_republisher
  115.   if(this.ros.groovyCompatibility) {
  116.     if (this.currentGoal) {
  117.       this.currentGoal.cancel();
  118.     }
  119.     this.currentGoal = new Goal({
  120.       actionClient : this.actionClient,
  121.       goalMessage : goalMessage
  122.     });
  124.     this.currentGoal.on('feedback', this.processTFArray.bind(this));
  125.     this.currentGoal.send();
  126.   }
  127.   else {
  128.     // otherwise, use the service interface
  129.     // The service interface has the same parameters as the action,
  130.     // plus the timeout
  131.     goalMessage.timeout = this.topicTimeout;
  132.     var request = new ServiceRequest(goalMessage);
  134.     this.serviceClient.callService(request, this.processResponse.bind(this));
  135.   }
  137.   this.republisherUpdateRequested = false;
  138. };
  140. /**
  141.  * Process the service response and subscribe to the tf republisher
  142.  * topic.
  143.  *
  144.  * @param {Object} response - The service response containing the topic name.
  145.  */
  146. TFClient.prototype.processResponse = function(response) {
  147.   // Do not setup a topic subscription if already disposed. Prevents a race condition where
  148.   // The dispose() function is called before the service call receives a response.
  149.   if (this._isDisposed) {
  150.     return;
  151.   }
  153.   // if we subscribed to a topic before, unsubscribe so
  154.   // the republisher stops publishing it
  155.   if (this.currentTopic) {
  156.     this.currentTopic.unsubscribe(this._subscribeCB);
  157.   }
  159.   this.currentTopic = new Topic({
  160.     ros: this.ros,
  161.     name: response.topic_name,
  162.     messageType: 'tf2_web_republisher/TFArray'
  163.   });
  164.   this._subscribeCB = this.processTFArray.bind(this);
  165.   this.currentTopic.subscribe(this._subscribeCB);
  166. };
  168. /**
  169.  * Subscribe to the given TF frame.
  170.  *
  171.  * @param {string} frameID - The TF frame to subscribe to.
  172.  * @param {function} callback - Function with the following params:
  173.  * @param {Transform} callback.transform - The transform data.
  174.  */
  175. TFClient.prototype.subscribe = function(frameID, callback) {
  176.   // remove leading slash, if it's there
  177.   if (frameID[0] === '/')
  178.   {
  179.     frameID = frameID.substring(1);
  180.   }
  181.   // if there is no callback registered for the given frame, create empty callback list
  182.   if (!this.frameInfos[frameID]) {
  183.     this.frameInfos[frameID] = {
  184.       cbs: []
  185.     };
  186.     if (!this.republisherUpdateRequested) {
  187.       setTimeout(this.updateGoal.bind(this), this.updateDelay);
  188.       this.republisherUpdateRequested = true;
  189.     }
  190.   }
  191.   // if we already have a transform, callback immediately
  192.   else if (this.frameInfos[frameID].transform) {
  193.     callback(this.frameInfos[frameID].transform);
  194.   }
  195.   this.frameInfos[frameID].cbs.push(callback);
  196. };
  198. /**
  199.  * Unsubscribe from the given TF frame.
  200.  *
  201.  * @param {string} frameID - The TF frame to unsubscribe from.
  202.  * @param {function} callback - The callback function to remove.
  203.  */
  204. TFClient.prototype.unsubscribe = function(frameID, callback) {
  205.   // remove leading slash, if it's there
  206.   if (frameID[0] === '/')
  207.   {
  208.     frameID = frameID.substring(1);
  209.   }
  210.   var info = this.frameInfos[frameID];
  211.   for (var cbs = info && info.cbs || [], idx = cbs.length; idx--;) {
  212.     if (cbs[idx] === callback) {
  213.       cbs.splice(idx, 1);
  214.     }
  215.   }
  216.   if (!callback || cbs.length === 0) {
  217.     delete this.frameInfos[frameID];
  218.   }
  219. };
  221. /**
  222.  * Unsubscribe and unadvertise all topics associated with this TFClient.
  223.  */
  224. TFClient.prototype.dispose = function() {
  225.   this._isDisposed = true;
  226.   this.actionClient.dispose();
  227.   if (this.currentTopic) {
  228.     this.currentTopic.unsubscribe(this._subscribeCB);
  229.   }
  230. };
  232. module.exports = TFClient;