proxy_action_client.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 import rospy
3 import actionlib
4 from threading import Timer
5 
6 from flexbe_core.logger import Logger
7 
8 
9 class ProxyActionClient(object):
10  """
11  A proxy for calling actions.
12  """
13  _clients = {}
14 
15  _result = {}
16  _feedback = {}
17 
18  def __init__(self, topics={}, wait_duration=10):
19  """
20  Initializes the proxy with optionally a given set of clients.
21 
22  @type topics: dictionary string - message class
23  @param topics: A dictionay containing a collection of topic - message type pairs.
24 
25  @type wait_duration: int
26  @param wait_duration: Defines how long to wait for each client in the
27  given set to become available (if it is not already available).
28  """
29  for topic, msg_type in topics.items():
30  self.setupClient(topic, msg_type, wait_duration)
31 
32  def setupClient(self, topic, msg_type, wait_duration=10):
33  """
34  Tries to set up an action client for calling it later.
35 
36  @type topic: string
37  @param topic: The topic of the action to call.
38 
39  @type msg_type: msg type
40  @param msg_type: The type of messages of this action client.
41 
42  @type wait_duration: int
43  @param wait_duration: Defines how long to wait for the given client if it is not available right now.
44  """
45  if topic not in ProxyActionClient._clients:
46  ProxyActionClient._clients[topic] = actionlib.SimpleActionClient(topic, msg_type)
47  self._check_topic_available(topic, wait_duration)
48 
49  def send_goal(self, topic, goal):
50  """
51  Performs an action call on the given topic.
52 
53  @type topic: string
54  @param topic: The topic to call.
55 
56  @type goal: action goal
57  @param goal: The request to send to the action server.
58  """
59  if not self._check_topic_available(topic):
60  raise ValueError('Cannot send goal for action client %s: Topic not available.' % topic)
61  # reset previous results
62  ProxyActionClient._result[topic] = None
63  ProxyActionClient._feedback[topic] = None
64  # send goal
65  ProxyActionClient._clients[topic].send_goal(
66  goal,
67  done_cb=lambda ts, r: self._done_callback(topic, ts, r),
68  feedback_cb=lambda f: self._feedback_callback(topic, f)
69  )
70 
71  def _done_callback(self, topic, terminal_state, result):
72  ProxyActionClient._result[topic] = result
73 
74  def _feedback_callback(self, topic, feedback):
75  ProxyActionClient._feedback[topic] = feedback
76 
77  def is_available(self, topic):
78  """
79  Checks if the client on the given action topic is available.
80 
81  @type topic: string
82  @param topic: The topic of interest.
83  """
84  return self._check_topic_available(topic)
85 
86  def has_result(self, topic):
87  """
88  Checks if the given action call already has a result.
89 
90  @type topic: string
91  @param topic: The topic of interest.
92  """
93  return ProxyActionClient._result.get(topic) is not None
94 
95  def get_result(self, topic):
96  """
97  Returns the result message of the given action call.
98 
99  @type topic: string
100  @param topic: The topic of interest.
101  """
102  return ProxyActionClient._result.get(topic)
103 
104  def remove_result(self, topic):
105  """
106  Removes the latest result message of the given action call.
107 
108  @type topic: string
109  @param topic: The topic of interest.
110  """
111  ProxyActionClient._result[topic] = None
112 
113  def has_feedback(self, topic):
114  """
115  Checks if the given action call has any feedback.
116 
117  @type topic: string
118  @param topic: The topic of interest.
119  """
120  return ProxyActionClient._feedback.get(topic) is not None
121 
122  def get_feedback(self, topic):
123  """
124  Returns the latest feedback message of the given action call.
125 
126  @type topic: string
127  @param topic: The topic of interest.
128  """
129  return ProxyActionClient._feedback.get(topic)
130 
131  def remove_feedback(self, topic):
132  """
133  Removes the latest feedback message of the given action call.
134 
135  @type topic: string
136  @param topic: The topic of interest.
137  """
138  ProxyActionClient._feedback[topic] = None
139 
140  def get_state(self, topic):
141  """
142  Determines the current actionlib state of the given action topic.
143  A list of possible states is defined in actionlib_msgs/GoalStatus.
144 
145  @type topic: string
146  @param topic: The topic of interest.
147  """
148  return ProxyActionClient._clients[topic].get_state()
149 
150  def is_active(self, topic):
151  """
152  Determines if an action request is already being processed on the given topic.
153 
154  @type topic: string
155  @param topic: The topic of interest.
156  """
157  return ProxyActionClient._clients[topic].simple_state != actionlib.SimpleGoalState.DONE
158 
159  def cancel(self, topic):
160  """
161  Cancels the current action call on the given action topic.
162 
163  @type topic: string
164  @param topic: The topic of interest.
165  """
166  ProxyActionClient._clients[topic].cancel_goal()
167 
168  def _check_topic_available(self, topic, wait_duration=1):
169  """
170  Checks whether a topic is available.
171 
172  @type topic: string
173  @param topic: The topic of the action.
174 
175  @type wait_duration: int
176  @param wait_duration: Defines how long to wait for the given client if it is not available right now.
177  """
178  client = ProxyActionClient._clients.get(topic)
179  if client is None:
180  Logger.logerr("Action client %s not yet registered, need to add it first!" % topic)
181  return False
182  t = Timer(.5, self._print_wait_warning, [topic])
183  t.start()
184  available = client.wait_for_server(rospy.Duration.from_sec(wait_duration))
185  warning_sent = False
186  try:
187  t.cancel()
188  except Exception:
189  # already printed the warning
190  warning_sent = True
191 
192  if not available:
193  Logger.logerr("Action client %s timed out!" % topic)
194  return False
195  else:
196  if warning_sent:
197  Logger.loginfo("Finally found action client %s..." % (topic))
198  return True
199 
200  def _print_wait_warning(self, topic):
201  Logger.logwarn("Waiting for action client %s..." % (topic))
def _done_callback(self, topic, terminal_state, result)
def setupClient(self, topic, msg_type, wait_duration=10)


flexbe_core
Author(s): Philipp Schillinger
autogenerated on Sun Dec 13 2020 04:01:39