コード例 #1
0
 /**
  * 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);
   }
 }
コード例 #2
0
  /**
   * 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);
    }
  }