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 3867329ae..b44eb4199 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 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 new file mode 100644 index 000000000..8e7efc3a8 --- /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 be9616185..da5785133 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 @@ 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 index 4f2509590..7ce4eca55 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 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 @@ *

* {@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> transforms; + private final Map> 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( + private void add(FrameName source, LazyFrameTransform lazyFrameTransform) { + if (!transforms.containsKey(source)) { + transforms.put(source, new CircularBlockingDeque( 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 deque = transforms.get(resolvedSource); + private FrameTransform getLatest(FrameName source) { + CircularBlockingDeque 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 deque = transforms.get(resolvedSource); + private FrameTransform get(FrameName source, Time time) { + CircularBlockingDeque deque = transforms.get(source); if (deque == null) { return null; } @@ -176,32 +168,30 @@ 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 { @@ -209,7 +199,7 @@ public FrameTransform transform(GraphName source, GraphName target) { } } 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()); } } } 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 3d1727deb..85f09154e 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 @@ 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 Transform(Vector3 translation, Quaternion rotation) { /** * 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 geometry_msgs.Pose toPoseMessage(geometry_msgs.Pose result) { 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);