TFListener.java
Go to the documentation of this file.
00001 /* 
00002  * Copyright (c) 2011, Sjoerd van den Dries
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Technische Universiteit Eindhoven nor the
00014  *       names of its contributors may be used to endorse or promote products
00015  *       derived from this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 
00031 package tfjava;
00032 
00033 import ros.*;
00034 import ros.communication.*;
00035 
00036 import ros.pkg.tf.msg.tfMessage;
00037 import ros.pkg.geometry_msgs.msg.TransformStamped;
00038 
00039 import javax.vecmath.Quat4d;
00040 import javax.vecmath.Vector3d;
00041 import javax.vecmath.Point3d;
00042 import javax.vecmath.Matrix4d;
00043 
00044 import java.util.HashMap;
00045 import java.util.PriorityQueue;
00046 import java.util.LinkedList;
00047 
00068 public class TFListener {
00069     
00071     public static final long MAX_STORAGE_TIME = (new Duration(10, 0)).totalNsecs(); 
00072     
00074     protected static Ros ros;
00076     protected static NodeHandle rosNode;
00078     protected static TFListener instance;    
00080     protected HashMap<String, Frame> frames;
00082     protected String tfPrefix = "";
00083     
00087     public synchronized static TFListener getInstance() {
00088         if (instance == null) {
00089             instance = new TFListener();
00090         }
00091         return instance;
00092     }
00093     
00094     /* **********************************************************************
00095      * *                           INITIALIZATION                           *
00096      * ********************************************************************** */     
00097     
00102     protected TFListener() {
00103                 initRos("tfjava_listener");
00104                 
00105                 if (initListener()) {
00106                     spinInSeperateThread();
00107                 }
00108         }       
00109         
00113         protected void initRos(String node_name) {
00114                 ros = Ros.getInstance();
00115 
00116                 if(!ros.isInitialized()) {
00117                         ros.init(node_name);
00118                 }
00119                 rosNode = ros.createNodeHandle();
00120         }
00121         
00125         protected boolean initListener() {
00126         frames = new HashMap<String, Frame>();    
00127             
00128             try {           
00129             TFCallback callback = new TFCallback();         
00130             Subscriber<tfMessage> sub = rosNode.subscribe("/tf", new tfMessage(), callback, 10);  // 10: magic numer
00131             
00132         } catch (Exception e) {
00133             ros.logError("Main: subscribe to /tf failed");
00134             return false;
00135         }           
00136         return true;
00137         }
00138         
00142     protected class TFCallback implements Subscriber.Callback<tfMessage> {
00143         public void call(tfMessage tfm) {
00144             if (tfm != null) {
00145                 for(TransformStamped tf : tfm.transforms) {                 
00146                     setTransform(tf);               
00147                 }
00148             }
00149         }
00150     }           
00151         
00156     protected void spinInSeperateThread() {
00157             Thread t = new Thread() {
00158                 public void run() {
00159                     rosNode.spin();
00160                 }
00161             };      
00162             t.start();
00163         }       
00164         
00165     /* **********************************************************************
00166      * *                            TF LISTENER                             *
00167      * ********************************************************************** */        
00168         
00172     protected boolean setTransform(TransformStamped transform) {
00173             // resolve the frame ID's
00174         String childFrameID = assertResolved(tfPrefix, transform.child_frame_id);
00175             String frameID = assertResolved(tfPrefix, transform.header.frame_id);
00176  
00177             boolean errorExists = false;
00178             if (childFrameID == frameID) {
00179                 ros.logError("TF_SELF_TRANSFORM: Ignoring transform with frame_id and child_frame_id  \"" + childFrameID + "\" because they are the same");
00180                 errorExists = true;
00181             }
00182 
00183             if (childFrameID == "/") { //empty frame id will be mapped to "/"
00184                 ros.logError("TF_NO_CHILD_FRAME_ID: Ignoring transform because child_frame_id not set ");
00185                 errorExists = true;
00186             }
00187 
00188             if (frameID == "/") { //empty parent id will be mapped to "/"
00189                 ros.logError("TF_NO_FRAME_ID: Ignoring transform with child_frame_id \"" + childFrameID + "\" because frame_id not set");
00190                 errorExists = true;
00191             }
00192 
00193             if (errorExists) return false;          
00194             
00195             // lookup or insert child frame
00196             Frame frame = lookupOrInsertFrame(childFrameID);
00197             
00198             // convert tf message to JTransform datastructure
00199             transform.child_frame_id = childFrameID;
00200             transform.header.frame_id = frameID; 
00201             TransformStorage tf = transformStampedMsgToTF(transform);
00202             
00203             // try to insert tf in corresponding time cache. If result is FALSE, the tf contains old data.
00204             if (!frame.insertData(tf)) {
00205                 ros.logWarn("TF_OLD_DATA ignoring data from the past for frame \"" + childFrameID + "\" at time " + ((double)tf.getTimeStamp() / 1E9));
00206             return false;
00207             }
00208 
00209         return true;
00210         }
00211         
00216         protected Frame lookupOrInsertFrame(String frameID) {
00217         Frame frame = frames.get(frameID);
00218         if (frame == null) {
00219             frame = new Frame(frameID, MAX_STORAGE_TIME);
00220             frames.put(frameID, frame);
00221         }           
00222         return frame;
00223         }
00224     
00225     /* **********************************************************************
00226      * *                         TRANSFORM METHODS                          *
00227      * ********************************************************************** */ 
00228         
00232         public void transformPoint(String targetFrameID, Stamped<Point3d> stampedIn, Stamped<Point3d> stampedOut) {
00233             StampedTransform transform = lookupTransform(targetFrameID, stampedIn.frameID, stampedIn.timeStamp);
00234             transform.transformPoint(stampedIn.getData(), stampedOut.getData());
00235             stampedOut.frameID = targetFrameID;
00236             stampedOut.timeStamp = stampedIn.timeStamp;
00237         }
00238         
00243         public void transformPoint(String targetFrameID, Time targetTime, Stamped<Point3d> stampedIn,
00244                                                     String fixedFrameID, Stamped<Point3d> stampedOut) {
00245             StampedTransform transform = lookupTransform(targetFrameID, targetTime, stampedIn.frameID, stampedIn.timeStamp, fixedFrameID);
00246         transform.transformPoint(stampedIn.getData(), stampedOut.getData()); 
00247         stampedOut.frameID = targetFrameID;
00248         stampedOut.timeStamp = stampedIn.timeStamp;
00249         }
00253         public void transformPose(String targetFrameID, Stamped<Matrix4d> stampedIn, Stamped<Matrix4d> stampedOut) {
00254             StampedTransform transform = lookupTransform(targetFrameID, stampedIn.frameID, stampedIn.timeStamp);
00255             transform.transformPose(stampedIn.getData(), stampedOut.getData());     
00256         stampedOut.frameID = targetFrameID;
00257         stampedOut.timeStamp = stampedIn.timeStamp;
00258         }
00259          
00264         public void transformPose(String targetFrameID, Time targetTime, Stamped<Matrix4d> stampedIn,
00265                                                     String fixedFrameID, Stamped<Matrix4d> stampedOut) {
00266             StampedTransform transform = lookupTransform(targetFrameID, targetTime, stampedIn.frameID, stampedIn.timeStamp, fixedFrameID);
00267             transform.transformPose(stampedIn.getData(), stampedOut.getData());
00268         stampedOut.frameID = targetFrameID;
00269         stampedOut.timeStamp = stampedIn.timeStamp;
00270     }
00271         
00272     /* **********************************************************************
00273      * *                          LOOKUP METHODS                            *
00274      * ********************************************************************** */    
00275 
00280         public StampedTransform lookupTransform(String targetFrameID, String sourceFrameID, Time time) {
00281         // resolve the source and target IDs
00282             String resolvedTargetID = assertResolved(tfPrefix, targetFrameID);
00283         String resolvedSourceID = assertResolved(tfPrefix, sourceFrameID);
00284         
00285         // if source and target are the same, return the identity transform
00286         if (resolvedSourceID == resolvedTargetID) {
00287             StampedTransform out = StampedTransform.getIdentity();
00288             out.timeStamp = time;
00289             out.frameID = resolvedSourceID;
00290             out.childFrameID = resolvedTargetID;            
00291             return out;
00292         }
00293 
00294         // lookup source and target frame
00295         Frame sourceFrame = frames.get(resolvedSourceID);
00296         Frame targetFrame = frames.get(resolvedTargetID);
00297         
00298         if (sourceFrame == null) {
00299             ros.logError("Cannot transform: source frame \"" + resolvedSourceID + "\" does not exist.");
00300             return null;
00301         }    
00302             
00303         if (targetFrame == null) {
00304             ros.logError("Cannot transform: target frame \"" + resolvedTargetID + "\" does not exist.");
00305             return null;
00306         } 
00307             
00308         // list that will contain transformations from source frame to some frame F        
00309             LinkedList<TransformStorage> inverseTransforms = new LinkedList<TransformStorage>();
00310             // list that will contain transformations from frame F to target frame
00311             LinkedList<TransformStorage> forwardTransforms = new LinkedList<TransformStorage>();
00312             
00313             // fill the lists using lookupLists. If it returns FALSE, no transformation could be found.
00314             if (!lookupLists(targetFrame, sourceFrame, time.totalNsecs(), inverseTransforms, forwardTransforms)) {
00315                 // TODO give warning
00316                 ros.logError("Cannot transform: source + \"" + resolvedSourceID + "\" and target \""
00317                                 + resolvedTargetID + "\" are not connected.");
00318                 return null;
00319             }        
00320             
00321             // create an identity transform with the correct time stamp
00322             StampedTransform out = StampedTransform.getIdentity();          
00323             out.timeStamp = time;
00324             
00325         // multiply all transforms from source frame to frame F TODO: right?
00326         for(TransformStorage t : inverseTransforms) {           
00327             out.mul(StorageToStampedTransform(t));
00328         }
00329             
00330             // multiply all transforms from frame F to target frame TODO: right?
00331             for(TransformStorage t : forwardTransforms) {               
00332                 out.mul(StorageToStampedTransform(t).invert(), out);
00333         }           
00334             
00335             // return transform
00336             return out;
00337         }
00338         
00343         public StampedTransform lookupTransform(String targetID, Time targetTime, String sourceID, Time sourceTime, String fixedID) {       
00344             // lookup transform from source to fixed frame, at sourceTime
00345             StampedTransform t1 = lookupTransform(fixedID, sourceID, sourceTime);
00346             // lookup transform from fixed frame to target frame, at targetTime
00347             StampedTransform t2 = lookupTransform(targetID, fixedID, targetTime);
00348             
00349             // if either of the two transformations did not succeed, return null
00350             if (t1 == null || t2 == null) return null;      
00351             
00352             // multiply transformation t2 with t1, and return
00353             t2.mul(t1);
00354             return t2;
00355         }
00356         
00370     protected boolean lookupLists(Frame targetFrame, Frame sourceFrame, long time,
00371             LinkedList<TransformStorage> inverseTransforms, LinkedList<TransformStorage> forwardTransforms) {
00372         
00373         // wrap the source and target frames in search nodes
00374         SearchNode<Frame> sourceNode = new SearchNode<Frame>(sourceFrame);
00375         SearchNode<Frame> targetNode = new SearchNode<Frame>(targetFrame);
00376         
00377         // set beginning of forward path (from source)
00378         sourceNode.backwardStep = sourceNode;
00379         // set beginning of backward path (form target)
00380         targetNode.forwardStep = targetNode;        
00381         
00382         // create a hash map that map frames to search nodes. This is necessary to keep track of
00383         // which frames have already been visited (and from which direction).
00384         HashMap<Frame, SearchNode<Frame>> frameToNode = new HashMap<Frame, SearchNode<Frame>>();
00385         
00386         // add source and target search nodes to the map
00387         frameToNode.put(sourceFrame, sourceNode);
00388         frameToNode.put(targetFrame, targetNode);
00389         
00390         // create a priority queue, which will hold the search nodes ordered by cost (descending)
00391         PriorityQueue<SearchNode<Frame>> Q = new PriorityQueue<SearchNode<Frame>>();
00392         
00393         // at the source and target search nodes to the queue
00394         Q.add(sourceNode); 
00395         Q.add(targetNode);
00396         
00397         // perform the search
00398         while(!Q.isEmpty()) {
00399             // poll most potential search node from queue
00400             SearchNode<Frame> frameNode = Q.poll();
00401             Frame frame = frameNode.content;
00402             
00403             // if the node is both visited from the source and from the target node, a path has been found
00404             if (frameNode.backwardStep != null && frameNode.forwardStep != null) {
00405                 // found the best path from source to target through FRAME.
00406                 
00407                 // create inverse list (from source to FRAME)
00408                 SearchNode<Frame> node = frameNode;                
00409                 while(node.content != sourceNode.content) {                    
00410                     inverseTransforms.addLast(node.backwardStep.content.getData(time, node.content));
00411                     node = node.backwardStep;
00412                 }
00413                 
00414                 // create forward list (from FRAME to target)
00415                 node = frameNode;
00416                 while(node.content != targetNode.content) {
00417                     forwardTransforms.addLast(node.forwardStep.content.getData(time, node.content));
00418                     node = node.forwardStep;
00419                 }
00420                 return true;
00421             }
00422             
00423             // expand search node
00424             for(Frame parentFrame : frame.getParentFrames()) {                
00425                 SearchNode<Frame> parentFrameNode = frameToNode.get(parentFrame);
00426                                 
00427                 boolean addToQueue = false;
00428                 if (parentFrameNode == null) {
00429                     // node was not yet visited
00430                     parentFrameNode = new SearchNode<Frame>(parentFrame);                    
00431                     frameToNode.put(parentFrame, parentFrameNode);
00432                     addToQueue = true;
00433                 } else {
00434                     // node is already visited
00435                     if ((parentFrameNode.backwardStep == null && frameNode.forwardStep == null)
00436                             || (parentFrameNode.forwardStep == null && frameNode.backwardStep == null)) {
00437                         // node was visited, but from other direction.
00438                         // create new search node that represents this frame, visited from both sides
00439                         // this allows the other search node of this frame to still be expanded first                        
00440                         parentFrameNode = new SearchNode<Frame>(parentFrameNode);
00441                         addToQueue = true;                        
00442                     }
00443                 }                
00444                                 
00445                 // add search node belonging to parent frame to the queue
00446                 if (addToQueue) {
00447                     // determine cost (based on max absolute difference in time stamp) 
00448                     TimeCache cache = frame.getTimeCache(parentFrame);
00449                     parentFrameNode.cost = Math.max((double)cache.timeToNearestTransform(time),
00450                                                 Math.max(parentFrameNode.cost, frameNode.cost));
00451                     // if visiting forward (from source), set backward step to remember path 
00452                     if (frameNode.backwardStep != null) parentFrameNode.backwardStep = frameNode;
00453                     // if visiting backward (from target), set forward step to remember path
00454                     if (frameNode.forwardStep != null) parentFrameNode.forwardStep = frameNode;
00455                     // add node to queue
00456                     Q.add(parentFrameNode);
00457                 }
00458             }
00459         }    
00460         
00461         // target and source frames are not connected.        
00462         return false;
00463     }
00464     
00473     protected class SearchNode<V> implements Comparable<SearchNode<V>> {        
00475         V content;
00477         double cost;
00479         SearchNode<V> backwardStep;
00481         SearchNode<V> forwardStep;        
00482         
00484         SearchNode(V content) {
00485             this.content = content;
00486             this.cost = 0;
00487             this.backwardStep = null;
00488             this.forwardStep = null;
00489         }
00490         
00492         SearchNode(SearchNode<V> orig) {
00493             this.content = orig.content;
00494             this.cost = orig.cost;
00495             this.backwardStep = orig.backwardStep;
00496             this.forwardStep = orig.forwardStep;
00497         }
00498         
00500         public int compareTo(SearchNode<V> other) {
00501             if (this.cost < other.cost) return -1;
00502             if (this.cost > other.cost) return 1;
00503             return 0;
00504         }        
00505         
00506     }
00507         
00508     /* **********************************************************************
00509      * *                          HELPER METHODS                            *
00510      * ********************************************************************** */        
00511         
00515         protected TransformStorage transformStampedMsgToTF(TransformStamped msg) {
00516             ros.pkg.geometry_msgs.msg.Vector3 tMsg = msg.transform.translation;
00517             ros.pkg.geometry_msgs.msg.Quaternion rMsg = msg.transform.rotation; 
00518             
00519             // add frames to map
00520             Frame childFrame = lookupOrInsertFrame(msg.child_frame_id);
00521             Frame parentFrame = lookupOrInsertFrame(msg.header.frame_id);
00522             
00523             return new TransformStorage(new Vector3d(tMsg.x, tMsg.y, tMsg.z),
00524                                         new Quat4d(rMsg.x, rMsg.y, rMsg.z, rMsg.w),
00525                                         msg.header.stamp.totalNsecs(),
00526                                         parentFrame, childFrame);
00527         }
00528         
00532         protected TransformStamped TFToTransformStampedMsg(TransformStorage tf) {
00533             Vector3d tTF = tf.getTranslation();
00534             Quat4d rTF = tf.getRotation();
00535             
00536             // convert quaternion and translation vector to corresponding messages
00537             ros.pkg.geometry_msgs.msg.Vector3 tMsg = new ros.pkg.geometry_msgs.msg.Vector3();
00538             ros.pkg.geometry_msgs.msg.Quaternion rMsg = new ros.pkg.geometry_msgs.msg.Quaternion();
00539             tMsg.x = tTF.x; tMsg.y = tTF.y; tMsg.z = tTF.z;
00540         rMsg.x = rTF.x; rMsg.y = rTF.y; rMsg.z = rTF.z; rMsg.w = rTF.w;
00541         
00542         // create TransformStamped message
00543             TransformStamped msg = new TransformStamped();          
00544             msg.header.frame_id = tf.getParentFrame().getFrameID();
00545             msg.header.stamp = new Time(tf.getTimeStamp());
00546             msg.child_frame_id = tf.getChildFrame().getFrameID();
00547             msg.transform.translation = tMsg;
00548             msg.transform.rotation = rMsg;
00549             
00550             return msg;
00551         }
00552         
00557         protected StampedTransform StorageToStampedTransform(TransformStorage ts) {
00558             return new StampedTransform(ts.getTranslation(), ts.getRotation(), new Time(ts.getTimeStamp()), 
00559                             ts.getParentFrame().getFrameID(), ts.getChildFrame().getFrameID());
00560         }
00561         
00566         private String assertResolved(String prefix, String frameID) {
00567           if (!frameID.startsWith("/"))     
00568               ros.logDebug("TF operating on not fully resolved frame id " + frameID +", resolving using local prefix " + prefix);
00569           return resolve(prefix, frameID);
00570         }
00571         
00575         private static String resolve(String prefix, String frameID)    {                           
00576             if (frameID.startsWith("/")) {
00577                         return frameID;                 
00578                 }         
00579                 
00580                 if (prefix.length() > 0) {
00581                         if (prefix.startsWith("/")) {
00582                                 return prefix + "/" + frameID;
00583                         } else {
00584                             return "/" + prefix + "/" + frameID;
00585                         }
00586                 }  else {                   
00587                         return "/" + frameID;                   
00588                 }
00589         }       
00590         
00591         /* Returns the tf prefix from the parameter list
00592          * 
00593          * TODO: does not work yet
00594         private static String getPrefixParam(NodeHandle nh) {
00595                 String param; 
00596                 if (!nh.hasParam("tf_prefix")) return ""; 
00597                 
00598                 try {
00599                         return nh.getStringParam("tf_prefix", false);
00600                 } catch (Exception e) {
00601                         e.printStackTrace();
00602                 }
00603                 return "";
00604         }
00605         */      
00606         
00607 }


tfjava
Author(s): Sjoerd van den Dries
autogenerated on Sun Oct 5 2014 22:54:19