private void readGeom() {
try {
// Buffer for reading pose and size
byte[] buffer = new byte[12 + 8];
// Read pose and size
is.readFully(buffer, 0, 12 + 8);
pp1dgeom = new PlayerPosition1dGeom();
PlayerPose pose = new PlayerPose();
PlayerBbox size = new PlayerBbox();
// Begin decoding the XDR buffer
XdrBufferDecodingStream xdr = new XdrBufferDecodingStream(buffer);
xdr.beginDecoding();
// pose of the robot base [m, m, rad]
pose.setPx(xdr.xdrDecodeFloat());
pose.setPy(xdr.xdrDecodeFloat());
pose.setPa(xdr.xdrDecodeFloat());
pp1dgeom.setPose(pose);
// dimensions of the base [m, m]
size.setSw(xdr.xdrDecodeFloat());
size.setSl(xdr.xdrDecodeFloat());
pp1dgeom.setSize(size);
xdr.endDecoding();
xdr.close();
} catch (IOException e) {
throw new PlayerException("[Position1D] : Error reading geometry data: " + e.toString(), e);
} catch (OncRpcException e) {
throw new PlayerException(
"[Position1D] : Error while XDR-decoding geometry data: " + e.toString(), e);
}
}
/** Read the Position1D data. */
public synchronized void readData(PlayerMsgHdr header) {
try {
switch (header.getSubtype()) {
case PLAYER_POSITION1D_DATA_STATE:
{
this.timestamp = header.getTimestamp();
// Buffer for reading pos, vel, stall and status
byte[] buffer = new byte[16];
// Read pos, vel, stall and status
is.readFully(buffer, 0, 16);
pp1ddata = new PlayerPosition1dData();
// Begin decoding the XDR buffer
XdrBufferDecodingStream xdr = new XdrBufferDecodingStream(buffer);
xdr.beginDecoding();
pp1ddata.setPos(xdr.xdrDecodeFloat()); // position [m]
pp1ddata.setVel(xdr.xdrDecodeFloat()); // tr.vel.[m/s]
pp1ddata.setStall(xdr.xdrDecodeByte()); // stalled?
pp1ddata.setStatus(xdr.xdrDecodeByte()); // status
xdr.endDecoding();
xdr.close();
readyPp1ddata = true;
break;
}
case PLAYER_POSITION1D_DATA_GEOM:
{
this.timestamp = header.getTimestamp();
readGeom();
readyPp1dgeom = true;
break;
}
}
} catch (IOException e) {
throw new PlayerException("[Position1D] : Error reading payload: " + e.toString(), e);
} catch (OncRpcException e) {
throw new PlayerException(
"[Position1D] : Error while XDR-decoding payload: " + e.toString(), e);
}
}
