Skip to content
Snippets Groups Projects
Commit 523496d9 authored by Daniel Stonier's avatar Daniel Stonier
Browse files

migrating hydro frame naming, now just strings -...

parent 08d4d2ec
No related branches found
No related tags found
No related merge requests found
...@@ -59,7 +59,7 @@ public class TransformBenchmark extends AbstractNodeMain { ...@@ -59,7 +59,7 @@ public class TransformBenchmark extends AbstractNodeMain {
connectedNode.getTopicMessageFactory().newFromType(geometry_msgs.TransformStamped._TYPE); connectedNode.getTopicMessageFactory().newFromType(geometry_msgs.TransformStamped._TYPE);
turtle2.getHeader().setFrameId("world"); turtle2.getHeader().setFrameId("world");
turtle2.setChildFrameId("turtle2"); turtle2.setChildFrameId("turtle2");
final FrameTransformTree frameTransformTree = new FrameTransformTree(NameResolver.newRoot()); final FrameTransformTree frameTransformTree = new FrameTransformTree();
connectedNode.executeCancellableLoop(new CancellableLoop() { connectedNode.executeCancellableLoop(new CancellableLoop() {
@Override @Override
protected void loop() throws InterruptedException { protected void loop() throws InterruptedException {
......
/*
* 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;
}
}
...@@ -19,7 +19,6 @@ package org.ros.rosjava_geometry; ...@@ -19,7 +19,6 @@ package org.ros.rosjava_geometry;
import com.google.common.base.Preconditions; import com.google.common.base.Preconditions;
import org.ros.message.Time; import org.ros.message.Time;
import org.ros.namespace.GraphName;
/** /**
* Describes a {@link Transform} from data in the source frame to data in the * Describes a {@link Transform} from data in the source frame to data in the
...@@ -30,8 +29,8 @@ import org.ros.namespace.GraphName; ...@@ -30,8 +29,8 @@ import org.ros.namespace.GraphName;
public class FrameTransform { public class FrameTransform {
private final Transform transform; private final Transform transform;
private final GraphName source; private final FrameName source;
private final GraphName target; private final FrameName target;
private final Time time; private final Time time;
public static FrameTransform fromTransformStampedMessage( public static FrameTransform fromTransformStampedMessage(
...@@ -40,7 +39,7 @@ public class FrameTransform { ...@@ -40,7 +39,7 @@ public class FrameTransform {
String target = transformStamped.getHeader().getFrameId(); String target = transformStamped.getHeader().getFrameId();
String source = transformStamped.getChildFrameId(); String source = transformStamped.getChildFrameId();
Time stamp = transformStamped.getHeader().getStamp(); 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);
} }
/** /**
...@@ -57,7 +56,7 @@ public class FrameTransform { ...@@ -57,7 +56,7 @@ public class FrameTransform {
* the time associated with this {@link FrameTransform}, can be * the time associated with this {@link FrameTransform}, can be
* {@null} * {@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(transform);
Preconditions.checkNotNull(source); Preconditions.checkNotNull(source);
Preconditions.checkNotNull(target); Preconditions.checkNotNull(target);
...@@ -71,11 +70,11 @@ public class FrameTransform { ...@@ -71,11 +70,11 @@ public class FrameTransform {
return transform; return transform;
} }
public GraphName getSourceFrame() { public FrameName getSourceFrame() {
return source; return source;
} }
public GraphName getTargetFrame() { public FrameName getTargetFrame() {
return target; return target;
} }
......
...@@ -23,9 +23,6 @@ import com.google.common.collect.Maps; ...@@ -23,9 +23,6 @@ import com.google.common.collect.Maps;
import geometry_msgs.TransformStamped; import geometry_msgs.TransformStamped;
import org.ros.concurrent.CircularBlockingDeque; import org.ros.concurrent.CircularBlockingDeque;
import org.ros.message.Time; import org.ros.message.Time;
import org.ros.namespace.GraphName;
import org.ros.namespace.NameResolver;
import java.util.Map; import java.util.Map;
/** /**
...@@ -41,7 +38,6 @@ public class FrameTransformTree { ...@@ -41,7 +38,6 @@ public class FrameTransformTree {
private static final int TRANSFORM_QUEUE_CAPACITY = 16; private static final int TRANSFORM_QUEUE_CAPACITY = 16;
private final NameResolver nameResolver;
private final Object mutex; private final Object mutex;
/** /**
...@@ -49,11 +45,9 @@ public class FrameTransformTree { ...@@ -49,11 +45,9 @@ public class FrameTransformTree {
* frame. Lookups by target frame or by the pair of source and target are both * 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. * 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) { public FrameTransformTree() {
Preconditions.checkNotNull(nameResolver);
this.nameResolver = nameResolver;
mutex = new Object(); mutex = new Object();
transforms = Maps.newConcurrentMap(); transforms = Maps.newConcurrentMap();
} }
...@@ -71,26 +65,26 @@ public class FrameTransformTree { ...@@ -71,26 +65,26 @@ public class FrameTransformTree {
*/ */
public void update(geometry_msgs.TransformStamped transformStamped) { public void update(geometry_msgs.TransformStamped transformStamped) {
Preconditions.checkNotNull(transformStamped); Preconditions.checkNotNull(transformStamped);
GraphName resolvedSource = nameResolver.resolve(transformStamped.getChildFrameId()); FrameName transformName = FrameName.of(transformStamped.getChildFrameId());
LazyFrameTransform lazyFrameTransform = new LazyFrameTransform(transformStamped); LazyFrameTransform lazyFrameTransform = new LazyFrameTransform(transformStamped);
add(resolvedSource, lazyFrameTransform); add(transformName, lazyFrameTransform);
} }
@VisibleForTesting @VisibleForTesting
void update(FrameTransform frameTransform) { void update(FrameTransform frameTransform) {
Preconditions.checkNotNull(frameTransform); Preconditions.checkNotNull(frameTransform);
GraphName resolvedSource = frameTransform.getSourceFrame(); FrameName source = frameTransform.getSourceFrame();
LazyFrameTransform lazyFrameTransform = new LazyFrameTransform(frameTransform); LazyFrameTransform lazyFrameTransform = new LazyFrameTransform(frameTransform);
add(resolvedSource, lazyFrameTransform); add(source, lazyFrameTransform);
} }
private void add(GraphName resolvedSource, LazyFrameTransform lazyFrameTransform) { private void add(FrameName source, LazyFrameTransform lazyFrameTransform) {
if (!transforms.containsKey(resolvedSource)) { if (!transforms.containsKey(source)) {
transforms.put(resolvedSource, new CircularBlockingDeque<LazyFrameTransform>( transforms.put(source, new CircularBlockingDeque<LazyFrameTransform>(
TRANSFORM_QUEUE_CAPACITY)); TRANSFORM_QUEUE_CAPACITY));
} }
synchronized (mutex) { synchronized (mutex) {
transforms.get(resolvedSource).addFirst(lazyFrameTransform); transforms.get(source).addFirst(lazyFrameTransform);
} }
} }
...@@ -102,14 +96,13 @@ public class FrameTransformTree { ...@@ -102,14 +96,13 @@ public class FrameTransformTree {
* @return the most recent {@link FrameTransform} for {@code source} or * @return the most recent {@link FrameTransform} for {@code source} or
* {@code null} if no transform for {@code source} is available * {@code null} if no transform for {@code source} is available
*/ */
public FrameTransform lookUp(GraphName source) { public FrameTransform lookUp(FrameName source) {
Preconditions.checkNotNull(source); Preconditions.checkNotNull(source);
GraphName resolvedSource = nameResolver.resolve(source); return getLatest(source);
return getLatest(resolvedSource);
} }
private FrameTransform getLatest(GraphName resolvedSource) { private FrameTransform getLatest(FrameName source) {
CircularBlockingDeque<LazyFrameTransform> deque = transforms.get(resolvedSource); CircularBlockingDeque<LazyFrameTransform> deque = transforms.get(source);
if (deque == null) { if (deque == null) {
return null; return null;
} }
...@@ -121,11 +114,11 @@ public class FrameTransformTree { ...@@ -121,11 +114,11 @@ public class FrameTransformTree {
} }
/** /**
* @see #lookUp(GraphName) * @see #lookUp(FrameName)
*/ */
public FrameTransform get(String source) { public FrameTransform get(String source) {
Preconditions.checkNotNull(source); Preconditions.checkNotNull(source);
return lookUp(GraphName.of(source)); return lookUp(FrameName.of(source));
} }
/** /**
...@@ -140,16 +133,15 @@ public class FrameTransformTree { ...@@ -140,16 +133,15 @@ public class FrameTransformTree {
* @return the most recent {@link FrameTransform} for {@code source} or * @return the most recent {@link FrameTransform} for {@code source} or
* {@code null} if no transform for {@code source} is available * {@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(source);
Preconditions.checkNotNull(time); Preconditions.checkNotNull(time);
GraphName resolvedSource = nameResolver.resolve(source); return get(source, time);
return get(resolvedSource, time);
} }
// TODO(damonkohler): Use an efficient search. // TODO(damonkohler): Use an efficient search.
private FrameTransform get(GraphName resolvedSource, Time time) { private FrameTransform get(FrameName source, Time time) {
CircularBlockingDeque<LazyFrameTransform> deque = transforms.get(resolvedSource); CircularBlockingDeque<LazyFrameTransform> deque = transforms.get(source);
if (deque == null) { if (deque == null) {
return null; return null;
} }
...@@ -176,32 +168,30 @@ public class FrameTransformTree { ...@@ -176,32 +168,30 @@ public class FrameTransformTree {
} }
/** /**
* @see #lookUp(GraphName, Time) * @see #lookUp(FrameName, Time)
*/ */
public FrameTransform get(String source, Time time) { public FrameTransform get(String source, Time time) {
Preconditions.checkNotNull(source); 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 * @return the {@link FrameTransform} from source the frame to the target
* frame, or {@code null} if no {@link FrameTransform} could be found * 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(source);
Preconditions.checkNotNull(target); Preconditions.checkNotNull(target);
GraphName resolvedSource = nameResolver.resolve(source); if (source.equals(target)) {
GraphName resolvedTarget = nameResolver.resolve(target); return new FrameTransform(Transform.identity(), source, target, null);
if (resolvedSource.equals(resolvedTarget)) {
return new FrameTransform(Transform.identity(), resolvedSource, resolvedTarget, null);
} }
FrameTransform sourceToRoot = transformToRoot(resolvedSource); FrameTransform sourceToRoot = transformToRoot(source);
FrameTransform targetToRoot = transformToRoot(resolvedTarget); FrameTransform targetToRoot = transformToRoot(target);
if (sourceToRoot == null && targetToRoot == null) { if (sourceToRoot == null && targetToRoot == null) {
return null; return null;
} }
if (sourceToRoot == null) { if (sourceToRoot == null) {
if (targetToRoot.getTargetFrame().equals(resolvedSource)) { if (targetToRoot.getTargetFrame().equals(source)) {
// resolvedSource is root. // resolvedSource is root.
return targetToRoot.invert(); return targetToRoot.invert();
} else { } else {
...@@ -209,7 +199,7 @@ public class FrameTransformTree { ...@@ -209,7 +199,7 @@ public class FrameTransformTree {
} }
} }
if (targetToRoot == null) { if (targetToRoot == null) {
if (sourceToRoot.getTargetFrame().equals(resolvedTarget)) { if (sourceToRoot.getTargetFrame().equals(target)) {
// resolvedTarget is root. // resolvedTarget is root.
return sourceToRoot; return sourceToRoot;
} else { } else {
...@@ -221,29 +211,29 @@ public class FrameTransformTree { ...@@ -221,29 +211,29 @@ public class FrameTransformTree {
// same root. // same root.
Transform transform = Transform transform =
targetToRoot.getTransform().invert().multiply(sourceToRoot.getTransform()); 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. // No known transform.
return null; return null;
} }
/** /**
* @see #transform(GraphName, GraphName) * @see #transform(FrameName, FrameName)
*/ */
public FrameTransform transform(String source, String target) { public FrameTransform transform(String source, String target) {
Preconditions.checkNotNull(source); Preconditions.checkNotNull(source);
Preconditions.checkNotNull(target); 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 * the resolved source frame
* @return the {@link Transform} from {@code source} to root * @return the {@link Transform} from {@code source} to root
*/ */
@VisibleForTesting @VisibleForTesting
FrameTransform transformToRoot(GraphName resolvedSource) { FrameTransform transformToRoot(FrameName source) {
FrameTransform result = getLatest(resolvedSource); FrameTransform result = getLatest(source);
if (result == null) { if (result == null) {
return null; return null;
} }
...@@ -254,8 +244,8 @@ public class FrameTransformTree { ...@@ -254,8 +244,8 @@ public class FrameTransformTree {
} }
// Now resultToParent.getSourceFrame() == result.getTargetFrame() // Now resultToParent.getSourceFrame() == result.getTargetFrame()
Transform transform = resultToParent.getTransform().multiply(result.getTransform()); Transform transform = resultToParent.getTransform().multiply(result.getTransform());
GraphName resolvedTarget = resultToParent.getTargetFrame(); FrameName target = resultToParent.getTargetFrame();
result = new FrameTransform(transform, resolvedSource, resolvedTarget, result.getTime()); result = new FrameTransform(transform, source, target, result.getTime());
} }
} }
} }
...@@ -19,7 +19,6 @@ package org.ros.rosjava_geometry; ...@@ -19,7 +19,6 @@ package org.ros.rosjava_geometry;
import com.google.common.annotations.VisibleForTesting; import com.google.common.annotations.VisibleForTesting;
import org.ros.message.Time; import org.ros.message.Time;
import org.ros.namespace.GraphName;
/** /**
* A transformation in terms of translation, rotation, and scale. * A transformation in terms of translation, rotation, and scale.
...@@ -133,7 +132,7 @@ public class Transform { ...@@ -133,7 +132,7 @@ public class Transform {
return 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) { geometry_msgs.PoseStamped result) {
result.getHeader().setFrameId(frame.toString()); result.getHeader().setFrameId(frame.toString());
result.getHeader().setStamp(stamp); result.getHeader().setStamp(stamp);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment