bugfix to tfclient/improxy: correctly initialize tf transforms if alr…

…eady subscribed

improxy.js

@@ -97,6 +97,9 @@
       var intMarkerHandle = new ImProxy.IntMarkerHandle(imMsg, that.feedbackTopic, that.tf);
       that.intMarkerHandles[imMsg.name] = intMarkerHandle;
       that.emit('created_marker', intMarkerHandle);
+      // this might trigger a transform update event immediately,
+      // so we need to call it after emitting a created_marker event.
+      intMarkerHandle.subscribeTf(imMsg);
     });
   };
 
@@ -114,12 +117,6 @@
     this.tfTransform    = new TfClient.Transform();
     this.pose           = new TfClient.Pose();
     this.setPoseFromServer( imMsg.pose );
-
-
-    // subscribe to tf updates if frame-fixed
-    if ( imMsg.header.stamp.secs === 0.0 && imMsg.header.stamp.nsecs === 0.0 ) {
-      this.tf.subscribe( imMsg.header.frame_id, this.tfUpdate.bind(this) );
-    }
   };
 
   IntMarkerHandle.prototype.__proto__ = EventEmitter2.prototype;
@@ -131,6 +128,13 @@
   var MOUSE_DOWN = 4;
   var MOUSE_UP = 5;
 
+  IntMarkerHandle.prototype.subscribeTf = function(imMsg) {
+    // subscribe to tf updates if frame-fixed
+    if ( imMsg.header.stamp.secs === 0.0 && imMsg.header.stamp.nsecs === 0.0 ) {
+      this.tf.subscribe( imMsg.header.frame_id, this.tfUpdate.bind(this) );
+    }
+  }
+
   IntMarkerHandle.prototype.emitServerPoseUpdate = function() {
     var poseTransformed = new TfClient.Pose( this.pose.position, this.pose.orientation );
     this.tfTransform.apply( poseTransformed );

tfclient.js

@@ -23,7 +23,7 @@
 
     this.actionClient = new ActionClient( options );
     this.currentGoal = false;
-    this.frame_cbs = {};
+    this.frame_infos = {};
     this.goalUpdateRequested = false;
   };
 
@@ -33,11 +33,11 @@
     var that = this;
     tfMsg.transforms.forEach( function(transform) {
       var frameId = transform.child_frame_id;
-      var cbs = that.frame_cbs[frameId];
-      if ( cbs != undefined ) {
-        cbs.forEach(function(cb) {
-          var t = new Transform(transform.transform.translation,transform.transform.rotation);
-          cb(t);
+      var info = that.frame_infos[frameId];
+      if ( info != undefined ) {
+        info.transform = new Transform(transform.transform.translation,transform.transform.rotation);
+        info.cbs.forEach(function(cb) {
+          cb(info.transform);
         });
       }
     });
@@ -67,7 +67,7 @@
     };
 
     var source_frames = [];
-    for (frame in this.frame_cbs ) {
+    for (frame in this.frame_infos ) {
       goalMsg.source_frames.push(frame);
     };
 
@@ -84,21 +84,27 @@
     }
     // if there is no callback registered for the given frame,
     // create emtpy callback list
-    if ( this.frame_cbs[frameId] == undefined ) {
-      this.frame_cbs[frameId] = [];
+    if ( this.frame_infos[frameId] == undefined ) {
+      this.frame_infos[frameId] = {
+        cbs: [] };
       this.requestGoalUpdate();
+    } else {
+      // if we already have a transform, call back immediately
+      if ( this.frame_infos[frameId].transform != undefined ) {
+        callback( this.frame_infos[frameId].transform );
+      }
     }
-    this.frame_cbs[frameId].push( callback );
+    this.frame_infos[frameId].cbs.push( callback );
   };
 
   TfClient.prototype.unsubscribe = function(frameId,callback) {
-    var cbs = this.frame_cbs[frameId];
-    if ( cbs != undefined ) {
-      var cbIndex = cbs.indexOf( callback );
+    var info = this.frame_infos[frameId];
+    if ( info != undefined ) {
+      var cbIndex = info.cbs.indexOf( callback );
       if ( cbIndex >= 0 ) {
-        cbs.splice(cbIndex, 1);
-        if ( cbs.length == 0 ) {
-          delete this.frame_cbs[frameId];
+        info.cbs.splice(cbIndex, 1);
+        if ( info.cbs.length == 0 ) {
+          delete this.frame_infos[frameId];
         }
       this.needUpdate = true;
       }