/** 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());
}