Changes FrameTransformTree to use GraphName instead of FrameName but …

…still support tf2.

rosjava_benchmarks/src/main/java/org/ros/rosjava_benchmarks/TransformBenchmark.java

@@ -22,7 +22,6 @@
 import org.ros.message.Duration;
 import org.ros.message.Time;
 import org.ros.namespace.GraphName;
-import org.ros.namespace.NameResolver;
 import org.ros.node.AbstractNodeMain;
 import org.ros.node.ConnectedNode;
 import org.ros.node.topic.Publisher;

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

@@ -16,9 +16,10 @@
 
 package org.ros.rosjava_geometry;
 
-import com.google.common.base.Preconditions;
-
 import org.ros.message.Time;
+import org.ros.namespace.GraphName;
+
+import com.google.common.base.Preconditions;
 
 /**
  * Describes a {@link Transform} from data in the source frame to data in the
@@ -29,8 +30,8 @@
 public class FrameTransform {
 
   private final Transform transform;
-  private final FrameName source;
-  private final FrameName target;
+  private final GraphName source;
+  private final GraphName target;
   private final Time time;
 
   public static FrameTransform fromTransformStampedMessage(
@@ -39,7 +40,7 @@ public static FrameTransform fromTransformStampedMessage(
     String target = transformStamped.getHeader().getFrameId();
     String source = transformStamped.getChildFrameId();
     Time stamp = transformStamped.getHeader().getStamp();
-    return new FrameTransform(transform, FrameName.of(source), FrameName.of(target), stamp);
+    return new FrameTransform(transform, GraphName.of(source), GraphName.of(target), stamp);
   }
 
   /**
@@ -56,25 +57,25 @@ public static FrameTransform fromTransformStampedMessage(
    *          the time associated with this {@link FrameTransform}, can be
    *          {@null}
    */
