@Test public void testTransformBazToRoot() { updateFrameTransformTree(); checkBazToFooTransform(frameTransformTree.transformToRoot(GraphName .of("baz"))); }
@VisibleForTesting FrameTransform transformToRoot(GraphName source) { FrameTransform result = getLatest(source); if (result == null) { return null; } while (true) { FrameTransform resultToParent = lookUp(result.getTargetFrame(), result.getTime()); if (resultToParent == null) { return result; } Transform transform = resultToParent.getTransform().multiply(result.getTransform()); GraphName target = resultToParent.getTargetFrame(); result = new FrameTransform(transform, source, target, result.getTime()); } }
FrameTransformTree { @VisibleForTesting FrameTransform transformToRoot(GraphName source) { FrameTransform result = getLatest(source); if (result == null) { return null; } while (true) { FrameTransform resultToParent = lookUp(result.getTargetFrame(), result.getTime()); if (resultToParent == null) { return result; } Transform transform = resultToParent.getTransform().multiply(result.getTransform()); GraphName target = resultToParent.getTargetFrame(); result = new FrameTransform(transform, source, target, result.getTime()); } } }
FrameTransformTree { @VisibleForTesting FrameTransform transformToRoot(GraphName source) { FrameTransform result = getLatest(source); if (result == null) { return null; } while (true) { FrameTransform resultToParent = lookUp(result.getTargetFrame(), result.getTime()); if (resultToParent == null) { return result; } Transform transform = resultToParent.getTransform().multiply(result.getTransform()); GraphName target = resultToParent.getTargetFrame(); result = new FrameTransform(transform, source, target, result.getTime()); } } FrameTransformTree(); }
FrameTransformTree { @VisibleForTesting FrameTransform transformToRoot(GraphName source) { FrameTransform result = getLatest(source); if (result == null) { return null; } while (true) { FrameTransform resultToParent = lookUp(result.getTargetFrame(), result.getTime()); if (resultToParent == null) { return result; } Transform transform = resultToParent.getTransform().multiply(result.getTransform()); GraphName target = resultToParent.getTargetFrame(); result = new FrameTransform(transform, source, target, result.getTime()); } } FrameTransformTree(); 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 { @VisibleForTesting FrameTransform transformToRoot(GraphName source) { FrameTransform result = getLatest(source); if (result == null) { return null; } while (true) { FrameTransform resultToParent = lookUp(result.getTargetFrame(), result.getTime()); if (resultToParent == null) { return result; } Transform transform = resultToParent.getTransform().multiply(result.getTransform()); GraphName target = resultToParent.getTargetFrame(); result = new FrameTransform(transform, source, target, result.getTime()); } } FrameTransformTree(); 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);  }