Go to the documentation of this file.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 import threading
00033 import traceback
00034
00035 from actionlib_msgs.msg import GoalStatus
00036
00037 from actionlib import ActionServer
00038 from actionlib.server_goal_handle import ServerGoalHandle
00039
00040
00041 def nop_cb(goal_handle):
00042 pass
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055 class SimpleActionServer:
00056
00057
00058
00059
00060
00061
00062 def __init__(self, name, ActionSpec, execute_cb=None, auto_start=True):
00063
00064 self.new_goal = False
00065 self.preempt_request = False
00066 self.new_goal_preempt_request = False
00067
00068 self.execute_callback = execute_cb
00069 self.goal_callback = None
00070 self.preempt_callback = None
00071
00072 self.need_to_terminate = False
00073 self.terminate_mutex = threading.RLock()
00074
00075
00076
00077
00078
00079 self.lock = threading.RLock()
00080
00081 self.execute_condition = threading.Condition(self.lock)
00082
00083 self.current_goal = ServerGoalHandle()
00084 self.next_goal = ServerGoalHandle()
00085
00086 if self.execute_callback:
00087 self.execute_thread = threading.Thread(None, self.executeLoop)
00088 self.execute_thread.start()
00089 else:
00090 self.execute_thread = None
00091
00092
00093 self.action_server = ActionServer(name, ActionSpec, self.internal_goal_callback, self.internal_preempt_callback, auto_start)
00094
00095 def __del__(self):
00096 if hasattr(self, 'execute_callback') and self.execute_callback:
00097 with self.terminate_mutex:
00098 self.need_to_terminate = True
00099
00100 assert(self.execute_thread)
00101 self.execute_thread.join()
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112 def accept_new_goal(self):
00113 with self.action_server.lock, self.lock:
00114 if not self.new_goal or not self.next_goal.get_goal():
00115 rospy.logerr("Attempting to accept the next goal when a new goal is not available")
00116 return None
00117
00118
00119 if self.is_active() and self.current_goal.get_goal() and self.current_goal != self.next_goal:
00120 self.current_goal.set_canceled(None, "This goal was canceled because another goal was received by the simple action server")
00121
00122 rospy.logdebug("Accepting a new goal")
00123
00124
00125 self.current_goal = self.next_goal
00126 self.new_goal = False
00127
00128
00129 self.preempt_request = self.new_goal_preempt_request
00130 self.new_goal_preempt_request = False
00131
00132
00133 self.current_goal.set_accepted("This goal has been accepted by the simple action server")
00134
00135 return self.current_goal.get_goal()
00136
00137
00138
00139 def is_new_goal_available(self):
00140 return self.new_goal
00141
00142
00143
00144 def is_preempt_requested(self):
00145 return self.preempt_request
00146
00147
00148
00149 def is_active(self):
00150 if not self.current_goal.get_goal():
00151 return False
00152
00153 status = self.current_goal.get_goal_status().status
00154 return status == GoalStatus.ACTIVE or status == GoalStatus.PREEMPTING
00155
00156
00157
00158 def set_succeeded(self, result=None, text=""):
00159 with self.action_server.lock, self.lock:
00160 if not result:
00161 result = self.get_default_result()
00162 self.current_goal.set_succeeded(result, text)
00163
00164
00165
00166 def set_aborted(self, result=None, text=""):
00167 with self.action_server.lock, self.lock:
00168 if not result:
00169 result = self.get_default_result()
00170 self.current_goal.set_aborted(result, text)
00171
00172
00173
00174 def publish_feedback(self, feedback):
00175 self.current_goal.publish_feedback(feedback)
00176
00177 def get_default_result(self):
00178 return self.action_server.ActionResultType()
00179
00180
00181
00182 def set_preempted(self, result=None, text=""):
00183 if not result:
00184 result = self.get_default_result()
00185 with self.action_server.lock, self.lock:
00186 rospy.logdebug("Setting the current goal as canceled")
00187 self.current_goal.set_canceled(result, text)
00188
00189
00190
00191 def register_goal_callback(self, cb):
00192 if self.execute_callback:
00193 rospy.logwarn("Cannot call SimpleActionServer.register_goal_callback() because an executeCallback exists. Not going to register it.")
00194 else:
00195 self.goal_callback = cb
00196
00197
00198
00199 def register_preempt_callback(self, cb):
00200 self.preempt_callback = cb
00201
00202
00203 def start(self):
00204 self.action_server.start()
00205
00206
00207 def internal_goal_callback(self, goal):
00208 self.execute_condition.acquire()
00209
00210 try:
00211 rospy.logdebug("A new goal %shas been recieved by the single goal action server", goal.get_goal_id().id)
00212
00213
00214 if((not self.current_goal.get_goal() or goal.get_goal_id().stamp >= self.current_goal.get_goal_id().stamp)
00215 and (not self.next_goal.get_goal() or goal.get_goal_id().stamp >= self.next_goal.get_goal_id().stamp)):
00216
00217 if(self.next_goal.get_goal() and (not self.current_goal.get_goal() or self.next_goal != self.current_goal)):
00218 self.next_goal.set_canceled(None, "This goal was canceled because another goal was received by the simple action server")
00219
00220 self.next_goal = goal
00221 self.new_goal = True
00222 self.new_goal_preempt_request = False
00223
00224
00225 if(self.is_active()):
00226 self.preempt_request = True
00227
00228 if(self.preempt_callback):
00229 self.preempt_callback()
00230
00231
00232 if self.goal_callback:
00233 self.goal_callback()
00234
00235
00236 self.execute_condition.notify()
00237 self.execute_condition.release()
00238 else:
00239
00240 goal.set_canceled(None, "This goal was canceled because another goal was received by the simple action server")
00241 self.execute_condition.release()
00242 except Exception as e:
00243 rospy.logerr("SimpleActionServer.internal_goal_callback - exception %s", str(e))
00244 self.execute_condition.release()
00245
00246
00247 def internal_preempt_callback(self, preempt):
00248 with self.lock:
00249 rospy.logdebug("A preempt has been received by the SimpleActionServer")
00250
00251
00252 if(preempt == self.current_goal):
00253 rospy.logdebug("Setting preempt_request bit for the current goal to TRUE and invoking callback")
00254 self.preempt_request = True
00255
00256
00257 if(self.preempt_callback):
00258 self.preempt_callback()
00259
00260 elif(preempt == self.next_goal):
00261 rospy.logdebug("Setting preempt request bit for the next goal to TRUE")
00262 self.new_goal_preempt_request = True
00263
00264
00265 def executeLoop(self):
00266 loop_duration = rospy.Duration.from_sec(.1)
00267
00268 while (not rospy.is_shutdown()):
00269 with self.terminate_mutex:
00270 if (self.need_to_terminate):
00271 break
00272
00273
00274
00275
00276
00277 if (self.is_active()):
00278 rospy.logerr("Should never reach this code with an active goal")
00279 return
00280
00281 if (self.is_new_goal_available()):
00282
00283 goal = self.accept_new_goal()
00284 if not self.execute_callback:
00285 rospy.logerr("execute_callback_ must exist. This is a bug in SimpleActionServer")
00286 return
00287
00288 try:
00289 self.execute_callback(goal)
00290
00291 if self.is_active():
00292 rospy.logwarn("Your executeCallback did not set the goal to a terminal status. " +
00293 "This is a bug in your ActionServer implementation. Fix your code! " +
00294 "For now, the ActionServer will set this goal to aborted")
00295 self.set_aborted(None, "No terminal state was set.")
00296 except Exception as ex:
00297 rospy.logerr("Exception in your execute callback: %s\n%s", str(ex),
00298 traceback.format_exc())
00299 self.set_aborted(None, "Exception in execute callback: %s" % str(ex))
00300
00301 with self.execute_condition:
00302 self.execute_condition.wait(loop_duration.to_sec())