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 1516857ceef94978aebaa1bd9be99a913c5ed83f..9c2b3f740b34f9e5e5e45da676758db120e2e316 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
@@ -54,6 +54,7 @@ public class ActionClient<T_ACTION_GOAL extends Message,
 
   private void publishClient(ConnectedNode node) {
     goalPublisher = node.newPublisher(actionName + "/goal", actionGoalType);
+    goalPublisher.setLatchMode(false);
     cancelPublisher = node.newPublisher(actionName + "/cancel", GoalID._TYPE);
   }
 
diff --git a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/GoalIDGenerator.java b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/GoalIDGenerator.java
index 35654c6f483dad257592b36690be380cb322126b..20b0e09b47f448441ea036401b89d44d6f48478f 100644
--- a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/GoalIDGenerator.java
+++ b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/GoalIDGenerator.java
@@ -19,8 +19,6 @@ package com.github.ekumen.rosjava_actionlib;
 import org.ros.message.Time;
 import actionlib_msgs.GoalID;
 import org.ros.node.ConnectedNode;
-import org.ros.node.NodeConfiguration;
-import org.ros.message.MessageFactory;
 
 import java.util.concurrent.atomic.AtomicLong;
 
@@ -62,7 +60,7 @@ public class GoalIDGenerator {
    *
    * @return GoalID object
    */
-  public String generateID(ConnectedNode node, GoalID goal) {
+  public String generateID(GoalID goalId) {
     String id;
     Time t = node.getCurrentTime();
     //NodeConfiguration nc = NodeConfiguration.newPrivate();
@@ -74,8 +72,8 @@ public class GoalIDGenerator {
     id = node.getName().toString() + "-" + goalCount.incrementAndGet()
       + "-" + t.secs + "." + t.nsecs;
 
-    goal.setId(id);
-    goal.setStamp(t);
+    goalId.setId(id);
+    goalId.setStamp(t);
 
     return id;
   }
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 d27aa704af5904ba5b696e84305318167ad591cd..7abf0a1bbfad18eada761cc5eaafb3bdfdd0aca7 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
@@ -1,5 +1,6 @@
 package com.github.ekumen.rosjava_actionlib;
 
+import java.util.List;
 import org.ros.namespace.GraphName;
 import org.ros.node.AbstractNodeMain;
 import org.ros.node.ConnectedNode;
@@ -11,10 +12,13 @@ import actionlib_tutorials.FibonacciGoal;
 import actionlib_tutorials.FibonacciFeedback;
 import actionlib_tutorials.FibonacciResult;
 import actionlib_msgs.GoalStatusArray;
+import actionlib_msgs.GoalID;
+import actionlib_msgs.GoalStatus;
 
 public class TestClient extends AbstractNodeMain implements ActionClientListener<FibonacciActionFeedback, FibonacciActionResult> {
-  ActionClient ac;
-  volatile private boolean resultReceived = false;
+  private ActionClient ac = null;
+  private volatile boolean resultReceived = false;
+  private GoalIDGenerator goalIdGenerator = null;
 
   @Override
   public GraphName getDefaultNodeName() {
@@ -26,6 +30,8 @@ public class TestClient extends AbstractNodeMain implements ActionClientListener
     ac = new ActionClient<FibonacciActionGoal, FibonacciActionFeedback, FibonacciActionResult>(node, "/fibonacci", FibonacciActionGoal._TYPE, FibonacciActionFeedback._TYPE, FibonacciActionResult._TYPE);
     int repeat = 3;
 
+    goalIdGenerator = new GoalIDGenerator(node);
+
     // Attach listener for the callbacks
     ac.attachListener(this);
 
@@ -40,7 +46,7 @@ public class TestClient extends AbstractNodeMain implements ActionClientListener
     while (repeat > 0) {
       sleep(10000);
       System.out.println("Sending goal #" + repeat + "...");
-      ac.sendGoal(goalMessage);
+      sendGoal(goalMessage);
       System.out.println("Goal sent.");
       //while(!resultReceived) sleep(100);
       resultReceived = false;
@@ -78,7 +84,15 @@ public class TestClient extends AbstractNodeMain implements ActionClientListener
 
   @Override
   public void statusReceived(GoalStatusArray status) {
+    List<GoalStatus> statusList = status.getStatusList();
+    for(GoalStatus gs:statusList) {
+      //System.out.println("GoalID: " + gs.getGoalId().getId() + " -- GoalStatus: " + gs.getStatus() + " -- " + gs.getText());
+    }
+  }
 
+  private void sendGoal(FibonacciActionGoal goal) {
+    goalIdGenerator.generateID(goal.getGoalId());
+    ac.sendGoal(goal);
   }
 
   void sleep(long msec) {