Skip to content
Snippets Groups Projects
Commit 3b3c5f1d authored by Damon Kohler's avatar Damon Kohler
Browse files

Improves performance of FrameTransformTree.

parent 25969881
No related branches found
No related tags found
No related merge requests found
...@@ -50,7 +50,6 @@ public class LazyMessage<T> { ...@@ -50,7 +50,6 @@ public class LazyMessage<T> {
this.buffer = buffer; this.buffer = buffer;
this.deserializer = deserializer; this.deserializer = deserializer;
mutex = new Object(); mutex = new Object();
message = null;
} }
@VisibleForTesting @VisibleForTesting
......
...@@ -29,6 +29,14 @@ public class FrameTransform { ...@@ -29,6 +29,14 @@ public class FrameTransform {
private final GraphName source; private final GraphName source;
private final GraphName target; private final GraphName target;
public static FrameTransform
fromTransformStamped(geometry_msgs.TransformStamped transformStamped) {
Transform transform = Transform.newFromTransformMessage(transformStamped.getTransform());
String source = transformStamped.getHeader().getFrameId();
String target = transformStamped.getChildFrameId();
return new FrameTransform(transform, GraphName.of(source), GraphName.of(target));
}
public FrameTransform(Transform transform, GraphName source, GraphName target) { public FrameTransform(Transform transform, GraphName source, GraphName target) {
this.transform = transform; this.transform = transform;
this.source = source; this.source = source;
......
...@@ -40,10 +40,10 @@ public class FrameTransformTree { ...@@ -40,10 +40,10 @@ public class FrameTransformTree {
private final NameResolver nameResolver; private final NameResolver nameResolver;
/** /**
* A {@link Map} from child frame ID to the child frame's most recent * A {@link Map} of the most recent {@link LazyFrameTransform} by source
* transform. * frame.
*/ */
private final Map<GraphName, geometry_msgs.TransformStamped> transforms; private final Map<GraphName, LazyFrameTransform> transforms;
public FrameTransformTree(NameResolver nameResolver) { public FrameTransformTree(NameResolver nameResolver) {
this.nameResolver = nameResolver; this.nameResolver = nameResolver;
...@@ -53,26 +53,20 @@ public class FrameTransformTree { ...@@ -53,26 +53,20 @@ public class FrameTransformTree {
/** /**
* Updates the transform tree with the provided transform. * Updates the transform tree with the provided transform.
* *
* @param transform * @param transformStamped
* the transform to update * the transform to update
*/ */
public void updateTransform(geometry_msgs.TransformStamped transform) { public void updateTransform(geometry_msgs.TransformStamped transformStamped) {
transforms.put(nameResolver.resolve(transform.getChildFrameId()), transform); GraphName source = nameResolver.resolve(transformStamped.getChildFrameId());
transforms.put(source, new LazyFrameTransform(transformStamped));
} }
public GraphName findRootFrame(GraphName sourceFrame) { private FrameTransform getLatestTransform(GraphName frame) {
GraphName targetFrame = sourceFrame; LazyFrameTransform lazyFrameTransform = transforms.get(nameResolver.resolve(frame));
while (true) { if (lazyFrameTransform != null) {
TransformStamped transformStamped = getLatestTransform(targetFrame); return lazyFrameTransform.get();
if (transformStamped == null) {
return targetFrame;
}
targetFrame = nameResolver.resolve(transformStamped.getHeader().getFrameId());
} }
} return null;
private geometry_msgs.TransformStamped getLatestTransform(GraphName frame) {
return transforms.get(nameResolver.resolve(frame));
} }
/** /**
...@@ -86,26 +80,25 @@ public class FrameTransformTree { ...@@ -86,26 +80,25 @@ public class FrameTransformTree {
public boolean canTransform(GraphName sourceFrame, GraphName targetFrame) { public boolean canTransform(GraphName sourceFrame, GraphName targetFrame) {
Preconditions.checkNotNull(sourceFrame); Preconditions.checkNotNull(sourceFrame);
Preconditions.checkNotNull(targetFrame); Preconditions.checkNotNull(targetFrame);
FrameTransform sourceFrameTransform = newFrameTransformToRoot(sourceFrame); FrameTransform source = newFrameTransformToRoot(sourceFrame);
FrameTransform targetFrameTransform = newFrameTransformToRoot(targetFrame); FrameTransform target = newFrameTransformToRoot(targetFrame);
return sourceFrameTransform.getTargetFrame().equals(targetFrameTransform.getTargetFrame()); return source.getTargetFrame().equals(target.getTargetFrame());
} }
/** /**
* @return the {@link FrameTransform} from source the frame to the target * @return the {@link FrameTransform} from source the frame to the target
* frame * frame, or {@code null} if no {@link FrameTransform} could be found
*/ */
public FrameTransform newFrameTransform(GraphName sourceFrame, GraphName targetFrame) { public FrameTransform newFrameTransform(GraphName sourceFrame, GraphName targetFrame) {
Preconditions.checkNotNull(sourceFrame); Preconditions.checkNotNull(sourceFrame);
Preconditions.checkNotNull(targetFrame); Preconditions.checkNotNull(targetFrame);
Preconditions.checkArgument(canTransform(sourceFrame, targetFrame), FrameTransform source = newFrameTransformToRoot(sourceFrame);
String.format("Cannot transform between %s and %s.", sourceFrame, targetFrame)); FrameTransform target = newFrameTransformToRoot(targetFrame);
FrameTransform sourceFrameTransform = newFrameTransformToRoot(sourceFrame); if (source.getTargetFrame().equals(target.getTargetFrame())) {
FrameTransform targetFrameTransform = newFrameTransformToRoot(targetFrame); Transform transform = target.getTransform().invert().multiply(source.getTransform());
Transform transform = return new FrameTransform(transform, source.getSourceFrame(), target.getSourceFrame());
targetFrameTransform.getTransform().invert().multiply(sourceFrameTransform.getTransform()); }
return new FrameTransform(transform, sourceFrameTransform.getSourceFrame(), return null;
targetFrameTransform.getSourceFrame());
} }
/** /**
...@@ -115,15 +108,16 @@ public class FrameTransformTree { ...@@ -115,15 +108,16 @@ public class FrameTransformTree {
*/ */
private FrameTransform newFrameTransformToRoot(GraphName frame) { private FrameTransform newFrameTransformToRoot(GraphName frame) {
GraphName sourceFrame = nameResolver.resolve(frame); GraphName sourceFrame = nameResolver.resolve(frame);
Transform result = Transform.newIdentityTransform(); FrameTransform result =
GraphName targetFrame = sourceFrame; new FrameTransform(Transform.newIdentityTransform(), sourceFrame, sourceFrame);
while (true) { while (true) {
TransformStamped transformStamped = getLatestTransform(targetFrame); FrameTransform parent = getLatestTransform(result.getTargetFrame());
if (transformStamped == null) { if (parent == null) {
return new FrameTransform(result, sourceFrame, targetFrame); return result;
} }
result = Transform.newFromTransformMessage(transformStamped.getTransform()).multiply(result); Transform transform = result.getTransform().multiply(parent.getTransform());
targetFrame = nameResolver.resolve(transformStamped.getHeader().getFrameId()); GraphName targetFrame = nameResolver.resolve(parent.getSourceFrame());
result = new FrameTransform(transform, sourceFrame, targetFrame);
} }
} }
} }
/*
* Copyright (C) 2012 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;
/**
* Lazily converts a {@link geometry_msgs.Transform} message to a
* {@link Transform} on the first call to {@link #get()} and caches the result.
* <p>
* This class is thread-safe.
*
* @author damonkohler@google.com (Damon Kohler)
*/
public class LazyFrameTransform {
private final geometry_msgs.TransformStamped message;
private final Object mutex;
private FrameTransform frameTransform;
public LazyFrameTransform(geometry_msgs.TransformStamped message) {
this.message = message;
mutex = new Object();
}
/**
* @return the {@link FrameTransform} for the wrapped
* {@link geometry_msgs.TransformStamped} message
*/
public FrameTransform get() {
if (frameTransform != null) {
return frameTransform;
}
synchronized (mutex) {
if (frameTransform == null) {
frameTransform = FrameTransform.fromTransformStamped(message);
}
}
return frameTransform;
}
}
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment