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 15148408c80d4601c379c701ec2c83439eab90d6..a2fbed963684ff9871ab376e67ca3ecc6cc37d2d 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 @@ -314,6 +314,21 @@ public class ActionServer<T_ACTION_GOAL extends Message, } } + /** + * Set goal ID and state information to the goal status message. + * @param gstat GoalStatus message. + * @param gidString String identifying the goal. + * @see actionlib_msgs.GoalStatus + */ + public void setGoalStatus(GoalStatus gstat, String gidString) { + try { + ServerGoal serverGoal = goalTracker.get(gidString); + gstat.setGoalId(getGoalId(serverGoal.goal)); + gstat.setStatus((byte)serverGoal.state.getState()); + } + catch (Exception e) { + } + } /** * Publishes the server's topics and suscribes to the client's topics. */