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)