From 16e33d40c17f24904fa28f26d8bbe437fc761bf1 Mon Sep 17 00:00:00 2001
From: Ernesto Corbellini <ecorbellini@ekumenlabs.com>
Date: Wed, 20 Jan 2016 16:18:05 -0300
Subject: [PATCH] Added result received event status update.
---
.../rosjava_actionlib/ActionClient.java | 6 ++
.../rosjava_actionlib/ActionResult.java | 81 +++++++++++++++++++
.../rosjava_actionlib/ClientGoalManager.java | 4 +
.../rosjava_actionlib/ClientStateMachine.java | 8 +-
.../ekumen/rosjava_actionlib/TestClient.java | 17 ++--
5 files changed, 107 insertions(+), 9 deletions(-)
create mode 100644 src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionResult.java
diff --git a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionClient.java b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionClient.java
index aa48d33..50c43ea 100644
--- a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionClient.java
+++ b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionClient.java
@@ -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);
diff --git a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionResult.java b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionResult.java
new file mode 100644
index 0000000..601a48a
--- /dev/null
+++ b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionResult.java
@@ -0,0 +1,81 @@
+/**
+ * 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;
+ }
+}
diff --git a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ClientGoalManager.java b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ClientGoalManager.java
index 93a8b17..c61f53e 100644
--- a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ClientGoalManager.java
+++ b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ClientGoalManager.java
@@ -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);
}
diff --git a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ClientStateMachine.java b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ClientStateMachine.java
index e923224..5fd7a2a 100644
--- a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ClientStateMachine.java
+++ b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ClientStateMachine.java
@@ -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()
{
}
diff --git a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/TestClient.java b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/TestClient.java
index f90553f..fd45bfb 100644
--- a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/TestClient.java
+++ b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/TestClient.java
@@ -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);
}
--
GitLab