/** 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; }
/** 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); }
@Override public void run() { try { Float32 proximity = ultasonicPublisher.newMessage(); proximity.setData(brick.SENSOR.readUltrasonic(NXT.Sensor.P4)); ultasonicPublisher.publish(proximity); Float32 voltage = voltagePublisher.newMessage(); voltage.setData((float) brick.SYSTEM.getVBatt()); voltagePublisher.publish(voltage); Odometry odometry = odometryPublisher.newMessage(); setupHeader(odometry.getHeader(), seq); odometry.setChildFrameId("center"); TransformStamped tfs = tfPublisher.newMessage(); setupHeader(tfs.getHeader(), seq); tfs.setChildFrameId("center"); long tachoL = brick.MOTOR.getAbsoluteTacho(NXT.Motor.A); long tachoR = brick.MOTOR.getAbsoluteTacho(NXT.Motor.B); odoComputer.computeOdometry(tachoL, tachoR, odometry, tfs); seq++; odometryPublisher.publish(odometry); tfPublisher.publish(tfs); } catch (IOException e) { connectedNode.getLog().error("Sensors error", e); } }
/** 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; }