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
00034 from actionlib_msgs.msg import GoalID, GoalStatus, GoalStatusArray
00035
00036 from actionlib.goal_id_generator import GoalIDGenerator
00037 from actionlib.status_tracker import StatusTracker
00038
00039 from actionlib.handle_tracker_deleter import HandleTrackerDeleter
00040 from actionlib.server_goal_handle import ServerGoalHandle
00041
00042 from actionlib.exceptions import ActionException
00043
00044
00045 def nop_cb(goal_handle):
00046 pass
00047
00048
00049 def ros_timer(callable, frequency):
00050 rate = rospy.Rate(frequency)
00051
00052 rospy.logdebug("Starting timer")
00053 while not rospy.is_shutdown():
00054 try:
00055 rate.sleep()
00056 callable()
00057 except rospy.exceptions.ROSInterruptException:
00058 rospy.logdebug("Sleep interrupted")
00059
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 self.goal_sub = None
00091 self.cancel_sub = None
00092 self.status_pub = None
00093 self.result_pub = None
00094 self.feedback_pub = None
00095
00096 self.lock = threading.RLock()
00097
00098 self.status_timer = None
00099
00100 self.status_list = []
00101
00102 self.last_cancel = rospy.Time()
00103 self.status_list_timeout = rospy.Duration()
00104
00105 self.id_generator = GoalIDGenerator()
00106
00107 self.goal_callback = goal_cb
00108 assert(self.goal_callback)
00109
00110 self.cancel_callback = cancel_cb
00111 self.auto_start = auto_start
00112
00113 self.started = False
00114
00115 if self.auto_start:
00116 rospy.logwarn("You've passed in true for auto_start to the python action server, you should always pass "
00117 "in false to avoid race conditions.")
00118 self.start()
00119
00120
00121
00122 def register_goal_callback(self, cb):
00123 self.goal_callback = cb
00124
00125
00126
00127 def register_cancel_callback(self, cancel_cb):
00128 self.cancel_callback = cancel_cb
00129
00130
00131 def start(self):
00132 with self.lock:
00133 self.initialize()
00134 self.started = True
00135 self.publish_status()
00136
00137
00138 def initialize(self):
00139 self.pub_queue_size = rospy.get_param('actionlib_server_pub_queue_size', 50)
00140 if self.pub_queue_size < 0:
00141 self.pub_queue_size = 50
00142 self.status_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/status", GoalStatusArray, latch=True, queue_size=self.pub_queue_size)
00143 self.result_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/result", self.ActionResult, queue_size=self.pub_queue_size)
00144 self.feedback_pub = rospy.Publisher(rospy.remap_name(self.ns)+"/feedback", self.ActionFeedback, queue_size=self.pub_queue_size)
00145
00146 self.sub_queue_size = rospy.get_param('actionlib_server_sub_queue_size', -1)
00147 if self.sub_queue_size < 0:
00148 self.sub_queue_size = None
00149 self.goal_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/goal", self.ActionGoal, callback=self.internal_goal_callback, queue_size=self.sub_queue_size)
00150
00151 self.cancel_sub = rospy.Subscriber(rospy.remap_name(self.ns)+"/cancel", GoalID, callback=self.internal_cancel_callback, queue_size=self.sub_queue_size)
00152
00153
00154
00155 resolved_status_frequency_name = rospy.remap_name(self.ns)+"/status_frequency"
00156 if rospy.has_param(resolved_status_frequency_name):
00157 self.status_frequency = rospy.get_param(resolved_status_frequency_name, 5.0)
00158 rospy.logwarn("You're using the deprecated status_frequency parameter, please switch to actionlib_status_frequency.")
00159 else:
00160 search_status_frequency_name = rospy.search_param("actionlib_status_frequency")
00161 if search_status_frequency_name is None:
00162 self.status_frequency = 5.0
00163 else:
00164 self.status_frequency = rospy.get_param(search_status_frequency_name, 5.0)
00165
00166 status_list_timeout = rospy.get_param(rospy.remap_name(self.ns)+"/status_list_timeout", 5.0)
00167 self.status_list_timeout = rospy.Duration(status_list_timeout)
00168
00169 self.status_timer = threading.Thread(None, ros_timer, None, (self.publish_status_async, self.status_frequency))
00170 self.status_timer.start()
00171
00172
00173
00174
00175 def publish_result(self, status, result):
00176 with self.lock:
00177 ar = self.ActionResult()
00178 ar.header.stamp = rospy.Time.now()
00179 ar.status = status
00180 ar.result = result
00181 if not rospy.is_shutdown():
00182 self.result_pub.publish(ar)
00183 self.publish_status()
00184
00185
00186
00187
00188 def publish_feedback(self, status, feedback):
00189 with self.lock:
00190 af = self.ActionFeedback()
00191 af.header.stamp = rospy.Time.now()
00192 af.status = status
00193 af.feedback = feedback
00194 if not rospy.is_shutdown():
00195 self.feedback_pub.publish(af)
00196
00197
00198 def internal_cancel_callback(self, goal_id):
00199 with self.lock:
00200
00201
00202 if not self.started:
00203 return
00204
00205
00206 rospy.logdebug("The action server has received a new cancel request")
00207
00208 goal_id_found = False
00209 for st in self.status_list[:]:
00210
00211
00212
00213 cancel_everything = (goal_id.id == "" and goal_id.stamp == rospy.Time())
00214 cancel_this_one = (goal_id.id == st.status.goal_id.id)
00215 cancel_before_stamp = (goal_id.stamp != rospy.Time() and st.status.goal_id.stamp <= goal_id.stamp)
00216
00217 if cancel_everything or cancel_this_one or cancel_before_stamp:
00218
00219 if goal_id.id == st.status.goal_id.id:
00220 goal_id_found = True
00221
00222
00223 handle_tracker = st.handle_tracker
00224
00225 if handle_tracker is None:
00226
00227 handle_tracker = HandleTrackerDeleter(self, st)
00228 st.handle_tracker = handle_tracker
00229
00230
00231 st.handle_destruction_time = rospy.Time.now()
00232
00233
00234
00235 gh = ServerGoalHandle(st, self, handle_tracker)
00236 if gh.set_cancel_requested():
00237
00238 self.cancel_callback(gh)
00239
00240
00241 if goal_id.id != "" and not goal_id_found:
00242 tracker = StatusTracker(goal_id, GoalStatus.RECALLING)
00243 self.status_list.append(tracker)
00244
00245 tracker.handle_destruction_time = rospy.Time.now()
00246
00247
00248 if goal_id.stamp > self.last_cancel:
00249 self.last_cancel = goal_id.stamp
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 == GoalStatus.RECALLING:
00266 st.status.status = 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 def publish_status_async(self):
00297 with self.lock:
00298
00299 if not self.started:
00300 return
00301 self.publish_status()
00302
00303
00304 def publish_status(self):
00305 with self.lock:
00306
00307 status_array = GoalStatusArray()
00308
00309
00310
00311 i = 0
00312 while i < len(self.status_list):
00313 st = self.status_list[i]
00314
00315 if st.handle_destruction_time != rospy.Time() and st.handle_destruction_time + self.status_list_timeout < rospy.Time.now():
00316 rospy.logdebug("Item %s with destruction time of %.3f being removed from list. Now = %.3f" %
00317 (st.status.goal_id, st.handle_destruction_time.to_sec(), rospy.Time.now().to_sec()))
00318 del self.status_list[i]
00319 else:
00320 status_array.status_list.append(st.status)
00321 i += 1
00322
00323 status_array.header.stamp = rospy.Time.now()
00324 if not rospy.is_shutdown():
00325 self.status_pub.publish(status_array)