TFListener.java

Go to the documentation of this file.

/* 
 * Copyright (c) 2011, Sjoerd van den Dries
 * All rights reserved.
 * 
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Technische Universiteit Eindhoven nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */


package tfjava;

import ros.*;
import ros.communication.*;

import ros.pkg.tf.msg.tfMessage;
import ros.pkg.geometry_msgs.msg.TransformStamped;

import javax.vecmath.Quat4d;
import javax.vecmath.Vector3d;
import javax.vecmath.Point3d;
import javax.vecmath.Matrix4d;

import java.util.HashMap;
import java.util.PriorityQueue;
import java.util.LinkedList;

public class TFListener {
    
    public static final long MAX_STORAGE_TIME = (new Duration(10, 0)).totalNsecs(); 
    
    protected static Ros ros;
    protected static NodeHandle rosNode;
    protected static TFListener instance;    
    protected HashMap<String, Frame> frames;
    protected String tfPrefix = "";
    
    public synchronized static TFListener getInstance() {
        if (instance == null) {
            instance = new TFListener();
        }
        return instance;
    }
    
    /* **********************************************************************
     * *                           INITIALIZATION                           *
     * ********************************************************************** */     
    
    protected TFListener() {
                initRos("tfjava_listener");
                
                if (initListener()) {
                    spinInSeperateThread();
                }
        }       
        
        protected void initRos(String node_name) {
                ros = Ros.getInstance();

                if(!ros.isInitialized()) {
                        ros.init(node_name);
                }
                rosNode = ros.createNodeHandle();
        }
        
        protected boolean initListener() {
        frames = new HashMap<String, Frame>();    
            
            try {           
            TFCallback callback = new TFCallback();         
            Subscriber<tfMessage> sub = rosNode.subscribe("/tf", new tfMessage(), callback, 10);  // 10: magic numer
            
        } catch (Exception e) {
            ros.logError("Main: subscribe to /tf failed");
            return false;
        }           
        return true;
        }
        
    protected class TFCallback implements Subscriber.Callback<tfMessage> {
        public void call(tfMessage tfm) {
            if (tfm != null) {
                for(TransformStamped tf : tfm.transforms) {                 
                    setTransform(tf);               
                }
            }
        }
    }           
        
    protected void spinInSeperateThread() {
            Thread t = new Thread() {
                public void run() {
                    rosNode.spin();
                }
            };      
            t.start();
        }       
        
    /* **********************************************************************
     * *                            TF LISTENER                             *
     * ********************************************************************** */        
        
    protected boolean setTransform(TransformStamped transform) {
            // resolve the frame ID's
        String childFrameID = assertResolved(tfPrefix, transform.child_frame_id);
            String frameID = assertResolved(tfPrefix, transform.header.frame_id);
 
            boolean errorExists = false;
            if (childFrameID == frameID) {
                ros.logError("TF_SELF_TRANSFORM: Ignoring transform with frame_id and child_frame_id  \"" + childFrameID + "\" because they are the same");
                errorExists = true;
            }

            if (childFrameID == "/") { //empty frame id will be mapped to "/"
                ros.logError("TF_NO_CHILD_FRAME_ID: Ignoring transform because child_frame_id not set ");
                errorExists = true;
            }

            if (frameID == "/") { //empty parent id will be mapped to "/"
                ros.logError("TF_NO_FRAME_ID: Ignoring transform with child_frame_id \"" + childFrameID + "\" because frame_id not set");
                errorExists = true;
            }

            if (errorExists) return false;          
            
            // lookup or insert child frame
            Frame frame = lookupOrInsertFrame(childFrameID);
            
            // convert tf message to JTransform datastructure
            transform.child_frame_id = childFrameID;
            transform.header.frame_id = frameID; 
            TransformStorage tf = transformStampedMsgToTF(transform);
            
            // try to insert tf in corresponding time cache. If result is FALSE, the tf contains old data.
            if (!frame.insertData(tf)) {
                ros.logWarn("TF_OLD_DATA ignoring data from the past for frame \"" + childFrameID + "\" at time " + ((double)tf.getTimeStamp() / 1E9));
            return false;
            }

        return true;
        }
        
        protected Frame lookupOrInsertFrame(String frameID) {
        Frame frame = frames.get(frameID);
        if (frame == null) {
            frame = new Frame(frameID, MAX_STORAGE_TIME);
            frames.put(frameID, frame);
        }           
        return frame;
        }
    
