/** 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 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);
}
private static void packQuat4dToGeometry_msgsQuaternion(Quat4d quat, Quaternion orientation) {
orientation.setW(quat.getW());
orientation.setX(quat.getX());
orientation.setY(quat.getY());
orientation.setZ(quat.getZ());
}
public static void packRosQuaternionToQuat4d(Quaternion rosQuat, Quat4d quat) {
quat.setX(rosQuat.getX());
quat.setY(rosQuat.getY());
quat.setZ(rosQuat.getZ());
quat.setW(rosQuat.getW());
}