rosjava · stonier · Aug 29, 2013 · Aug 28, 2013
diff --git a/rosjava_benchmarks/src/main/java/org/ros/rosjava_benchmarks/TransformBenchmark.java b/rosjava_benchmarks/src/main/java/org/ros/rosjava_benchmarks/TransformBenchmark.java
@@ -59,7 +59,7 @@ public void onStart(final ConnectedNode connectedNode) {
         connectedNode.getTopicMessageFactory().newFromType(geometry_msgs.TransformStamped._TYPE);
     turtle2.getHeader().setFrameId("world");
     turtle2.setChildFrameId("turtle2");
-    final FrameTransformTree frameTransformTree = new FrameTransformTree(NameResolver.newRoot());
+    final FrameTransformTree frameTransformTree = new FrameTransformTree();
     connectedNode.executeCancellableLoop(new CancellableLoop() {
       @Override
       protected void loop() throws InterruptedException {

diff --git a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameName.java b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameName.java
@@ -0,0 +1,72 @@
+/*
+ * Copyright (C) 2013 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;
+
+import java.lang.String;
+
+/**
+ * Provides a simple wrapper around strings to represent
+ * frame names with backwards compatibility (pre ros hydro)
+ * catered for by ignoring graph name style leading slashes.
+ *
+ * @author d.stonier@gmail.com (Daniel Stonier)
+ */
+public class FrameName {
+    private static final String LEGACY_SEPARATOR = "/";
+    private String name;
+
+    public static FrameName of(String name) {
+        return new FrameName(name);
+    }
+
+    private FrameName(String name) {
+        this.name = stripLeadingSlash(name);
+    }
+
+    /**
+     * TF2 names (from hydro on) do not make use of leading slashes.
+     */
+    private static String stripLeadingSlash(String name) {
+        return name.replaceFirst("^/", "");
+    }
+
+    public String toString() {
+        return name;
+    }
+
+    public int hashCode() {
+        return name.hashCode();
+    }
+
+    public boolean equals(Object obj) {
+        if (this == obj)
+            return true;
+        if (obj == null)
+            return false;
+        if (getClass() != obj.getClass())
+            return false;
+        FrameName other = (FrameName) obj;
+        if (name == null) {
+            if (other.name != null)
+                return false;
+        } else if (!name.equals(other.name)) {
+            return false;
+        }
+        return true;
+    }
+
+}
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
@@ -19,19 +19,18 @@
 import com.google.common.base.Preconditions;
 
 import org.ros.message.Time;
-import org.ros.namespace.GraphName;
 
 /**
  * Describes a {@link Transform} from data in the source frame to data in the
  * target frame at a specified {@link Time}.
- * 
+ *
  * @author damonkohler@google.com (Damon Kohler)
  */
 public class FrameTransform {
 
   private final Transform transform;
-  private final GraphName source;
-  private final GraphName target;
+  private final FrameName source;
+  private final FrameName target;
   private final Time time;
 
   public static FrameTransform fromTransformStampedMessage(
@@ -40,12 +39,12 @@ public static FrameTransform fromTransformStampedMessage(
     String target = transformStamped.getHeader().getFrameId();
     String source = transformStamped.getChildFrameId();
     Time stamp = transformStamped.getHeader().getStamp();
-    return new FrameTransform(transform, GraphName.of(source), GraphName.of(target), stamp);
+    return new FrameTransform(transform, FrameName.of(source), FrameName.of(target), stamp);
   }
 
   /**
    * Allocates a new {@link FrameTransform}.
-   * 
+   *
    * @param transform
    *          the {@link Transform} that transforms data in the {@code source}
    *          frame to data in the {@code target} frame
@@ -57,7 +56,7 @@ public static FrameTransform fromTransformStampedMessage(
    *          the time associated with this {@link FrameTransform}, can be
    *          {@null}
    */
-  public FrameTransform(Transform transform, GraphName source, GraphName target, Time time) {
+  public FrameTransform(Transform transform, FrameName source, FrameName target, Time time) {
     Preconditions.checkNotNull(transform);
     Preconditions.checkNotNull(source);
     Preconditions.checkNotNull(target);
@@ -71,11 +70,11 @@ public Transform getTransform() {
     return transform;
   }
 
-  public GraphName getSourceFrame() {
+  public FrameName getSourceFrame() {
     return source;
   }
 
-  public GraphName getTargetFrame() {
+  public FrameName getTargetFrame() {
     return target;
   }
 

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
@@ -23,37 +23,31 @@
 import geometry_msgs.TransformStamped;
 import org.ros.concurrent.CircularBlockingDeque;
 import org.ros.message.Time;
-import org.ros.namespace.GraphName;
-import org.ros.namespace.NameResolver;
-
 import java.util.Map;
 
 /**
  * A tree of {@link FrameTransform}s.
  * <p>
  * {@link FrameTransformTree} does not currently support time travel. Lookups
  * always use the newest {@link TransformStamped}.
- * 
+ *
  * @author damonkohler@google.com (Damon Kohler)
  * @author moesenle@google.com (Lorenz Moesenlechner)
  */
 public class FrameTransformTree {
 
   private static final int TRANSFORM_QUEUE_CAPACITY = 16;
 
-  private final NameResolver nameResolver;
   private final Object mutex;
 
   /**
    * A {@link Map} of the most recent {@link LazyFrameTransform} by source
    * 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<GraphName, CircularBlockingDeque<LazyFrameTransform>> transforms;
+  private final Map<FrameName, CircularBlockingDeque<LazyFrameTransform>> transforms;
 
-  public FrameTransformTree(NameResolver nameResolver) {
-    Preconditions.checkNotNull(nameResolver);
-    this.nameResolver = nameResolver;
+  public FrameTransformTree() {
     mutex = new Object();
     transforms = Maps.newConcurrentMap();
   }
@@ -65,51 +59,50 @@ public FrameTransformTree(NameResolver nameResolver) {
    * Note that the tree is updated lazily. Modifications to the provided
    * {@link geometry_msgs.TransformStamped} message may cause unpredictable
    * results.
-   * 
+   *
    * @param transformStamped
    *          the {@link geometry_msgs.TransformStamped} message to update with
    */
   public void update(geometry_msgs.TransformStamped transformStamped) {
     Preconditions.checkNotNull(transformStamped);
-    GraphName resolvedSource = nameResolver.resolve(transformStamped.getChildFrameId());
+    FrameName transformName = FrameName.of(transformStamped.getChildFrameId());
     LazyFrameTransform lazyFrameTransform = new LazyFrameTransform(transformStamped);
-    add(resolvedSource, lazyFrameTransform);
+    add(transformName, lazyFrameTransform);
   }
 
   @VisibleForTesting
   void update(FrameTransform frameTransform) {
     Preconditions.checkNotNull(frameTransform);
-    GraphName resolvedSource = frameTransform.getSourceFrame();
+    FrameName source = frameTransform.getSourceFrame();
     LazyFrameTransform lazyFrameTransform = new LazyFrameTransform(frameTransform);
-    add(resolvedSource, lazyFrameTransform);
+    add(source, lazyFrameTransform);
   }
 
-  private void add(GraphName resolvedSource, LazyFrameTransform lazyFrameTransform) {
-    if (!transforms.containsKey(resolvedSource)) {
-      transforms.put(resolvedSource, new CircularBlockingDeque<LazyFrameTransform>(
+  private void add(FrameName source, LazyFrameTransform lazyFrameTransform) {
+    if (!transforms.containsKey(source)) {
+      transforms.put(source, new CircularBlockingDeque<LazyFrameTransform>(
           TRANSFORM_QUEUE_CAPACITY));
     }
     synchronized (mutex) {
-      transforms.get(resolvedSource).addFirst(lazyFrameTransform);
+      transforms.get(source).addFirst(lazyFrameTransform);
     }
   }
 
   /**
    * Returns the most recent {@link FrameTransform} for target {@code source}.
-   * 
+   *
    * @param source
    *          the frame to look up
    * @return the most recent {@link FrameTransform} for {@code source} or
    *         {@code null} if no transform for {@code source} is available
    */
-  public FrameTransform lookUp(GraphName source) {
+  public FrameTransform lookUp(FrameName source) {
     Preconditions.checkNotNull(source);
-    GraphName resolvedSource = nameResolver.resolve(source);
-    return getLatest(resolvedSource);
+    return getLatest(source);
   }
 
-  private FrameTransform getLatest(GraphName resolvedSource) {
-    CircularBlockingDeque<LazyFrameTransform> deque = transforms.get(resolvedSource);
+  private FrameTransform getLatest(FrameName source) {
+    CircularBlockingDeque<LazyFrameTransform> deque = transforms.get(source);
     if (deque == null) {
       return null;
     }
@@ -121,17 +114,17 @@ private FrameTransform getLatest(GraphName resolvedSource) {
   }
 
   /**
-   * @see #lookUp(GraphName)
+   * @see #lookUp(FrameName)
    */
   public FrameTransform get(String source) {
     Preconditions.checkNotNull(source);
-    return lookUp(GraphName.of(source));
+    return lookUp(FrameName.of(source));
   }
 
   /**
    * Returns the {@link FrameTransform} for {@code source} closest to
    * {@code time}.
-   * 
+   *
    * @param source
    *          the frame to look up
    * @param time
@@ -140,16 +133,15 @@ 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(GraphName source, Time time) {
+  public FrameTransform lookUp(FrameName source, Time time) {
     Preconditions.checkNotNull(source);
     Preconditions.checkNotNull(time);
-    GraphName resolvedSource = nameResolver.resolve(source);
-    return get(resolvedSource, time);
+    return get(source, time);
   }
 
   // TODO(damonkohler): Use an efficient search.
-  private FrameTransform get(GraphName resolvedSource, Time time) {
-    CircularBlockingDeque<LazyFrameTransform> deque = transforms.get(resolvedSource);
+  private FrameTransform get(FrameName source, Time time) {
+    CircularBlockingDeque<LazyFrameTransform> deque = transforms.get(source);
     if (deque == null) {
       return null;
     }
@@ -176,40 +168,38 @@ private FrameTransform get(GraphName resolvedSource, Time time) {
   }
 
   /**
-   * @see #lookUp(GraphName, Time)
+   * @see #lookUp(FrameName, Time)
    */
   public FrameTransform get(String source, Time time) {
     Preconditions.checkNotNull(source);
-    return lookUp(GraphName.of(source), time);
+    return lookUp(FrameName.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(GraphName source, GraphName target) {
+  public FrameTransform transform(FrameName source, FrameName target) {
     Preconditions.checkNotNull(source);
     Preconditions.checkNotNull(target);
-    GraphName resolvedSource = nameResolver.resolve(source);
-    GraphName resolvedTarget = nameResolver.resolve(target);
-    if (resolvedSource.equals(resolvedTarget)) {
-      return new FrameTransform(Transform.identity(), resolvedSource, resolvedTarget, null);
+    if (source.equals(target)) {
+      return new FrameTransform(Transform.identity(), source, target, null);
     }
-    FrameTransform sourceToRoot = transformToRoot(resolvedSource);
-    FrameTransform targetToRoot = transformToRoot(resolvedTarget);
+    FrameTransform sourceToRoot = transformToRoot(source);
+    FrameTransform targetToRoot = transformToRoot(target);
     if (sourceToRoot == null && targetToRoot == null) {
       return null;
     }
     if (sourceToRoot == null) {
-      if (targetToRoot.getTargetFrame().equals(resolvedSource)) {
+      if (targetToRoot.getTargetFrame().equals(source)) {
         // resolvedSource is root.
         return targetToRoot.invert();
       } else {
         return null;
       }
     }
     if (targetToRoot == null) {
-      if (sourceToRoot.getTargetFrame().equals(resolvedTarget)) {
+      if (sourceToRoot.getTargetFrame().equals(target)) {
         // resolvedTarget is root.
         return sourceToRoot;
       } else {
@@ -221,29 +211,29 @@ public FrameTransform transform(GraphName source, GraphName target) {
       // same root.
       Transform transform =
           targetToRoot.getTransform().invert().multiply(sourceToRoot.getTransform());
-      return new FrameTransform(transform, resolvedSource, resolvedTarget, sourceToRoot.getTime());
+      return new FrameTransform(transform, source, target, sourceToRoot.getTime());
     }
     // No known transform.
     return null;
   }
 
   /**
-   * @see #transform(GraphName, GraphName)
+   * @see #transform(FrameName, FrameName)
    */
   public FrameTransform transform(String source, String target) {
     Preconditions.checkNotNull(source);
     Preconditions.checkNotNull(target);
-    return transform(GraphName.of(source), GraphName.of(target));
+    return transform(FrameName.of(source), FrameName.of(target));
   }
 
   /**
-   * @param resolvedSource
+   * @param source
    *          the resolved source frame
    * @return the {@link Transform} from {@code source} to root
    */
   @VisibleForTesting
-  FrameTransform transformToRoot(GraphName resolvedSource) {
-    FrameTransform result = getLatest(resolvedSource);
+  FrameTransform transformToRoot(FrameName source) {
+    FrameTransform result = getLatest(source);
     if (result == null) {
       return null;
     }
@@ -254,8 +244,8 @@ FrameTransform transformToRoot(GraphName resolvedSource) {
       }
       // Now resultToParent.getSourceFrame() == result.getTargetFrame()
       Transform transform = resultToParent.getTransform().multiply(result.getTransform());
-      GraphName resolvedTarget = resultToParent.getTargetFrame();
-      result = new FrameTransform(transform, resolvedSource, resolvedTarget, result.getTime());
+      FrameName target = resultToParent.getTargetFrame();
+      result = new FrameTransform(transform, source, target, result.getTime());
     }
   }
 }