    /* **********************************************************************
     * *                         TRANSFORM METHODS                          *
     * ********************************************************************** */ 
        
        public void transformPoint(String targetFrameID, Stamped<Point3d> stampedIn, Stamped<Point3d> stampedOut) {
            StampedTransform transform = lookupTransform(targetFrameID, stampedIn.frameID, stampedIn.timeStamp);
            transform.transformPoint(stampedIn.getData(), stampedOut.getData());
            stampedOut.frameID = targetFrameID;
            stampedOut.timeStamp = stampedIn.timeStamp;
        }
        
        public void transformPoint(String targetFrameID, Time targetTime, Stamped<Point3d> stampedIn,
                                                    String fixedFrameID, Stamped<Point3d> stampedOut) {
            StampedTransform transform = lookupTransform(targetFrameID, targetTime, stampedIn.frameID, stampedIn.timeStamp, fixedFrameID);
        transform.transformPoint(stampedIn.getData(), stampedOut.getData()); 
        stampedOut.frameID = targetFrameID;
        stampedOut.timeStamp = stampedIn.timeStamp;
        }
        public void transformPose(String targetFrameID, Stamped<Matrix4d> stampedIn, Stamped<Matrix4d> stampedOut) {
            StampedTransform transform = lookupTransform(targetFrameID, stampedIn.frameID, stampedIn.timeStamp);
            transform.transformPose(stampedIn.getData(), stampedOut.getData());     
        stampedOut.frameID = targetFrameID;
        stampedOut.timeStamp = stampedIn.timeStamp;
        }
         
        public void transformPose(String targetFrameID, Time targetTime, Stamped<Matrix4d> stampedIn,
                                                    String fixedFrameID, Stamped<Matrix4d> stampedOut) {
            StampedTransform transform = lookupTransform(targetFrameID, targetTime, stampedIn.frameID, stampedIn.timeStamp, fixedFrameID);
            transform.transformPose(stampedIn.getData(), stampedOut.getData());
        stampedOut.frameID = targetFrameID;
        stampedOut.timeStamp = stampedIn.timeStamp;
    }
        
    /* **********************************************************************
     * *                          LOOKUP METHODS                            *
     * ********************************************************************** */    

        public StampedTransform lookupTransform(String targetFrameID, String sourceFrameID, Time time) {
        // resolve the source and target IDs
            String resolvedTargetID = assertResolved(tfPrefix, targetFrameID);
        String resolvedSourceID = assertResolved(tfPrefix, sourceFrameID);
        
        // if source and target are the same, return the identity transform
        if (resolvedSourceID == resolvedTargetID) {
            StampedTransform out = StampedTransform.getIdentity();
            out.timeStamp = time;
            out.frameID = resolvedSourceID;
            out.childFrameID = resolvedTargetID;            
            return out;
        }

        // lookup source and target frame
        Frame sourceFrame = frames.get(resolvedSourceID);
        Frame targetFrame = frames.get(resolvedTargetID);
        
        if (sourceFrame == null) {
            ros.logError("Cannot transform: source frame \"" + resolvedSourceID + "\" does not exist.");
            return null;
        }    
            
        if (targetFrame == null) {
            ros.logError("Cannot transform: target frame \"" + resolvedTargetID + "\" does not exist.");
            return null;
        } 
            
        // list that will contain transformations from source frame to some frame F        
            LinkedList<TransformStorage> inverseTransforms = new LinkedList<TransformStorage>();
            // list that will contain transformations from frame F to target frame
            LinkedList<TransformStorage> forwardTransforms = new LinkedList<TransformStorage>();
            
            // fill the lists using lookupLists. If it returns FALSE, no transformation could be found.
            if (!lookupLists(targetFrame, sourceFrame, time.totalNsecs(), inverseTransforms, forwardTransforms)) {
                // TODO give warning
                ros.logError("Cannot transform: source + \"" + resolvedSourceID + "\" and target \""
                                + resolvedTargetID + "\" are not connected.");
                return null;
            }        
            
            // create an identity transform with the correct time stamp
            StampedTransform out = StampedTransform.getIdentity();          
            out.timeStamp = time;
            
        // multiply all transforms from source frame to frame F TODO: right?
        for(TransformStorage t : inverseTransforms) {           
            out.mul(StorageToStampedTransform(t));
        }
            
            // multiply all transforms from frame F to target frame TODO: right?
            for(TransformStorage t : forwardTransforms) {               
                out.mul(StorageToStampedTransform(t).invert(), out);
        }           
            
            // return transform
            return out;
        }
        
