From b21bdcfdf85274f6cc24c56553f754ae47dc4e2d Mon Sep 17 00:00:00 2001 From: Ernesto Corbellini <ecorbellini@ekumenlabs.com> Date: Fri, 18 Dec 2015 11:52:22 -0300 Subject: [PATCH] Added goal tracker to the server. --- .../rosjava_actionlib/ActionServer.java | 37 ++++++++++++++++--- 1 file changed, 32 insertions(+), 5 deletions(-) diff --git a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionServer.java b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionServer.java index 3b1d9f0..ef72bb1 100644 --- a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionServer.java +++ b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionServer.java @@ -1,4 +1,4 @@ - package com.github.ekumen.rosjava_actionlib; +package com.github.ekumen.rosjava_actionlib; import org.ros.namespace.GraphName; import org.ros.node.AbstractNodeMain; @@ -10,6 +10,8 @@ import org.ros.internal.message.Message; import java.util.concurrent.TimeUnit; import java.util.Timer; import java.util.TimerTask; +import java.util.HashMap; +import java.lang.reflect.Method; import actionlib_msgs.GoalStatusArray; import actionlib_msgs.GoalID; @@ -17,6 +19,15 @@ public class ActionServer<T_ACTION_GOAL extends Message, T_ACTION_FEEDBACK extends Message, T_ACTION_RESULT extends Message> { + private class ServerGoal { + T_ACTION_GOAL goal; + ServerStateMachine state = new ServerStateMachine(); + + ServerGoal(T_ACTION_GOAL g) { + goal = g; + } + } + private T_ACTION_GOAL actionGoal; private String actionGoalType; private String actionResultType; @@ -31,6 +42,8 @@ public class ActionServer<T_ACTION_GOAL extends Message, private String actionName; private ActionServerListener callbackTarget = null; private Timer statusTick = new Timer(); + private HashMap<String, ServerGoal> goalTracker = new HashMap<String, + ServerGoal>(1); ActionServer (ConnectedNode node, String actionName, String actionGoalType, String actionFeedbackType, String actionResultType) { @@ -116,17 +129,18 @@ public class ActionServer<T_ACTION_GOAL extends Message, } } - public void gotGoal(T_ACTION_GOAL message) { + public void gotGoal(T_ACTION_GOAL goal) { + goalTracker.put(getGoalId(goal).getId(), new ServerGoal(goal)); // Propagate the callback if (callbackTarget != null) { - callbackTarget.goalReceived(message); + callbackTarget.goalReceived(goal); } } - public void gotCancel(GoalID message) { + public void gotCancel(GoalID gid) { // Propagate the callback if (callbackTarget != null) { - callbackTarget.cancelReceived(message); + callbackTarget.cancelReceived(gid); } } @@ -143,6 +157,19 @@ public class ActionServer<T_ACTION_GOAL extends Message, return feedbackPublisher.newMessage(); } + public GoalID getGoalId(T_ACTION_GOAL goal) { + GoalID gid = null; + try { + Method m = goal.getClass().getMethod("getGoalId"); + m.setAccessible(true); // workaround for known bug http://bugs.java.com/bugdatabase/view_bug.do?bug_id=6924232 + gid = (GoalID)m.invoke(goal); + } + catch (Exception e) { + e.printStackTrace(System.out); + } + return gid; + } + /** * Publishes the server's topics and suscribes to the client's topics. */ -- GitLab