Ejemplo n.º 1
0
  public void onClickFunc(double goalX, double goalY) {
    final srs_env_model_percp.EstimateBBRequest request = serviceClient.newMessage();
    short[] bod1 = new short[2];
    short[] bod2 = new short[2];
    bod1[0] = (short) goalX;
    bod1[1] = (short) goalY;
    bod2[0] = (short) ((goalX > 600) ? (goalX - 10) : (goalX + 10));
    bod2[1] = (short) ((goalY > 440) ? (goalY - 10) : (goalY + 10));
    request.getHeader().setFrameId("/map");
    request.getHeader().setStamp(connectedNode.getCurrentTime());
    request.setP1(bod1);
    request.setP2(bod2);
    request.setMode((byte) 1);
    serviceClient.call(
        request,
        new ServiceResponseListener<srs_env_model_percp.EstimateBBResponse>() {
          @Override
          public void onFailure(RemoteException arg0) {
            throw new RosRuntimeException(arg0);
          }

          @Override
          public void onSuccess(EstimateBBResponse response) {
            x =
                (response.getP1()[0]
                        + response.getP2()[0]
                        + response.getP3()[0]
                        + response.getP4()[0]
                        + response.getP5()[0]
                        + response.getP6()[0]
                        + response.getP7()[0]
                        + response.getP8()[0])
                    / 8.0f; // * 10;
            y =
                (response.getP1()[1]
                        + response.getP2()[1]
                        + response.getP3()[1]
                        + response.getP4()[1]
                        + response.getP5()[1]
                        + response.getP6()[1]
                        + response.getP7()[1]
                        + response.getP8()[1])
                    / 8.0f; // * 10;
            move = true;
          }
        });
  }
  public void saveMap() {
    ServiceClient<SaveMapRequest, SaveMapResponse> saveMapClient = null;
    if (connectedNode != null) {
      try {
        if (nameResolverSet) {
          saveSrvName = nameResolver.resolve(saveSrvName).toString();
        }
        saveMapClient = connectedNode.newServiceClient(saveSrvName, SaveMap._TYPE);
      } catch (ServiceNotFoundException e) {
        try {
          Thread.sleep(1000L);
        } catch (Exception ex) {
        }
        statusCallback.onFailureCallback(e);
      }
      if (saveMapClient != null) {
        final SaveMapRequest request = saveMapClient.newMessage();
        request.setMapName(mapName);
        saveMapClient.call(
            request,
            new ServiceResponseListener<SaveMapResponse>() {
              @Override
              public void onSuccess(SaveMapResponse saveMapResponse) {
                if (waitingFlag) {
                  clearWaitFor();
                  statusCallback.onSuccessCallback(saveMapResponse);
                }
              }

              @Override
              public void onFailure(RemoteException e) {
                if (waitingFlag) {
                  clearWaitFor();
                  statusCallback.onFailureCallback(e);
                }
              }
            });
        if (!waitFor(10)) {
          statusCallback.timeoutCallback();
        }
      }
    }
  }