        public StampedTransform lookupTransform(String targetID, Time targetTime, String sourceID, Time sourceTime, String fixedID) {       
            // lookup transform from source to fixed frame, at sourceTime
            StampedTransform t1 = lookupTransform(fixedID, sourceID, sourceTime);
            // lookup transform from fixed frame to target frame, at targetTime
            StampedTransform t2 = lookupTransform(targetID, fixedID, targetTime);
            
            // if either of the two transformations did not succeed, return null
            if (t1 == null || t2 == null) return null;      
            
            // multiply transformation t2 with t1, and return
            t2.mul(t1);
            return t2;
        }
        
    protected boolean lookupLists(Frame targetFrame, Frame sourceFrame, long time,
            LinkedList<TransformStorage> inverseTransforms, LinkedList<TransformStorage> forwardTransforms) {
        
        // wrap the source and target frames in search nodes
        SearchNode<Frame> sourceNode = new SearchNode<Frame>(sourceFrame);
        SearchNode<Frame> targetNode = new SearchNode<Frame>(targetFrame);
        
        // set beginning of forward path (from source)
        sourceNode.backwardStep = sourceNode;
        // set beginning of backward path (form target)
        targetNode.forwardStep = targetNode;        
        
        // create a hash map that map frames to search nodes. This is necessary to keep track of
        // which frames have already been visited (and from which direction).
        HashMap<Frame, SearchNode<Frame>> frameToNode = new HashMap<Frame, SearchNode<Frame>>();
        
        // add source and target search nodes to the map
        frameToNode.put(sourceFrame, sourceNode);
        frameToNode.put(targetFrame, targetNode);
        
        // create a priority queue, which will hold the search nodes ordered by cost (descending)
        PriorityQueue<SearchNode<Frame>> Q = new PriorityQueue<SearchNode<Frame>>();
        
        // at the source and target search nodes to the queue
        Q.add(sourceNode); 
        Q.add(targetNode);
        
        // perform the search
        while(!Q.isEmpty()) {
            // poll most potential search node from queue
            SearchNode<Frame> frameNode = Q.poll();
            Frame frame = frameNode.content;
            
            // if the node is both visited from the source and from the target node, a path has been found
            if (frameNode.backwardStep != null && frameNode.forwardStep != null) {
                // found the best path from source to target through FRAME.
                
                // create inverse list (from source to FRAME)
                SearchNode<Frame> node = frameNode;                
                while(node.content != sourceNode.content) {                    
                    inverseTransforms.addLast(node.backwardStep.content.getData(time, node.content));
                    node = node.backwardStep;
                }
                
                // create forward list (from FRAME to target)
                node = frameNode;
                while(node.content != targetNode.content) {
                    forwardTransforms.addLast(node.forwardStep.content.getData(time, node.content));
                    node = node.forwardStep;
                }
                return true;
            }
            
            // expand search node
            for(Frame parentFrame : frame.getParentFrames()) {                
                SearchNode<Frame> parentFrameNode = frameToNode.get(parentFrame);
                                
                boolean addToQueue = false;
                if (parentFrameNode == null) {
                    // node was not yet visited
                    parentFrameNode = new SearchNode<Frame>(parentFrame);                    
                    frameToNode.put(parentFrame, parentFrameNode);
                    addToQueue = true;
                } else {
                    // node is already visited
                    if ((parentFrameNode.backwardStep == null && frameNode.forwardStep == null)
                            || (parentFrameNode.forwardStep == null && frameNode.backwardStep == null)) {
                        // node was visited, but from other direction.
                        // create new search node that represents this frame, visited from both sides
                        // this allows the other search node of this frame to still be expanded first                        
                        parentFrameNode = new SearchNode<Frame>(parentFrameNode);
                        addToQueue = true;                        
                    }
                }                
                                
                // add search node belonging to parent frame to the queue
                if (addToQueue) {
                    // determine cost (based on max absolute difference in time stamp) 
                    TimeCache cache = frame.getTimeCache(parentFrame);
                    parentFrameNode.cost = Math.max((double)cache.timeToNearestTransform(time),
                                                Math.max(parentFrameNode.cost, frameNode.cost));
                    // if visiting forward (from source), set backward step to remember path 
                    if (frameNode.backwardStep != null) parentFrameNode.backwardStep = frameNode;
                    // if visiting backward (from target), set forward step to remember path
                    if (frameNode.forwardStep != null) parentFrameNode.forwardStep = frameNode;
                    // add node to queue
                    Q.add(parentFrameNode);
                }
            }
        }    
        
        // target and source frames are not connected.        
        return false;
    }
    
