Added class to wrap an action goal.

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

@@ -1,34 +1,243 @@
+/**
+ * Copyright 2015 Ekumen www.ekumenlabs.com
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
 
 package com.github.ekumen.rosjava_actionlib;
 
-public class ActionGoal
-{
-  private int state;
+import java.lang.reflect.Method;
+import org.ros.internal.message.Message;
+import org.ros.message.Time;
+import std_msgs.Header;
+import actionlib_msgs.GoalID;
+
+/**
+ * Class to encapsulate the action goal object.
+ * @author Ernesto Corbellini [email protected]
+ */
+public class ActionGoal<T_ACTION_GOAL extends Message, T_GOAL extends Message> {
+  private T_ACTION_GOAL goalMessage = null;
+
+  /**
+   * 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.
+   * @see std_msgs.Header
+   */
+  public int getHeaderSequence() {
+    int seq = -1;
+    Header h = getHeaderMessage();
+    if (h != null) {
+      try {
+        Method m = h.getClass().getMethod("getSeq");
+        m.setAccessible(true); // workaround for known bug http://bugs.java.com/bugdatabase/view_bug.do?bug_id=6924232
+        seq = (int)m.invoke(h);
+      }
+      catch (Exception e) {
+        e.printStackTrace(System.out);
+      }
+    }
+    return seq;
+  }
+
+  /**
+   * Set the sequence number of the action goal message's header.
+   * @param  seq The sequence number for the std_msgs.Header.
+   * @see std_msgs.Header
+   */
+  public void setHeaderSequence(int seq) {
+    Header h = getHeaderMessage();
+    if (h != null) {
+      try {
+        Method m = h.getClass().getMethod("setSeq", Integer.class);
+        m.setAccessible(true); // workaround for known bug http://bugs.java.com/bugdatabase/view_bug.do?bug_id=6924232
+        m.invoke(h, seq);
+      }
+      catch (Exception e) {
+        e.printStackTrace(System.out);
+      }
+    }
+  }
+
+  /**
+   * Return the time stamp of the action goal message's header.
+   * @return The time stamp (org.ros.message.Time) of the std_msgs.Header or null otherwise.
+   * @see org.ros.message.Time
+   */
+  public Time getHeaderTimestamp() {
+    Time t = null;
+    Header h = getHeaderMessage();
+    if (h != null) {
+      try {
+        Method m = h.getClass().getMethod("getStamp");
+        m.setAccessible(true); // workaround for known bug http://bugs.java.com/bugdatabase/view_bug.do?bug_id=6924232
+        t = (Time)m.invoke(h);
+      }
+      catch (Exception e) {
+        e.printStackTrace(System.out);
+      }
+    }
+    return t;
+  }
+
+  /**
+   * Sets the time stamp for the action goal message's header.
+   * @param t The time stamp (org.ros.message.Time) of the std_msgs.Header.
+   * @see org.ros.message.Time
+   */
+  public void setHeaderTimestamp(Time t) {
+    Header h = getHeaderMessage();
+    if (h != null) {
+      try {
+        Method m = h.getClass().getMethod("setStamp", Time.class);
+        m.setAccessible(true); // workaround for known bug http://bugs.java.com/bugdatabase/view_bug.do?bug_id=6924232
+        m.invoke(h, t);
+      }
+      catch (Exception e) {
+        e.printStackTrace(System.out);
+      }
+    }
+  }
+
+  public String getHeaderFrameId() {
+    return null;
+  }
+
+  public void setHeaderFrameId(String id) {
+
+  }
+
+  /**
+   * Return the standard actionlib header message for this action goal.
+   * @return The std_msgs.Header object or null otherwise.
+   * @see std_msgs.Header
+   */
+  public Header getHeaderMessage() {
+    Header h = null;
+    if (goalMessage != null) {
+      try {
+        Method m = goalMessage.getClass().getMethod("getHeader");
+        m.setAccessible(true); // workaround for known bug http://bugs.java.com/bugdatabase/view_bug.do?bug_id=6924232
+        h = (Header)m.invoke(goalMessage);
+      }
+      catch (Exception e) {
+        e.printStackTrace(System.out);
+      }
+    }
+    return h;
+  }
+
+  public void setHeaderMessage(Header h) {
+
+  }
+
+  public String getGoalId() {
+    String id = null;
+    GoalID gid = getGoalIdMessage();
+    if (gid != null) {
+      try {
+        Method m = gid.getClass().getMethod("getId");
+        m.setAccessible(true); // workaround for known bug http://bugs.java.com/bugdatabase/view_bug.do?bug_id=6924232
+        id = (String)m.invoke(gid);
+      }
+      catch (Exception e) {
+        e.printStackTrace(System.out);
+      }
+    }
+    return id;
+  }
+
+  /**
+   * Set the action goal's goal ID string and timestamp.
+   * @param id Identification string for this goal.
+   * @param t Time stamp (org.ros.message.Time).
+   */
+  public void setGoalId(String id, Time t) {
+    GoalID gid = getGoalIdMessage();
+    if (gid != null) {
+      try {
+        Method m = gid.getClass().getMethod("setId", String.class);
+        m.setAccessible(true); // workaround for known bug http://bugs.java.com/bugdatabase/view_bug.do?bug_id=6924232
+        m.invoke(gid, id);
+        m = gid.getClass().getMethod("setStamp", Time.class);
+        m.setAccessible(true); // workaround for known bug http://bugs.java.com/bugdatabase/view_bug.do?bug_id=6924232
+        m.invoke(gid, t);
+      }
+      catch (Exception e) {
+        e.printStackTrace(System.out);
+      }
+    }
+  }
+
+  public Time getGoalIdTimestamp() {
+    return null;
+  }
+
+  public void setGoalIdTimestamp(Time t) {
+
+  }
+
+  /**
+   * Return the actionlib GoalID message for this action goal.
+   * @return The actionlib_msgs.GoalID object or null otherwise.
+   * @see actionlib_msgs.GoalID
+   */
+  public GoalID getGoalIdMessage() {
+    GoalID gid = null;
+    if (goalMessage != null) {
+      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);
+      }
+      catch (Exception e) {
+        e.printStackTrace(System.out);
+      }
+    }
+    return gid;
+  }
+
+  public void setGoalIdMessage(GoalID gid) {
+
+  }
+
+  public T_ACTION_GOAL getActionGoalMessage() {
+    return goalMessage;
+  }
+
+  public void setActionGoalMessage(T_ACTION_GOAL agm) {
 
-  public static class GoalStates
-  {
-    public final static int PENDING = 0;  // The goal has yet to be processed by the action server
-    public final static int ACTIVE = 1;  // The goal is currently being processed by the action server
-    public final static int REJECTED = 2;  // The goal was rejected by the action server without being processed and without a request from the action client to cancel
-    public final static int SUCCEEDED = 3;  // The goal was achieved successfully by the action server
-    public final static int ABORTED = 4;  // The goal was aborted by the action server
-    public final static int PREEMPTING = 5;  // Processing of the goal was canceled by either another goal, or a cancel request sent to the action server
-    public final static int PREEMPTED = 6;  // The goal was preempted by either another goal, or a preempt message being sent to the action server
-    public final static int RECALLING = 7;  // The goal has not been processed and a cancel request has been received from the action client, but the action server has not confirmed the goal is canceled
-    public final static int RECALLED = 8; // The goal was canceled by either another goal, or a cancel request before the action server began processing the goal
-    public final static int LOST = 9;  // The goal was sent
   }
 
-  int findStatus(int statusArray)
-  {
-    return 0;
+  public T_GOAL getGoalMessage() {
+    T_GOAL 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);
+      }
+      catch (Exception e) {
+        e.printStackTrace(System.out);
+      }
+    }
+    return g;
   }
+  public void setGoalMessage(T_GOAL gm) {
 
-  public synchronized void setState(int state) {
-    this.state = state;
   }
 
-  public synchronized int getState() {
-    return this.state;
+  public boolean equals(ActionGoal<T_ACTION_GOAL> ag) {
+    return (this.getGoalId() == ag.getGoalId());
   }
 }