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();
}
}
}
}