    protected class SearchNode<V> implements Comparable<SearchNode<V>> {        
        V content;
        double cost;
        SearchNode<V> backwardStep;
        SearchNode<V> forwardStep;        
        
        SearchNode(V content) {
            this.content = content;
            this.cost = 0;
            this.backwardStep = null;
            this.forwardStep = null;
        }
        
        SearchNode(SearchNode<V> orig) {
            this.content = orig.content;
            this.cost = orig.cost;
            this.backwardStep = orig.backwardStep;
            this.forwardStep = orig.forwardStep;
        }
        
        public int compareTo(SearchNode<V> other) {
            if (this.cost < other.cost) return -1;
            if (this.cost > other.cost) return 1;
            return 0;
        }        
        
    }
        
    /* **********************************************************************
     * *                          HELPER METHODS                            *
     * ********************************************************************** */        
        
        protected TransformStorage transformStampedMsgToTF(TransformStamped msg) {
            ros.pkg.geometry_msgs.msg.Vector3 tMsg = msg.transform.translation;
            ros.pkg.geometry_msgs.msg.Quaternion rMsg = msg.transform.rotation; 
            
            // add frames to map
            Frame childFrame = lookupOrInsertFrame(msg.child_frame_id);
            Frame parentFrame = lookupOrInsertFrame(msg.header.frame_id);
            
            return new TransformStorage(new Vector3d(tMsg.x, tMsg.y, tMsg.z),
                                        new Quat4d(rMsg.x, rMsg.y, rMsg.z, rMsg.w),
                                        msg.header.stamp.totalNsecs(),
                                        parentFrame, childFrame);
        }
        
        protected TransformStamped TFToTransformStampedMsg(TransformStorage tf) {
            Vector3d tTF = tf.getTranslation();
            Quat4d rTF = tf.getRotation();
            
            // convert quaternion and translation vector to corresponding messages
            ros.pkg.geometry_msgs.msg.Vector3 tMsg = new ros.pkg.geometry_msgs.msg.Vector3();
            ros.pkg.geometry_msgs.msg.Quaternion rMsg = new ros.pkg.geometry_msgs.msg.Quaternion();
            tMsg.x = tTF.x; tMsg.y = tTF.y; tMsg.z = tTF.z;
        rMsg.x = rTF.x; rMsg.y = rTF.y; rMsg.z = rTF.z; rMsg.w = rTF.w;
        
        // create TransformStamped message
            TransformStamped msg = new TransformStamped();          
            msg.header.frame_id = tf.getParentFrame().getFrameID();
            msg.header.stamp = new Time(tf.getTimeStamp());
            msg.child_frame_id = tf.getChildFrame().getFrameID();
            msg.transform.translation = tMsg;
            msg.transform.rotation = rMsg;
            
            return msg;
        }
        
        protected StampedTransform StorageToStampedTransform(TransformStorage ts) {
            return new StampedTransform(ts.getTranslation(), ts.getRotation(), new Time(ts.getTimeStamp()), 
                            ts.getParentFrame().getFrameID(), ts.getChildFrame().getFrameID());
        }
        
        private String assertResolved(String prefix, String frameID) {
          if (!frameID.startsWith("/"))     
              ros.logDebug("TF operating on not fully resolved frame id " + frameID +", resolving using local prefix " + prefix);
          return resolve(prefix, frameID);
        }
        
        private static String resolve(String prefix, String frameID)    {                           
            if (frameID.startsWith("/")) {
                        return frameID;                 
                }         
                
                if (prefix.length() > 0) {
                        if (prefix.startsWith("/")) {
                                return prefix + "/" + frameID;
                        } else {
                            return "/" + prefix + "/" + frameID;
                        }
                }  else {                   
                        return "/" + frameID;                   
                }
        }       
        
        /* Returns the tf prefix from the parameter list
         * 
         * TODO: does not work yet
        private static String getPrefixParam(NodeHandle nh) {
                String param; 
                if (!nh.hasParam("tf_prefix")) return ""; 
                
                try {
                        return nh.getStringParam("tf_prefix", false);
                } catch (Exception e) {
                        e.printStackTrace();
                }
                return "";
        }
        */      
        
}