server_goal_handle.py
Go to the documentation of this file.
00001 # Copyright (c) 2009, Willow Garage, Inc.
00002 # All rights reserved.
00003 # 
00004 # Redistribution and use in source and binary forms, with or without
00005 # modification, are permitted provided that the following conditions are met:
00006 # 
00007 #     * Redistributions of source code must retain the above copyright
00008 #       notice, this list of conditions and the following disclaimer.
00009 #     * Redistributions in binary form must reproduce the above copyright
00010 #       notice, this list of conditions and the following disclaimer in the
00011 #       documentation and/or other materials provided with the distribution.
00012 #     * Neither the name of the Willow Garage, Inc. nor the names of its
00013 #       contributors may be used to endorse or promote products derived from
00014 #       this software without specific prior written permission.
00015 # 
00016 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00017 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00018 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00019 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00020 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00021 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00022 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00023 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00024 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00025 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00026 # POSSIBILITY OF SUCH DAMAGE.
00027 
00028 # Author: Alexander Sorokin. 
00029 # Based on C++ goal_id_generator.h/cpp
00030 
00031 import rospy
00032 
00033 import actionlib_msgs.msg
00034 
00035 class ServerGoalHandle:
00036     """
00037     * @class ServerGoalHandle
00038     * @brief Encapsulates a state machine for a given goal that the user can
00039     * trigger transisions on. All ROS interfaces for the goal are managed by
00040     * the ActionServer to lessen the burden on the user.
00041 
00042     """
00043 
00044     def __init__(self, status_tracker=None, action_server=None, handle_tracker=None):
00045         """
00046         A private constructor used by the ActionServer to initialize a ServerGoalHandle.
00047         @node  The default constructor was not ported.
00048         """
00049         self.status_tracker = status_tracker;
00050         self.action_server = action_server
00051         self.handle_tracker = handle_tracker;
00052 
00053         if status_tracker:
00054             self.goal=status_tracker.goal;
00055         else:
00056             self.goal=None
00057 
00058 
00059     def get_default_result(self):
00060         return self.action_server.ActionResultType();
00061 
00062     def set_accepted(self, text=""):
00063       """
00064       Accept the goal referenced by the goal handle. This will
00065       transition to the ACTIVE state or the PREEMPTING state depending
00066       on whether a cancel request has been received for the goal
00067       """
00068 
00069       rospy.logdebug("Accepting goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec());
00070       if self.goal:
00071           with self.action_server.lock:
00072               status = self.status_tracker.status.status;
00073 
00074               #if we were pending before, then we'll go active
00075               if(status == actionlib_msgs.msg.GoalStatus.PENDING):
00076                   self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.ACTIVE;
00077                   self.status_tracker.status.text = text
00078                   self.action_server.publish_status();
00079                   
00080               #if we were recalling before, now we'll go to preempting
00081               elif(status == actionlib_msgs.msg.GoalStatus.RECALLING) :
00082                   self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.PREEMPTING;
00083                   self.status_tracker.status.text = text
00084                   self.action_server.publish_status();
00085                   
00086               else:
00087                   rospy.logerr("To transition to an active state, the goal must be in a pending or recalling state, it is currently in state: %d",  self.status_tracker.status.status);
00088 
00089       else:
00090           rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle");
00091 
00092 
00093     def set_canceled(self, result=None, text=""):
00094         """
00095         Set the status of the goal associated with the ServerGoalHandle to RECALLED or PREEMPTED
00096         depending on what the current status of the goal is
00097         @param  result Optionally, the user can pass in a result to be sent to any clients of the goal
00098         """
00099         if not result:
00100             result=self.get_default_result();
00101 
00102         rospy.logdebug("Setting status to canceled on goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec());
00103 
00104         if(self.goal):
00105             with self.action_server.lock:
00106                 status = self.status_tracker.status.status;
00107                 if(status == actionlib_msgs.msg.GoalStatus.PENDING or status == actionlib_msgs.msg.GoalStatus.RECALLING):
00108                     self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.RECALLED;
00109                     self.status_tracker.status.text = text
00110                     #on transition to a terminal state, we'll also set the handle destruction time
00111                     self.status_tracker.handle_destruction_time = rospy.Time.now()
00112                     self.action_server.publish_result(self.status_tracker.status, result);
00113                 elif(status == actionlib_msgs.msg.GoalStatus.ACTIVE or status == actionlib_msgs.msg.GoalStatus.PREEMPTING):
00114                     self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.PREEMPTED;
00115                     self.status_tracker.status.text = text
00116                     #on transition to a terminal state, we'll also set the handle destruction time
00117                     self.status_tracker.handle_destruction_time = rospy.Time.now()
00118                     self.action_server.publish_result(self.status_tracker.status, result);
00119       
00120                 else:
00121                     rospy.logerr("To transition to a cancelled state, the goal must be in a pending, recalling, active, or preempting state, it is currently in state: %d",
00122                                  self.status_tracker.status.status);
00123     
00124         else:
00125             rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle");
00126 
00127 
00128     def set_rejected(self, result=None, text=""):
00129         """
00130         * @brief  Set the status of the goal associated with the ServerGoalHandle to rejected
00131         * @param  result Optionally, the user can pass in a result to be sent to any clients of the goal
00132         """
00133         if not result:
00134             result=self.get_default_result();
00135 
00136         rospy.logdebug("Setting status to rejected on goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec());
00137         if self.goal :
00138             with self.action_server.lock:
00139                 status = self.status_tracker.status.status;
00140                 if(status == actionlib_msgs.msg.GoalStatus.PENDING or status == actionlib_msgs.msg.GoalStatus.RECALLING):
00141                     self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.REJECTED;
00142                     self.status_tracker.status.text = text
00143                     #on transition to a terminal state, we'll also set the handle destruction time
00144                     self.status_tracker.handle_destruction_time = rospy.Time.now()
00145                     self.action_server.publish_result(self.status_tracker.status, result);
00146       
00147                 else:
00148                     rospy.logerr("To transition to a rejected state, the goal must be in a pending or recalling state, it is currently in state: %d",
00149                                  self.status_tracker.status.status);
00150     
00151         else:
00152             rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle");
00153 
00154 
00155     def set_aborted(self, result=None, text=""):
00156         """
00157         Set the status of the goal associated with the ServerGoalHandle to aborted
00158         @param  result Optionally, the user can pass in a result to be sent to any clients of the goal
00159         """
00160         if not result:
00161             result=self.get_default_result();
00162 
00163         rospy.logdebug("Setting status to aborted on goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec());
00164         if self.goal:
00165             with self.action_server.lock:
00166                 status = self.status_tracker.status.status;
00167                 if status == actionlib_msgs.msg.GoalStatus.PREEMPTING or status == actionlib_msgs.msg.GoalStatus.ACTIVE:
00168                     self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.ABORTED;
00169                     self.status_tracker.status.text = text
00170                     #on transition to a terminal state, we'll also set the handle destruction time
00171                     self.status_tracker.handle_destruction_time = rospy.Time.now()
00172                     self.action_server.publish_result(self.status_tracker.status, result);
00173                     
00174                 else:
00175                     rospy.logerr("To transition to an aborted state, the goal must be in a preempting or active state, it is currently in state: %d",
00176                                  status);
00177     
00178         else:
00179             rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle");
00180 
00181 
00182     def set_succeeded(self,result=None, text=""):
00183         """
00184         Set the status of the goal associated with the ServerGoalHandle to succeeded
00185         @param  result Optionally, the user can pass in a result to be sent to any clients of the goal
00186         """
00187         if not result:
00188             result=self.get_default_result();
00189 
00190         rospy.logdebug("Setting status to succeeded on goal, id: %s, stamp: %.2f",
00191                        self.get_goal_id().id, self.get_goal_id().stamp.to_sec());
00192         if self.goal:
00193             with self.action_server.lock:
00194                 status = self.status_tracker.status.status;
00195                 if status == actionlib_msgs.msg.GoalStatus.PREEMPTING or status == actionlib_msgs.msg.GoalStatus.ACTIVE :
00196                     self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.SUCCEEDED;
00197                     self.status_tracker.status.text = text
00198                     #on transition to a terminal state, we'll also set the handle destruction time
00199                     self.status_tracker.handle_destruction_time = rospy.Time.now()
00200                     self.action_server.publish_result(self.status_tracker.status, result);
00201       
00202                 else:
00203                     rospy.logerr("To transition to a succeeded state, the goal must be in a preempting or active state, it is currently in state: %d",
00204                                  status);
00205                 
00206         else:
00207             rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle");
00208 
00209 
00210     def publish_feedback(self,feedback):
00211         """
00212         Send feedback to any clients of the goal associated with this ServerGoalHandle
00213         @param feedback The feedback to send to the client
00214         """
00215         rospy.logdebug("Publishing feedback for goal, id: %s, stamp: %.2f", 
00216                        self.get_goal_id().id, self.get_goal_id().stamp.to_sec());
00217         if self.goal:
00218             with self.action_server.lock:
00219                 self.action_server.publish_feedback(self.status_tracker.status, feedback);
00220         else:
00221             rospy.logerr("Attempt to publish feedback on an uninitialized ServerGoalHandle");
00222 
00223 
00224     def get_goal(self):
00225         """
00226         Accessor for the goal associated with the ServerGoalHandle
00227         @return A shared_ptr to the goal object
00228         """
00229           #if we have a goal that is non-null
00230         if self.goal:
00231             #@todo Test that python reference counting automatically handles this.
00232             #create the deleter for our goal subtype
00233             #d = EnclosureDeleter(self.goal);
00234             #weakref.ref(boost::shared_ptr<const Goal>(&(goal_->goal), d);
00235             return self.goal.goal
00236         
00237         return None
00238 
00239 
00240     def get_goal_id(self):
00241         """
00242         Accessor for the goal id associated with the ServerGoalHandle
00243         @return The goal id
00244         """
00245         if self.goal:
00246             with self.action_server.lock:
00247                 return self.status_tracker.status.goal_id;
00248         else:
00249             rospy.logerr("Attempt to get a goal id on an uninitialized ServerGoalHandle");
00250             return actionlib_msgs.msg.GoalID();
00251         
00252         
00253     def get_goal_status(self):
00254         """
00255         Accessor for the status associated with the ServerGoalHandle
00256         @return The goal status
00257         """
00258         if self.goal:
00259             with self.action_server.lock:
00260                 return self.status_tracker.status;
00261         else:
00262             rospy.logerr("Attempt to get goal status on an uninitialized ServerGoalHandle");
00263             return actionlib_msgs.msg.GoalStatus();
00264         
00265         
00266     def __eq__(self, other):
00267         """
00268         Equals operator for ServerGoalHandles
00269         @param other The ServerGoalHandle to compare to
00270         @return True if the ServerGoalHandles refer to the same goal, false otherwise
00271         """
00272         
00273         if( not self.goal or not other.goal):
00274             return False;
00275         my_id = self.get_goal_id();
00276         their_id = other.get_goal_id();
00277         return my_id.id == their_id.id;
00278 
00279     def __ne__(self, other):
00280         """
00281         != operator for ServerGoalHandles
00282         @param other The ServerGoalHandle to compare to
00283         @return True if the ServerGoalHandles refer to different goals, false otherwise
00284         """
00285         if( not self.goal or not other.goal):
00286             return True;
00287         my_id = self.get_goal_id();
00288         their_id = other.get_goal_id();
00289         
00290         return my_id.id != their_id.id;
00291 
00292 
00293     def set_cancel_requested(self):
00294         """
00295         A private method to set status to PENDING or RECALLING
00296         @return True if the cancel request should be passed on to the user, false otherwise
00297         """
00298         rospy.logdebug("Transisitoning to a cancel requested state on goal id: %s, stamp: %.2f", 
00299                        self.get_goal_id().id, self.get_goal_id().stamp.to_sec());
00300         if self.goal:
00301             with self.action_server.lock:
00302                 status = self.status_tracker.status.status;
00303                 if (status == actionlib_msgs.msg.GoalStatus.PENDING):
00304                     self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.RECALLING;
00305                     self.action_server.publish_status();
00306                     return True;
00307 
00308                 if(status == actionlib_msgs.msg.GoalStatus.ACTIVE):
00309                     self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.PREEMPTING;
00310                     self.action_server.publish_status();
00311                     return True;
00312 
00313         return False;
00314 
00315 
00316 
00317 
00318 
00319 
00320 
00321 
00322 
00323 
00324 


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep
autogenerated on Fri Aug 28 2015 10:04:41