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:
# now, we need to create a goal handle and call the user's callback
self.goal_callback(gh)