def internal_cancel_callback(self, goal_id):
with self.lock:
#if we're not started... then we're not actually going to do anything
if not self.started:
return
#we need to handle a cancel for the user
rospy.logdebug(
"The action server has received a new cancel request")
goal_id_found = False
for st in self.status_list[:]:
#check if the goal id is zero or if it is equal to the goal id of
#the iterator or if the time of the iterator warrants a cancel
cancel_everything = (
goal_id.id == "" and goal_id.stamp == rospy.Time()
) #rospy::Time()) #id and stamp 0 --> cancel everything
cancel_this_one = (goal_id.id == st.status.goal_id.id
) #ids match... cancel that goal
cancel_before_stamp = (
goal_id.stamp != rospy.Time()
and st.status.goal_id.stamp <= goal_id.stamp
) #//stamp != 0 --> cancel everything before stamp
if cancel_everything or cancel_this_one or cancel_before_stamp:
#we need to check if we need to store this cancel request for later
if goal_id.id == st.status.goal_id.id:
goal_id_found = True
#attempt to get the handle_tracker for the list item if it exists
handle_tracker = st.handle_tracker
if handle_tracker is None:
#if the handle tracker is expired, then we need to create a new one
handle_tracker = HandleTrackerDeleter(self, st)
st.handle_tracker = handle_tracker
#we also need to reset the time that the status is supposed to be removed from the list
st.handle_destruction_time = rospy.Time.now()
#set the status of the goal to PREEMPTING or RECALLING as approriate
#and check if the request should be passed on to the user
gh = ServerGoalHandle(st, self, handle_tracker)
if gh.set_cancel_requested():
#call the user's cancel callback on the relevant goal
self.cancel_callback(gh)
#if the requested goal_id was not found, and it is non-zero, then we need to store the cancel request
if goal_id.id != "" and not goal_id_found:
tracker = StatusTracker(
goal_id, actionlib_msgs.msg.GoalStatus.RECALLING)
self.status_list.append(tracker)
#start the timer for how long the status will live in the list without a goal handle to it
tracker.handle_destruction_time = rospy.Time.now()
#make sure to set last_cancel_ based on the stamp associated with this cancel request
if goal_id.stamp > self.last_cancel:
self.last_cancel = goal_id.stamp
def internal_goal_callback(self, goal):
with self.lock:
#if we're not started... then we're not actually going to do anything
if not self.started:
return
rospy.logdebug("The action server has received a new goal request")
#we need to check if this goal already lives in the status list
for st in self.status_list:
if goal.goal_id.id == st.status.goal_id.id:
rospy.logdebug(
"Goal %s was already in the status list with status %i"
% (goal.goal_id.id, st.status.status))
# Goal could already be in recalling state if a cancel came in before the goal
if st.status.status == actionlib_msgs.msg.GoalStatus.RECALLING:
st.status.status = actionlib_msgs.msg.GoalStatus.RECALLED
self.publish_result(st.status, self.ActionResultType())
#if this is a request for a goal that has no active handles left,
#we'll bump how long it stays in the list
if st.handle_tracker is None:
st.handle_destruction_time = rospy.Time.now()
#make sure not to call any user callbacks or add duplicate status onto the list
return
#if the goal is not in our list, we need to create a StatusTracker associated with this goal and push it on
st = StatusTracker(None, None, goal)
self.status_list.append(st)
#we need to create a handle tracker for the incoming goal and update the StatusTracker
handle_tracker = HandleTrackerDeleter(self, st)
st.handle_tracker = handle_tracker
#check if this goal has already been canceled based on its timestamp
gh = ServerGoalHandle(st, self, handle_tracker)
if goal.goal_id.stamp != rospy.Time(
) and goal.goal_id.stamp <= self.last_cancel:
#if it has... just create a GoalHandle for it and setCanceled
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"
)
else:
#make sure that we're unlocked before we call the users callback
self.lock.release()
#now, we need to create a goal handle and call the user's callback
self.goal_callback(gh)
self.lock.acquire()
