Merge Pull Reqest #144 from T045T:tf2_republisher_service

Change TFClient to use the new version of tf2_web_republisher (see RobotWebTools/tf2_web_republisher#15 and RobotWebTools/tf2_web_republisher#16 )

build/roslib.js

@@ -636,13 +636,21 @@ var EventEmitter2 = require('./../util/shim/EventEmitter2.js').EventEmitter2;
  * @constructor
  * @param options - possible keys include:
  *   * url (optional) - the WebSocket URL for rosbridge (can be specified later with `connect`)
+ *   * groovyCompatibility - don't use interfaces that changed after the last groovy release or rosbridge_suite and related tools (defaults to true)
  */
 function Ros(options) {
   options = options || {};
   this.socket = null;
   this.idCounter = 0;
   this.isConnected = false;
 
+  if (typeof options.groovyCompatibility === 'undefined') {
+    this.groovyCompatibility = true;
+  }
+  else {
+    this.groovyCompatibility = options.groovyCompatibility;
+  }
+
   // Sets unlimited event listeners.
   this.setMaxListeners(0);
 
@@ -890,6 +898,7 @@ Ros.prototype.decodeTypeDefs = function(defs) {
 
 
 module.exports = Ros;
+
 },{"./../util/shim/EventEmitter2.js":32,"./../util/shim/WebSocket.js":33,"./Service":11,"./ServiceRequest":12,"./SocketAdapter.js":14,"object-assign":1}],11:[function(require,module,exports){
 /**
  * @author Brandon Alexander - baalexander@gmail.com
@@ -1493,6 +1502,10 @@ module.exports = Vector3;
 
 var ActionClient = require('../actionlib/ActionClient');
 var Goal = require('../actionlib/Goal');
+
+var Service = require('../core/Service.js');
+var ServiceRequest = require('../core/ServiceRequest.js');
+
 var Transform = require('../math/Transform');
 
 /**
@@ -1505,7 +1518,9 @@ var Transform = require('../math/Transform');
  *   * angularThres - the angular threshold for the TF republisher
  *   * transThres - the translation threshold for the TF republisher
  *   * rate - the rate for the TF republisher
- *   * goalUpdateDelay - the goal update delay for the TF republisher
+ *   * updateDelay - the time (in ms) to wait after a new subscription
+ *                   to update the TF republisher's list of TFs
+ *   * topicTimeout - the timeout parameter for the TF republisher
  */
 function TFClient(options) {
   options = options || {};
@@ -1514,18 +1529,31 @@ function TFClient(options) {
   this.angularThres = options.angularThres || 2.0;
   this.transThres = options.transThres || 0.01;
   this.rate = options.rate || 10.0;
-  this.goalUpdateDelay = options.goalUpdateDelay || 50;
+  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.currentGoal = false;
+  this.currentTopic = false;
   this.frameInfos = {};
-  this.goalUpdateRequested = false;
+  this.republisherUpdateRequested = false;
 
-  // Create an ActionClient
-  this.actionClient = new ActionClient({
-    ros : this.ros,
+  // Create an Action client
+  this.actionclient = this.ros.ActionClien({
     serverName : '/tf2_web_republisher',
     actionName : 'tf2_web_republisher/TFSubscriptionAction'
   });
+
+  // Create a Service client
+  this.serviceClient = this.ros.Service({
+    name: '/republish_tfs',
+    serviceType: 'tf2_web_republisher/RepublishTFs'
+  });
 }
 
 /**
@@ -1534,7 +1562,7 @@ function TFClient(options) {
  *
  * @param tf - the TF message from the server
  */
-TFClient.prototype.processFeedback = function(tf) {
+TFClient.prototype.processTFArray = function(tf) {
   var that = this;
   tf.transforms.forEach(function(transform) {
     var frameID = transform.child_frame_id;
@@ -1555,15 +1583,10 @@ TFClient.prototype.processFeedback = function(tf) {
 };
 
 /**
- * Create and send a new goal to the tf2_web_republisher based on the current
- * list of TFs.
+ * Create and send a new goal (or service request) to the tf2_web_republisher
+ * based on the current list of TFs.
  */
 TFClient.prototype.updateGoal = function() {
-  // Anytime the list of frames changes, we will need to send a new goal.
-  if (this.currentGoal) {
-    this.currentGoal.cancel();
-  }
-
   var goalMessage = {
     source_frames : [],
     target_frame : this.fixedFrame,
@@ -1573,16 +1596,54 @@ TFClient.prototype.updateGoal = function() {
   };
 
   for (var frame in this.frameInfos) {
-    goalMessage.source_frames.push(frame);
+    request.source_frames.push(frame);
+  }
+
+  // if we're running in groovy compatibility mode (the default)
+  // then use the action interface to tf2_web_republisher
+  if(this.ros.groovyCompatibility) {
+    if (this.currentGoal) {
+      this.currentGoal.cancel();
+    }
+    this.currentGoal = new Goal({
+      actionClient : this.actionClient,
+      goalMessage : goalMessage
+    });
+
+    this.currentGoal.on('feedback', this.processTFArray.bind(this));
+    this.currentGoal.send();
   }
+  else {
+    // otherwise, use the service interface
+    // The service interface has the same parameters as the action,
+    // plus the timeout
+    goalMessage.timeout = this.topicTimeout;
+    var request = new ServiceRequest(goalMessage);
+
+    this.serviceClient.callService(request, this.processResponse.bind(this));
+  }
+
+  this.republisherUpdateRequested = false;
+};
 
-  this.currentGoal = new Goal({
-    actionClient : this.actionClient,
-    goalMessage : goalMessage
+/**
+ * Process the service response and subscribe to the tf republisher
+ * topic
+ *
+ * @param response the service response containing the topic name
+ */
+TFClient.prototype.processResponse = function(response) {
+  // if we subscribed to a topic before, unsubscribe so
+  // the republisher stops publishing it
+  if (this.currentTopic) {
+    this.currentTopic.unsubscribe();
+  }
+
+  this.currentTopic = this.ros.Topic({
+    name: response.topic_name,
+    messageType: 'tf2_web_republisher/TFArray'
   });
-  this.currentGoal.on('feedback', this.processFeedback.bind(this));
-  this.currentGoal.send();
-  this.goalUpdateRequested = false;
+  this.currentTopic.subscribe(this.processTFArray.bind(this));
 };
 
 /**
@@ -1602,9 +1663,9 @@ TFClient.prototype.subscribe = function(frameID, callback) {
     this.frameInfos[frameID] = {
       cbs : []
     };
-    if (!this.goalUpdateRequested) {
-      setTimeout(this.updateGoal.bind(this), this.goalUpdateDelay);
-      this.goalUpdateRequested = true;
+    if (!this.republisherUpdateRequested) {
+      setTimeout(this.updateGoal.bind(this), this.updateDelay);
+      this.republisherUpdateRequested = true;
     }
   } else {
     // if we already have a transform, call back immediately
@@ -1640,7 +1701,8 @@ TFClient.prototype.unsubscribe = function(frameID, callback) {
 };
 
 module.exports = TFClient;
-},{"../actionlib/ActionClient":5,"../actionlib/Goal":6,"../math/Transform":18}],21:[function(require,module,exports){
+
+},{"../actionlib/ActionClient":5,"../actionlib/Goal":6,"../core/Service.js":11,"../core/ServiceRequest.js":12,"../math/Transform":18}],21:[function(require,module,exports){
 /**
  * @author Benjamin Pitzer - ben.pitzer@gmail.com
  * @author Russell Toris - rctoris@wpi.edu

examples/tf.html

@@ -8,7 +8,12 @@
 <script>
   // Connecting to ROS
   // -----------------
-  var ros = new ROSLIB.Ros();
+  var ros = new ROSLIB.Ros({
+    // set this to false to use the new service interface to
+    // tf2_web_republisher. true is the default and means roslibjs
+    // will use the action interface
+    groovyCompatibility : true
+  });
 
   // If there is an error on the backend, an 'error' emit will be emitted.
   ros.on('error', function(error) {

src/core/Ros.js

@@ -24,13 +24,21 @@ var EventEmitter2 = require('eventemitter2').EventEmitter2;
  * @constructor
  * @param options - possible keys include:
  *   * url (optional) - the WebSocket URL for rosbridge (can be specified later with `connect`)
+ *   * groovyCompatibility - don't use interfaces that changed after the last groovy release or rosbridge_suite and related tools (defaults to true)
  */
 function Ros(options) {
   options = options || {};
   this.socket = null;
   this.idCounter = 0;
   this.isConnected = false;
 
+  if (typeof options.groovyCompatibility === 'undefined') {
+    this.groovyCompatibility = true;
+  }
+  else {
+    this.groovyCompatibility = options.groovyCompatibility;
+  }
+
   // Sets unlimited event listeners.
   this.setMaxListeners(0);
 
@@ -277,4 +285,4 @@ Ros.prototype.decodeTypeDefs = function(defs) {
 };
 
 
-module.exports = Ros;
+module.exports = Ros;