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 import rospy
00031
00032 import actionlib_msgs.msg
00033
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 def get_default_result(self):
00059 return self.action_server.ActionResultType()
00060
00061 def set_accepted(self, text=""):
00062 """
00063 Accept the goal referenced by the goal handle. This will
00064 transition to the ACTIVE state or the PREEMPTING state depending
00065 on whether a cancel request has been received for the goal
00066 """
00067
00068 rospy.logdebug("Accepting goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec())
00069 if self.goal:
00070 with self.action_server.lock:
00071 status = self.status_tracker.status.status
00072
00073
00074 if status == actionlib_msgs.msg.GoalStatus.PENDING:
00075 self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.ACTIVE
00076 self.status_tracker.status.text = text
00077 self.action_server.publish_status()
00078
00079
00080 elif status == actionlib_msgs.msg.GoalStatus.RECALLING:
00081 self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.PREEMPTING
00082 self.status_tracker.status.text = text
00083 self.action_server.publish_status()
00084
00085 else:
00086 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)
00087
00088 else:
00089 rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle")
00090
00091 def set_canceled(self, result=None, text=""):
00092 """
00093 Set the status of the goal associated with the ServerGoalHandle to RECALLED or PREEMPTED
00094 depending on what the current status of the goal is
00095 @param result Optionally, the user can pass in a result to be sent to any clients of the goal
00096 """
00097 if not result:
00098 result = self.get_default_result()
00099
00100 rospy.logdebug("Setting status to canceled on goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec())
00101
00102 if self.goal:
00103 with self.action_server.lock:
00104 status = self.status_tracker.status.status
00105 if status == actionlib_msgs.msg.GoalStatus.PENDING or status == actionlib_msgs.msg.GoalStatus.RECALLING:
00106 self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.RECALLED
00107 self.status_tracker.status.text = text
00108
00109 self.status_tracker.handle_destruction_time = rospy.Time.now()
00110 self.action_server.publish_result(self.status_tracker.status, result)
00111 elif status == actionlib_msgs.msg.GoalStatus.ACTIVE or status == actionlib_msgs.msg.GoalStatus.PREEMPTING:
00112 self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.PREEMPTED
00113 self.status_tracker.status.text = text
00114
00115 self.status_tracker.handle_destruction_time = rospy.Time.now()
00116 self.action_server.publish_result(self.status_tracker.status, result)
00117
00118 else:
00119 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",
00120 self.status_tracker.status.status)
00121
00122 else:
00123 rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle")
00124
00125 def set_rejected(self, result=None, text=""):
00126 """
00127 * @brief Set the status of the goal associated with the ServerGoalHandle to rejected
00128 * @param result Optionally, the user can pass in a result to be sent to any clients of the goal
00129 """
00130 if not result:
00131 result = self.get_default_result()
00132
00133 rospy.logdebug("Setting status to rejected on goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec())
00134 if self.goal:
00135 with self.action_server.lock:
00136 status = self.status_tracker.status.status
00137 if status == actionlib_msgs.msg.GoalStatus.PENDING or status == actionlib_msgs.msg.GoalStatus.RECALLING:
00138 self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.REJECTED
00139 self.status_tracker.status.text = text
00140
00141 self.status_tracker.handle_destruction_time = rospy.Time.now()
00142 self.action_server.publish_result(self.status_tracker.status, result)
00143
00144 else:
00145 rospy.logerr("To transition to a rejected state, the goal must be in a pending or recalling state, it is currently in state: %d",
00146 self.status_tracker.status.status)
00147
00148 else:
00149 rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle")
00150
00151 def set_aborted(self, result=None, text=""):
00152 """
00153 Set the status of the goal associated with the ServerGoalHandle to aborted
00154 @param result Optionally, the user can pass in a result to be sent to any clients of the goal
00155 """
00156 if not result:
00157 result = self.get_default_result()
00158
00159 rospy.logdebug("Setting status to aborted on goal, id: %s, stamp: %.2f", self.get_goal_id().id, self.get_goal_id().stamp.to_sec())
00160 if self.goal:
00161 with self.action_server.lock:
00162 status = self.status_tracker.status.status
00163 if status == actionlib_msgs.msg.GoalStatus.PREEMPTING or status == actionlib_msgs.msg.GoalStatus.ACTIVE:
00164 self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.ABORTED
00165 self.status_tracker.status.text = text
00166
00167 self.status_tracker.handle_destruction_time = rospy.Time.now()
00168 self.action_server.publish_result(self.status_tracker.status, result)
00169
00170 else:
00171 rospy.logerr("To transition to an aborted state, the goal must be in a preempting or active state, it is currently in state: %d",
00172 status)
00173
00174 else:
00175 rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle")
00176
00177 def set_succeeded(self, result=None, text=""):
00178 """
00179 Set the status of the goal associated with the ServerGoalHandle to succeeded
00180 @param result Optionally, the user can pass in a result to be sent to any clients of the goal
00181 """
00182 if not result:
00183 result = self.get_default_result()
00184
00185 rospy.logdebug("Setting status to succeeded on goal, id: %s, stamp: %.2f",
00186 self.get_goal_id().id, self.get_goal_id().stamp.to_sec())
00187 if self.goal:
00188 with self.action_server.lock:
00189 status = self.status_tracker.status.status
00190 if status == actionlib_msgs.msg.GoalStatus.PREEMPTING or status == actionlib_msgs.msg.GoalStatus.ACTIVE:
00191 self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.SUCCEEDED
00192 self.status_tracker.status.text = text
00193
00194 self.status_tracker.handle_destruction_time = rospy.Time.now()
00195 self.action_server.publish_result(self.status_tracker.status, result)
00196
00197 else:
00198 rospy.logerr("To transition to a succeeded state, the goal must be in a preempting or active state, it is currently in state: %d",
00199 status)
00200
00201 else:
00202 rospy.logerr("Attempt to set status on an uninitialized ServerGoalHandle")
00203
00204 def publish_feedback(self, feedback):
00205 """
00206 Send feedback to any clients of the goal associated with this ServerGoalHandle
00207 @param feedback The feedback to send to the client
00208 """
00209 rospy.logdebug("Publishing feedback for goal, id: %s, stamp: %.2f",
00210 self.get_goal_id().id, self.get_goal_id().stamp.to_sec())
00211 if self.goal:
00212 with self.action_server.lock:
00213 self.action_server.publish_feedback(self.status_tracker.status, feedback)
00214 else:
00215 rospy.logerr("Attempt to publish feedback on an uninitialized ServerGoalHandle")
00216
00217 def get_goal(self):
00218 """
00219 Accessor for the goal associated with the ServerGoalHandle
00220 @return A shared_ptr to the goal object
00221 """
00222
00223 if self.goal:
00224
00225
00226
00227
00228 return self.goal.goal
00229
00230 return None
00231
00232 def get_goal_id(self):
00233 """
00234 Accessor for the goal id associated with the ServerGoalHandle
00235 @return The goal id
00236 """
00237 if self.goal:
00238 with self.action_server.lock:
00239 return self.status_tracker.status.goal_id
00240 else:
00241 rospy.logerr("Attempt to get a goal id on an uninitialized ServerGoalHandle")
00242 return actionlib_msgs.msg.GoalID()
00243
00244 def get_goal_status(self):
00245 """
00246 Accessor for the status associated with the ServerGoalHandle
00247 @return The goal status
00248 """
00249 if self.goal:
00250 with self.action_server.lock:
00251 return self.status_tracker.status
00252 else:
00253 rospy.logerr("Attempt to get goal status on an uninitialized ServerGoalHandle")
00254 return actionlib_msgs.msg.GoalStatus()
00255
00256 def __eq__(self, other):
00257 """
00258 Equals operator for ServerGoalHandles
00259 @param other The ServerGoalHandle to compare to
00260 @return True if the ServerGoalHandles refer to the same goal, false otherwise
00261 """
00262
00263 if not self.goal or not other.goal:
00264 return False
00265 my_id = self.get_goal_id()
00266 their_id = other.get_goal_id()
00267 return my_id.id == their_id.id
00268
00269 def __ne__(self, other):
00270 """
00271 != operator for ServerGoalHandles
00272 @param other The ServerGoalHandle to compare to
00273 @return True if the ServerGoalHandles refer to different goals, false otherwise
00274 """
00275 if not self.goal or not other.goal:
00276 return True
00277 my_id = self.get_goal_id()
00278 their_id = other.get_goal_id()
00279
00280 return my_id.id != their_id.id
00281
00282 def __hash__(self):
00283 """
00284 hash function for ServerGoalHandles
00285 @return hash of the goal ID
00286 """
00287 return hash(self.get_goal_id().id)
00288
00289 def set_cancel_requested(self):
00290 """
00291 A private method to set status to PENDING or RECALLING
00292 @return True if the cancel request should be passed on to the user, false otherwise
00293 """
00294 rospy.logdebug("Transisitoning to a cancel requested state on goal id: %s, stamp: %.2f",
00295 self.get_goal_id().id, self.get_goal_id().stamp.to_sec())
00296 if self.goal:
00297 with self.action_server.lock:
00298 status = self.status_tracker.status.status
00299 if status == actionlib_msgs.msg.GoalStatus.PENDING:
00300 self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.RECALLING
00301 self.action_server.publish_status()
00302 return True
00303
00304 if status == actionlib_msgs.msg.GoalStatus.ACTIVE:
00305 self.status_tracker.status.status = actionlib_msgs.msg.GoalStatus.PREEMPTING
00306 self.action_server.publish_status()
00307 return True
00308
00309 return False