From 08885cec1625f34a16a16210fa9b3d2d9690574c Mon Sep 17 00:00:00 2001 From: Ernesto Corbellini Date: Thu, 14 Jan 2016 18:36:04 -0300 Subject: [PATCH] Added class to wrap an action goal. --- .../ekumen/rosjava_actionlib/ActionGoal.java | 253 ++++++++++++++++-- 1 file changed, 231 insertions(+), 22 deletions(-) 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 babb6d0..1a368c3 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 @@ -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 ecorbellini@ekumenlabs.com + */ +public class ActionGoal { + 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 ag) { + return (this.getGoalId() == ag.getGoalId()); } }