예제 #1
0
  /** 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);
    }
  }
예제 #2
0
  /**
   * 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);
    }
  }