/**
* Request/reply: Get/set sensor field of view. <br>
* <br>
* The field of view of the fiducial device can be set using the PLAYER_FIDUCIAL_REQ_GET_FOV
* request (response will be null), and queried using a null PLAYER_FIDUCIAL_REQ_GET_FOV request.
* <br>
* <br>
* See the player_fiducial_fov structure from player.h
*
* @param pff a PlayerFiducialFov structure holding the required data
*/
public void setFov(PlayerFiducialFov pff) {
try {
sendHeader(PLAYER_MSGTYPE_REQ, PLAYER_FIDUCIAL_REQ_SET_FOV, 12);
XdrBufferEncodingStream xdr = new XdrBufferEncodingStream(12);
xdr.beginEncoding(null, 0);
xdr.xdrEncodeFloat(pff.getMin_range());
xdr.xdrEncodeFloat(pff.getMax_range());
xdr.xdrEncodeFloat(pff.getView_angle());
xdr.endEncoding();
os.write(xdr.getXdrData(), 0, xdr.getXdrLength());
xdr.close();
os.flush();
} catch (IOException e) {
throw new PlayerException(
"[Fiducial] : Couldn't request PLAYER_FIDUCIAL_REQ_SET_FOV: " + e.toString(), e);
} catch (OncRpcException e) {
throw new PlayerException(
"[Fiducial] : Error while XDR-encoding SET_FOV request: " + 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);
}
}