diff --git a/rosjava/src/main/java/org/ros/concurrent/EventDispatcher.java b/rosjava/src/main/java/org/ros/concurrent/EventDispatcher.java
index 2475aeb1f92787db2ced5a50feb6e900e6ce4a21..428d92569f663b17eecd4533a875c41b840d58f6 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 95bdb3039b17df377882ecb3f0c5f920bcee8f8f..f87672fc6dc90a934399c11c8e7fcc85b22129a2 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 7299281a5d9fdfe183b6e494855df06d1bf88d07..b44bf398f254833d2f4a65beec2b2e1ffb2880e1 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 2cd4f762e4f309dae189c38446919edc69b308df..33392eefed5ee11e9e5e666e792382b26f8aa154 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 c7ebe441212ccd11a7ebdf3124c3568008389f14..5163a17643f087dc61ace7f8057587dcc56043a9 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 164851b255247091c6f53a2623f23f7183f74d9b..b12af1fe9dbd8432d103c8827c3161afc3adeab2 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 b3f61d5e07a5cd2ae8e6f6ac75471fc278212274..5b5038b520c30821f5fd0877650cffbeea37ce80 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 9a38cb1fea4ae79c006c0e486140b142a711bf49..3867329ae842722e4caff3a11c756911b9fe113e 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 a91536e027a08d0f25b67e0bcec14a0f253ee44b..1d0e99d4f26917fb69ee16a46d57a773a473225e 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 83c27b3a8fb5a1e9a338ef33331bc25227bfed46..e4e9571de165f3330e635cb406154128975c4cac 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 a41b2dc7baa59af964a8cd0753192fa3f385ba63..41a4a981abb9638205a24c029e66c09996bb49a1 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 0b4385f886ed6b6fff2f50b69149c152685ab31b..c9353a2696111ec84a1f560db8f2f4cc5b8fd3f6 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 32e40f3538141cbabdba75f3699d79d99cd3a339..cf8cec28aff86e7f61e19659281a9884bcd9ce53 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 0510075a957c109e3455af78c805b42aa58c9e3e..13fcd367513da738d47e2f033dbf652871ae65b7 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)));
+ }
}