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 
00031 import threading
00032 import time
00033 import rospy
00034 from rospy import Header
00035 from actionlib_msgs.msg import *
00036 from action_client import ActionClient, CommState, get_name_of_constant
00037 
00038 class SimpleGoalState:
00039     PENDING = 0
00040     ACTIVE = 1
00041     DONE = 2
00042 SimpleGoalState.to_string = classmethod(get_name_of_constant)
00043 
00044 
00045 class SimpleActionClient:
00046     ## @brief Constructs a SimpleActionClient and opens connections to an ActionServer.
00047     ##
00048     ## @param ns The namespace in which to access the action.  For
00049     ## example, the "goal" topic should occur under ns/goal
00050     ##
00051     ## @param ActionSpec The *Action message type.  The SimpleActionClient
00052     ## will grab the other message types from this type.
00053     def __init__(self, ns, ActionSpec):
00054         self.action_client = ActionClient(ns, ActionSpec)
00055         self.simple_state = SimpleGoalState.DONE
00056         self.gh = None
00057         self.done_condition = threading.Condition()
00058 
00059     ## @brief Blocks until the action server connects to this client
00060     ##
00061     ## @param timeout Max time to block before returning. A zero
00062     ## timeout is interpreted as an infinite timeout.
00063     ##
00064     ## @return True if the server connected in the allocated time. False on timeout
00065     def wait_for_server(self, timeout = rospy.Duration()):
00066         return self.action_client.wait_for_server(timeout)
00067 
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 
00095     ## @brief Sends a goal to the ActionServer, waits for the goal to complete, and preempts goal is necessary
00096     ##
00097     ## If a previous goal is already active when this is called. We simply forget
00098     ## about that goal and start tracking the new goal. No cancel requests are made.
00099     ##
00100     ## If the goal does not complete within the execute_timeout, the goal gets preempted
00101     ##
00102     ## If preemption of the goal does not complete withing the preempt_timeout, this
00103     ## method simply returns
00104     ##
00105     ## @param execute_timeout The time to wait for the goal to complete
00106     ##
00107     ## @param preempt_timeout The time to wait for preemption to complete
00108     ##
00109     ## @return The goal's state.
00110     def send_goal_and_wait(self, goal, execute_timeout = rospy.Duration(), preempt_timeout = rospy.Duration()):
00111         self.send_goal(goal)
00112         if not self.wait_for_result(execute_timeout):
00113             # preempt action
00114             rospy.logdebug("Canceling goal")
00115             self.cancel_goal()
00116             if self.wait_for_result(preempt_timeout):
00117                 rospy.logdebug("Preempt finished within specified preempt_timeout [%.2f]", preempt_timeout.to_sec());
00118             else:
00119                 rospy.logdebug("Preempt didn't finish specified preempt_timeout [%.2f]", preempt_timeout.to_sec());
00120         return self.get_state()
00121 
00122 
00123     ## @brief Blocks until this goal transitions to done
00124     ## @param timeout Max time to block before returning. A zero timeout is interpreted as an infinite timeout.
00125     ## @return True if the goal finished. False if the goal didn't finish within the allocated timeout
00126     def wait_for_result(self, timeout = rospy.Duration()):
00127         if not self.gh:
00128             rospy.logerr("Called wait_for_goal_to_finish when no goal exists")
00129             return False
00130 
00131         timeout_time = rospy.get_rostime() + timeout
00132         loop_period = rospy.Duration(0.1)
00133         with self.done_condition:
00134             while not rospy.is_shutdown():
00135                 time_left = timeout_time - rospy.get_rostime()
00136                 if timeout > rospy.Duration(0.0) and time_left <= rospy.Duration(0.0):
00137                     break
00138 
00139                 if self.simple_state == SimpleGoalState.DONE:
00140                     break
00141 
00142                 if time_left > loop_period or timeout == rospy.Duration():
00143                     time_left = loop_period
00144 
00145                 self.done_condition.wait(time_left.to_sec())
00146 
00147         return self.simple_state == SimpleGoalState.DONE
00148 
00149 
00150     ## @brief Gets the Result of the current goal
00151     def get_result(self):
00152         if not self.gh:
00153             rospy.logerr("Called get_result when no goal is running")
00154             return None
00155 
00156         return self.gh.get_result()
00157 
00158 
00159     ## @brief Get the state information for this goal
00160     ##
00161     ## Possible States Are: PENDING, ACTIVE, RECALLED, REJECTED,
00162     ## PREEMPTED, ABORTED, SUCCEEDED, LOST.
00163     ##
00164     ## @return The goal's state. Returns LOST if this
00165     ## SimpleActionClient isn't tracking a goal.
00166     def get_state(self):
00167         if not self.gh:
00168             rospy.logerr("Called get_state when no goal is running")
00169             return GoalStatus.LOST
00170         status = self.gh.get_goal_status()
00171 
00172         if status == GoalStatus.RECALLING:
00173             status = GoalStatus.PENDING
00174         elif status == GoalStatus.PREEMPTING:
00175             status = GoalStatus.ACTIVE
00176 
00177         return status
00178 
00179 
00180     ## @brief Returns the current status text of the goal.
00181     ##
00182     ## The text is sent by the action server. It is designed to
00183     ## help debugging issues on the server side.
00184     ##
00185     ## @return The current status text of the goal.
00186     def get_goal_status_text(self):
00187         if not self.gh:
00188             rospy.logerr("Called get_goal_status_text when no goal is running")
00189             return "ERROR: Called get_goal_status_text when no goal is running"
00190 
00191         return self.gh.get_goal_status_text()
00192 
00193 
00194 
00195 
00196     ## @brief Cancels all goals currently running on the action server
00197     ##
00198     ## This preempts all goals running on the action server at the point that
00199     ## this message is serviced by the ActionServer.
00200     def cancel_all_goals(self):
00201         self.action_client.cancel_all_goals()
00202 
00203 
00204     ## @brief Cancels all goals prior to a given timestamp
00205     ##
00206     ## This preempts all goals running on the action server for which the 
00207     ## time stamp is earlier than the specified time stamp
00208     ## this message is serviced by the ActionServer.
00209 
00210     def cancel_goals_at_and_before_time(self, time):
00211         self.action_client.cancel_goals_at_and_before_time(time)
00212 
00213 
00214     ## @brief Cancels the goal that we are currently pursuing
00215     def cancel_goal(self):
00216         if self.gh:
00217             self.gh.cancel()
00218 
00219 
00220     ## @brief Stops tracking the state of the current goal. Unregisters this goal's callbacks
00221     ##
00222     ## This is useful if we want to make sure we stop calling our callbacks before sending a new goal.
00223     ## Note that this does not cancel the goal, it simply stops looking for status info about this goal.
00224     def stop_tracking_goal(self):
00225         self.gh = None
00226 
00227     def _handle_transition(self, gh):
00228         comm_state = gh.get_comm_state()
00229 
00230         error_msg = "Received comm state %s when in simple state %s with SimpleActionClient in NS %s" % \
00231             (CommState.to_string(comm_state), SimpleGoalState.to_string(self.simple_state), rospy.resolve_name(self.action_client.ns))
00232 
00233         if comm_state == CommState.ACTIVE:
00234             if self.simple_state == SimpleGoalState.PENDING:
00235                 self._set_simple_state(SimpleGoalState.ACTIVE)
00236                 if self.active_cb:
00237                     self.active_cb()
00238             elif self.simple_state == SimpleGoalState.DONE:
00239                 rospy.logerr(error_msg)
00240         elif comm_state == CommState.RECALLING:
00241             if self.simple_state != SimpleGoalState.PENDING:
00242                 rospy.logerr(error_msg)
00243         elif comm_state == CommState.PREEMPTING:
00244             if self.simple_state == SimpleGoalState.PENDING:
00245                 self._set_simple_state(SimpleGoalState.ACTIVE)
00246                 if self.active_cb:
00247                     self.active_cb()
00248             elif self.simple_state == SimpleGoalState.DONE:
00249                 rospy.logerr(error_msg)
00250         elif comm_state == CommState.DONE:
00251             if self.simple_state in [SimpleGoalState.PENDING, SimpleGoalState.ACTIVE]:
00252                 self._set_simple_state(SimpleGoalState.DONE)
00253                 if self.done_cb:
00254                     self.done_cb(gh.get_goal_status(), gh.get_result())
00255                 with self.done_condition:
00256                     self.done_condition.notifyAll()
00257             elif self.simple_state == SimpleGoalState.DONE:
00258                 rospy.logerr("SimpleActionClient received DONE twice")
00259 
00260     def _handle_feedback(self, gh, feedback):
00261         if not self.gh:
00262             rospy.logerr("Got a feedback callback when we're not tracking a goal. (id: %s)" % \
00263                              gh.comm_state_machine.action_goal.goal_id.id)
00264             return
00265         if gh != self.gh:
00266             rospy.logerr("Got a feedback callback on a goal handle that we're not tracking. %s vs %s" % \
00267                              (self.gh.comm_state_machine.action_goal.goal_id.id,
00268                               gh.comm_state_machine.action_goal.goal_id.id))
00269             return
00270         if self.feedback_cb:
00271             self.feedback_cb(feedback)
00272 
00273 
00274     def _set_simple_state(self, state):
00275         self.simple_state = state


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep
autogenerated on Thu Jan 2 2014 11:03:49