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
00036 from actionlib_msgs.msg import *
00037
00038 from goal_id_generator import GoalIDGenerator
00039 from status_tracker import StatusTracker
00040
00041 from handle_tracker_deleter import HandleTrackerDeleter
00042 from server_goal_handle import ServerGoalHandle
00043
00044 from actionlib.exceptions import *
00045
00046
00047 def nop_cb(goal_handle):
00048 pass
00049
00050 def ros_timer(callable,frequency):
00051 rate = rospy.Rate(frequency)
00052
00053 rospy.logdebug("Starting timer");
00054 while not rospy.is_shutdown():
00055 try:
00056 rate.sleep()
00057 callable()
00058 except rospy.exceptions.ROSInterruptException:
00059 rospy.logdebug("Sleep interrupted");
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069 class ActionServer:
00070
00071
00072
00073
00074
00075
00076 def __init__(self, ns, ActionSpec, goal_cb, cancel_cb = nop_cb, auto_start = True):
00077 self.ns=ns;
00078
00079 try:
00080 a = ActionSpec()
00081
00082 self.ActionSpec = ActionSpec
00083 self.ActionGoal = type(a.action_goal)
00084 self.ActionResult = type(a.action_result)
00085 self.ActionResultType = type(a.action_result.result)
00086 self.ActionFeedback = type(a.action_feedback)
00087 except AttributeError:
00088 raise ActionException("Type is not an action spec: %s" % str(ActionSpec))
00089
00090
00091 self.goal_sub = None
00092 self.cancel_sub = None
00093 self.status_pub = None
00094 self.result_pub = None
00095 self.feedback_pub = None
00096
00097 self.lock = threading.RLock()
00098
00099 self.status_timer = None;
00100
00101 self.status_list = [];
00102
00103 self.last_cancel = rospy.Time();
00104 self.status_list_timeout = rospy.Duration();
00105
00106 self.id_generator = GoalIDGenerator();
00107
00108 self.goal_callback = goal_cb;
00109 assert(self.goal_callback);
00110
00111 self.cancel_callback = cancel_cb;
00112 self.auto_start = auto_start;
00113
00114 self.started = False;
00115
00116 if self.auto_start:
00117 rospy.logwarn("You've passed in true for auto_start to the python action server, you should always pass in false to avoid race conditions.")
00118 self.start()
00119
00120
00121
00122
00123 def register_goal_callback(self, cb):
00124 self.goal_callback = cb
00125
00126
00127
00128 def register_cancel_callback(self,cancel_cb):
00129 self.cancel_callback = cancel_cb
00130
00131
00132 def start(self):
00133 with self.lock:
00134 self.initialize();
00135 self.started = True;
00136 self.publish_status();
00137
00138
00139 def initialize(self):
00140 self.status_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/status", GoalStatusArray, latch=True);
00141 self.result_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/result", self.ActionResult);
00142 self.feedback_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/feedback", self.ActionFeedback);
00143
00144 self.goal_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/goal", self.ActionGoal,self.internal_goal_callback);
00145
00146 self.cancel_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/cancel", GoalID,self.internal_cancel_callback);
00147
00148
00149
00150 resolved_status_frequency_name = rospy.remap_name(self.ns)+"/status_frequency"
00151 if rospy.has_param(resolved_status_frequency_name):
00152 self.status_frequency = rospy.get_param(resolved_status_frequency_name, 5.0);
00153 rospy.logwarn("You're using the deprecated status_frequency parameter, please switch to actionlib_status_frequency.")
00154 else:
00155 search_status_frequency_name = rospy.search_param("actionlib_status_frequency")
00156 if search_status_frequency_name is None:
00157 self.status_frequency = 5.0
00158 else:
00159 self.status_frequency = rospy.get_param(search_status_frequency_name, 5.0)
00160
00161 status_list_timeout = rospy.get_param(rospy.remap_name(self.ns)+"/status_list_timeout", 5.0);
00162 self.status_list_timeout = rospy.Duration(status_list_timeout);
00163
00164
00165 self.status_timer = threading.Thread( None, ros_timer, None, (self.publish_status_async,self.status_frequency) );
00166 self.status_timer.start();
00167
00168
00169
00170
00171 def publish_result(self, status, result):
00172 with self.lock:
00173 ar = self.ActionResult();
00174 ar.header.stamp = rospy.Time.now();
00175 ar.status = status;
00176 ar.result = result;
00177 self.result_pub.publish(ar);
00178 self.publish_status()
00179
00180
00181
00182
00183
00184 def publish_feedback(self, status, feedback):
00185 with self.lock:
00186
00187 af=self.ActionFeedback();
00188 af.header.stamp = rospy.Time.now();
00189 af.status = status;
00190 af.feedback = feedback;
00191 self.feedback_pub.publish(af);
00192
00193
00194
00195 def internal_cancel_callback(self, goal_id):
00196 with self.lock:
00197
00198
00199 if not self.started:
00200 return;
00201
00202
00203 rospy.logdebug("The action server has received a new cancel request");
00204
00205 goal_id_found = False;
00206 for st in self.status_list[:]:
00207
00208
00209
00210 cancel_everything = (goal_id.id == "" and goal_id.stamp == rospy.Time() )
00211 cancel_this_one = ( goal_id.id == st.status.goal_id.id)
00212 cancel_before_stamp = (goal_id.stamp != rospy.Time() and st.status.goal_id.stamp <= goal_id.stamp)
00213
00214 if cancel_everything or cancel_this_one or cancel_before_stamp:
00215
00216 if goal_id.id == st.status.goal_id.id:
00217 goal_id_found = True;
00218
00219
00220 handle_tracker = st.handle_tracker;
00221
00222 if handle_tracker is None:
00223
00224 handle_tracker = HandleTrackerDeleter(self, st);
00225 st.handle_tracker = handle_tracker;
00226
00227
00228 st.handle_destruction_time = rospy.Time.now()
00229
00230
00231
00232
00233 gh = ServerGoalHandle(st, self, handle_tracker);
00234 if gh.set_cancel_requested():
00235
00236 self.cancel_callback(gh);
00237
00238
00239
00240 if goal_id.id != "" and not goal_id_found:
00241 tracker= StatusTracker(goal_id, actionlib_msgs.msg.GoalStatus.RECALLING);
00242 self.status_list.append(tracker)
00243
00244 tracker.handle_destruction_time = rospy.Time.now()
00245
00246
00247 if goal_id.stamp > self.last_cancel:
00248 self.last_cancel = goal_id.stamp;
00249
00250
00251
00252 def internal_goal_callback(self, goal):
00253 with self.lock:
00254
00255 if not self.started:
00256 return;
00257
00258 rospy.logdebug("The action server has received a new goal request");
00259
00260
00261 for st in self.status_list[:]:
00262 if goal.goal_id.id == st.status.goal_id.id:
00263 rospy.logdebug("Goal %s was already in the status list with status %i" % (goal.goal_id.id, st.status.status))
00264
00265 if st.status.status == actionlib_msgs.msg.GoalStatus.RECALLING:
00266 st.status.status = actionlib_msgs.msg.GoalStatus.RECALLED
00267 self.publish_result(st.status, self.ActionResultType())
00268
00269
00270
00271 if st.handle_tracker is None:
00272 st.handle_destruction_time = rospy.Time.now()
00273
00274
00275 return;
00276
00277
00278 st = StatusTracker(None,None,goal)
00279 self.status_list.append(st);
00280
00281
00282 handle_tracker = HandleTrackerDeleter(self, st);
00283
00284 st.handle_tracker = handle_tracker;
00285
00286
00287 gh= ServerGoalHandle(st, self, handle_tracker);
00288 if goal.goal_id.stamp != rospy.Time() and goal.goal_id.stamp <= self.last_cancel:
00289
00290 gh.set_canceled(None, "This goal handle was canceled by the action server because its timestamp is before the timestamp of the last cancel request");
00291 else:
00292
00293 self.goal_callback(gh);
00294
00295
00296
00297 def publish_status_async(self):
00298 rospy.logdebug("Status async");
00299 with self.lock:
00300
00301 if not self.started:
00302 return
00303 self.publish_status();
00304
00305
00306
00307
00308 def publish_status(self):
00309 with self.lock:
00310
00311 status_array = actionlib_msgs.msg.GoalStatusArray()
00312
00313
00314
00315 i=0;
00316 while i<len(self.status_list):
00317 st = self.status_list[i]
00318
00319 if st.handle_destruction_time != rospy.Time() and st.handle_destruction_time + self.status_list_timeout < rospy.Time.now():
00320 rospy.logdebug("Item %s with destruction time of %.3f being removed from list. Now = %.3f" % \
00321 (st.status.goal_id, st.handle_destruction_time.to_sec(), rospy.Time.now().to_sec()))
00322 del self.status_list[i]
00323 else:
00324 status_array.status_list.append(st.status);
00325 i+=1
00326
00327
00328 status_array.header.stamp = rospy.Time.now()
00329 self.status_pub.publish(status_array)
00330
00331