diff --git a/rosjava/src/main/java/org/ros/internal/transport/queue/LazyMessage.java b/rosjava/src/main/java/org/ros/internal/transport/queue/LazyMessage.java index 789e45c35bce2b30a5db7ff15df88073358afd5e..1be527b950d36f0ed480a239a1151192e647fdba 100644 --- a/rosjava/src/main/java/org/ros/internal/transport/queue/LazyMessage.java +++ b/rosjava/src/main/java/org/ros/internal/transport/queue/LazyMessage.java @@ -50,7 +50,6 @@ public class LazyMessage<T> { this.buffer = buffer; this.deserializer = deserializer; mutex = new Object(); - message = null; } @VisibleForTesting 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 eac23953872ba2be067f76074048cb8edd482fa3..d258da88c29e10daee05ad2eb8d5e414d9110b90 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 @@ -29,6 +29,14 @@ public class FrameTransform { private final GraphName source; 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) { this.transform = transform; this.source = source; 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 f4e33d618dd6e7733eaecb85138a048af8683c2d..fdbe0c814281b54e837ce7ac49ea8c3267314dad 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 @@ -40,10 +40,10 @@ public class FrameTransformTree { private final NameResolver nameResolver; /** - * A {@link Map} from child frame ID to the child frame's most recent - * transform. + * A {@link Map} of the most recent {@link LazyFrameTransform} by source + * frame. */ - private final Map<GraphName, geometry_msgs.TransformStamped> transforms; + private final Map<GraphName, LazyFrameTransform> transforms; public FrameTransformTree(NameResolver nameResolver) { this.nameResolver = nameResolver; @@ -53,26 +53,20 @@ public class FrameTransformTree { /** * Updates the transform tree with the provided transform. * - * @param transform + * @param transformStamped * the transform to update */ - public void updateTransform(geometry_msgs.TransformStamped transform) { - transforms.put(nameResolver.resolve(transform.getChildFrameId()), transform); + public void updateTransform(geometry_msgs.TransformStamped transformStamped) { + GraphName source = nameResolver.resolve(transformStamped.getChildFrameId()); + transforms.put(source, new LazyFrameTransform(transformStamped)); } - public GraphName findRootFrame(GraphName sourceFrame) { - GraphName targetFrame = sourceFrame; - while (true) { - TransformStamped transformStamped = getLatestTransform(targetFrame); - if (transformStamped == null) { - return targetFrame; - } - targetFrame = nameResolver.resolve(transformStamped.getHeader().getFrameId()); + private FrameTransform getLatestTransform(GraphName frame) { + LazyFrameTransform lazyFrameTransform = transforms.get(nameResolver.resolve(frame)); + if (lazyFrameTransform != null) { + return lazyFrameTransform.get(); } - } - - private geometry_msgs.TransformStamped getLatestTransform(GraphName frame) { - return transforms.get(nameResolver.resolve(frame)); + return null; } /** @@ -86,26 +80,25 @@ public class FrameTransformTree { public boolean canTransform(GraphName sourceFrame, GraphName targetFrame) { Preconditions.checkNotNull(sourceFrame); Preconditions.checkNotNull(targetFrame); - FrameTransform sourceFrameTransform = newFrameTransformToRoot(sourceFrame); - FrameTransform targetFrameTransform = newFrameTransformToRoot(targetFrame); - return sourceFrameTransform.getTargetFrame().equals(targetFrameTransform.getTargetFrame()); + FrameTransform source = newFrameTransformToRoot(sourceFrame); + FrameTransform target = newFrameTransformToRoot(targetFrame); + return source.getTargetFrame().equals(target.getTargetFrame()); } /** * @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) { Preconditions.checkNotNull(sourceFrame); Preconditions.checkNotNull(targetFrame); - Preconditions.checkArgument(canTransform(sourceFrame, targetFrame), - String.format("Cannot transform between %s and %s.", sourceFrame, targetFrame)); - FrameTransform sourceFrameTransform = newFrameTransformToRoot(sourceFrame); - FrameTransform targetFrameTransform = newFrameTransformToRoot(targetFrame); - Transform transform = - targetFrameTransform.getTransform().invert().multiply(sourceFrameTransform.getTransform()); - return new FrameTransform(transform, sourceFrameTransform.getSourceFrame(), - targetFrameTransform.getSourceFrame()); + 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()); + } + return null; } /** @@ -115,15 +108,16 @@ public class FrameTransformTree { */ private FrameTransform newFrameTransformToRoot(GraphName frame) { GraphName sourceFrame = nameResolver.resolve(frame); - Transform result = Transform.newIdentityTransform(); - GraphName targetFrame = sourceFrame; + FrameTransform result = + new FrameTransform(Transform.newIdentityTransform(), sourceFrame, sourceFrame); while (true) { - TransformStamped transformStamped = getLatestTransform(targetFrame); - if (transformStamped == null) { - return new FrameTransform(result, sourceFrame, targetFrame); + FrameTransform parent = getLatestTransform(result.getTargetFrame()); + if (parent == null) { + return result; } - result = Transform.newFromTransformMessage(transformStamped.getTransform()).multiply(result); - targetFrame = nameResolver.resolve(transformStamped.getHeader().getFrameId()); + Transform transform = result.getTransform().multiply(parent.getTransform()); + GraphName targetFrame = nameResolver.resolve(parent.getSourceFrame()); + result = new FrameTransform(transform, sourceFrame, targetFrame); } } } 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 new file mode 100644 index 0000000000000000000000000000000000000000..a41b2dc7baa59af964a8cd0753192fa3f385ba63 --- /dev/null +++ b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/LazyFrameTransform.java @@ -0,0 +1,54 @@ +/* + * 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