/** 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()); }
/** Converts transform (a geometry msg) to a TransformStorage object and adds it to the buffer. */ protected boolean setTransform(TransformStamped transform) { // resolve the frame ID's String childFrameID = assertResolved(tfPrefix, transform.getChildFrameId()); String frameID = assertResolved(tfPrefix, transform.getHeader().getFrameId()); boolean errorExists = false; if (childFrameID == frameID) { node.getLog() .error( "TF_SELF_TRANSFORM: Ignoring transform with frame_id and child_frame_id \"" + childFrameID + "\" because they are the same"); errorExists = true; } if (childFrameID == "/") { // empty frame id will be mapped to "/" node.getLog() .error("TF_NO_CHILD_FRAME_ID: Ignoring transform because child_frame_id not set "); errorExists = true; } if (frameID == "/") { // empty parent id will be mapped to "/" node.getLog() .error( "TF_NO_FRAME_ID: Ignoring transform with child_frame_id \"" + childFrameID + "\" because frame_id not set"); errorExists = true; } if (errorExists) return false; // lookup or insert child frame Frame frame = lookupOrInsertFrame(childFrameID); // convert tf message to JTransform datastructure transform.setChildFrameId(childFrameID); transform.getHeader().setFrameId(frameID); TransformStorage tf = transformStampedMsgToTF(transform); // try to insert tf in corresponding time cache. If result is FALSE, the tf contains old data. if (!frame.insertData(tf)) { node.getLog() .warn( "TF_OLD_DATA ignoring data from the past for frame \"" + childFrameID + "\" at time " + ((double) tf.getTimeStamp() / 1E9)); return false; } return true; }