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 9c2b3f740b34f9e5e5e45da676758db120e2e316..4633d23e46e7f4f3460aa6ac7ae1dca47a2f0b9a 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
@@ -9,9 +9,14 @@ import org.ros.node.topic.Publisher;
 import org.ros.message.MessageListener;
 import org.ros.internal.message.Message;
 import java.util.concurrent.TimeUnit;
+import java.lang.reflect.Method;
 import actionlib_msgs.GoalStatusArray;
 import actionlib_msgs.GoalID;
 
+/**
+ * Client implementation for actionlib.
+ * @author Ernesto Corbellini <ecorbellini@ekumenlabs.com>
+ */
 public class ActionClient<T_ACTION_GOAL extends Message,
   T_ACTION_FEEDBACK extends Message,
   T_ACTION_RESULT extends Message> {
@@ -28,6 +33,7 @@ public class ActionClient<T_ACTION_GOAL extends Message,
   ConnectedNode node = null;
   String actionName;
   ActionClientListener callbackTarget = null;
+  GoalIDGenerator goalIdGenerator = null;
 
   ActionClient (ConnectedNode node, String actionName, String actionGoalType,
     String actionFeedbackType, String actionResultType) {
@@ -36,7 +42,7 @@ public class ActionClient<T_ACTION_GOAL extends Message,
     this.actionGoalType = actionGoalType;
     this.actionFeedbackType = actionFeedbackType;
     this.actionResultType = actionResultType;
-
+    goalIdGenerator = new GoalIDGenerator(node);
     connect(node);
   }
 
@@ -44,10 +50,44 @@ public class ActionClient<T_ACTION_GOAL extends Message,
     callbackTarget = target;
   }
 
-  public void sendGoal(T_ACTION_GOAL goal) {
+  public void sendGoal(T_ACTION_GOAL goal, String id) {
+    GoalID gid = getGoalId(goal);
+    if (id == "") {
+      goalIdGenerator.generateID(gid);
+    } else {
+      gid.setId(id);
+    }
     goalPublisher.publish(goal);
   }
 
+  public void sendGoal(T_ACTION_GOAL goal) {
+    sendGoal(goal, "");
+  }
+
+  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;
+  }
+
+  public void setGoalId(T_ACTION_GOAL goal, GoalID gid) {
+    try {
+      Method m = goal.getClass().getMethod("setGoalId", GoalID.class);
+      m.setAccessible(true); // workaround for known bug http://bugs.java.com/bugdatabase/view_bug.do?bug_id=6924232
+      m.invoke(goal, gid);
+    }
+    catch (Exception e) {
+      e.printStackTrace(System.out);
+    }
+  }
+
   public void sendCancel(GoalID id) {
     cancelPublisher.publish(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 7abf0a1bbfad18eada761cc5eaafb3bdfdd0aca7..75cf59c74658fc6e146210f5a9554a210e43c578 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
@@ -18,7 +18,6 @@ import actionlib_msgs.GoalStatus;
 public class TestClient extends AbstractNodeMain implements ActionClientListener<FibonacciActionFeedback, FibonacciActionResult> {
   private ActionClient ac = null;
   private volatile boolean resultReceived = false;
-  private GoalIDGenerator goalIdGenerator = null;
 
   @Override
   public GraphName getDefaultNodeName() {
@@ -30,23 +29,20 @@ 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);
 
-    // publish a goal message
+    // Create Fibonacci goal message
     FibonacciActionGoal goalMessage = (FibonacciActionGoal)ac.newGoalMessage();
     FibonacciGoal fibonacciGoal = goalMessage.getGoal();
 
     // set Fibonacci parameter
     fibonacciGoal.setOrder(6);
-    goalMessage.setGoal(fibonacciGoal);
 
     while (repeat > 0) {
       sleep(10000);
       System.out.println("Sending goal #" + repeat + "...");
-      sendGoal(goalMessage);
+      ac.sendGoal(goalMessage);
       System.out.println("Goal sent.");
       //while(!resultReceived) sleep(100);
       resultReceived = false;
@@ -90,17 +86,11 @@ public class TestClient extends AbstractNodeMain implements ActionClientListener
     }
   }
 
-  private void sendGoal(FibonacciActionGoal goal) {
-    goalIdGenerator.generateID(goal.getGoalId());
-    ac.sendGoal(goal);
-  }
-
   void sleep(long msec) {
     try {
       Thread.sleep(msec);
     }
     catch (InterruptedException ex) {
-      ;
     }
   }
 }