simple_action_client.py
Go to the documentation of this file.
1 #! /usr/bin/env python
2 # Copyright (c) 2009, Willow Garage, Inc.
3 # All rights reserved.
4 #
5 # Redistribution and use in source and binary forms, with or without
6 # modification, are permitted provided that the following conditions are met:
7 #
8 # * Redistributions of source code must retain the above copyright
9 # notice, this list of conditions and the following disclaimer.
10 # * Redistributions in binary form must reproduce the above copyright
11 # notice, this list of conditions and the following disclaimer in the
12 # documentation and/or other materials provided with the distribution.
13 # * Neither the name of the Willow Garage, Inc. nor the names of its
14 # contributors may be used to endorse or promote products derived from
15 # this software without specific prior written permission.
16 #
17 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 # POSSIBILITY OF SUCH DAMAGE.
28 
29 # Author: Stuart Glaser
30 import rospy
31 import threading
32 
33 from actionlib_msgs.msg import GoalStatus
34 from actionlib.action_client import ActionClient, CommState, get_name_of_constant
35 
36 
38  PENDING = 0
39  ACTIVE = 1
40  DONE = 2
41 
42 
43 SimpleGoalState.to_string = classmethod(get_name_of_constant)
44 
45 
47  ## @brief Constructs a SimpleActionClient and opens connections to an ActionServer.
48 
54  def __init__(self, ns, ActionSpec):
55  self.action_client = ActionClient(ns, ActionSpec)
56  self.simple_state = SimpleGoalState.DONE
57  self.gh = None
58  self.done_condition = threading.Condition()
59 
60  ## @brief Blocks until the action server connects to this client
61 
66  def wait_for_server(self, timeout=rospy.Duration()):
67  return self.action_client.wait_for_server(timeout)
68 
69  ## @brief Sends a goal to the ActionServer, and also registers callbacks
70 
83  def send_goal(self, goal, done_cb=None, active_cb=None, feedback_cb=None):
84  # destroys the old goal handle
85  self.stop_tracking_goal()
86 
87  self.done_cb = done_cb
88  self.active_cb = active_cb
89  self.feedback_cb = feedback_cb
90 
91  self.simple_state = SimpleGoalState.PENDING
92  self.gh = self.action_client.send_goal(goal, self._handle_transition, self._handle_feedback)
93 
94  ## @brief Sends a goal to the ActionServer, waits for the goal to complete, and preempts goal is necessary
95 
109  def send_goal_and_wait(self, goal, execute_timeout=rospy.Duration(), preempt_timeout=rospy.Duration()):
110  self.send_goal(goal)
111  if not self.wait_for_result(execute_timeout):
112  # preempt action
113  rospy.logdebug("Canceling goal")
114  self.cancel_goal()
115  if self.wait_for_result(preempt_timeout):
116  rospy.logdebug("Preempt finished within specified preempt_timeout [%.2f]", preempt_timeout.to_sec())
117  else:
118  rospy.logdebug("Preempt didn't finish specified preempt_timeout [%.2f]", preempt_timeout.to_sec())
119  return self.get_state()
120 
121  ## @brief Blocks until this goal transitions to done
122  ## @param timeout Max time to block before returning. A zero timeout is interpreted as an infinite timeout.
123  ## @return True if the goal finished. False if the goal didn't finish within the allocated timeout
124  def wait_for_result(self, timeout=rospy.Duration()):
125  if not self.gh:
126  rospy.logerr("Called wait_for_result when no goal exists")
127  return False
128 
129  timeout_time = rospy.get_rostime() + timeout
130  loop_period = rospy.Duration(0.1)
131  with self.done_condition:
132  while not rospy.is_shutdown():
133  time_left = timeout_time - rospy.get_rostime()
134  if timeout > rospy.Duration(0.0) and time_left <= rospy.Duration(0.0):
135  break
136 
137  if self.simple_state == SimpleGoalState.DONE:
138  break
139 
140  if time_left > loop_period or timeout == rospy.Duration():
141  time_left = loop_period
142 
143  self.done_condition.wait(time_left.to_sec())
144 
145  return self.simple_state == SimpleGoalState.DONE
146 
147  ## @brief Gets the Result of the current goal
148  def get_result(self):
149  if not self.gh:
150  rospy.logerr("Called get_result when no goal is running")
151  return None
152 
153  return self.gh.get_result()
154 
155  ## @brief Get the state information for this goal
156 
162  def get_state(self):
163  if not self.gh:
164  return GoalStatus.LOST
165  status = self.gh.get_goal_status()
166 
167  if status == GoalStatus.RECALLING:
168  status = GoalStatus.PENDING
169  elif status == GoalStatus.PREEMPTING:
170  status = GoalStatus.ACTIVE
171 
172  return status
173 
174  ## @brief Returns the current status text of the goal.
175 
181  if not self.gh:
182  rospy.logerr("Called get_goal_status_text when no goal is running")
183  return "ERROR: Called get_goal_status_text when no goal is running"
184 
185  return self.gh.get_goal_status_text()
186 
187  ## @brief Cancels all goals currently running on the action server
188 
191  def cancel_all_goals(self):
192  self.action_client.cancel_all_goals()
193 
194  ## @brief Cancels all goals prior to a given timestamp
195 
200  self.action_client.cancel_goals_at_and_before_time(time)
201 
202  ## @brief Cancels the goal that we are currently pursuing
203  def cancel_goal(self):
204  if self.gh:
205  self.gh.cancel()
206 
207  ## @brief Stops tracking the state of the current goal. Unregisters this goal's callbacks
208 
212  self.gh = None
213 
214  def _handle_transition(self, gh):
215  comm_state = gh.get_comm_state()
216 
217  error_msg = "Received comm state %s when in simple state %s with SimpleActionClient in NS %s" % \
218  (CommState.to_string(comm_state), SimpleGoalState.to_string(self.simple_state), rospy.resolve_name(self.action_client.ns))
219 
220  if comm_state == CommState.ACTIVE:
221  if self.simple_state == SimpleGoalState.PENDING:
222  self._set_simple_state(SimpleGoalState.ACTIVE)
223  if self.active_cb:
224  self.active_cb()
225  elif self.simple_state == SimpleGoalState.DONE:
226  rospy.logerr(error_msg)
227  elif comm_state == CommState.RECALLING:
228  if self.simple_state != SimpleGoalState.PENDING:
229  rospy.logerr(error_msg)
230  elif comm_state == CommState.PREEMPTING:
231  if self.simple_state == SimpleGoalState.PENDING:
232  self._set_simple_state(SimpleGoalState.ACTIVE)
233  if self.active_cb:
234  self.active_cb()
235  elif self.simple_state == SimpleGoalState.DONE:
236  rospy.logerr(error_msg)
237  elif comm_state == CommState.DONE:
238  if self.simple_state in [SimpleGoalState.PENDING, SimpleGoalState.ACTIVE]:
239  self._set_simple_state(SimpleGoalState.DONE)
240  if self.done_cb:
241  self.done_cb(gh.get_goal_status(), gh.get_result())
242  with self.done_condition:
243  self.done_condition.notifyAll()
244  elif self.simple_state == SimpleGoalState.DONE:
245  rospy.logerr("SimpleActionClient received DONE twice")
246 
247  def _handle_feedback(self, gh, feedback):
248  if not self.gh:
249  # this is not actually an error - there can be a small window in which old feedback
250  # can be received between the time this variable is reset and a new goal is
251  # sent and confirmed
252  return
253  if gh != self.gh:
254  rospy.logerr("Got a feedback callback on a goal handle that we're not tracking. %s vs %s" %
255  (self.gh.comm_state_machine.action_goal.goal_id.id, gh.comm_state_machine.action_goal.goal_id.id))
256  return
257  if self.feedback_cb:
258  self.feedback_cb(feedback)
259 
260  def _set_simple_state(self, state):
261  self.simple_state = state
def cancel_goal(self)
Cancels the goal that we are currently pursuing.
def get_result(self)
Gets the Result of the current goal.
def send_goal(self, goal, done_cb=None, active_cb=None, feedback_cb=None)
Sends a goal to the ActionServer, and also registers callbacks.
def __init__(self, ns, ActionSpec)
Constructs a SimpleActionClient and opens connections to an ActionServer.
def wait_for_server(self, timeout=rospy.Duration())
Blocks until the action server connects to this client.
def cancel_goals_at_and_before_time(self, time)
Cancels all goals prior to a given timestamp.
def get_goal_status_text(self)
Returns the current status text of the goal.
def get_state(self)
Get the state information for this goal.
def send_goal_and_wait(self, goal, execute_timeout=rospy.Duration(), preempt_timeout=rospy.Duration())
Sends a goal to the ActionServer, waits for the goal to complete, and preempts goal is necessary...
def cancel_all_goals(self)
Cancels all goals currently running on the action server.
def stop_tracking_goal(self)
Stops tracking the state of the current goal.
def wait_for_result(self, timeout=rospy.Duration())
Blocks until this goal transitions to done.


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep, Mikael Arguedas
autogenerated on Mon Aug 24 2020 03:40:47