simple_action_client.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 # Copyright (c) 2009, Willow Garage, Inc.
00003 # All rights reserved.
00004 #
00005 # Redistribution and use in source and binary forms, with or without
00006 # modification, are permitted provided that the following conditions are met:
00007 #
00008 #     * Redistributions of source code must retain the above copyright
00009 #       notice, this list of conditions and the following disclaimer.
00010 #     * Redistributions in binary form must reproduce the above copyright
00011 #       notice, this list of conditions and the following disclaimer in the
00012 #       documentation and/or other materials provided with the distribution.
00013 #     * Neither the name of the Willow Garage, Inc. nor the names of its
00014 #       contributors may be used to endorse or promote products derived from
00015 #       this software without specific prior written permission.
00016 #
00017 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027 # POSSIBILITY OF SUCH DAMAGE.
00028 
00029 # Author: Stuart Glaser
00030 import rospy
00031 import threading
00032 
00033 from actionlib_msgs.msg import GoalStatus
00034 from actionlib.action_client import ActionClient, CommState, get_name_of_constant
00035 
00036 
00037 class SimpleGoalState:
00038     PENDING = 0
00039     ACTIVE = 1
00040     DONE = 2
00041 
00042 
00043 SimpleGoalState.to_string = classmethod(get_name_of_constant)
00044 
00045 
00046 class SimpleActionClient:
00047     ## @brief Constructs a SimpleActionClient and opens connections to an ActionServer.
00048     ##
00049     ## @param ns The namespace in which to access the action.  For
00050     ## example, the "goal" topic should occur under ns/goal
00051     ##
00052     ## @param ActionSpec The *Action message type.  The SimpleActionClient
00053     ## will grab the other message types from this type.
00054     def __init__(self, ns, ActionSpec):
00055         self.action_client = ActionClient(ns, ActionSpec)
00056         self.simple_state = SimpleGoalState.DONE
00057         self.gh = None
00058         self.done_condition = threading.Condition()
00059 
00060     ## @brief Blocks until the action server connects to this client
00061     ##
00062     ## @param timeout Max time to block before returning. A zero
00063     ## timeout is interpreted as an infinite timeout.
00064     ##
00065     ## @return True if the server connected in the allocated time. False on timeout
00066     def wait_for_server(self, timeout=rospy.Duration()):
00067         return self.action_client.wait_for_server(timeout)
00068 
00069     ## @brief Sends a goal to the ActionServer, and also registers callbacks
00070     ##
00071     ## If a previous goal is already active when this is called. We simply forget
00072     ## about that goal and start tracking the new goal. No cancel requests are made.
00073     ##
00074     ## @param done_cb Callback that gets called on transitions to
00075     ## Done.  The callback should take two parameters: the terminal
00076     ## state (as an integer from actionlib_msgs/GoalStatus) and the
00077     ## result.
00078     ##
00079     ## @param active_cb   No-parameter callback that gets called on transitions to Active.
00080     ##
00081     ## @param feedback_cb Callback that gets called whenever feedback
00082     ## for this goal is received.  Takes one parameter: the feedback.
00083     def send_goal(self, goal, done_cb=None, active_cb=None, feedback_cb=None):
00084         # destroys the old goal handle
00085         self.stop_tracking_goal()
00086 
00087         self.done_cb = done_cb
00088         self.active_cb = active_cb
00089         self.feedback_cb = feedback_cb
00090 
00091         self.simple_state = SimpleGoalState.PENDING
00092         self.gh = self.action_client.send_goal(goal, self._handle_transition, self._handle_feedback)
00093 
00094     ## @brief Sends a goal to the ActionServer, waits for the goal to complete, and preempts goal is necessary
00095     ##
00096     ## If a previous goal is already active when this is called. We simply forget
00097     ## about that goal and start tracking the new goal. No cancel requests are made.
00098     ##
00099     ## If the goal does not complete within the execute_timeout, the goal gets preempted
00100     ##
00101     ## If preemption of the goal does not complete withing the preempt_timeout, this
00102     ## method simply returns
00103     ##
00104     ## @param execute_timeout The time to wait for the goal to complete
00105     ##
00106     ## @param preempt_timeout The time to wait for preemption to complete
00107     ##
00108     ## @return The goal's state.
00109     def send_goal_and_wait(self, goal, execute_timeout=rospy.Duration(), preempt_timeout=rospy.Duration()):
00110         self.send_goal(goal)
00111         if not self.wait_for_result(execute_timeout):
00112             # preempt action
00113             rospy.logdebug("Canceling goal")
00114             self.cancel_goal()
00115             if self.wait_for_result(preempt_timeout):
00116                 rospy.logdebug("Preempt finished within specified preempt_timeout [%.2f]", preempt_timeout.to_sec())
00117             else:
00118                 rospy.logdebug("Preempt didn't finish specified preempt_timeout [%.2f]", preempt_timeout.to_sec())
00119         return self.get_state()
00120 
00121     ## @brief Blocks until this goal transitions to done
00122     ## @param timeout Max time to block before returning. A zero timeout is interpreted as an infinite timeout.
00123     ## @return True if the goal finished. False if the goal didn't finish within the allocated timeout
00124     def wait_for_result(self, timeout=rospy.Duration()):
00125         if not self.gh:
00126             rospy.logerr("Called wait_for_goal_to_finish when no goal exists")
00127             return False
00128 
00129         timeout_time = rospy.get_rostime() + timeout
00130         loop_period = rospy.Duration(0.1)
00131         with self.done_condition:
00132             while not rospy.is_shutdown():
00133                 time_left = timeout_time - rospy.get_rostime()
00134                 if timeout > rospy.Duration(0.0) and time_left <= rospy.Duration(0.0):
00135                     break
00136 
00137                 if self.simple_state == SimpleGoalState.DONE:
00138                     break
00139 
00140                 if time_left > loop_period or timeout == rospy.Duration():
00141                     time_left = loop_period
00142 
00143                 self.done_condition.wait(time_left.to_sec())
00144 
00145         return self.simple_state == SimpleGoalState.DONE
00146 
00147     ## @brief Gets the Result of the current goal
00148     def get_result(self):
00149         if not self.gh:
00150             rospy.logerr("Called get_result when no goal is running")
00151             return None
00152 
00153         return self.gh.get_result()
00154 
00155     ## @brief Get the state information for this goal
00156     ##
00157     ## Possible States Are: PENDING, ACTIVE, RECALLED, REJECTED,
00158     ## PREEMPTED, ABORTED, SUCCEEDED, LOST.
00159     ##
00160     ## @return The goal's state. Returns LOST if this
00161     ## SimpleActionClient isn't tracking a goal.
00162     def get_state(self):
00163         if not self.gh:
00164             rospy.logerr("Called get_state when no goal is running")
00165             return GoalStatus.LOST
00166         status = self.gh.get_goal_status()
00167 
00168         if status == GoalStatus.RECALLING:
00169             status = GoalStatus.PENDING
00170         elif status == GoalStatus.PREEMPTING:
00171             status = GoalStatus.ACTIVE
00172 
00173         return status
00174 
00175     ## @brief Returns the current status text of the goal.
00176     ##
00177     ## The text is sent by the action server. It is designed to
00178     ## help debugging issues on the server side.
00179     ##
00180     ## @return The current status text of the goal.
00181     def get_goal_status_text(self):
00182         if not self.gh:
00183             rospy.logerr("Called get_goal_status_text when no goal is running")
00184             return "ERROR: Called get_goal_status_text when no goal is running"
00185 
00186         return self.gh.get_goal_status_text()
00187 
00188     ## @brief Cancels all goals currently running on the action server
00189     ##
00190     ## This preempts all goals running on the action server at the point that
00191     ## this message is serviced by the ActionServer.
00192     def cancel_all_goals(self):
00193         self.action_client.cancel_all_goals()
00194 
00195     ## @brief Cancels all goals prior to a given timestamp
00196     ##
00197     ## This preempts all goals running on the action server for which the
00198     ## time stamp is earlier than the specified time stamp
00199     ## this message is serviced by the ActionServer.
00200     def cancel_goals_at_and_before_time(self, time):
00201         self.action_client.cancel_goals_at_and_before_time(time)
00202 
00203     ## @brief Cancels the goal that we are currently pursuing
00204     def cancel_goal(self):
00205         if self.gh:
00206             self.gh.cancel()
00207 
00208     ## @brief Stops tracking the state of the current goal. Unregisters this goal's callbacks
00209     ##
00210     ## This is useful if we want to make sure we stop calling our callbacks before sending a new goal.
00211     ## Note that this does not cancel the goal, it simply stops looking for status info about this goal.
00212     def stop_tracking_goal(self):
00213         self.gh = None
00214 
00215     def _handle_transition(self, gh):
00216         comm_state = gh.get_comm_state()
00217 
00218         error_msg = "Received comm state %s when in simple state %s with SimpleActionClient in NS %s" % \
00219             (CommState.to_string(comm_state), SimpleGoalState.to_string(self.simple_state), rospy.resolve_name(self.action_client.ns))
00220 
00221         if comm_state == CommState.ACTIVE:
00222             if self.simple_state == SimpleGoalState.PENDING:
00223                 self._set_simple_state(SimpleGoalState.ACTIVE)
00224                 if self.active_cb:
00225                     self.active_cb()
00226             elif self.simple_state == SimpleGoalState.DONE:
00227                 rospy.logerr(error_msg)
00228         elif comm_state == CommState.RECALLING:
00229             if self.simple_state != SimpleGoalState.PENDING:
00230                 rospy.logerr(error_msg)
00231         elif comm_state == CommState.PREEMPTING:
00232             if self.simple_state == SimpleGoalState.PENDING:
00233                 self._set_simple_state(SimpleGoalState.ACTIVE)
00234                 if self.active_cb:
00235                     self.active_cb()
00236             elif self.simple_state == SimpleGoalState.DONE:
00237                 rospy.logerr(error_msg)
00238         elif comm_state == CommState.DONE:
00239             if self.simple_state in [SimpleGoalState.PENDING, SimpleGoalState.ACTIVE]:
00240                 self._set_simple_state(SimpleGoalState.DONE)
00241                 if self.done_cb:
00242                     self.done_cb(gh.get_goal_status(), gh.get_result())
00243                 with self.done_condition:
00244                     self.done_condition.notifyAll()
00245             elif self.simple_state == SimpleGoalState.DONE:
00246                 rospy.logerr("SimpleActionClient received DONE twice")
00247 
00248     def _handle_feedback(self, gh, feedback):
00249         if not self.gh:
00250             # this is not actually an error - there can be a small window in which old feedback
00251             # can be received between the time this variable is reset and a new goal is
00252             # sent and confirmed
00253             return
00254         if gh != self.gh:
00255             rospy.logerr("Got a feedback callback on a goal handle that we're not tracking. %s vs %s" %
00256                          (self.gh.comm_state_machine.action_goal.goal_id.id, gh.comm_state_machine.action_goal.goal_id.id))
00257             return
00258         if self.feedback_cb:
00259             self.feedback_cb(feedback)
00260 
00261     def _set_simple_state(self, state):
00262         self.simple_state = state


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep
autogenerated on Thu Sep 28 2017 02:51:16