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 6bc5fd2ff9db6c0a48f5aa0530e85e34b44ad9dc..c45658aa725d49a3df67493281f3ded44ed24318 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
@@ -26,9 +26,16 @@ import actionlib_msgs.GoalID;
  * Class to encapsulate the action goal object.
  * @author Ernesto Corbellini ecorbellini@ekumenlabs.com
  */
-public class ActionGoal<T_ACTION_GOAL extends Message, T_GOAL extends Message> {
+public class ActionGoal<T_ACTION_GOAL extends Message> {
   private T_ACTION_GOAL goalMessage = null;
 
+  public ActionGoal(T_ACTION_GOAL ag) {
+    goalMessage = ag;
+  }
+
+  public ActionGoal() {
+  }
+
   /**
    * Return the sequence number of the action goal message's header.
    * @return The sequence number of the std_msgs.Header or -1 if there is an error.
@@ -198,7 +205,7 @@ public class ActionGoal<T_ACTION_GOAL extends Message, T_GOAL extends Message> {
       try {
         Method m = goalMessage.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(goalMessage);
+        gid = (GoalID)m.invoke(goalMessage);
       }
       catch (Exception e) {
         e.printStackTrace(System.out);
@@ -219,13 +226,13 @@ public class ActionGoal<T_ACTION_GOAL extends Message, T_GOAL extends Message> {
 
   }
 
-  public T_GOAL getGoalMessage() {
-    T_GOAL g = null;
+  public Message getGoalMessage() {
+    Message g = null;
     if (goalMessage != null) {
       try {
         Method m = goalMessage.getClass().getMethod("getGoal");
         m.setAccessible(true); // workaround for known bug http://bugs.java.com/bugdatabase/view_bug.do?bug_id=6924232
-        g= (T_GOAL)m.invoke(goalMessage);
+        g = (Message)m.invoke(goalMessage);
       }
       catch (Exception e) {
         e.printStackTrace(System.out);
@@ -233,8 +240,18 @@ public class ActionGoal<T_ACTION_GOAL extends Message, T_GOAL extends Message> {
     }
     return g;
   }
-  public void setGoalMessage(T_GOAL gm) {
 
+  public void setGoalMessage(Message gm) {
+    if (goalMessage != null) {
+      try {
+        Method m = goalMessage.getClass().getMethod("setGoal", Message.class);
+        m.setAccessible(true); // workaround for known bug http://bugs.java.com/bugdatabase/view_bug.do?bug_id=6924232
+        m.invoke(goalMessage, gm);
+      }
+      catch (Exception e) {
+        e.printStackTrace(System.out);
+      }
+    }
   }
 
   public boolean equals(ActionGoal ag) {