Skip to content
Snippets Groups Projects
Commit 0fd55c88 authored by Damon Kohler's avatar Damon Kohler Committed by Daniel Stonier
Browse files

Changes FrameTransformTree to use GraphName instead of FrameName but still support tf2.

parent 52d14497
Branches
Tags
No related merge requests found
Show whitespace changes
Inline Side-by-side
Showing
with 132 additions and 189 deletions
rosjava_benchmarks/src/main/java/org/ros/rosjava_benchmarks/TransformBenchmark.java
+ 0
− 1
View file @ 0fd55c88
......@@ -22,7 +22,6 @@ import org.ros.concurrent.WallTimeRate;
import org.ros.message.Duration;
import org.ros.message.Time;
import org.ros.namespace.GraphName;
import org.ros.namespace.NameResolver;
import org.ros.node.AbstractNodeMain;
import org.ros.node.ConnectedNode;
import org.ros.node.topic.Publisher;
......
rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameName.java deleted 100644 → 0
+ 0
− 72
View file @ 52d14497
/*
* Copyright (C) 2013 Google Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License"); you may not
* use this file except in compliance with the License. You may obtain a copy of
* the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
* License for the specific language governing permissions and limitations under
* the License.
*/
package org.ros.rosjava_geometry;
import java.lang.String;
/**
* Provides a simple wrapper around strings to represent
* frame names with backwards compatibility (pre ros hydro)
* catered for by ignoring graph name style leading slashes.
*
* @author d.stonier@gmail.com (Daniel Stonier)
*/
public class FrameName {
private static final String LEGACY_SEPARATOR = "/";
private String name;
public static FrameName of(String name) {
return new FrameName(name);
}
private FrameName(String name) {
this.name = stripLeadingSlash(name);
}
/**
* TF2 names (from hydro on) do not make use of leading slashes.
*/
private static String stripLeadingSlash(String name) {
return name.replaceFirst("^/", "");
}
public String toString() {
return name;
}
public int hashCode() {
return name.hashCode();
}
public boolean equals(Object obj) {
if (this == obj)
return true;
if (obj == null)
return false;
if (getClass() != obj.getClass())
return false;
FrameName other = (FrameName) obj;
if (name == null) {
if (other.name != null)
return false;
} else if (!name.equals(other.name)) {
return false;
}
return true;
}
}
rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransform.java
+ 11
− 10
View file @ 0fd55c88
......@@ -16,9 +16,10 @@
package org.ros.rosjava_geometry;
import com.google.common.base.Preconditions;
import org.ros.message.Time;
import org.ros.namespace.GraphName;
import com.google.common.base.Preconditions;
/**
* Describes a {@link Transform} from data in the source frame to data in the
......@@ -29,8 +30,8 @@ import org.ros.message.Time;
public class FrameTransform {
private final Transform transform;
private final FrameName source;
private final FrameName target;
private final GraphName source;
private final GraphName target;
private final Time time;
public static FrameTransform fromTransformStampedMessage(
......@@ -39,7 +40,7 @@ public class FrameTransform {
String target = transformStamped.getHeader().getFrameId();
String source = transformStamped.getChildFrameId();
Time stamp = transformStamped.getHeader().getStamp();
return new FrameTransform(transform, FrameName.of(source), FrameName.of(target), stamp);
return new FrameTransform(transform, GraphName.of(source), GraphName.of(target), stamp);
}
/**
......@@ -56,13 +57,13 @@ public class FrameTransform {
* the time associated with this {@link FrameTransform}, can be
* {@null}
*/
public FrameTransform(Transform transform, FrameName source, FrameName target, Time time) {
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.source = source.toRelative();
this.target = target.toRelative();
this.time = time;
}
......@@ -70,11 +71,11 @@ public class FrameTransform {
return transform;
}
public FrameName getSourceFrame() {
public GraphName getSourceFrame() {
return source;
}
public FrameName getTargetFrame() {
public GraphName getTargetFrame() {
return target;
}
......
rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransformTree.java
+ 41
− 33
View file @ 0fd55c88
......@@ -23,6 +23,8 @@ 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 java.util.Map;
/**
......@@ -45,7 +47,7 @@ public class FrameTransformTree {
* 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<FrameName, CircularBlockingDeque<LazyFrameTransform>> transforms;
private final Map<GraphName, CircularBlockingDeque<LazyFrameTransform>> transforms;
public FrameTransformTree() {
mutex = new Object();
......@@ -65,26 +67,28 @@ public class FrameTransformTree {
*/
public void update(geometry_msgs.TransformStamped transformStamped) {
Preconditions.checkNotNull(transformStamped);
FrameName transformName = FrameName.of(transformStamped.getChildFrameId());
GraphName source = GraphName.of(transformStamped.getChildFrameId());
LazyFrameTransform lazyFrameTransform = new LazyFrameTransform(transformStamped);
add(transformName, lazyFrameTransform);
add(source, lazyFrameTransform);
}
@VisibleForTesting
void update(FrameTransform frameTransform) {
Preconditions.checkNotNull(frameTransform);
FrameName source = frameTransform.getSourceFrame();
GraphName source = frameTransform.getSourceFrame();
LazyFrameTransform lazyFrameTransform = new LazyFrameTransform(frameTransform);
add(source, lazyFrameTransform);
}
private void add(FrameName source, LazyFrameTransform lazyFrameTransform) {
if (!transforms.containsKey(source)) {
transforms.put(source, new CircularBlockingDeque<LazyFrameTransform>(
private void add(GraphName source, LazyFrameTransform lazyFrameTransform) {
// This adds support for tf2 while maintaining backward compatibility with tf.
GraphName relativeSource = source.toRelative();
if (!transforms.containsKey(relativeSource)) {
transforms.put(relativeSource, new CircularBlockingDeque<LazyFrameTransform>(
TRANSFORM_QUEUE_CAPACITY));
}
synchronized (mutex) {
transforms.get(source).addFirst(lazyFrameTransform);
transforms.get(relativeSource).addFirst(lazyFrameTransform);
}
}
......@@ -96,12 +100,13 @@ public class FrameTransformTree {
* @return the most recent {@link FrameTransform} for {@code source} or
* {@code null} if no transform for {@code source} is available
*/
public FrameTransform lookUp(FrameName source) {
public FrameTransform lookUp(GraphName source) {
Preconditions.checkNotNull(source);
return getLatest(source);
// This adds support for tf2 while maintaining backward compatibility with tf.
return getLatest(source.toRelative());
}
private FrameTransform getLatest(FrameName source) {
private FrameTransform getLatest(GraphName source) {
CircularBlockingDeque<LazyFrameTransform> deque = transforms.get(source);
if (deque == null) {
return null;
......@@ -114,11 +119,11 @@ public class FrameTransformTree {
}
/**
* @see #lookUp(FrameName)
* @see #lookUp(GraphName)
*/
public FrameTransform get(String source) {
Preconditions.checkNotNull(source);
return lookUp(FrameName.of(source));
return lookUp(GraphName.of(source));
}
/**
......@@ -133,14 +138,14 @@ public class FrameTransformTree {
* @return the most recent {@link FrameTransform} for {@code source} or
* {@code null} if no transform for {@code source} is available
*/
public FrameTransform lookUp(FrameName source, Time time) {
public FrameTransform lookUp(GraphName source, Time time) {
Preconditions.checkNotNull(source);
Preconditions.checkNotNull(time);
return get(source, time);
}
// TODO(damonkohler): Use an efficient search.
private FrameTransform get(FrameName source, Time time) {
private FrameTransform get(GraphName source, Time time) {
CircularBlockingDeque<LazyFrameTransform> deque = transforms.get(source);
if (deque == null) {
return null;
......@@ -168,71 +173,74 @@ public class FrameTransformTree {
}
/**
* @see #lookUp(FrameName, Time)
* @see #lookUp(GraphName, Time)
*/
public FrameTransform get(String source, Time time) {
Preconditions.checkNotNull(source);
return lookUp(FrameName.of(source), time);
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 transform(FrameName source, FrameName target) {
public FrameTransform transform(GraphName source, GraphName target) {
Preconditions.checkNotNull(source);
Preconditions.checkNotNull(target);
if (source.equals(target)) {
return new FrameTransform(Transform.identity(), source, target, null);
}
FrameTransform sourceToRoot = transformToRoot(source);
FrameTransform targetToRoot = transformToRoot(target);
// This adds support for tf2 while maintaining backward compatibility with tf.
GraphName relativeSource = source.toRelative();
GraphName relativeTarget = target.toRelative();
if (relativeSource.equals(relativeTarget)) {
return new FrameTransform(Transform.identity(), relativeSource, relativeTarget, null);
}
FrameTransform sourceToRoot = transformToRoot(relativeSource);
FrameTransform targetToRoot = transformToRoot(relativeTarget);
if (sourceToRoot == null && targetToRoot == null) {
return null;
}
if (sourceToRoot == null) {
if (targetToRoot.getTargetFrame().equals(source)) {
// resolvedSource is root.
if (targetToRoot.getTargetFrame().equals(relativeSource)) {
// relativeSource is root.
return targetToRoot.invert();
} else {
return null;
}
}
if (targetToRoot == null) {
if (sourceToRoot.getTargetFrame().equals(target)) {
// resolvedTarget is root.
if (sourceToRoot.getTargetFrame().equals(relativeTarget)) {
// relativeTarget is root.
return sourceToRoot;
} else {
return null;
}
}
if (sourceToRoot.getTargetFrame().equals(targetToRoot.getTargetFrame())) {
// Neither resolvedSource nor resolvedTarget is root and both share the
// Neither relativeSource nor relativeTarget is root and both share the
// same root.
Transform transform =
targetToRoot.getTransform().invert().multiply(sourceToRoot.getTransform());
return new FrameTransform(transform, source, target, sourceToRoot.getTime());
return new FrameTransform(transform, relativeSource, relativeTarget, sourceToRoot.getTime());
}
// No known transform.
return null;
}
/**
* @see #transform(FrameName, FrameName)
* @see #transform(GraphName, GraphName)
*/
public FrameTransform transform(String source, String target) {
Preconditions.checkNotNull(source);
Preconditions.checkNotNull(target);
return transform(FrameName.of(source), FrameName.of(target));
return transform(GraphName.of(source), GraphName.of(target));
}
/**
* @param source
* the resolved source frame
* the source frame
* @return the {@link Transform} from {@code source} to root
*/
@VisibleForTesting
FrameTransform transformToRoot(FrameName source) {
FrameTransform transformToRoot(GraphName source) {
FrameTransform result = getLatest(source);
if (result == null) {
return null;
......@@ -244,7 +252,7 @@ public class FrameTransformTree {
}
// Now resultToParent.getSourceFrame() == result.getTargetFrame()
Transform transform = resultToParent.getTransform().multiply(result.getTransform());
FrameName target = resultToParent.getTargetFrame();
GraphName target = resultToParent.getTargetFrame();
result = new FrameTransform(transform, source, target, result.getTime());
}
}
......
rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Transform.java
+ 4
− 7
View file @ 0fd55c88
......@@ -16,9 +16,8 @@
package org.ros.rosjava_geometry;
import com.google.common.annotations.VisibleForTesting;
import org.ros.message.Time;
import org.ros.namespace.GraphName;
/**
* A transformation in terms of translation, rotation, and scale.
......@@ -132,7 +131,7 @@ public class Transform {
return result;
}
public geometry_msgs.PoseStamped toPoseStampedMessage(FrameName frame, Time stamp,
public geometry_msgs.PoseStamped toPoseStampedMessage(GraphName frame, Time stamp,
geometry_msgs.PoseStamped result) {
result.getHeader().setFrameId(frame.toString());
result.getHeader().setStamp(stamp);
......@@ -145,13 +144,11 @@ public class Transform {
&& rotationAndScale.almostEquals(other.rotationAndScale, epsilon);
}
@VisibleForTesting
Vector3 getTranslation() {
public Vector3 getTranslation() {
return translation;
}
@VisibleForTesting
Quaternion getRotationAndScale() {
public Quaternion getRotationAndScale() {
return rotationAndScale;
}
......
rosjava_geometry/src/test/java/org/ros/rosjava_geometry/FrameTransformTreeTest.java
+ 76
− 66
View file @ 0fd55c88
......@@ -26,105 +26,105 @@ 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.NameResolver;
import org.ros.namespace.GraphName;
/**
* @author damonkohler@google.com (Damon Kohler)
*/
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);
frameTransformTree = new FrameTransformTree();
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());
FrameTransform frameTransform = new FrameTransform(Transform.identity(),
GraphName.of("foo"), GraphName.of("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()));
FrameTransform frameTransform = new FrameTransform(Transform.identity(),
GraphName.of("foo"), GraphName.of("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);
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,
GraphName.of(source), GraphName.of(target), time);
frameTransform.toTransformStampedMessage(message);
return message;
}
@Test
public void testIdentityTransforms() {
frameTransformTree.update(newTransformStampedMessage(Transform.identity(), "baz", "bar",
new Time()));
frameTransformTree.update(newTransformStampedMessage(Transform.identity(), "bar", "foo",
new Time()));
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");
FrameTransform frameTransform = frameTransformTree
.transform("baz", "foo");
assertTrue(frameTransform != null);
assertEquals(nameResolver.resolve("baz"), frameTransform.getSourceFrame());
assertEquals(nameResolver.resolve("foo"), frameTransform.getTargetFrame());
assertEquals(GraphName.of("baz"), frameTransform.getSourceFrame());
assertEquals(GraphName.of("foo"), frameTransform.getTargetFrame());
assertEquals(Transform.identity(), frameTransform.getTransform());
}
// Same node transform.
{
FrameTransform frameTransform = frameTransformTree.transform("baz", "baz");
FrameTransform frameTransform = frameTransformTree
.transform("baz", "baz");
assertTrue(frameTransform != null);
assertEquals(nameResolver.resolve("baz"), frameTransform.getSourceFrame());
assertEquals(nameResolver.resolve("baz"), frameTransform.getTargetFrame());
assertEquals(GraphName.of("baz"), frameTransform.getSourceFrame());
assertEquals(GraphName.of("baz"), frameTransform.getTargetFrame());
assertEquals(Transform.identity(), frameTransform.getTransform());
}
// Same node transform.
{
FrameTransform frameTransform = frameTransformTree.transform("bar", "bar");
FrameTransform frameTransform = frameTransformTree
.transform("bar", "bar");
assertTrue(frameTransform != null);
assertEquals(nameResolver.resolve("bar"), frameTransform.getSourceFrame());
assertEquals(nameResolver.resolve("bar"), frameTransform.getTargetFrame());
assertEquals(GraphName.of("bar"), frameTransform.getSourceFrame());
assertEquals(GraphName.of("bar"), frameTransform.getTargetFrame());
assertEquals(Transform.identity(), frameTransform.getTransform());
}
// Root-to-root transform.
{
FrameTransform frameTransform = frameTransformTree.transform("foo", "foo");
FrameTransform frameTransform = frameTransformTree
.transform("foo", "foo");
assertTrue(frameTransform != null);
assertEquals(nameResolver.resolve("foo"), frameTransform.getSourceFrame());
assertEquals(nameResolver.resolve("foo"), frameTransform.getTargetFrame());
assertEquals(GraphName.of("foo"), frameTransform.getSourceFrame());
assertEquals(GraphName.of("foo"), frameTransform.getTargetFrame());
assertEquals(Transform.identity(), frameTransform.getTransform());
}
// Root-to-leaf transform.
{
FrameTransform frameTransform = frameTransformTree.transform("foo", "baz");
FrameTransform frameTransform = frameTransformTree
.transform("foo", "baz");
assertTrue(frameTransform != null);
assertEquals(nameResolver.resolve("foo"), frameTransform.getSourceFrame());
assertEquals(nameResolver.resolve("baz"), frameTransform.getTargetFrame());
assertEquals(GraphName.of("foo"), frameTransform.getSourceFrame());
assertEquals(GraphName.of("baz"), frameTransform.getTargetFrame());
assertEquals(Transform.identity(), frameTransform.getTransform());
}
}
......@@ -141,38 +141,44 @@ public class FrameTransformTreeTest {
private void updateFrameTransformTree() {
{
Transform transform = Transform.translation(0, 1, 0);
frameTransformTree.update(newTransformStampedMessage(transform, "bar", "foo", new Time()));
frameTransformTree.update(newTransformStampedMessage(transform, "bar",
"foo", new Time()));
}
{
Transform transform = Transform.xRotation(Math.PI / 2);
frameTransformTree.update(newTransformStampedMessage(transform, "baz", "bar", new Time()));
frameTransformTree.update(newTransformStampedMessage(transform, "baz",
"bar", new Time()));
}
{
Transform transform = Transform.translation(1, 0, 0);
frameTransformTree.update(newTransformStampedMessage(transform, "bop", "foo", new Time()));
frameTransformTree.update(newTransformStampedMessage(transform, "bop",
"foo", new Time()));
}
{
Transform transform = Transform.yRotation(Math.PI / 2);
frameTransformTree.update(newTransformStampedMessage(transform, "fuz", "bop", new Time()));
frameTransformTree.update(newTransformStampedMessage(transform, "fuz",
"bop", new Time()));
}
}
private void checkBazToFooTransform(FrameTransform frameTransform) {
// 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.
Transform transform = Transform.translation(0, 1, 0).multiply(Transform.xRotation(Math.PI / 2));
Transform transform = Transform.translation(0, 1, 0).multiply(
Transform.xRotation(Math.PI / 2));
Quaternion rotationAndScale = transform.getRotationAndScale();
assertTrue(String.format("%s is not neutral.", rotationAndScale),
rotationAndScale.isAlmostNeutral(1e-9));
assertEquals(nameResolver.resolve("baz"), frameTransform.getSourceFrame());
assertEquals(nameResolver.resolve("foo"), frameTransform.getTargetFrame());
assertEquals(GraphName.of("baz"), frameTransform.getSourceFrame());
assertEquals(GraphName.of("foo"), frameTransform.getTargetFrame());
assertTrue(transform.almostEquals(frameTransform.getTransform(), 1e-9));
}
@Test
public void testTransformBazToRoot() {
updateFrameTransformTree();
checkBazToFooTransform(frameTransformTree.transformToRoot(nameResolver.resolve("baz")));
checkBazToFooTransform(frameTransformTree.transformToRoot(GraphName
.of("baz")));
}
@Test
......@@ -185,20 +191,24 @@ public class FrameTransformTreeTest {
private void checkFuzToFooTransform(FrameTransform frameTransform) {
// 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.
Transform transform = Transform.translation(1, 0, 0).multiply(Transform.yRotation(Math.PI / 2));
Transform transform = Transform.translation(1, 0, 0).multiply(
Transform.yRotation(Math.PI / 2));
Quaternion rotationAndScale = transform.getRotationAndScale();
assertTrue(String.format("%s is not neutral.", rotationAndScale),
rotationAndScale.isAlmostNeutral(1e-9));
assertEquals(nameResolver.resolve("fuz"), frameTransform.getSourceFrame());
assertEquals(nameResolver.resolve("foo"), frameTransform.getTargetFrame());
assertTrue(String.format("Expected %s != %s", transform, frameTransform.getTransform()),
assertEquals(GraphName.of("fuz"), frameTransform.getSourceFrame());
assertEquals(GraphName.of("foo"), frameTransform.getTargetFrame());
assertTrue(
String.format("Expected %s != %s", transform,
frameTransform.getTransform()),
transform.almostEquals(frameTransform.getTransform(), 1e-9));
}
@Test
public void testTransformFuzToRoot() {
updateFrameTransformTree();
checkFuzToFooTransform(frameTransformTree.transformToRoot(nameResolver.resolve("fuz")));
checkFuzToFooTransform(frameTransformTree.transformToRoot(GraphName
.of("fuz")));
}
@Test
......@@ -212,27 +222,27 @@ public class FrameTransformTreeTest {
public void testTransformBazToFuz() {
updateFrameTransformTree();
FrameTransform frameTransform = frameTransformTree.transform("baz", "fuz");
Transform transform =
Transform.yRotation(Math.PI / 2).invert().multiply(Transform.translation(1, 0, 0).invert())
.multiply(Transform.translation(0, 1, 0)).multiply(Transform.xRotation(Math.PI / 2));
Transform transform = Transform.yRotation(Math.PI / 2).invert()
.multiply(Transform.translation(1, 0, 0).invert())
.multiply(Transform.translation(0, 1, 0))
.multiply(Transform.xRotation(Math.PI / 2));
assertTrue(transform.getRotationAndScale().isAlmostNeutral(1e-9));
assertEquals(nameResolver.resolve("baz"), frameTransform.getSourceFrame());
assertEquals(nameResolver.resolve("fuz"), frameTransform.getTargetFrame());
assertTrue(String.format("Expected %s != %s", transform, frameTransform.getTransform()),
assertEquals(GraphName.of("baz"), frameTransform.getSourceFrame());
assertEquals(GraphName.of("fuz"), frameTransform.getTargetFrame());
assertTrue(
String.format("Expected %s != %s", transform,
frameTransform.getTransform()),
transform.almostEquals(frameTransform.getTransform(), 1e-9));
}
@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));
FrameTransform frameTransform1 = new FrameTransform(Transform.identity(),
GraphName.of("foo"), GraphName.of("bar"), new Time());
FrameTransform frameTransform2 = new FrameTransform(Transform.identity(),
GraphName.of("foo"), GraphName.of("bar"), new Time(2));
FrameTransform frameTransform3 = new FrameTransform(Transform.identity(),
GraphName.of("foo"), GraphName.of("bar"), new Time(4));
frameTransformTree.update(frameTransform1);
frameTransformTree.update(frameTransform2);
frameTransformTree.update(frameTransform3);
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment