/**
* 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);
}
}
