Esempio n. 1
0
  private void readGeom() {
    try {
      // Buffer for reading pose and size
      byte[] buffer = new byte[12 + 8];
      // Read pose and size
      is.readFully(buffer, 0, 12 + 8);

      pp1dgeom = new PlayerPosition1dGeom();
      PlayerPose pose = new PlayerPose();
      PlayerBbox size = new PlayerBbox();

      // Begin decoding the XDR buffer
      XdrBufferDecodingStream xdr = new XdrBufferDecodingStream(buffer);
      xdr.beginDecoding();

      // pose of the robot base [m, m, rad]
      pose.setPx(xdr.xdrDecodeFloat());
      pose.setPy(xdr.xdrDecodeFloat());
      pose.setPa(xdr.xdrDecodeFloat());
      pp1dgeom.setPose(pose);
      // dimensions of the base [m, m]
      size.setSw(xdr.xdrDecodeFloat());
      size.setSl(xdr.xdrDecodeFloat());
      pp1dgeom.setSize(size);
      xdr.endDecoding();
      xdr.close();
    } catch (IOException e) {
      throw new PlayerException("[Position1D] : Error reading geometry data: " + e.toString(), e);
    } catch (OncRpcException e) {
      throw new PlayerException(
          "[Position1D] : Error while XDR-decoding geometry data: " + e.toString(), e);
    }
  }
Esempio n. 2
0
  /** Read the Position1D data. */
  public synchronized void readData(PlayerMsgHdr header) {
    try {
      switch (header.getSubtype()) {
        case PLAYER_POSITION1D_DATA_STATE:
          {
            this.timestamp = header.getTimestamp();

            // Buffer for reading pos, vel, stall and status
            byte[] buffer = new byte[16];
            // Read pos, vel, stall and status
            is.readFully(buffer, 0, 16);

            pp1ddata = new PlayerPosition1dData();

            // Begin decoding the XDR buffer
            XdrBufferDecodingStream xdr = new XdrBufferDecodingStream(buffer);
            xdr.beginDecoding();
            pp1ddata.setPos(xdr.xdrDecodeFloat()); // position [m]
            pp1ddata.setVel(xdr.xdrDecodeFloat()); // tr.vel.[m/s]
            pp1ddata.setStall(xdr.xdrDecodeByte()); // stalled?
            pp1ddata.setStatus(xdr.xdrDecodeByte()); // status
            xdr.endDecoding();
            xdr.close();

            readyPp1ddata = true;
            break;
          }
        case PLAYER_POSITION1D_DATA_GEOM:
          {
            this.timestamp = header.getTimestamp();

            readGeom();
            readyPp1dgeom = true;
            break;
          }
      }
    } catch (IOException e) {
      throw new PlayerException("[Position1D] : Error reading payload: " + e.toString(), e);
    } catch (OncRpcException e) {
      throw new PlayerException(
          "[Position1D] : Error while XDR-decoding payload: " + e.toString(), e);
    }
  }