@Test public void testTransformBazToFoo() { updateFrameTransformTree(); checkBazToFooTransform(frameTransformTree.transform("baz", "foo")); checkBazToFooTransform(frameTransformTree.transform("foo", "baz").invert()); }
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); FrameTransform targetToRoot = transformToRoot(resolvedTarget); if (sourceToRoot == null && targetToRoot == null) { return null; } if (sourceToRoot == null) { if (targetToRoot.getTargetFrame().equals(resolvedSource)) { return targetToRoot.invert(); } else { return null; } } if (targetToRoot == null) { if (sourceToRoot.getTargetFrame().equals(resolvedTarget)) { return sourceToRoot; } else { 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; }
FrameTransformTree { 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); FrameTransform targetToRoot = transformToRoot(resolvedTarget); if (sourceToRoot == null && targetToRoot == null) { return null; } if (sourceToRoot == null) { if (targetToRoot.getTargetFrame().equals(resolvedSource)) { return targetToRoot.invert(); } else { return null; } } if (targetToRoot == null) { if (sourceToRoot.getTargetFrame().equals(resolvedTarget)) { return sourceToRoot; } else { 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; } }
FrameTransformTree { 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); FrameTransform targetToRoot = transformToRoot(resolvedTarget); if (sourceToRoot == null && targetToRoot == null) { return null; } if (sourceToRoot == null) { if (targetToRoot.getTargetFrame().equals(resolvedSource)) { return targetToRoot.invert(); } else { return null; } } if (targetToRoot == null) { if (sourceToRoot.getTargetFrame().equals(resolvedTarget)) { return sourceToRoot; } else { 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; } FrameTransformTree(NameResolver nameResolver); }
FrameTransformTree { 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); FrameTransform targetToRoot = transformToRoot(resolvedTarget); if (sourceToRoot == null && targetToRoot == null) { return null; } if (sourceToRoot == null) { if (targetToRoot.getTargetFrame().equals(resolvedSource)) { return targetToRoot.invert(); } else { return null; } } if (targetToRoot == null) { if (sourceToRoot.getTargetFrame().equals(resolvedTarget)) { return sourceToRoot; } else { 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; } FrameTransformTree(NameResolver nameResolver); void update(geometry_msgs.TransformStamped transformStamped); FrameTransform lookUp(GraphName source); FrameTransform get(String source); FrameTransform lookUp(GraphName source, Time time); FrameTransform get(String source, Time time); FrameTransform transform(GraphName source, GraphName target); FrameTransform transform(String source, String target); }
FrameTransformTree { 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); FrameTransform targetToRoot = transformToRoot(resolvedTarget); if (sourceToRoot == null && targetToRoot == null) { return null; } if (sourceToRoot == null) { if (targetToRoot.getTargetFrame().equals(resolvedSource)) { return targetToRoot.invert(); } else { return null; } } if (targetToRoot == null) { if (sourceToRoot.getTargetFrame().equals(resolvedTarget)) { return sourceToRoot; } else { 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; } FrameTransformTree(NameResolver nameResolver); void update(geometry_msgs.TransformStamped transformStamped); FrameTransform lookUp(GraphName source); FrameTransform get(String source); FrameTransform lookUp(GraphName source, Time time); FrameTransform get(String source, Time time); FrameTransform transform(GraphName source, GraphName target); FrameTransform transform(String source, String target);  }