/** Converts transform (a geometry msg) to a TransformStorage object and adds it to the buffer. */
protected boolean setTransform(TransformStamped transform) {
// resolve the frame ID's
String childFrameID = assertResolved(tfPrefix, transform.getChildFrameId());
String frameID = assertResolved(tfPrefix, transform.getHeader().getFrameId());
boolean errorExists = false;
if (childFrameID == frameID) {
node.getLog()
.error(
"TF_SELF_TRANSFORM: Ignoring transform with frame_id and child_frame_id \""
+ childFrameID
+ "\" because they are the same");
errorExists = true;
}
if (childFrameID == "/") { // empty frame id will be mapped to "/"
node.getLog()
.error("TF_NO_CHILD_FRAME_ID: Ignoring transform because child_frame_id not set ");
errorExists = true;
}
if (frameID == "/") { // empty parent id will be mapped to "/"
node.getLog()
.error(
"TF_NO_FRAME_ID: Ignoring transform with child_frame_id \""
+ childFrameID
+ "\" because frame_id not set");
errorExists = true;
}
if (errorExists) return false;
// lookup or insert child frame
Frame frame = lookupOrInsertFrame(childFrameID);
// convert tf message to JTransform datastructure
transform.setChildFrameId(childFrameID);
transform.getHeader().setFrameId(frameID);
TransformStorage tf = transformStampedMsgToTF(transform);
// try to insert tf in corresponding time cache. If result is FALSE, the tf contains old data.
if (!frame.insertData(tf)) {
node.getLog()
.warn(
"TF_OLD_DATA ignoring data from the past for frame \""
+ childFrameID
+ "\" at time "
+ ((double) tf.getTimeStamp() / 1E9));
return false;
}
return true;
}
/**
* Returns the resolves version of the given frame ID, and asserts a debug message if the name was
* not fully resolved.
*/
private String assertResolved(String prefix, String frameID) {
if (!frameID.startsWith("/"))
node.getLog()
.debug(
"TF operating on not fully resolved frame id "
+ frameID
+ ", resolving using local prefix "
+ prefix);
return resolve(prefix, frameID);
}
/**
* Returns the transform from the specified source frame to the target frame at a given time;
* returns null if no transformation could be found.
*/
public StampedTransform lookupTransform(String targetFrameID, String sourceFrameID, Time time) {
// resolve the source and target IDs
String resolvedTargetID = assertResolved(tfPrefix, targetFrameID);
String resolvedSourceID = assertResolved(tfPrefix, sourceFrameID);
// if source and target are the same, return the identity transform
if (resolvedSourceID == resolvedTargetID) {
StampedTransform out = StampedTransform.getIdentity();
out.timeStamp = time;
out.frameID = resolvedSourceID;
out.childFrameID = resolvedTargetID;
return out;
}
// lookup source and target frame
Frame sourceFrame = frames.get(resolvedSourceID);
Frame targetFrame = frames.get(resolvedTargetID);
if (sourceFrame == null) {
node.getLog()
.error("Cannot transform: source frame \"" + resolvedSourceID + "\" does not exist.");
return null;
}
if (targetFrame == null) {
node.getLog()
.error("Cannot transform: target frame \"" + resolvedTargetID + "\" does not exist.");
return null;
}
// list that will contain transformations from source frame to some frame F
LinkedList<TransformStorage> inverseTransforms = new LinkedList<TransformStorage>();
// list that will contain transformations from frame F to target frame
LinkedList<TransformStorage> forwardTransforms = new LinkedList<TransformStorage>();
// fill the lists using lookupLists. If it returns FALSE, no transformation could be found.
if (!lookupLists(
targetFrame, sourceFrame, time.totalNsecs(), inverseTransforms, forwardTransforms)) {
// TODO give warning
node.getLog()
.error(
"Cannot transform: source + \""
+ resolvedSourceID
+ "\" and target \""
+ resolvedTargetID
+ "\" are not connected.");
return null;
}
// create an identity transform with the correct time stamp
StampedTransform out = StampedTransform.getIdentity();
out.timeStamp = time;
// multiply all transforms from source frame to frame F TODO: right?
for (TransformStorage t : inverseTransforms) {
out.mul(StorageToStampedTransform(t));
}
// multiply all transforms from frame F to target frame TODO: right?
for (TransformStorage t : forwardTransforms) {
out.mul(StorageToStampedTransform(t).invert(), out);
}
// return transform
return out;
}
