/** Read the fiducial data packet (all fiducials). */
public synchronized void readData(PlayerMsgHdr header) {
try {
switch (header.getSubtype()) {
case PLAYER_FIDUCIAL_DATA_SCAN:
{
this.timestamp = header.getTimestamp();
// Buffer for reading fiducials_count
byte[] buffer = new byte[8];
// Read fiducials_count
is.readFully(buffer, 0, 8);
// Begin decoding the XDR buffer
XdrBufferDecodingStream xdr = new XdrBufferDecodingStream(buffer);
xdr.beginDecoding();
int fiducialsCount = xdr.xdrDecodeInt();
xdr.endDecoding();
xdr.close();
// Buffer for reading fiducials
buffer = new byte[PLAYER_FIDUCIAL_MAX_SAMPLES * (4 + 24 * 2)];
// Read fiducials
is.readFully(buffer, 0, fiducialsCount * (4 + 24 * 2));
xdr = new XdrBufferDecodingStream(buffer);
xdr.beginDecoding();
PlayerFiducialItem[] pfis = new PlayerFiducialItem[fiducialsCount];
for (int i = 0; i < fiducialsCount; i++) {
PlayerFiducialItem pfi = new PlayerFiducialItem();
pfi.setId(xdr.xdrDecodeInt());
PlayerPose3d pose = new PlayerPose3d();
PlayerPose3d upose = new PlayerPose3d();
pose.setPx(xdr.xdrDecodeFloat());
pose.setPy(xdr.xdrDecodeFloat());
pose.setPz(xdr.xdrDecodeFloat());
pose.setProll(xdr.xdrDecodeFloat());
pose.setPpitch(xdr.xdrDecodeFloat());
pose.setPyaw(xdr.xdrDecodeFloat());
upose.setPx(xdr.xdrDecodeFloat());
upose.setPy(xdr.xdrDecodeFloat());
upose.setPz(xdr.xdrDecodeFloat());
upose.setProll(xdr.xdrDecodeFloat());
upose.setPpitch(xdr.xdrDecodeFloat());
upose.setPyaw(xdr.xdrDecodeFloat());
pfi.setPose(pose);
pfi.setUpose(upose);
pfis[i] = pfi;
}
xdr.endDecoding();
xdr.close();
pfdata = new PlayerFiducialData();
pfdata.setFiducials_count(fiducialsCount);
pfdata.setFiducials(pfis);
readyPfdata = true;
break;
}
}
} catch (IOException e) {
throw new PlayerException("[Fiducial] : Error reading payload: " + e.toString(), e);
} catch (OncRpcException e) {
throw new PlayerException(
"[Fiducial] : Error while XDR-decoding payload: " + e.toString(), e);
}
}
/**
* Handle acknowledgement response messages.
*
* @param header Player header
*/
protected void handleResponse(PlayerMsgHdr header) {
try {
switch (header.getSubtype()) {
case PLAYER_FIDUCIAL_REQ_GET_GEOM:
{
// Buffer for reading the geometry data
byte[] buffer = new byte[12 + 8 + 8];
// Read the geometry data
is.readFully(buffer, 0, 12 + 8 + 8);
pfgeom = new PlayerFiducialGeom();
PlayerPose pose = new PlayerPose();
PlayerBbox size = new PlayerBbox();
PlayerBbox fiducial_size = new PlayerBbox();
// Begin decoding the XDR buffer
XdrBufferDecodingStream xdr = new XdrBufferDecodingStream(buffer);
xdr.beginDecoding();
// pose of the detector in the robot cs [m, m, rad]
pose.setPx(xdr.xdrDecodeFloat());
pose.setPy(xdr.xdrDecodeFloat());
pose.setPa(xdr.xdrDecodeFloat());
pfgeom.setPose(pose);
// size of the detector [m, m]
size.setSw(xdr.xdrDecodeFloat());
size.setSl(xdr.xdrDecodeFloat());
pfgeom.setSize(size);
// dimensions of the fiducial in units of [m, m]
fiducial_size.setSw(xdr.xdrDecodeFloat());
fiducial_size.setSl(xdr.xdrDecodeFloat());
pfgeom.setFiducial_size(size);
xdr.endDecoding();
xdr.close();
readyPfgeom = true;
break;
}
case PLAYER_FIDUCIAL_REQ_GET_FOV:
{
// Buffer for reading the fov data
byte[] buffer = new byte[12];
// Read the fov data
is.readFully(buffer, 0, 12);
pffov = new PlayerFiducialFov();
// Begin decoding the XDR buffer
XdrBufferDecodingStream xdr = new XdrBufferDecodingStream(buffer);
xdr.beginDecoding();
pffov.setMin_range(xdr.xdrDecodeFloat());
pffov.setMax_range(xdr.xdrDecodeFloat());
pffov.setView_angle(xdr.xdrDecodeFloat());
xdr.endDecoding();
xdr.close();
readyPffov = true;
break;
}
case PLAYER_FIDUCIAL_REQ_SET_FOV:
{
break;
}
case PLAYER_FIDUCIAL_REQ_GET_ID:
{
// Buffer for reading the ID data
byte[] buffer = new byte[4];
// Read the ID data
is.readFully(buffer, 0, 4);
// Begin decoding the XDR buffer
XdrBufferDecodingStream xdr = new XdrBufferDecodingStream(buffer);
xdr.beginDecoding();
pfid = xdr.xdrDecodeInt();
xdr.endDecoding();
xdr.close();
readyPfid = true;
break;
}
case PLAYER_FIDUCIAL_REQ_SET_ID:
{
break;
}
default:
{
if (isDebugging)
logger.log(
Level.FINEST,
"[Fiducial][Debug] : "
+ "Unexpected response "
+ header.getSubtype()
+ " of size = "
+ header.getSize());
break;
}
}
} catch (IOException e) {
throw new PlayerException("[Fiducial] : Error reading payload: " + e.toString(), e);
} catch (OncRpcException e) {
throw new PlayerException(
"[Fiducial] : Error while XDR-decoding payload: " + e.toString(), e);
}
}