diff --git a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionGoal.java b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionGoal.java
index c45658aa725d49a3df67493281f3ded44ed24318..1db7244f736150472abf4b6d7498652d435151ca 100644
--- a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionGoal.java
+++ b/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() {
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 19719b5ff45ab04ff4d2ee12c8728358eafba4b4..93a8b17245908503465dd93d4e4bbae43b4ec04a 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
@@ -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;
+  }
 }