@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;
}
/** 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);
}
/**
* Updates the tree with the provided {@link geometry_msgs.TransformStamped} message.
*
* <p>Note that the tree is updated lazily. Modifications to the provided {@link
* geometry_msgs.TransformStamped} message may cause unpredictable results.
*
* @param transformStamped the {@link geometry_msgs.TransformStamped} message to update with
*/
public void update(geometry_msgs.TransformStamped transformStamped) {
Preconditions.checkNotNull(transformStamped);
GraphName source = GraphName.of(transformStamped.getChildFrameId());
LazyFrameTransform lazyFrameTransform = new LazyFrameTransform(transformStamped);
add(source, lazyFrameTransform);
}