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