Skip to content
Snippets Groups Projects
Commit 16e33d40 authored by Ernesto Corbellini's avatar Ernesto Corbellini
Browse files

Added result received event status update.

parent 01ef7c83
No related branches found
No related tags found
No related merge requests found
......@@ -162,6 +162,7 @@ public class ActionClient<T_ACTION_GOAL extends Message,
* @see actionlib_msgs.GoalID
*/
public void sendCancel(GoalID id) {
goalManager.cancelGoal();
cancelPublisher.publish(id);
}
......@@ -251,6 +252,11 @@ public class ActionClient<T_ACTION_GOAL extends Message,
* depends on the application.
*/
public void gotResult(T_ACTION_RESULT message) {
ActionResult<T_ACTION_RESULT> ar = new ActionResult(message);
if (ar.getGoalStatusMessage().getGoalId().getId().equals(goalManager.actionGoal.getGoalId())) {
goalManager.updateStatus(ar.getGoalStatusMessage().getStatus());
}
goalManager.resultReceived();
// Propagate the callback
if (callbackTarget != null) {
callbackTarget.resultReceived(message);
......
/**
* Copyright 2015 Ekumen www.ekumenlabs.com
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package com.github.ekumen.rosjava_actionlib;
import java.lang.reflect.Method;
import org.ros.internal.message.Message;
import org.ros.message.Time;
import std_msgs.Header;
import actionlib_msgs.GoalID;
import actionlib_msgs.GoalStatus;
/**
* Class to encapsulate the action feedback object.
* @author Ernesto Corbellini ecorbellini@ekumenlabs.com
*/
public class ActionResult<T_ACTION_RESULT extends Message> {
private T_ACTION_RESULT actionResultMessage = null;
public ActionResult(T_ACTION_RESULT msg) {
actionResultMessage = msg;
}
public Header getHeaderMessage() {
Header h = null;
if (actionResultMessage != null) {
try {
Method m = actionResultMessage.getClass().getMethod("getHeader");
m.setAccessible(true); // workaround for known bug http://bugs.java.com/bugdatabase/view_bug.do?bug_id=6924232
h = (Header)m.invoke(actionResultMessage);
}
catch (Exception e) {
e.printStackTrace(System.out);
}
}
return h;
}
public GoalStatus getGoalStatusMessage() {
GoalStatus gs = null;
if (actionResultMessage != null) {
try {
Method m = actionResultMessage.getClass().getMethod("getStatus");
m.setAccessible(true); // workaround for known bug http://bugs.java.com/bugdatabase/view_bug.do?bug_id=6924232
gs = (GoalStatus)m.invoke(actionResultMessage);
}
catch (Exception e) {
e.printStackTrace(System.out);
}
}
return gs;
}
public Message getResultMessage() {
Message x = null;
if (actionResultMessage != null) {
try {
Method m = actionResultMessage.getClass().getMethod("getResult");
m.setAccessible(true); // workaround for known bug http://bugs.java.com/bugdatabase/view_bug.do?bug_id=6924232
x = (Message)m.invoke(actionResultMessage);
}
catch (Exception e) {
e.printStackTrace(System.out);
}
}
return x;
}
}
......@@ -48,6 +48,10 @@ public class ClientGoalManager<T_ACTION_GOAL extends Message> {
return stateMachine.cancel();
}
public void resultReceived() {
stateMachine.resultReceived();
}
public void updateStatus(int status) {
stateMachine.transition(status);
}
......
......@@ -458,13 +458,19 @@ public class ClientStateMachine {
case ClientStates.WAITING_FOR_GOAL_ACK:
case ClientStates.PENDING:
case ClientStates.ACTIVE:
this.state = ClientStates.WAITING_FOR_CANCEL_ACK;
state = ClientStates.WAITING_FOR_CANCEL_ACK;
ret = true;
break;
}
return ret;
}
public void resultReceived() {
if (state == ClientStates.WAITING_FOR_RESULT) {
state = ClientStates.DONE;
}
}
public void markAsLost()
{
}
......
......@@ -83,15 +83,16 @@ public class TestClient extends AbstractNodeMain implements ActionClientListener
// send another message and cancel it
goalId += i;
goalMessage = (FibonacciActionGoal)ac.newGoalMessage();
goalMessage.getGoal().setOrder(2);
System.out.println("Sending goal ID: " + goalId + "...");
ac.sendGoal(goalMessage, goalId);
System.out.println("Goal sent.");
sleep(2000);
System.out.println("Cancelling goal ID: " + goalId);
goalMessage.getGoal().setOrder(3);
//System.out.println("Sending goal ID: " + goalId + "...");
//ac.sendGoal(goalMessage, goalId);
ac.sendGoal(goalMessage);
GoalID gid = ac.getGoalId(goalMessage);
ac.sendCancel(gid);
sleep(10000);
System.out.println("Goal sent. Goal ID: " + gid);
//sleep(1000);
//System.out.println("Cancelling goal ID: " + goalId);
//ac.sendCancel(gid);
sleep(5000);
System.exit(0);
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment