00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
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
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
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
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
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
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
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
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
00230 if self.goal:
00231
00232
00233
00234
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