/** Converts the given TransformStamped message to the TransformStorage datastructure */
protected TransformStorage transformStampedMsgToTF(TransformStamped msg) {
geometry_msgs.Vector3 tMsg = msg.getTransform().getTranslation();
geometry_msgs.Quaternion rMsg = msg.getTransform().getRotation();
// add frames to map
Frame childFrame = lookupOrInsertFrame(msg.getChildFrameId());
Frame parentFrame = lookupOrInsertFrame(msg.getHeader().getFrameId());
return new TransformStorage(
new Vector3d(tMsg.getX(), tMsg.getY(), tMsg.getZ()),
new Quat4d(rMsg.getX(), rMsg.getY(), rMsg.getZ(), rMsg.getW()),
msg.getHeader().getStamp().totalNsecs(),
parentFrame,
childFrame);
}
public static void packRosQuaternionToQuat4d(Quaternion rosQuat, Quat4d quat) {
quat.setX(rosQuat.getX());
quat.setY(rosQuat.getY());
quat.setZ(rosQuat.getZ());
quat.setW(rosQuat.getW());
}