From 2da352b91783b31e8a454baf32abd321794f789c Mon Sep 17 00:00:00 2001
From: Damon Kohler <damonkohler@google.com>
Date: Fri, 27 Jul 2012 16:24:46 +0200
Subject: [PATCH] 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.
---
.../org/ros/concurrent/EventDispatcher.java | 8 +-
.../transport/queue/IncomingMessageQueue.java | 6 +-
.../transport/queue/MessageDispatcher.java | 8 +-
.../transport/queue/MessageReceiver.java | 8 +-
.../transport/queue/OutgoingMessageQueue.java | 10 +-
.../java/org/ros/namespace/NameResolver.java | 8 +-
.../queue/MessageDispatcherTest.java | 8 +-
.../TransformBenchmark.java | 6 +-
.../ros/rosjava_geometry/FrameTransform.java | 54 ++++-
.../rosjava_geometry/FrameTransformTree.java | 207 ++++++++++++++----
.../rosjava_geometry/LazyFrameTransform.java | 11 +-
.../org/ros/rosjava_geometry/Quaternion.java | 22 +-
.../org/ros/rosjava_geometry/Vector3.java | 17 +-
.../FrameTransformTreeTest.java | 157 +++++++++----
14 files changed, 396 insertions(+), 134 deletions(-)
diff --git a/rosjava/src/main/java/org/ros/concurrent/EventDispatcher.java b/rosjava/src/main/java/org/ros/concurrent/EventDispatcher.java
index 2475aeb1..428d9256 100644
--- a/rosjava/src/main/java/org/ros/concurrent/EventDispatcher.java
+++ b/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
diff --git a/rosjava/src/main/java/org/ros/internal/transport/queue/IncomingMessageQueue.java b/rosjava/src/main/java/org/ros/internal/transport/queue/IncomingMessageQueue.java
index 95bdb303..f87672fc 100644
--- a/rosjava/src/main/java/org/ros/internal/transport/queue/IncomingMessageQueue.java
+++ b/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);
diff --git a/rosjava/src/main/java/org/ros/internal/transport/queue/MessageDispatcher.java b/rosjava/src/main/java/org/ros/internal/transport/queue/MessageDispatcher.java
index 7299281a..b44bf398 100644
--- a/rosjava/src/main/java/org/ros/internal/transport/queue/MessageDispatcher.java
+++ b/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) {
diff --git a/rosjava/src/main/java/org/ros/internal/transport/queue/MessageReceiver.java b/rosjava/src/main/java/org/ros/internal/transport/queue/MessageReceiver.java
index 2cd4f762..33392eef 100644
--- a/rosjava/src/main/java/org/ros/internal/transport/queue/MessageReceiver.java
+++ b/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
diff --git a/rosjava/src/main/java/org/ros/internal/transport/queue/OutgoingMessageQueue.java b/rosjava/src/main/java/org/ros/internal/transport/queue/OutgoingMessageQueue.java
index c7ebe441..5163a176 100644
--- a/rosjava/src/main/java/org/ros/internal/transport/queue/OutgoingMessageQueue.java
+++ b/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);
}
diff --git a/rosjava/src/main/java/org/ros/namespace/NameResolver.java b/rosjava/src/main/java/org/ros/namespace/NameResolver.java
index 164851b2..b12af1fe 100644
--- a/rosjava/src/main/java/org/ros/namespace/NameResolver.java
+++ b/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;
diff --git a/rosjava/src/test/java/org/ros/internal/transport/queue/MessageDispatcherTest.java b/rosjava/src/test/java/org/ros/internal/transport/queue/MessageDispatcherTest.java
index b3f61d5e..5b5038b5 100644
--- a/rosjava/src/test/java/org/ros/internal/transport/queue/MessageDispatcherTest.java
+++ b/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));
diff --git a/rosjava_benchmarks/src/main/java/org/ros/rosjava_benchmarks/TransformBenchmark.java b/rosjava_benchmarks/src/main/java/org/ros/rosjava_benchmarks/TransformBenchmark.java
index 9a38cb1f..3867329a 100644
--- a/rosjava_benchmarks/src/main/java/org/ros/rosjava_benchmarks/TransformBenchmark.java
+++ b/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);
}
}
diff --git a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransform.java b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransform.java
index a91536e0..1d0e99d4 100644
--- a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransform.java
+++ b/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;
diff --git a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransformTree.java b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransformTree.java
index 83c27b3a..e4e9571d 100644
--- a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransformTree.java
+++ b/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 FrameTransform getLatestTransform(GraphName frame) {
- LazyFrameTransform lazyFrameTransform = transforms.get(nameResolver.resolve(frame));
- if (lazyFrameTransform != null) {
- return lazyFrameTransform.get();
+ private void add(GraphName resolvedSource, LazyFrameTransform lazyFrameTransform) {
+ if (!transforms.containsKey(resolvedSource)) {
+ transforms.put(resolvedSource, new CircularBlockingDeque<LazyFrameTransform>(
+ TRANSFORM_QUEUE_CAPACITY));
}
- return null;
+ 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 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());
}
}
}
diff --git a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/LazyFrameTransform.java b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/LazyFrameTransform.java
index a41b2dc7..41a4a981 100644
--- a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/LazyFrameTransform.java
+++ b/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;
diff --git a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Quaternion.java b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Quaternion.java
index 0b4385f8..c9353a26 100644
--- a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Quaternion.java
+++ b/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;
}
diff --git a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Vector3.java b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Vector3.java
index 32e40f35..cf8cec28 100644
--- a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Vector3.java
+++ b/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;
}
diff --git a/rosjava_geometry/src/test/java/org/ros/rosjava_geometry/FrameTransformTreeTest.java b/rosjava_geometry/src/test/java/org/ros/rosjava_geometry/FrameTransformTreeTest.java
index 0510075a..13fcd367 100644
--- a/rosjava_geometry/src/test/java/org/ros/rosjava_geometry/FrameTransformTreeTest.java
+++ b/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 testUpdateAndGet() {
+ FrameTransform frameTransform =
+ new FrameTransform(Transform.identity(), nameResolver.resolve("foo"),
+ nameResolver.resolve("bar"), new Time());
+ frameTransformTree.update(frameTransform);
+ assertEquals(frameTransform, frameTransformTree.get("foo"));
+ }
+
+ @Test
+ public void testUpdateAndGetWithTransformStampedMessage() {
+ FrameTransform frameTransform =
+ 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);
+ FrameTransform frameTransform =
+ new FrameTransform(transform, nameResolver.resolve(source), nameResolver.resolve(target),
+ time);
+ frameTransform.toTransformStampedMessage(message);
+ return message;
+ }
+
@Test
public void testIdentityTransforms() {
- MessageDefinitionProvider messageDefinitionProvider = new MessageDefinitionReflectionProvider();
- MessageFactory messageFactory = new DefaultMessageFactory(messageDefinitionProvider);
- NameResolver nameResolver = NameResolver.newRoot();
- FrameTransformTree frameTransformTree = new FrameTransformTree(nameResolver);
+ frameTransformTree.update(newTransformStampedMessage(Transform.identity(), "baz", "bar",
+ new Time()));
+ frameTransformTree.update(newTransformStampedMessage(Transform.identity(), "bar", "foo",
+ new Time()));
+ // Full tree transform.
{
- geometry_msgs.TransformStamped message =
- messageFactory.newFromType(geometry_msgs.TransformStamped._TYPE);
- Transform transform = Transform.identity();
- FrameTransform frameTransform =
- new FrameTransform(transform, GraphName.of("baz"), GraphName.of("bar"));
- frameTransform.toTransformStampedMessage(new Time(), message);
- frameTransformTree.updateTransform(message);
+ 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.
{
- 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);
+ 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());
}
- FrameTransform frameTransform =
- frameTransformTree.newFrameTransform(GraphName.of("baz"), GraphName.of("foo"));
- assertEquals(nameResolver.resolve("baz"), frameTransform.getSourceFrame());
- assertEquals(nameResolver.resolve("foo"), 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)));
+ }
}
--
GitLab