Fixed goal setter.

Added method to obtain the goal state.

src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionGoal.java

@@ -223,7 +223,7 @@ public class ActionGoal<T_ACTION_GOAL extends Message> {
   }
 
   public void setActionGoalMessage(T_ACTION_GOAL agm) {
-
+    goalMessage = agm;
   }
 
   public Message getGoalMessage() {

src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ClientGoalManager.java

@@ -25,8 +25,8 @@ import actionlib_msgs.GoalStatus;
  * @author Ernesto Corbellini ecorbellini@ekumenlabs.com
  */
 public class ClientGoalManager<T_ACTION_GOAL extends Message> {
-  public ActionGoal<T_ACTION_GOAL> actionGoal = null;
-  public ClientStateMachine stateMachine = null;
+  ActionGoal<T_ACTION_GOAL> actionGoal = null;
+  ClientStateMachine stateMachine = null;
 
   public ClientGoalManager(ActionGoal<T_ACTION_GOAL> ag) {
     actionGoal = ag;
@@ -40,7 +40,8 @@ public class ClientGoalManager<T_ACTION_GOAL extends Message> {
 
   public void setGoal(T_ACTION_GOAL agm) {
     ActionGoal<T_ACTION_GOAL> ag = new ActionGoal();
-    ag.setGoalMessage(agm);
+    ag.setActionGoalMessage(agm);
+    setGoal(ag);
   }
 
   public boolean cancelGoal() {
@@ -50,4 +51,12 @@ public class ClientGoalManager<T_ACTION_GOAL extends Message> {
   public void updateStatus(int status) {
     stateMachine.transition(status);
   }
+
+  public int getGoalState() {
+    int ret = -666;
+    if (stateMachine != null) {
+      ret = stateMachine.getState();
+    }
+    return ret;
+  }
 }