Go to the documentation of this file.00001
00002
00003 import roslib; roslib.load_manifest('flexbe_core')
00004 import rospy
00005 import actionlib
00006 from threading import Timer
00007 import time
00008
00009 from flexbe_core.logger import Logger
00010
00011
00012 class ProxyActionClient(object):
00013 """
00014 A proxy for calling actions.
00015 """
00016 _clients = {}
00017
00018 _result = {}
00019 _feedback = {}
00020
00021 def __init__(self, topics={}, wait_duration=10):
00022 """
00023 Initializes the proxy with optionally a given set of clients.
00024
00025 @type topics: dictionary string - message class
00026 @param topics: A dictionay containing a collection of topic - message type pairs.
00027
00028 @type wait_duration: int
00029 @param wait_duration: Defines how long to wait for each client in the
00030 given set to become available (if it is not already available).
00031 """
00032
00033 for topic, msg_type in topics.iteritems():
00034 self.setupClient(topic, msg_type, wait_duration)
00035
00036
00037 def setupClient(self, topic, msg_type, wait_duration=10):
00038 """
00039 Tries to set up an action client for calling it later.
00040
00041 @type topic: string
00042 @param topic: The topic of the action to call.
00043
00044 @type msg_type: msg type
00045 @param msg_type: The type of messages of this action client.
00046
00047 @type wait_duration: int
00048 @param wait_duration: Defines how long to wait for the given client if it is not available right now.
00049 """
00050 if topic not in ProxyActionClient._clients:
00051 client = actionlib.SimpleActionClient(topic, msg_type)
00052 t = Timer(1, self._print_wait_warning, [topic])
00053 t.start()
00054 available = client.wait_for_server(rospy.Duration.from_sec(wait_duration))
00055 warning_sent = False
00056 try:
00057 t.cancel()
00058 except Exception as ve:
00059
00060 warning_sent = True
00061
00062 if not available:
00063 Logger.logerr("Action client %s timed out!" % topic)
00064 else:
00065 ProxyActionClient._clients[topic] = client
00066 if warning_sent:
00067 Logger.loginfo("Finally found action client %s..." % (topic))
00068
00069
00070 def send_goal(self, topic, goal):
00071 """
00072 Performs an action call on the given topic.
00073
00074 @type topic: string
00075 @param topic: The topic to call.
00076
00077 @type goal: action goal
00078 @param goal: The request to send to the action server.
00079 """
00080 if topic not in ProxyActionClient._clients:
00081 raise ValueError('ProxyActionClient: topic %s not yet registered!' % topic)
00082
00083 ProxyActionClient._result[topic] = None
00084 ProxyActionClient._feedback[topic] = None
00085
00086 ProxyActionClient._clients[topic].send_goal(goal,
00087 done_cb = lambda ts, r: self._done_callback(topic, ts, r),
00088 feedback_cb = lambda f: self._feedback_callback(topic, f)
00089 )
00090
00091 def _done_callback(self, topic, terminal_state, result):
00092 ProxyActionClient._result[topic] = result
00093
00094 def _feedback_callback(self, topic, feedback):
00095 ProxyActionClient._feedback[topic] = feedback
00096
00097
00098 def is_available(self, topic):
00099 """
00100 Checks if the client on the given action topic is available.
00101
00102 @type topic: string
00103 @param topic: The topic of interest.
00104 """
00105 return topic in ProxyActionClient._clients
00106
00107 def has_result(self, topic):
00108 """
00109 Checks if the given action call already has a result.
00110
00111 @type topic: string
00112 @param topic: The topic of interest.
00113 """
00114 return ProxyActionClient._result[topic] is not None
00115
00116 def get_result(self, topic):
00117 """
00118 Returns the result message of the given action call.
00119
00120 @type topic: string
00121 @param topic: The topic of interest.
00122 """
00123 return ProxyActionClient._result[topic]
00124
00125 def remove_result(self, topic):
00126 """
00127 Removes the latest result message of the given action call.
00128
00129 @type topic: string
00130 @param topic: The topic of interest.
00131 """
00132 ProxyActionClient._result[topic] = None
00133
00134 def has_feedback(self, topic):
00135 """
00136 Checks if the given action call has any feedback.
00137
00138 @type topic: string
00139 @param topic: The topic of interest.
00140 """
00141 return ProxyActionClient._feedback[topic] is not None
00142
00143 def get_feedback(self, topic):
00144 """
00145 Returns the latest feedback message of the given action call.
00146
00147 @type topic: string
00148 @param topic: The topic of interest.
00149 """
00150 return ProxyActionClient._feedback[topic]
00151
00152 def remove_feedback(self, topic):
00153 """
00154 Removes the latest feedback message of the given action call.
00155
00156 @type topic: string
00157 @param topic: The topic of interest.
00158 """
00159 ProxyActionClient._feedback[topic] = None
00160
00161 def get_state(self, topic):
00162 """
00163 Determines the current actionlib state of the given action topic.
00164 A list of possible states is defined in actionlib_msgs/GoalStatus.
00165
00166 @type topic: string
00167 @param topic: The topic of interest.
00168 """
00169 return ProxyActionClient._clients[topic].get_state()
00170
00171 def is_active(self, topic):
00172 """
00173 Determines if an action request is already being processed on the given topic.
00174
00175 @type topic: string
00176 @param topic: The topic of interest.
00177 """
00178 return ProxyActionClient._clients[topic].simple_state != actionlib.SimpleGoalState.DONE
00179
00180 def cancel(self, topic):
00181 """
00182 Cancels the current action call on the given action topic.
00183
00184 @type topic: string
00185 @param topic: The topic of interest.
00186 """
00187 ProxyActionClient._clients[topic].cancel_goal()
00188
00189 def _print_wait_warning(self, topic):
00190 Logger.logwarn("Waiting for action client %s..." % (topic))