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
index 3867329ae842722e4caff3a11c756911b9fe113e..b44eb4199d5494657c2bf662b760dd6ac91e895c 100644
--- 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 class TransformBenchmark extends AbstractNodeMain {
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
new file mode 100644
index 0000000000000000000000000000000000000000..8e7efc3a8a671a2cda13fac33f569bcd002da01d
--- /dev/null
+++ 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
index be9616185adfbec71c91d9c9cd9a71a3da2e7162..da578513347cc616b916ca65f0cb7491c9f33c9d 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
@@ -19,19 +19,18 @@ package org.ros.rosjava_geometry;
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 class FrameTransform {
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 class FrameTransform {
* 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 class FrameTransform {
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
index 4f25095906d00e966be70292cf36f0e298d2f98f..7ce4eca55e5b22e517565198f414b6af08024985 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
@@ -23,9 +23,6 @@ import com.google.common.collect.Maps;
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;
/**
@@ -33,7 +30,7 @@ import java.util.Map;
* <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)
*/
@@ -41,7 +38,6 @@ public class FrameTransformTree {
private static final int TRANSFORM_QUEUE_CAPACITY = 16;
- private final NameResolver nameResolver;
private final Object mutex;
/**
@@ -49,11 +45,9 @@ 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<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 class FrameTransformTree {
* 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 @@ public class FrameTransformTree {
}
/**
- * @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 class FrameTransformTree {
* @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,32 +168,30 @@ public class FrameTransformTree {
}
/**
- * @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 {
@@ -209,7 +199,7 @@ public class FrameTransformTree {
}
}
if (targetToRoot == null) {
- if (sourceToRoot.getTargetFrame().equals(resolvedTarget)) {
+ if (sourceToRoot.getTargetFrame().equals(target)) {
// resolvedTarget is root.
return sourceToRoot;
} else {
@@ -221,29 +211,29 @@ public class FrameTransformTree {
// 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 @@ public class FrameTransformTree {
}
// 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());
}
}
}
diff --git a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Transform.java b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Transform.java
index 3d1727deb044c56314a59755b5243978822bdc8c..85f09154efa91c34a10ee3456de49541d9258b2f 100644
--- a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Transform.java
+++ b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Transform.java
@@ -19,11 +19,10 @@ 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.
- *
+ *
* @author damonkohler@google.com (Damon Kohler)
* @author moesenle@google.com (Lorenz Moesenlechner)
*/
@@ -73,7 +72,7 @@ public class Transform {
/**
* Apply another {@link Transform} to this {@link Transform}.
- *
+ *
* @param other
* the {@link Transform} to apply to this {@link Transform}
* @return the resulting {@link Transform}
@@ -133,7 +132,7 @@ public class Transform {
return result;
}
- public geometry_msgs.PoseStamped toPoseStampedMessage(GraphName frame, Time stamp,
+ public geometry_msgs.PoseStamped toPoseStampedMessage(FrameName frame, Time stamp,
geometry_msgs.PoseStamped result) {
result.getHeader().setFrameId(frame.toString());
result.getHeader().setStamp(stamp);