/** Converts the given TransformStorage datastructure to a TransformStamped message */
protected TransformStamped TFToTransformStampedMsg(TransformStorage tf) {
Vector3d tTF = tf.getTranslation();
Quat4d rTF = tf.getRotation();
// convert quaternion and translation vector to corresponding messages
geometry_msgs.Vector3 tMsg =
node.getTopicMessageFactory().newFromType(geometry_msgs.Vector3._TYPE);
geometry_msgs.Quaternion rMsg =
node.getTopicMessageFactory().newFromType(geometry_msgs.Quaternion._TYPE);
tMsg.setX(tTF.x);
tMsg.setY(tTF.y);
tMsg.setZ(tTF.z);
rMsg.setX(rTF.x);
rMsg.setY(rTF.y);
rMsg.setZ(rTF.z);
rMsg.setW(rTF.w);
// create TransformStamped message
TransformStamped msg = node.getTopicMessageFactory().newFromType(TransformStamped._TYPE);
msg.getHeader().setFrameId(tf.getParentFrame().getFrameID());
msg.getHeader().setStamp(new Time(tf.getTimeStamp()));
msg.setChildFrameId(tf.getChildFrame().getFrameID());
msg.getTransform().setTranslation(tMsg);
msg.getTransform().setRotation(rMsg);
return msg;
}
/**
* Converts the TransformStorage datastructure (represented by quaternion and vector) to the
* StampedTransform datastructure (represented by a 4x4 matrix)
*/
protected StampedTransform StorageToStampedTransform(TransformStorage ts) {
return new StampedTransform(
ts.getTranslation(),
ts.getRotation(),
new Time(ts.getTimeStamp()),
ts.getParentFrame().getFrameID(),
ts.getChildFrame().getFrameID());
}
/** 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;
}
