Changes CircularBlockingQueue into CircularBlockingDeque. This allows us to...

Changes CircularBlockingQueue into CircularBlockingDeque. This allows us to start supporting time travel for TF.
Adds an initial, limited implementation of time travel for TF. The buffer is very small because the current search is very inefficient.
Slightly improves the performance of NameResolver.resolve() by avoiding string concatenation of an error message during normal operation.
Improves equality and hash codes of Quaternions and Vector3s by ensureing 0 == -0.

rosjava/src/main/java/org/ros/concurrent/EventDispatcher.java

@@ -26,20 +26,20 @@ package org.ros.concurrent;
 public class EventDispatcher<T> extends CancellableLoop {
 
   private final T listener;
-  private final CircularBlockingQueue<SignalRunnable<T>> events;
+  private final CircularBlockingDeque<SignalRunnable<T>> events;
 
   public EventDispatcher(T listener, int queueCapacity) {
     this.listener = listener;
-    events = new CircularBlockingQueue<SignalRunnable<T>>(queueCapacity);
+    events = new CircularBlockingDeque<SignalRunnable<T>>(queueCapacity);
   }
 
   public void signal(final SignalRunnable<T> signalRunnable) {
-    events.add(signalRunnable);
+    events.addLast(signalRunnable);
   }
 
   @Override
   public void loop() throws InterruptedException {
-    SignalRunnable<T> signalRunnable = events.take();
+    SignalRunnable<T> signalRunnable = events.takeFirst();
     signalRunnable.run(listener);
   }
 }
\ No newline at end of file

rosjava/src/main/java/org/ros/internal/transport/queue/IncomingMessageQueue.java

@@ -16,7 +16,7 @@
 
 package org.ros.internal.transport.queue;
 
-import org.ros.concurrent.CircularBlockingQueue;
+import org.ros.concurrent.CircularBlockingDeque;
 import org.ros.internal.transport.tcp.NamedChannelHandler;
 import org.ros.message.MessageDeserializer;
 import org.ros.message.MessageListener;
