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