Improves performance of FrameTransformTree.

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

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;

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);
     }
   }
 }

rosjava_geometry/src/main/java/org/ros/rosjava_geometry/LazyFrameTransform.java

+/*
+ * 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