-  public FrameTransform(Transform transform, FrameName source, FrameName target, Time time) {
+  public FrameTransform(Transform transform, GraphName source, GraphName target, Time time) {
     Preconditions.checkNotNull(transform);
     Preconditions.checkNotNull(source);
     Preconditions.checkNotNull(target);
     this.transform = transform;
-    this.source = source;
-    this.target = target;
+    this.source = source.toRelative();
+    this.target = target.toRelative();
     this.time = time;
   }
 
   public Transform getTransform() {
     return transform;
   }
 
-  public FrameName getSourceFrame() {
+  public GraphName getSourceFrame() {
     return source;
   }
 
-  public FrameName getTargetFrame() {
+  public GraphName getTargetFrame() {
     return target;
   }
 

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

@@ -23,6 +23,8 @@
 import geometry_msgs.TransformStamped;
 import org.ros.concurrent.CircularBlockingDeque;
 import org.ros.message.Time;
+import org.ros.namespace.GraphName;
+
 import java.util.Map;
 
 /**
@@ -45,7 +47,7 @@ public class FrameTransformTree {
    * frame. Lookups by target frame or by the pair of source and target are both
    * unnecessary because every frame can only have exactly one target.
    */
-  private final Map<FrameName, CircularBlockingDeque<LazyFrameTransform>> transforms;
+  private final Map<GraphName, CircularBlockingDeque<LazyFrameTransform>> transforms;
 
   public FrameTransformTree() {
     mutex = new Object();
@@ -65,26 +67,28 @@ public FrameTransformTree() {
    */
   public void update(geometry_msgs.TransformStamped transformStamped) {
     Preconditions.checkNotNull(transformStamped);
-    FrameName transformName = FrameName.of(transformStamped.getChildFrameId());
+    GraphName source = GraphName.of(transformStamped.getChildFrameId());
     LazyFrameTransform lazyFrameTransform = new LazyFrameTransform(transformStamped);
-    add(transformName, lazyFrameTransform);
+    add(source, lazyFrameTransform);
   }
 
   @VisibleForTesting
   void update(FrameTransform frameTransform) {
     Preconditions.checkNotNull(frameTransform);
-    FrameName source = frameTransform.getSourceFrame();
+    GraphName source = frameTransform.getSourceFrame();
     LazyFrameTransform lazyFrameTransform = new LazyFrameTransform(frameTransform);
     add(source, lazyFrameTransform);
   }
 
-  private void add(FrameName source, LazyFrameTransform lazyFrameTransform) {
-    if (!transforms.containsKey(source)) {
-      transforms.put(source, new CircularBlockingDeque<LazyFrameTransform>(
+  private void add(GraphName source, LazyFrameTransform lazyFrameTransform) {
+    // This adds support for tf2 while maintaining backward compatibility with tf.
+    GraphName relativeSource = source.toRelative();
+    if (!transforms.containsKey(relativeSource)) {
+      transforms.put(relativeSource, new CircularBlockingDeque<LazyFrameTransform>(
           TRANSFORM_QUEUE_CAPACITY));
     }
     synchronized (mutex) {
-      transforms.get(source).addFirst(lazyFrameTransform);
+      transforms.get(relativeSource).addFirst(lazyFrameTransform);
     }
   }
 
@@ -96,12 +100,13 @@ private void add(FrameName source, LazyFrameTransform lazyFrameTransform) {
    * @return the most recent {@link FrameTransform} for {@code source} or
    *         {@code null} if no transform for {@code source} is available
    */
-  public FrameTransform lookUp(FrameName source) {
+  public FrameTransform lookUp(GraphName source) {
     Preconditions.checkNotNull(source);
-    return getLatest(source);
+    // This adds support for tf2 while maintaining backward compatibility with tf.
+    return getLatest(source.toRelative());
   }
 
-  private FrameTransform getLatest(FrameName source) {
+  private FrameTransform getLatest(GraphName source) {
     CircularBlockingDeque<LazyFrameTransform> deque = transforms.get(source);
     if (deque == null) {
       return null;
@@ -114,11 +119,11 @@ private FrameTransform getLatest(FrameName source) {
   }
 
   /**
-   * @see #lookUp(FrameName)
+   * @see #lookUp(GraphName)
    */
   public FrameTransform get(String source) {
     Preconditions.checkNotNull(source);
-    return lookUp(FrameName.of(source));
+    return lookUp(GraphName.of(source));
   }
 
   /**
@@ -133,14 +138,14 @@ public FrameTransform get(String source) {
    * @return the most recent {@link FrameTransform} for {@code source} or
    *         {@code null} if no transform for {@code source} is available
    */
-  public FrameTransform lookUp(FrameName source, Time time) {
+  public FrameTransform lookUp(GraphName source, Time time) {
     Preconditions.checkNotNull(source);
     Preconditions.checkNotNull(time);
     return get(source, time);
   }
 
   // TODO(damonkohler): Use an efficient search.
-  private FrameTransform get(FrameName source, Time time) {
+  private FrameTransform get(GraphName source, Time time) {
     CircularBlockingDeque<LazyFrameTransform> deque = transforms.get(source);
     if (deque == null) {
       return null;
@@ -168,71 +173,74 @@ private FrameTransform get(FrameName source, Time time) {
   }
 
   /**
-   * @see #lookUp(FrameName, Time)
+   * @see #lookUp(GraphName, Time)
    */
   public FrameTransform get(String source, Time time) {
     Preconditions.checkNotNull(source);
-    return lookUp(FrameName.of(source), time);
+    return lookUp(GraphName.of(source), time);
   }
 
   /**
    * @return the {@link FrameTransform} from source the frame to the target
    *         frame, or {@code null} if no {@link FrameTransform} could be found
    */
-  public FrameTransform transform(FrameName source, FrameName target) {
+  public FrameTransform transform(GraphName source, GraphName target) {
     Preconditions.checkNotNull(source);
     Preconditions.checkNotNull(target);
-    if (source.equals(target)) {
-      return new FrameTransform(Transform.identity(), source, target, null);
+    // This adds support for tf2 while maintaining backward compatibility with tf.
+    GraphName relativeSource = source.toRelative();
+    GraphName relativeTarget = target.toRelative();
+    if (relativeSource.equals(relativeTarget)) {
+      return new FrameTransform(Transform.identity(), relativeSource, relativeTarget, null);
     }
-    FrameTransform sourceToRoot = transformToRoot(source);
-    FrameTransform targetToRoot = transformToRoot(target);
+    FrameTransform sourceToRoot = transformToRoot(relativeSource);
+    FrameTransform targetToRoot = transformToRoot(relativeTarget);
     if (sourceToRoot == null && targetToRoot == null) {
       return null;
     }
     if (sourceToRoot == null) {
-      if (targetToRoot.getTargetFrame().equals(source)) {
-        // resolvedSource is root.
+      if (targetToRoot.getTargetFrame().equals(relativeSource)) {
+        // relativeSource is root.
         return targetToRoot.invert();
       } else {
         return null;
       }
     }
     if (targetToRoot == null) {
-      if (sourceToRoot.getTargetFrame().equals(target)) {
-        // resolvedTarget is root.
+      if (sourceToRoot.getTargetFrame().equals(relativeTarget)) {
+        // relativeTarget is root.
         return sourceToRoot;
       } else {
         return null;
       }
     }
     if (sourceToRoot.getTargetFrame().equals(targetToRoot.getTargetFrame())) {
-      // Neither resolvedSource nor resolvedTarget is root and both share the
+      // Neither relativeSource nor relativeTarget is root and both share the
       // same root.
       Transform transform =
           targetToRoot.getTransform().invert().multiply(sourceToRoot.getTransform());
-      return new FrameTransform(transform, source, target, sourceToRoot.getTime());
+      return new FrameTransform(transform, relativeSource, relativeTarget, sourceToRoot.getTime());
     }
     // No known transform.
     return null;
   }
 
   /**
-   * @see #transform(FrameName, FrameName)
+   * @see #transform(GraphName, GraphName)
    */
   public FrameTransform transform(String source, String target) {
     Preconditions.checkNotNull(source);
     Preconditions.checkNotNull(target);
-    return transform(FrameName.of(source), FrameName.of(target));
+    return transform(GraphName.of(source), GraphName.of(target));
   }
 
   /**
    * @param source
-   *          the resolved source frame
+   *          the source frame
    * @return the {@link Transform} from {@code source} to root
    */
   @VisibleForTesting
-  FrameTransform transformToRoot(FrameName source) {
+  FrameTransform transformToRoot(GraphName source) {
     FrameTransform result = getLatest(source);
     if (result == null) {
       return null;
@@ -244,7 +252,7 @@ FrameTransform transformToRoot(FrameName source) {
       }
       // Now resultToParent.getSourceFrame() == result.getTargetFrame()
       Transform transform = resultToParent.getTransform().multiply(result.getTransform());
-      FrameName target = resultToParent.getTargetFrame();
+      GraphName target = resultToParent.getTargetFrame();
       result = new FrameTransform(transform, source, target, result.getTime());
     }
   }

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

@@ -16,9 +16,8 @@
 
 package org.ros.rosjava_geometry;
 
-import com.google.common.annotations.VisibleForTesting;
-
 import org.ros.message.Time;
+import org.ros.namespace.GraphName;
 
 /**
  * A transformation in terms of translation, rotation, and scale.
@@ -132,7 +131,7 @@ public geometry_msgs.Pose toPoseMessage(geometry_msgs.Pose result) {
     return result;
   }
 
-  public geometry_msgs.PoseStamped toPoseStampedMessage(FrameName frame, Time stamp,
+  public geometry_msgs.PoseStamped toPoseStampedMessage(GraphName frame, Time stamp,
       geometry_msgs.PoseStamped result) {
     result.getHeader().setFrameId(frame.toString());
     result.getHeader().setStamp(stamp);
@@ -145,13 +144,11 @@ public boolean almostEquals(Transform other, double epsilon) {
         && rotationAndScale.almostEquals(other.rotationAndScale, epsilon);
   }
 
-  @VisibleForTesting
-  Vector3 getTranslation() {
+  public Vector3 getTranslation() {
     return translation;
   }
 
-  @VisibleForTesting
-  Quaternion getRotationAndScale() {
+  public Quaternion getRotationAndScale() {
     return rotationAndScale;
   }