@@ -43,8 +43,8 @@ public class IncomingMessageQueue<T> {
   private final MessageDispatcher<T> messageDispatcher;
 
   public IncomingMessageQueue(MessageDeserializer<T> deserializer, ExecutorService executorService) {
-    CircularBlockingQueue<LazyMessage<T>> lazyMessages =
-        new CircularBlockingQueue<LazyMessage<T>>(QUEUE_CAPACITY);
+    CircularBlockingDeque<LazyMessage<T>> lazyMessages =
+        new CircularBlockingDeque<LazyMessage<T>>(QUEUE_CAPACITY);
     messageReceiver = new MessageReceiver<T>(lazyMessages, deserializer);
     messageDispatcher = new MessageDispatcher<T>(lazyMessages, executorService);
     executorService.execute(messageDispatcher);

rosjava/src/main/java/org/ros/internal/transport/queue/MessageDispatcher.java

@@ -19,7 +19,7 @@ package org.ros.internal.transport.queue;
 import org.apache.commons.logging.Log;
 import org.apache.commons.logging.LogFactory;
 import org.ros.concurrent.CancellableLoop;
-import org.ros.concurrent.CircularBlockingQueue;
+import org.ros.concurrent.CircularBlockingDeque;
 import org.ros.concurrent.EventDispatcher;
 import org.ros.concurrent.ListenerGroup;
 import org.ros.concurrent.SignalRunnable;
@@ -38,7 +38,7 @@ public class MessageDispatcher<T> extends CancellableLoop {
   private static final boolean DEBUG = false;
   private static final Log log = LogFactory.getLog(MessageDispatcher.class);
 
-  private final CircularBlockingQueue<LazyMessage<T>> lazyMessages;
+  private final CircularBlockingDeque<LazyMessage<T>> lazyMessages;
   private final ListenerGroup<MessageListener<T>> messageListeners;
 
   /**
@@ -50,7 +50,7 @@ public class MessageDispatcher<T> extends CancellableLoop {
   private boolean latchMode;
   private LazyMessage<T> latchedMessage;
 
-  public MessageDispatcher(CircularBlockingQueue<LazyMessage<T>> lazyMessages,
+  public MessageDispatcher(CircularBlockingDeque<LazyMessage<T>> lazyMessages,
       ExecutorService executorService) {
     this.lazyMessages = lazyMessages;
     messageListeners = new ListenerGroup<MessageListener<T>>(executorService);
@@ -114,7 +114,7 @@ public class MessageDispatcher<T> extends CancellableLoop {
 
   @Override
   public void loop() throws InterruptedException {
-    LazyMessage<T> lazyMessage = lazyMessages.take();
+    LazyMessage<T> lazyMessage = lazyMessages.takeFirst();
     synchronized (mutex) {
       latchedMessage = lazyMessage;
       if (DEBUG) {

rosjava/src/main/java/org/ros/internal/transport/queue/MessageReceiver.java

@@ -16,7 +16,7 @@
 
 package org.ros.internal.transport.queue;
 
-import org.ros.concurrent.CircularBlockingQueue;
+import org.ros.concurrent.CircularBlockingDeque;
 
 import org.apache.commons.logging.Log;
 import org.apache.commons.logging.LogFactory;
@@ -37,10 +37,10 @@ public class MessageReceiver<T> extends AbstractNamedChannelHandler {
   private static final boolean DEBUG = false;
   private static final Log log = LogFactory.getLog(MessageReceiver.class);
 
-  private final CircularBlockingQueue<LazyMessage<T>> lazyMessages;
+  private final CircularBlockingDeque<LazyMessage<T>> lazyMessages;
   private final MessageDeserializer<T> deserializer;
 
-  public MessageReceiver(CircularBlockingQueue<LazyMessage<T>> lazyMessages,
+  public MessageReceiver(CircularBlockingDeque<LazyMessage<T>> lazyMessages,
       MessageDeserializer<T> deserializer) {
     this.lazyMessages = lazyMessages;
     this.deserializer = deserializer;
@@ -59,7 +59,7 @@ public class MessageReceiver<T> extends AbstractNamedChannelHandler {
     }
     // We have to make a defensive copy of the buffer here because Netty does
     // not guarantee that the returned ChannelBuffer will not be reused.
-    lazyMessages.add(new LazyMessage<T>(buffer.copy(), deserializer));
+    lazyMessages.addLast(new LazyMessage<T>(buffer.copy(), deserializer));
     super.messageReceived(ctx, e);
   }
 }
\ No newline at end of file

rosjava/src/main/java/org/ros/internal/transport/queue/OutgoingMessageQueue.java

@@ -27,7 +27,7 @@ import org.jboss.netty.channel.group.ChannelGroupFuture;
 import org.jboss.netty.channel.group.ChannelGroupFutureListener;
 import org.jboss.netty.channel.group.DefaultChannelGroup;
 import org.ros.concurrent.CancellableLoop;
-import org.ros.concurrent.CircularBlockingQueue;
+import org.ros.concurrent.CircularBlockingDeque;
 import org.ros.internal.message.MessageBufferPool;
 import org.ros.internal.message.MessageBuffers;
 import org.ros.message.MessageSerializer;
@@ -45,7 +45,7 @@ public class OutgoingMessageQueue<T> {
   private static final int QUEUE_CAPACITY = 16;
 
   private final MessageSerializer<T> serializer;
-  private final CircularBlockingQueue<T> queue;
+  private final CircularBlockingDeque<T> queue;
   private final ChannelGroup channelGroup;
   private final Writer writer;
   private final MessageBufferPool messageBufferPool;
@@ -57,7 +57,7 @@ public class OutgoingMessageQueue<T> {
   private final class Writer extends CancellableLoop {
     @Override
     public void loop() throws InterruptedException {
-      T message = queue.take();
+      T message = queue.takeFirst();
       final ChannelBuffer buffer = messageBufferPool.acquire();
       serializer.serialize(message, buffer);
       if (DEBUG) {
@@ -79,7 +79,7 @@ public class OutgoingMessageQueue<T> {
 
   public OutgoingMessageQueue(MessageSerializer<T> serializer, ExecutorService executorService) {
     this.serializer = serializer;
-    queue = new CircularBlockingQueue<T>(QUEUE_CAPACITY);
+    queue = new CircularBlockingDeque<T>(QUEUE_CAPACITY);
     channelGroup = new DefaultChannelGroup();
     writer = new Writer();
     messageBufferPool = new MessageBufferPool();
@@ -101,7 +101,7 @@ public class OutgoingMessageQueue<T> {
    *          the message to add to the queue
    */
   public void add(T message) {
-    queue.add(message);
+    queue.addLast(message);
     setLatchedMessage(message);
   }
 

rosjava/src/main/java/org/ros/namespace/NameResolver.java

@@ -16,8 +16,6 @@
 
 package org.ros.namespace;
 
-import com.google.common.base.Preconditions;
-
 import org.ros.exception.RosRuntimeException;
 
 import java.util.Collections;
@@ -76,8 +74,10 @@ public class NameResolver {
    */
   public GraphName resolve(GraphName namespace, GraphName name) {
     GraphName remappedNamespace = lookUpRemapping(namespace);
-    Preconditions.checkArgument(remappedNamespace.isGlobal(),
-        "Namespace must be global. Tried to resolve: " + remappedNamespace);
+    if (!remappedNamespace.isGlobal()) {
+      throw new IllegalStateException(String.format(
+          "Namespace %s (remapped from %s) must be global.", remappedNamespace, namespace));
+    }
     GraphName remappedName = lookUpRemapping(name);
     if (remappedName.isGlobal()) {
       return remappedName;

rosjava/src/test/java/org/ros/internal/transport/queue/MessageDispatcherTest.java

@@ -21,7 +21,7 @@ import static org.junit.Assert.fail;
 
 import org.junit.Before;
 import org.junit.Test;
-import org.ros.concurrent.CircularBlockingQueue;
+import org.ros.concurrent.CircularBlockingDeque;
 import org.ros.internal.message.DefaultMessageFactory;
 import org.ros.internal.message.definition.MessageDefinitionReflectionProvider;
 import org.ros.message.MessageFactory;
@@ -42,13 +42,13 @@ public class MessageDispatcherTest {
   private static final int QUEUE_CAPACITY = 128;
 
   private ExecutorService executorService;
-  private CircularBlockingQueue<LazyMessage<std_msgs.Int32>> lazyMessages;
+  private CircularBlockingDeque<LazyMessage<std_msgs.Int32>> lazyMessages;
   private MessageFactory messageFactory;
 
   @Before
   public void before() {
     executorService = Executors.newCachedThreadPool();
-    lazyMessages = new CircularBlockingQueue<LazyMessage<std_msgs.Int32>>(128);
+    lazyMessages = new CircularBlockingDeque<LazyMessage<std_msgs.Int32>>(128);
     messageFactory = new DefaultMessageFactory(new MessageDefinitionReflectionProvider());
   }
 
@@ -84,7 +84,7 @@ public class MessageDispatcherTest {
       final int count = i;
       std_msgs.Int32 message = messageFactory.newFromType(std_msgs.Int32._TYPE);
       message.setData(count);
-      lazyMessages.add(new LazyMessage<std_msgs.Int32>(message));
+      lazyMessages.addLast(new LazyMessage<std_msgs.Int32>(message));
     }
 
     assertTrue(latch.await(1, TimeUnit.SECONDS));

rosjava_benchmarks/src/main/java/org/ros/rosjava_benchmarks/TransformBenchmark.java

@@ -59,15 +59,13 @@ public class TransformBenchmark extends AbstractNodeMain {
         connectedNode.getTopicMessageFactory().newFromType(geometry_msgs.TransformStamped._TYPE);
     turtle2.getHeader().setFrameId("world");
     turtle2.setChildFrameId("turtle2");
-    final GraphName sourceFrame = GraphName.of("turtle1");
-    final GraphName targetFrame = GraphName.of("turtle2");
     final FrameTransformTree frameTransformTree = new FrameTransformTree(NameResolver.newRoot());
     connectedNode.executeCancellableLoop(new CancellableLoop() {
       @Override
       protected void loop() throws InterruptedException {
         updateTransform(turtle1, connectedNode, frameTransformTree);
         updateTransform(turtle2, connectedNode, frameTransformTree);
-        frameTransformTree.newFrameTransform(sourceFrame, targetFrame);
+        frameTransformTree.transform("turtle1", "turtle2");
         counter.incrementAndGet();
       }
     });
@@ -103,6 +101,6 @@ public class TransformBenchmark extends AbstractNodeMain {
     transformStamped.getTransform().getTranslation().setX(Math.random());
     transformStamped.getTransform().getTranslation().setY(Math.random());
     transformStamped.getTransform().getTranslation().setZ(Math.random());
-    frameTransformTree.updateTransform(transformStamped);
+    frameTransformTree.update(transformStamped);
   }
 }

rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransform.java

@@ -16,12 +16,14 @@
 
 package org.ros.rosjava_geometry;
 
+import com.google.common.base.Preconditions;
+
 import org.ros.message.Time;
 import org.ros.namespace.GraphName;
 
 /**
  * Describes a {@link Transform} from data in the source frame to data in the
- * target frame.
+ * target frame at a specified {@link Time}.
  * 
  * @author damonkohler@google.com (Damon Kohler)
  */
@@ -30,19 +32,39 @@ public class FrameTransform {
   private final Transform transform;
   private final GraphName source;
   private final GraphName target;
+  private final Time time;
 
-  public static FrameTransform
-      fromTransformStamped(geometry_msgs.TransformStamped transformStamped) {
+  public static FrameTransform fromTransformStampedMessage(
+      geometry_msgs.TransformStamped transformStamped) {
     Transform transform = Transform.fromTransformMessage(transformStamped.getTransform());
     String target = transformStamped.getHeader().getFrameId();
     String source = transformStamped.getChildFrameId();
-    return new FrameTransform(transform, GraphName.of(source), GraphName.of(target));
+    Time stamp = transformStamped.getHeader().getStamp();
+    return new FrameTransform(transform, GraphName.of(source), GraphName.of(target), stamp);
   }
 
-  public FrameTransform(Transform transform, GraphName source, GraphName target) {
+  /**
+   * Allocates a new {@link FrameTransform}.
+   * 
+   * @param transform
+   *          the {@link Transform} that transforms data in the {@code source}
+   *          frame to data in the {@code target} frame
+   * @param source
+   *          the source frame
+   * @param target
+   *          the target frame
+   * @param time
+   *          the time associated with this {@link FrameTransform}, can be
+   *          {@null}
+   */
+  public FrameTransform(Transform transform, GraphName source, GraphName target, Time time) {
+    Preconditions.checkNotNull(transform);
+    Preconditions.checkNotNull(source);
+    Preconditions.checkNotNull(target);
     this.transform = transform;
     this.source = source;
     this.target = target;
+    this.time = time;
   }
 
   public Transform getTransform() {
@@ -57,10 +79,19 @@ public class FrameTransform {
     return target;
   }
 
-  public geometry_msgs.TransformStamped toTransformStampedMessage(Time stamp,
+  /**
+   * @return the time associated with the {@link FrameTransform} or {@code null}
+   *         if there is no associated time
+   */
+  public Time getTime() {
+    return time;
+  }
+
+  public geometry_msgs.TransformStamped toTransformStampedMessage(
       geometry_msgs.TransformStamped result) {
+    Preconditions.checkNotNull(time);
     result.getHeader().setFrameId(target.toString());
-    result.getHeader().setStamp(stamp);
+    result.getHeader().setStamp(time);
     result.setChildFrameId(source.toString());
     transform.toTransformMessage(result.getTransform());
     return result;
@@ -68,7 +99,8 @@ public class FrameTransform {
 
   @Override
   public String toString() {
-    return String.format("FrameTransform<Source: %s, Target: %s, %s>", source, target, transform);
+    return String.format("FrameTransform<Source: %s, Target: %s, %s, Time: %s>", source, target,
+        transform, time);
   }
 
   @Override
@@ -77,6 +109,7 @@ public class FrameTransform {
     int result = 1;
     result = prime * result + ((source == null) ? 0 : source.hashCode());
     result = prime * result + ((target == null) ? 0 : target.hashCode());
+    result = prime * result + ((time == null) ? 0 : time.hashCode());
     result = prime * result + ((transform == null) ? 0 : transform.hashCode());
     return result;
   }
@@ -100,6 +133,11 @@ public class FrameTransform {
         return false;
     } else if (!target.equals(other.target))
       return false;
+    if (time == null) {
+      if (other.time != null)
+        return false;
+    } else if (!time.equals(other.time))
+      return false;
     if (transform == null) {
       if (other.transform != null)
         return false;

rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransformTree.java

@@ -16,10 +16,13 @@
 
 package org.ros.rosjava_geometry;
 
+import com.google.common.annotations.VisibleForTesting;
 import com.google.common.base.Preconditions;
 import com.google.common.collect.Maps;
 
 import geometry_msgs.TransformStamped;
+import org.ros.concurrent.CircularBlockingDeque;
+import org.ros.message.Time;
 import org.ros.namespace.GraphName;
 import org.ros.namespace.NameResolver;
 
@@ -36,16 +39,22 @@ import java.util.Map;
  */
 public class FrameTransformTree {
 
+  private static final int TRANSFORM_QUEUE_CAPACITY = 16;
+
   private final NameResolver nameResolver;
+  private final Object mutex;
 
   /**
-   * A {@link Map} of the most recent {@link LazyFrameTransform} by target
-   * frame.
+   * A {@link Map} of the most recent {@link LazyFrameTransform} by source
+   * frame. Lookups by target frame or by the pair of source and target are both
+   * unnecessary because every frame can only have exactly one target.
    */
-  private final Map<GraphName, LazyFrameTransform> transforms;
+  private final Map<GraphName, CircularBlockingDeque<LazyFrameTransform>> transforms;
 
   public FrameTransformTree(NameResolver nameResolver) {
+    Preconditions.checkNotNull(nameResolver);
     this.nameResolver = nameResolver;
+    mutex = new Object();
     transforms = Maps.newConcurrentMap();
   }
 
@@ -60,69 +69,185 @@ public class FrameTransformTree {
    * @param transformStamped
    *          the {@link geometry_msgs.TransformStamped} message to update with
    */
-  public void updateTransform(geometry_msgs.TransformStamped transformStamped) {
-    GraphName target = nameResolver.resolve(transformStamped.getChildFrameId());
-    transforms.put(target, new LazyFrameTransform(transformStamped));
+  public void update(geometry_msgs.TransformStamped transformStamped) {
+    Preconditions.checkNotNull(transformStamped);
+    GraphName resolvedSource = nameResolver.resolve(transformStamped.getChildFrameId());
+    LazyFrameTransform lazyFrameTransform = new LazyFrameTransform(transformStamped);
+    add(resolvedSource, lazyFrameTransform);
+  }
+
+  @VisibleForTesting
+  void update(FrameTransform frameTransform) {
+    Preconditions.checkNotNull(frameTransform);
+    GraphName resolvedSource = frameTransform.getSourceFrame();
+    LazyFrameTransform lazyFrameTransform = new LazyFrameTransform(frameTransform);
+    add(resolvedSource, lazyFrameTransform);
+  }
+
+  private void add(GraphName resolvedSource, LazyFrameTransform lazyFrameTransform) {
+    if (!transforms.containsKey(resolvedSource)) {
+      transforms.put(resolvedSource, new CircularBlockingDeque<LazyFrameTransform>(
+          TRANSFORM_QUEUE_CAPACITY));
+    }
+    synchronized (mutex) {
+      transforms.get(resolvedSource).addFirst(lazyFrameTransform);
+    }
+  }
+
+  /**
+   * Returns the most recent {@link FrameTransform} for target {@code source}.
+   * 
+   * @param source
+   *          the frame to look up
+   * @return the most recent {@link FrameTransform} for {@code source} or
+   *         {@code null} if no transform for {@code source} is available
+   */
+  public FrameTransform lookUp(GraphName source) {
+    Preconditions.checkNotNull(source);
+    GraphName resolvedSource = nameResolver.resolve(source);
+    return getLatest(resolvedSource);
   }
 
-  private FrameTransform getLatestTransform(GraphName frame) {
-    LazyFrameTransform lazyFrameTransform = transforms.get(nameResolver.resolve(frame));
-    if (lazyFrameTransform != null) {
+  private FrameTransform getLatest(GraphName resolvedSource) {
+    CircularBlockingDeque<LazyFrameTransform> deque = transforms.get(resolvedSource);
+    if (deque == null) {
+      return null;
+    }
+    LazyFrameTransform lazyFrameTransform = deque.peekFirst();
+    if (lazyFrameTransform == null) {
+      return null;
+    }
     return lazyFrameTransform.get();
   }
+
+  /**
+   * @see #lookUp(GraphName)
+   */
+  public FrameTransform get(String source) {
+    Preconditions.checkNotNull(source);
+    return lookUp(GraphName.of(source));
+  }
+
+  /**
+   * Returns the {@link FrameTransform} for {@code source} closest to
+   * {@code time}.
+   * 
+   * @param source
+   *          the frame to look up
+   * @param time
+   *          the transform for {@code frame} closest to this {@link Time} will
+   *          be returned
+   * @return the most recent {@link FrameTransform} for {@code source} or
+   *         {@code null} if no transform for {@code source} is available
+   */
+  public FrameTransform lookUp(GraphName source, Time time) {
+    Preconditions.checkNotNull(source);
+    Preconditions.checkNotNull(time);
+    GraphName resolvedSource = nameResolver.resolve(source);
+    return get(resolvedSource, time);
+  }
+
+  // TODO(damonkohler): Use an efficient search.
+  private FrameTransform get(GraphName resolvedSource, Time time) {
+    CircularBlockingDeque<LazyFrameTransform> deque = transforms.get(resolvedSource);
+    if (deque == null) {
+      return null;
+    }
+    LazyFrameTransform result = null;
+    synchronized (mutex) {
+      long offset = 0;
+      for (LazyFrameTransform lazyFrameTransform : deque) {
+        if (result == null) {
+          result = lazyFrameTransform;
+          offset = Math.abs(time.subtract(result.get().getTime()).totalNsecs());
+          continue;
+        }
+        long newOffset = Math.abs(time.subtract(lazyFrameTransform.get().getTime()).totalNsecs());
+        if (newOffset < offset) {
+          result = lazyFrameTransform;
+          offset = newOffset;
+        }
+      }
+    }
+    if (result == null) {
       return null;
     }
+    return result.get();
+  }
 
   /**
-   * @param sourceFrame
-   *          the source frame
-   * @param targetFrame
-   *          the target frame
-   * @return {@code true} if there exists a {@link FrameTransform} from
-   *         {@code sourceFrame} to {@code targetFrame}, {@code false} otherwise
+   * @see #lookUp(GraphName, Time)
    */
-  public boolean canTransform(GraphName sourceFrame, GraphName targetFrame) {
-    Preconditions.checkNotNull(sourceFrame);
-    Preconditions.checkNotNull(targetFrame);
-    FrameTransform source = newFrameTransformToRoot(sourceFrame);
-    FrameTransform target = newFrameTransformToRoot(targetFrame);
-    return source.getTargetFrame().equals(target.getTargetFrame());
+  public FrameTransform get(String source, Time time) {
+    Preconditions.checkNotNull(source);
+    return lookUp(GraphName.of(source), time);
   }
 
   /**
    * @return the {@link FrameTransform} from source the frame to the target
    *         frame, or {@code null} if no {@link FrameTransform} could be found
    */
-  public FrameTransform newFrameTransform(GraphName sourceFrame, GraphName targetFrame) {
-    Preconditions.checkNotNull(sourceFrame);
-    Preconditions.checkNotNull(targetFrame);
-    FrameTransform source = newFrameTransformToRoot(sourceFrame);
-    FrameTransform target = newFrameTransformToRoot(targetFrame);
-    if (source.getTargetFrame().equals(target.getTargetFrame())) {
-      Transform transform = target.getTransform().invert().multiply(source.getTransform());
-      return new FrameTransform(transform, source.getSourceFrame(), target.getSourceFrame());
+  public FrameTransform transform(GraphName source, GraphName target) {
+    Preconditions.checkNotNull(source);
+    Preconditions.checkNotNull(target);
+    GraphName resolvedSource = nameResolver.resolve(source);
+    GraphName resolvedTarget = nameResolver.resolve(target);
+    if (resolvedSource.equals(resolvedTarget)) {
+      return new FrameTransform(Transform.identity(), resolvedSource, resolvedTarget, null);
+    }
+    FrameTransform sourceToRoot = transformToRoot(resolvedSource);
+    if (sourceToRoot != null && sourceToRoot.getTargetFrame().equals(resolvedTarget)) {
+      return sourceToRoot;
+    }
+    FrameTransform targetToRoot = transformToRoot(resolvedTarget);
+    if (targetToRoot != null) {
+      if (targetToRoot.getTargetFrame().equals(resolvedTarget)) {
+        return targetToRoot;
+      }
+      if (targetToRoot.getTargetFrame().equals(resolvedSource)) {
+        Transform transform = targetToRoot.getTransform().invert();
+        return new FrameTransform(transform, resolvedSource, resolvedTarget, targetToRoot.getTime());
+      }
+    }
+    if (sourceToRoot == null || targetToRoot == null) {
+      return null;
+    }
+    if (sourceToRoot.getTargetFrame().equals(targetToRoot.getTargetFrame())) {
+      Transform transform =
+          targetToRoot.getTransform().invert().multiply(sourceToRoot.getTransform());
+      return new FrameTransform(transform, resolvedSource, resolvedTarget, sourceToRoot.getTime());
     }
     return null;
   }
 
   /**
-   * @param frame
-   *          the start frame
-   * @return the {@link Transform} from {@code frame} to root
+   * @see #transform(GraphName, GraphName)
    */
-  private FrameTransform newFrameTransformToRoot(GraphName frame) {
-    GraphName sourceFrame = nameResolver.resolve(frame);
-    FrameTransform result =
-        new FrameTransform(Transform.identity(), sourceFrame, sourceFrame);
+  public FrameTransform transform(String source, String target) {
+    Preconditions.checkNotNull(source);
+    Preconditions.checkNotNull(target);
+    return transform(GraphName.of(source), GraphName.of(target));
+  }
+
+  /**
+   * @param resolvedSource
+   *          the resolved source frame
+   * @return the {@link Transform} from {@code source} to root
+   */
+  private FrameTransform transformToRoot(GraphName resolvedSource) {
+    FrameTransform result = getLatest(resolvedSource);
+    if (result == null) {
+      return null;
+    }
     while (true) {
-      FrameTransform resultToParent = getLatestTransform(result.getTargetFrame());
+      FrameTransform resultToParent = lookUp(result.getTargetFrame(), result.getTime());
       if (resultToParent == null) {
         return result;
       }
       // Now resultToParent.getSourceFrame() == result.getTargetFrame()
       Transform transform = resultToParent.getTransform().multiply(result.getTransform());
-      GraphName targetFrame = nameResolver.resolve(resultToParent.getTargetFrame());
-      result = new FrameTransform(transform, sourceFrame, targetFrame);
+      GraphName resolvedTarget = resultToParent.getTargetFrame();
+      result = new FrameTransform(transform, resolvedSource, resolvedTarget, result.getTime());
     }
   }
 }

rosjava_geometry/src/main/java/org/ros/rosjava_geometry/LazyFrameTransform.java

@@ -16,6 +16,8 @@
 
 package org.ros.rosjava_geometry;
 
+import com.google.common.annotations.VisibleForTesting;
+
 /**
  * Lazily converts a {@link geometry_msgs.Transform} message to a
  * {@link Transform} on the first call to {@link #get()} and caches the result.
@@ -36,6 +38,13 @@ public class LazyFrameTransform {
     mutex = new Object();
   }
 
+  @VisibleForTesting
+  LazyFrameTransform(FrameTransform frameTransform) {
+    message = null;
+    mutex = null;
+    this.frameTransform = frameTransform;
+  }
+
   /**
    * @return the {@link FrameTransform} for the wrapped
    *         {@link geometry_msgs.TransformStamped} message
@@ -46,7 +55,7 @@ public class LazyFrameTransform {
     }
     synchronized (mutex) {
       if (frameTransform == null) {
-        frameTransform = FrameTransform.fromTransformStamped(message);
+        frameTransform = FrameTransform.fromTransformStampedMessage(message);
       }
     }
     return frameTransform;

rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Quaternion.java

@@ -131,6 +131,11 @@ public class Quaternion {
 
   @Override
   public int hashCode() {
+    // Ensure that -0 and 0 are considered equal.
+    double w = this.w == 0 ? 0 : this.w;
+    double x = this.x == 0 ? 0 : this.x;
+    double y = this.y == 0 ? 0 : this.y;
+    double z = this.z == 0 ? 0 : this.z;
     final int prime = 31;
     int result = 1;
     long temp;
@@ -154,13 +159,22 @@ public class Quaternion {
     if (getClass() != obj.getClass())
       return false;
     Quaternion other = (Quaternion) obj;
-    if (Double.doubleToLongBits(w) != Double.doubleToLongBits(other.w))
+    // Ensure that -0 and 0 are considered equal.
+    double w = this.w == 0 ? 0 : this.w;
+    double x = this.x == 0 ? 0 : this.x;
+    double y = this.y == 0 ? 0 : this.y;
+    double z = this.z == 0 ? 0 : this.z;
+    double otherW = other.w == 0 ? 0 : other.w;
+    double otherX = other.x == 0 ? 0 : other.x;
+    double otherY = other.y == 0 ? 0 : other.y;
+    double otherZ = other.z == 0 ? 0 : other.z;
+    if (Double.doubleToLongBits(w) != Double.doubleToLongBits(otherW))
       return false;
-    if (Double.doubleToLongBits(x) != Double.doubleToLongBits(other.x))
+    if (Double.doubleToLongBits(x) != Double.doubleToLongBits(otherX))
       return false;
-    if (Double.doubleToLongBits(y) != Double.doubleToLongBits(other.y))
+    if (Double.doubleToLongBits(y) != Double.doubleToLongBits(otherY))
       return false;
-    if (Double.doubleToLongBits(z) != Double.doubleToLongBits(other.z))
+    if (Double.doubleToLongBits(z) != Double.doubleToLongBits(otherZ))
       return false;
     return true;
   }

rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Vector3.java

@@ -126,6 +126,10 @@ public class Vector3 {
 
   @Override
   public int hashCode() {
+    // Ensure that -0 and 0 are considered equal.
+    double x = this.x == 0 ? 0 : this.x;
+    double y = this.y == 0 ? 0 : this.y;
+    double z = this.z == 0 ? 0 : this.z;
     final int prime = 31;
     int result = 1;
     long temp;
@@ -147,11 +151,18 @@ public class Vector3 {
     if (getClass() != obj.getClass())
       return false;
     Vector3 other = (Vector3) obj;
-    if (Double.doubleToLongBits(x) != Double.doubleToLongBits(other.x))
+    // Ensure that -0 and 0 are considered equal.
+    double x = this.x == 0 ? 0 : this.x;
+    double y = this.y == 0 ? 0 : this.y;
+    double z = this.z == 0 ? 0 : this.z;
+    double otherX = other.x == 0 ? 0 : other.x;
+    double otherY = other.y == 0 ? 0 : other.y;
+    double otherZ = other.z == 0 ? 0 : other.z;
+    if (Double.doubleToLongBits(x) != Double.doubleToLongBits(otherX))
       return false;
-    if (Double.doubleToLongBits(y) != Double.doubleToLongBits(other.y))
+    if (Double.doubleToLongBits(y) != Double.doubleToLongBits(otherY))
       return false;
-    if (Double.doubleToLongBits(z) != Double.doubleToLongBits(other.z))
+    if (Double.doubleToLongBits(z) != Double.doubleToLongBits(otherZ))
       return false;
     return true;
   }

rosjava_geometry/src/test/java/org/ros/rosjava_geometry/FrameTransformTreeTest.java

@@ -17,14 +17,15 @@
 package org.ros.rosjava_geometry;
 
 import static org.junit.Assert.assertEquals;
+import static org.junit.Assert.assertTrue;
 
+import org.junit.Before;
 import org.junit.Test;
 import org.ros.internal.message.DefaultMessageFactory;
 import org.ros.internal.message.definition.MessageDefinitionReflectionProvider;
 import org.ros.message.MessageDefinitionProvider;
 import org.ros.message.MessageFactory;
 import org.ros.message.Time;
-import org.ros.namespace.GraphName;
 import org.ros.namespace.NameResolver;
 
 /**
@@ -32,75 +33,119 @@ import org.ros.namespace.NameResolver;
  */
 public class FrameTransformTreeTest {
 
+  private NameResolver nameResolver;
+  private FrameTransformTree frameTransformTree;
+  private MessageDefinitionProvider messageDefinitionProvider;
+  private MessageFactory messageFactory;
+
+  @Before
+  public void before() {
+    nameResolver = NameResolver.newRoot();
+    frameTransformTree = new FrameTransformTree(nameResolver);
+    messageDefinitionProvider = new MessageDefinitionReflectionProvider();
+    messageFactory = new DefaultMessageFactory(messageDefinitionProvider);
+  }
+
   @Test
-  public void testIdentityTransforms() {
-    MessageDefinitionProvider messageDefinitionProvider = new MessageDefinitionReflectionProvider();
-    MessageFactory messageFactory = new DefaultMessageFactory(messageDefinitionProvider);
-    NameResolver nameResolver = NameResolver.newRoot();
-    FrameTransformTree frameTransformTree = new FrameTransformTree(nameResolver);
+  public void testUpdateAndGet() {
+    FrameTransform frameTransform =
+        new FrameTransform(Transform.identity(), nameResolver.resolve("foo"),
+            nameResolver.resolve("bar"), new Time());
+    frameTransformTree.update(frameTransform);
+    assertEquals(frameTransform, frameTransformTree.get("foo"));
+  }
 
-    {
-      geometry_msgs.TransformStamped message =
-          messageFactory.newFromType(geometry_msgs.TransformStamped._TYPE);
-      Transform transform = Transform.identity();
+  @Test
+  public void testUpdateAndGetWithTransformStampedMessage() {
     FrameTransform frameTransform =
-          new FrameTransform(transform, GraphName.of("baz"), GraphName.of("bar"));
-      frameTransform.toTransformStampedMessage(new Time(), message);
-      frameTransformTree.updateTransform(message);
+        new FrameTransform(Transform.identity(), nameResolver.resolve("foo"),
+            nameResolver.resolve("bar"), new Time());
+    frameTransformTree.update(newTransformStampedMessage(Transform.identity(), "foo", "bar",
+        new Time()));
+    assertEquals(frameTransform, frameTransformTree.get("foo"));
   }
 
-    {
+  private geometry_msgs.TransformStamped newTransformStampedMessage(Transform transform,
+      String source, String target, Time time) {
     geometry_msgs.TransformStamped message =
         messageFactory.newFromType(geometry_msgs.TransformStamped._TYPE);
-      Transform transform = Transform.identity();
     FrameTransform frameTransform =
-          new FrameTransform(transform, GraphName.of("bar"), GraphName.of("foo"));
-      frameTransform.toTransformStampedMessage(new Time(), message);
-      frameTransformTree.updateTransform(message);
+        new FrameTransform(transform, nameResolver.resolve(source), nameResolver.resolve(target),
+            time);
+    frameTransform.toTransformStampedMessage(message);
+    return message;
   }
 
-    FrameTransform frameTransform =
-        frameTransformTree.newFrameTransform(GraphName.of("baz"), GraphName.of("foo"));
+  @Test
+  public void testIdentityTransforms() {
+    frameTransformTree.update(newTransformStampedMessage(Transform.identity(), "baz", "bar",
+        new Time()));
+    frameTransformTree.update(newTransformStampedMessage(Transform.identity(), "bar", "foo",
+        new Time()));
+
+    // Full tree transform.
+    {
+      FrameTransform frameTransform = frameTransformTree.transform("baz", "foo");
+      assertTrue(frameTransform != null);
+      assertEquals(nameResolver.resolve("baz"), frameTransform.getSourceFrame());
+      assertEquals(nameResolver.resolve("foo"), frameTransform.getTargetFrame());
+      assertEquals(Transform.identity(), frameTransform.getTransform());
+    }
+
+    // Same node transform.
+    {
+      FrameTransform frameTransform = frameTransformTree.transform("baz", "baz");
+      assertTrue(frameTransform != null);
       assertEquals(nameResolver.resolve("baz"), frameTransform.getSourceFrame());
+      assertEquals(nameResolver.resolve("baz"), frameTransform.getTargetFrame());
+      assertEquals(Transform.identity(), frameTransform.getTransform());
+    }
+
+    // Same node transform.
+    {
+      FrameTransform frameTransform = frameTransformTree.transform("bar", "bar");
+      assertTrue(frameTransform != null);
+      assertEquals(nameResolver.resolve("bar"), frameTransform.getSourceFrame());
+      assertEquals(nameResolver.resolve("bar"), frameTransform.getTargetFrame());
+      assertEquals(Transform.identity(), frameTransform.getTransform());
+    }
+
+    // Root-to-root transform.
+    {
+      FrameTransform frameTransform = frameTransformTree.transform("foo", "foo");
+      assertTrue(frameTransform != null);
+      assertEquals(nameResolver.resolve("foo"), frameTransform.getSourceFrame());
       assertEquals(nameResolver.resolve("foo"), frameTransform.getTargetFrame());
       assertEquals(Transform.identity(), frameTransform.getTransform());
     }
 
+    // Root-to-leaf transform.
+    {
+      FrameTransform frameTransform = frameTransformTree.transform("foo", "baz");
+      assertTrue(frameTransform != null);
+      assertEquals(nameResolver.resolve("foo"), frameTransform.getSourceFrame());
+      assertEquals(nameResolver.resolve("baz"), frameTransform.getTargetFrame());
+      assertEquals(Transform.identity(), frameTransform.getTransform());
+    }
+  }
+
   @Test
   public void testTransformToRoot() {
-    MessageDefinitionProvider messageDefinitionProvider = new MessageDefinitionReflectionProvider();
-    MessageFactory messageFactory = new DefaultMessageFactory(messageDefinitionProvider);
-    NameResolver nameResolver = NameResolver.newRoot();
-    FrameTransformTree frameTransformTree = new FrameTransformTree(nameResolver);
-
     {
-      geometry_msgs.TransformStamped message =
-          messageFactory.newFromType(geometry_msgs.TransformStamped._TYPE);
       Vector3 vector = Vector3.zero();
       Quaternion quaternion = new Quaternion(Math.sqrt(0.5), 0, 0, Math.sqrt(0.5));
       Transform transform = new Transform(vector, quaternion);
-      GraphName source = GraphName.of("baz");
-      GraphName target = GraphName.of("bar");
-      FrameTransform frameTransform = new FrameTransform(transform, source, target);
-      frameTransform.toTransformStampedMessage(new Time(), message);
-      frameTransformTree.updateTransform(message);
+      frameTransformTree.update(newTransformStampedMessage(transform, "baz", "bar", new Time()));
     }
 
     {
-      geometry_msgs.TransformStamped message =
-          messageFactory.newFromType(geometry_msgs.TransformStamped._TYPE);
       Vector3 vector = new Vector3(0, 1, 0);
       Quaternion quaternion = Quaternion.identity();
       Transform transform = new Transform(vector, quaternion);
-      GraphName source = GraphName.of("bar");
-      GraphName target = GraphName.of("foo");
-      FrameTransform frameTransform = new FrameTransform(transform, source, target);
-      frameTransform.toTransformStampedMessage(new Time(), message);
-      frameTransformTree.updateTransform(message);
+      frameTransformTree.update(newTransformStampedMessage(transform, "bar", "foo", new Time()));
     }
 
-    FrameTransform frameTransform =
-        frameTransformTree.newFrameTransform(GraphName.of("baz"), GraphName.of("foo"));
+    FrameTransform frameTransform = frameTransformTree.transform("baz", "foo");
     // If we were to reverse the order of the transforms in our implementation,
     // we would expect the translation vector to be <0, 0, 1> instead.
     Vector3 vector = new Vector3(0, 1, 0);
@@ -110,4 +155,26 @@ public class FrameTransformTreeTest {
     assertEquals(nameResolver.resolve("foo"), frameTransform.getTargetFrame());
     assertEquals(transform, frameTransform.getTransform());
   }
+
+  @Test
+  public void testTimeTravel() {
+    FrameTransform frameTransform1 =
+        new FrameTransform(Transform.identity(), nameResolver.resolve("foo"),
+            nameResolver.resolve("bar"), new Time());
+    FrameTransform frameTransform2 =
+        new FrameTransform(Transform.identity(), nameResolver.resolve("foo"),
+            nameResolver.resolve("bar"), new Time(2));
+    FrameTransform frameTransform3 =
+        new FrameTransform(Transform.identity(), nameResolver.resolve("foo"),
+            nameResolver.resolve("bar"), new Time(4));
+    frameTransformTree.update(frameTransform1);
+    frameTransformTree.update(frameTransform2);
+    frameTransformTree.update(frameTransform3);
+    assertEquals(frameTransform3, frameTransformTree.get("foo"));
+    assertEquals(frameTransform1, frameTransformTree.get("foo", new Time()));
+    assertEquals(frameTransform1, frameTransformTree.get("foo", new Time(0.5)));
+    assertEquals(frameTransform2, frameTransformTree.get("foo", new Time(1.5)));
+    assertEquals(frameTransform2, frameTransformTree.get("foo", new Time(2)));
+    assertEquals(frameTransform3, frameTransformTree.get("foo", new Time(10)));
+  }
 }