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