/** Read the IR values. */
public synchronized void readData(PlayerMsgHdr header) {
try {
switch (header.getSubtype()) {
case PLAYER_IR_DATA_RANGES:
{
this.timestamp = header.getTimestamp();
// Buffer for reading voltages_count
byte[] buffer = new byte[4];
// Read voltages_count
is.readFully(buffer, 0, 4);
// Begin decoding the XDR buffer
XdrBufferDecodingStream xdr = new XdrBufferDecodingStream(buffer);
xdr.beginDecoding();
int voltagesCount = xdr.xdrDecodeInt();
xdr.endDecoding();
xdr.close();
// Buffer for reading voltage values
buffer = new byte[voltagesCount * 4 + 4];
// Read voltage values
is.readFully(buffer, 0, voltagesCount * 4 + 4);
xdr = new XdrBufferDecodingStream(buffer);
xdr.beginDecoding();
float[] voltages = xdr.xdrDecodeFloatVector();
xdr.endDecoding();
xdr.close();
// Buffer for reading ranges_count
buffer = new byte[4];
// Read ranges_count
is.readFully(buffer, 0, 4);
// Begin decoding the XDR buffer
xdr = new XdrBufferDecodingStream(buffer);
xdr.beginDecoding();
int rangesCount = xdr.xdrDecodeInt();
xdr.endDecoding();
xdr.close();
// Buffer for reading range values
buffer = new byte[rangesCount * 4 + 4];
// Read range values
is.readFully(buffer, 0, rangesCount * 4 + 4);
xdr = new XdrBufferDecodingStream(buffer);
xdr.beginDecoding();
float[] ranges = xdr.xdrDecodeFloatVector();
xdr.endDecoding();
xdr.close();
pidata = new PlayerIrData();
pidata.setVoltages(voltages);
pidata.setRanges(ranges);
readyPidata = true;
break;
}
}
} catch (IOException e) {
throw new PlayerException("[IR] : Error reading payload: " + e.toString(), e);
} catch (OncRpcException e) {
throw new PlayerException("[IR] : Error while XDR-decoding payload: " + e.toString(), e);
}
}
/**
* Handle acknowledgement response messages
*
* @param header Player header
*/
public void handleResponse(PlayerMsgHdr header) {
try {
switch (header.getSubtype()) {
case PLAYER_IR_REQ_POSE:
{
// Buffer for reading poses_count
byte[] buffer = new byte[4];
// Read poses_count
is.readFully(buffer, 0, 4);
// Begin decoding the XDR buffer
XdrBufferDecodingStream xdr = new XdrBufferDecodingStream(buffer);
xdr.beginDecoding();
int posesCount = xdr.xdrDecodeInt();
xdr.endDecoding();
xdr.close();
// Buffer for reading IR poses
buffer = new byte[posesCount * 48 + 4];
// Read IR poses
is.readFully(buffer, 0, posesCount * 48 + 4);
xdr = new XdrBufferDecodingStream(buffer);
xdr.beginDecoding();
xdr.xdrDecodeInt(); // skip poses count
PlayerPose3d[] pps = new PlayerPose3d[posesCount];
for (int i = 0; i < posesCount; i++) {
pps[i] = new PlayerPose3d();
pps[i].setPx(xdr.xdrDecodeDouble());
pps[i].setPy(xdr.xdrDecodeDouble());
pps[i].setPz(xdr.xdrDecodeDouble());
pps[i].setProll(xdr.xdrDecodeDouble());
pps[i].setPpitch(xdr.xdrDecodeDouble());
pps[i].setPyaw(xdr.xdrDecodeDouble());
}
xdr.endDecoding();
xdr.close();
pipose = new PlayerIrPose();
pipose.setPoses(pps);
readyPipose = true;
break;
}
case PLAYER_IR_REQ_POWER:
{
break;
}
default:
{
if (isDebugging)
logger.log(
Level.FINEST,
"[IR]Debug] : "
+ "Unexpected response "
+ header.getSubtype()
+ " of size = "
+ header.getSize());
break;
}
}
} catch (IOException e) {
throw new PlayerException("[IR] : Error reading payload: " + e.toString(), e);
} catch (OncRpcException e) {
throw new PlayerException("[IR] : Error while XDR-decoding payload: " + e.toString(), e);
}
}
