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_result 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             return GoalStatus.LOST
00165         status = self.gh.get_goal_status()
00166 
00167         if status == GoalStatus.RECALLING:
00168             status = GoalStatus.PENDING
00169         elif status == GoalStatus.PREEMPTING:
00170             status = GoalStatus.ACTIVE
00171 
00172         return status
00173 
00174     ## @brief Returns the current status text of the goal.
00175     ##
00176     ## The text is sent by the action server. It is designed to
00177     ## help debugging issues on the server side.
00178     ##
00179     ## @return The current status text of the goal.
00180     def get_goal_status_text(self):
00181         if not self.gh:
00182             rospy.logerr("Called get_goal_status_text when no goal is running")
00183             return "ERROR: Called get_goal_status_text when no goal is running"
00184 
00185         return self.gh.get_goal_status_text()
00186 
00187     ## @brief Cancels all goals currently running on the action server
00188     ##
00189     ## This preempts all goals running on the action server at the point that
00190     ## this message is serviced by the ActionServer.
00191     def cancel_all_goals(self):
00192         self.action_client.cancel_all_goals()
00193 
00194     ## @brief Cancels all goals prior to a given timestamp
00195     ##
00196     ## This preempts all goals running on the action server for which the
00197     ## time stamp is earlier than the specified time stamp
00198     ## this message is serviced by the ActionServer.
00199     def cancel_goals_at_and_before_time(self, time):
00200         self.action_client.cancel_goals_at_and_before_time(time)
00201 
00202     ## @brief Cancels the goal that we are currently pursuing
00203     def cancel_goal(self):
00204         if self.gh:
00205             self.gh.cancel()
00206 
00207     ## @brief Stops tracking the state of the current goal. Unregisters this goal's callbacks
00208     ##
00209     ## This is useful if we want to make sure we stop calling our callbacks before sending a new goal.
00210     ## Note that this does not cancel the goal, it simply stops looking for status info about this goal.
00211     def stop_tracking_goal(self):
00212         self.gh = None
00213 
00214     def _handle_transition(self, gh):
00215         comm_state = gh.get_comm_state()
00216 
00217         error_msg = "Received comm state %s when in simple state %s with SimpleActionClient in NS %s" % \
00218             (CommState.to_string(comm_state), SimpleGoalState.to_string(self.simple_state), rospy.resolve_name(self.action_client.ns))
00219 
00220         if comm_state == CommState.ACTIVE:
00221             if self.simple_state == SimpleGoalState.PENDING:
00222                 self._set_simple_state(SimpleGoalState.ACTIVE)
00223                 if self.active_cb:
00224                     self.active_cb()
00225             elif self.simple_state == SimpleGoalState.DONE:
00226                 rospy.logerr(error_msg)
00227         elif comm_state == CommState.RECALLING:
00228             if self.simple_state != SimpleGoalState.PENDING:
00229                 rospy.logerr(error_msg)
00230         elif comm_state == CommState.PREEMPTING:
00231             if self.simple_state == SimpleGoalState.PENDING:
00232                 self._set_simple_state(SimpleGoalState.ACTIVE)
00233                 if self.active_cb:
00234                     self.active_cb()
00235             elif self.simple_state == SimpleGoalState.DONE:
00236                 rospy.logerr(error_msg)
00237         elif comm_state == CommState.DONE:
00238             if self.simple_state in [SimpleGoalState.PENDING, SimpleGoalState.ACTIVE]:
00239                 self._set_simple_state(SimpleGoalState.DONE)
00240                 if self.done_cb:
00241                     self.done_cb(gh.get_goal_status(), gh.get_result())
00242                 with self.done_condition:
00243                     self.done_condition.notifyAll()
00244             elif self.simple_state == SimpleGoalState.DONE:
00245                 rospy.logerr("SimpleActionClient received DONE twice")
00246 
00247     def _handle_feedback(self, gh, feedback):
00248         if not self.gh:
00249             # this is not actually an error - there can be a small window in which old feedback
00250             # can be received between the time this variable is reset and a new goal is
00251             # sent and confirmed
00252             return
00253         if gh != self.gh:
00254             rospy.logerr("Got a feedback callback on a goal handle that we're not tracking. %s vs %s" %
00255                          (self.gh.comm_state_machine.action_goal.goal_id.id, gh.comm_state_machine.action_goal.goal_id.id))
00256             return
00257         if self.feedback_cb:
00258             self.feedback_cb(feedback)
00259 
00260     def _set_simple_state(self, state):
00261         self.simple_state = state


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep, Mikael Arguedas
autogenerated on Sat Feb 16 2019 03:21:28