예제 #1
0
class EnhancedActionServer(object):

    def __init__(self, name, actionMsg, execute_cb=None, auto_start=True, call_on_elected=False):
        self.execute_callback = execute_cb
        self.goal_callback = None
        self.preempt_callback = None
        self.call_on_elected = call_on_elected
        self.queue = Queue()
        self.lock = threading.Lock()
        self.electionList = dict()
        self.ghDict = dict()

        # create the action server
        self.action_server = ActionServer(name, actionMsg, self.get_next_gh, self.internal_preempt_callback, auto_start)

        # create the scheduling daemon thread
        if name+"/schedThread" not in [t.name for t in threading.enumerate()]:
            workingThread = threading.Thread(name=name+"/schedThread", target=self.scheduling)
            workingThread.setDaemon(True)
            workingThread.start()

    def start(self):
        """
        start server if not already started
        """
        self.action_server.start()

    def _set_gh_state(self, result=None, text="", succeeded=True):
        """
        :param result: An optional result to send back to any clients of the goal
        :param text: An optionnal text associated to the SUCCESS|ABORTED status
        :param succeeded: the goal handle is set to SUCCEEDED if true, else ABORTED
        """
        threadId = threading.current_thread().ident
        self.lock.acquire()
        if threadId not in self.ghDict:
            rospy.logwarn("set_succeeded called more than once in an actionlib server callback or outside one, ignoring call")
            return
        gh = self.ghDict[threadId]
        if result is None:
            result = self.get_default_result()
        rate = rospy.Rate(1000)
        self.lock.release()
        while not self.is_elected(gh):
            rate.sleep()
        sender, _, _ = gh.get_goal_id().id.split("-")
        self.electionList.get(sender).pop(0)
        if succeeded:
            gh.set_succeeded(result, text)
        else:
            gh.set_aborted(result, text)
        self.lock.acquire()
        self.ghDict.pop(threadId)
        self.lock.release()

    def set_succeeded(self, result=None, text=""):
        self._set_gh_state(result, text)

    def set_aborted(self, result=None, text=""):
        self._set_gh_state(result, text, succeeded=False)
        
    def get_default_result(self):
        """
        :return: default content for result message
        """
        return self.action_server.ActionResultType()

    def execute_callback_on_elected(self, goal_handle):
        rate = rospy.Rate(1000)
        while not self.is_elected(goal_handle):
            rate.sleep()
        self.execute_server_callback(goal_handle)

    def execute_server_callback(self, goal_handle):
        self.lock.acquire()
        self.ghDict[threading.current_thread().ident] = goal_handle
        self.lock.release()
        try:
            self.execute_callback(goal_handle.get_goal())
        except (KeyboardInterrupt, SystemExit):
            raise
        except Exception as e:
            rospy.logerr("Error in the actionlib server callback: {}".format(traceback.format_exc()))
        finally:
            if threading.current_thread().ident in self.ghDict:
                rospy.logwarn("The actionlib server callback did not set the goal as succeeded, sending unsuccessful result")
                self.set_aborted()

    def get_next_gh(self, goal_handle):
        """
        start new thread on new goal reception
        :type goal_handle: ServerGoalHandle
        :return:
        """
        try:
            rospy.logdebug("A new goal %s has been recieved by the single goal action server", goal_handle.get_goal_id().id)
            if self.execute_callback:
                goal_handle.status_tracker.status.status = 6
                self.queue.put(goal_handle.get_goal_id().id, block=False)
                try:
                    if self.call_on_elected:
                        t = threading.Thread(target=self.execute_callback_on_elected, args=(goal_handle,))
                    else:
                        t = threading.Thread(target=self.execute_server_callback, args=(goal_handle,))
                    t.setDaemon(True)
                    t.start()
                except threading.ThreadError:
                    rospy.logerr("Error: unable to start thread")
            else:
                rospy.logerr("DEFINE an execute callback")

        except Exception as e:
            rospy.logerr("CustomActionServer.internal_goal_callback - exception %s", str(e))

    # no preemption needed
    def internal_preempt_callback(self):
        pass

    def scheduling(self):
        """
        method for the daemon thread to schedule the goal threads
        """
        while True:
            goalId = self.queue.get()
            sender, _, stamp = goalId.split("-")
            stamp = float(stamp)
            self.lock.acquire()
            if sender in self.electionList:
                senderList = self.electionList.get(sender, [])
                senderList.append((stamp, goalId))
                senderList.sort(key=lambda x : x[0])
            else:
                self.electionList[sender] = [(stamp, goalId)]
            self.lock.release()

    def is_elected(self, goalHandle):
        """
        :type goalHandle: ServerGoalHandle
        :param goalHandle: goal to check if allowed to be finalized
        :return: boolean value to check if the goal is elected
        """
        isElected = False
        sender, _, _ = goalHandle.get_goal_id().id.split("-")
        self.lock.acquire()
        senderList = self.electionList.get(sender)
        if senderList:
            electedGoal = senderList[0][1]
            isElected = (goalHandle.get_goal_id().id == electedGoal)
        self.lock.release()
        return isElected

    def get_goal_handle(self):
        return self.ghDict[threading.current_thread().ident]
예제 #2
0
class MultiGoalActionServer():

    def __init__(self, name, ActionSpec, auto_start=True):
        self.preempt_request = False
        self.new_goal_preempt_request = False

        self.goal_callback = None
        self.preempt_callback = None

        self.goal_handle_lock = threading.RLock()
        self.goal_handles = {}
        self.preempt_requests = {}

        self.active_goals_pub = rospy.Publisher(name + "/active_goals", GoalList, queue_size=10)
        self.action_server = ActionServer(name, ActionSpec, self.internal_goal_callback, self.internal_preempt_callback, auto_start);

    def start(self):
        self.action_server.start()

    ## @brief Allows users to register a callback to be invoked when a new goal is available
    ## @param cb The callback to be invoked
    def register_goal_callback(self, goal_callback):
        self.goal_callback = goal_callback

    ## @brief Allows users to register a callback to be invoked when a new preempt request is available
    ## @param cb The callback to be invoked
    def register_preempt_callback(self, preempt_callback):
        self.preempt_callback = preempt_callback

    ## @brief Allows  polling implementations to query about preempt requests
    ## @return True if a preempt is requested, false otherwise
    def is_preempt_requested(self, goal_handle):
        goal_id = goal_handle.get_goal_id().id
        return goal_id in self.preempt_requests

    ## @brief Allows  polling implementations to query about the status of the current goal
    ## @return True if a goal is active, false otherwise
    def is_active(self, goal_handle):
        goal_id = goal_handle.get_goal_id().id

        if goal_id not in self.goal_handles:
            return False

        status = goal_handle.get_goal_status().status
        return status == GoalStatus.ACTIVE or status == GoalStatus.PREEMPTING

    def get_goal_handle(self, goal_handle):
        with self.goal_handle_lock:
            goal_id = goal_handle.get_goal_id().id

            if goal_id in self.goal_handles:
                return self.goal_handles[goal_id]
            return None

    def add_goal_handle(self, goal_handle):
        with self.goal_handle_lock:
            self.goal_handles[goal_handle.get_goal_id().id] = goal_handle

    def remove_goal_handle(self, goal_handle):
        with self.goal_handle_lock:
            goal_id = goal_handle.get_goal_id().id

            if goal_id in self.goal_handles:
                self.goal_handles.pop(goal_id)

                if goal_id in self.preempt_requests:
                    self.preempt_requests.pop(goal_id)

    def publish_active_goals(self):
        with self.goal_handle_lock:
            active_goals = GoalList()
            print(str(self.goal_handles))

            for key, goal_handle in self.goal_handles.iteritems():
                active_goals.goal_list.append(goal_handle.get_goal_id())
            self.active_goals_pub.publish(active_goals)

    ## @brief Callback for when the MultiGoalActionServer receives a new goal and passes it on
    def internal_goal_callback(self, goal_handle):
        rospy.loginfo("Goal received: %s", str(goal_handle.get_goal_id().id))

        with self.goal_handle_lock:
            if self.goal_callback:
                self.set_accepted(goal_handle)
                self.goal_callback(goal_handle)
            else:
                raise Exception('goal_callback not registered')

    ## @brief Sets the status of the active goal to preempted
    ## @param  result An optional result to send back to any clients of the goal
    def set_preempted(self, goal_handle, result=None, text=""):
        if not result:
            result = self.get_default_result()
        with self.goal_handle_lock:
            rospy.logdebug("Setting the current goal as canceled");
            goal_handle.set_canceled(result, text)
            self.remove_goal_handle(goal_handle)

    ## @brief Callback for when the ActionServer receives a new preempt and passes it on
    def internal_preempt_callback(self, goal_handle):
        rospy.loginfo("Preempt received: %s", str(id(goal_handle)))

        with self.goal_handle_lock:
            goal_id = goal_handle.get_goal_id().id
            self.preempt_requests[goal_id] = True

            if self.preempt_callback:
                self.preempt_callback(goal_handle)

    ## @brief Accepts a new goal when one is available
    def set_accepted(self, goal_handle):
        with self.goal_handle_lock:
            rospy.loginfo("Accepting a new goal: %s", goal_handle)
            self.add_goal_handle(goal_handle)
            goal_handle.set_accepted("This goal has been accepted by the multi goal action server")
            self.publish_active_goals()

    ## @brief Sets the status of the active goal to succeeded
    ## @param  result An optional result to send back to any clients of the goal
    def set_succeeded(self, goal_handle, result=None, text=""):
        with self.goal_handle_lock:
            if not result:
                result = self.get_default_result()
            goal_handle.set_succeeded(result, text)
            self.remove_goal_handle(goal_handle)
            self.publish_active_goals()

    ## @brief Sets the status of the active goal to aborted
    ## @param  result An optional result to send back to any clients of the goal
    def set_aborted(self, goal_handle, result=None, text=""):
        with self.goal_handle_lock:
            if not result:
                result = self.get_default_result()
            goal_handle.set_aborted(result, text)
            self.remove_goal_handle(goal_handle)
            self.publish_active_goals()

    ## @brief Publishes feedback for a given goal
    ## @param  feedback Shared pointer to the feedback to publish
    def publish_feedback(self, goal_handle, feedback):
        with self.goal_handle_lock:
            goal_handle.publish_feedback(feedback)

    def get_default_result(self):
        return self.action_server.ActionResultType();
예제 #3
0
class SimpleActionServer:
    ## @brief Constructor for a SimpleActionServer
    ## @param name A name for the action server
    ## @param execute_cb Optional callback that gets called in a separate thread whenever
    ## a new goal is received, allowing users to have blocking callbacks.
    ## Adding an execute callback also deactivates the goalCallback.
    ## @param  auto_start A boolean value that tells the ActionServer wheteher or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server.
    def __init__(self, name, ActionSpec, execute_cb=None, auto_start=True):

        self.new_goal = False
        self.preempt_request = False
        self.new_goal_preempt_request = False

        self.execute_callback = execute_cb
        self.goal_callback = None
        self.preempt_callback = None

        self.need_to_terminate = False
        self.terminate_mutex = threading.RLock()

        # since the internal_goal/preempt_callbacks are invoked from the
        # ActionServer while holding the self.action_server.lock
        # self.lock must always be locked after the action server lock
        # to avoid an inconsistent lock acquisition order
        self.lock = threading.RLock()

        self.execute_condition = threading.Condition(self.lock)

        self.current_goal = ServerGoalHandle()
        self.next_goal = ServerGoalHandle()

        if self.execute_callback:
            self.execute_thread = threading.Thread(None, self.executeLoop)
            self.execute_thread.start()
        else:
            self.execute_thread = None

        #create the action server
        self.action_server = ActionServer(name, ActionSpec,
                                          self.internal_goal_callback,
                                          self.internal_preempt_callback,
                                          auto_start)

    def __del__(self):
        if hasattr(self, 'execute_callback') and self.execute_callback:
            with self.terminate_mutex:
                self.need_to_terminate = True

            assert (self.execute_thread)
            self.execute_thread.join()

    ## @brief Accepts a new goal when one is available The status of this
    ## goal is set to active upon acceptance, and the status of any
    ## previously active goal is set to preempted. Preempts received for the
    ## new goal between checking if isNewGoalAvailable or invokation of a
    ## goal callback and the acceptNewGoal call will not trigger a preempt
    ## callback.  This means, isPreemptReqauested should be called after
    ## accepting the goal even for callback-based implementations to make
    ## sure the new goal does not have a pending preempt request.
    ## @return A shared_ptr to the new goal.
    def accept_new_goal(self):
        with self.action_server.lock, self.lock:
            if not self.new_goal or not self.next_goal.get_goal():
                rospy.logerr(
                    "Attempting to accept the next goal when a new goal is not available"
                )
                return None

            # check if we need to send a preempted message for the goal that we're currently pursuing
            if self.is_active() and self.current_goal.get_goal(
            ) and self.current_goal != self.next_goal:
                self.current_goal.set_canceled(
                    None,
                    "This goal was canceled because another goal was received by the simple action server"
                )

            rospy.logdebug("Accepting a new goal")

            # accept the next goal
            self.current_goal = self.next_goal
            self.new_goal = False

            # set preempt to request to equal the preempt state of the new goal
            self.preempt_request = self.new_goal_preempt_request
            self.new_goal_preempt_request = False

            # set the status of the current goal to be active
            self.current_goal.set_accepted(
                "This goal has been accepted by the simple action server")

            return self.current_goal.get_goal()

    ## @brief Allows  polling implementations to query about the availability of a new goal
    ## @return True if a new goal is available, false otherwise
    def is_new_goal_available(self):
        return self.new_goal

    ## @brief Allows  polling implementations to query about preempt requests
    ## @return True if a preempt is requested, false otherwise
    def is_preempt_requested(self):
        return self.preempt_request

    ## @brief Allows  polling implementations to query about the status of the current goal
    ## @return True if a goal is active, false otherwise
    def is_active(self):
        if not self.current_goal.get_goal():
            return False

        status = self.current_goal.get_goal_status().status
        return status == actionlib_msgs.msg.GoalStatus.ACTIVE or status == actionlib_msgs.msg.GoalStatus.PREEMPTING

    ## @brief Sets the status of the active goal to succeeded
    ## @param  result An optional result to send back to any clients of the goal
    def set_succeeded(self, result=None, text=""):
        with self.action_server.lock, self.lock:
            if not result:
                result = self.get_default_result()
            self.current_goal.set_succeeded(result, text)

    ## @brief Sets the status of the active goal to aborted
    ## @param  result An optional result to send back to any clients of the goal
    def set_aborted(self, result=None, text=""):
        with self.action_server.lock, self.lock:
            if not result:
                result = self.get_default_result()
            self.current_goal.set_aborted(result, text)

    ## @brief Publishes feedback for a given goal
    ## @param  feedback Shared pointer to the feedback to publish
    def publish_feedback(self, feedback):
        self.current_goal.publish_feedback(feedback)

    def get_default_result(self):
        return self.action_server.ActionResultType()

    ## @brief Sets the status of the active goal to preempted
    ## @param  result An optional result to send back to any clients of the goal
    def set_preempted(self, result=None, text=""):
        if not result:
            result = self.get_default_result()
        with self.action_server.lock, self.lock:
            rospy.logdebug("Setting the current goal as canceled")
            self.current_goal.set_canceled(result, text)

    ## @brief Allows users to register a callback to be invoked when a new goal is available
    ## @param cb The callback to be invoked
    def register_goal_callback(self, cb):
        if self.execute_callback:
            rospy.logwarn(
                "Cannot call SimpleActionServer.register_goal_callback() because an executeCallback exists. Not going to register it."
            )
        else:
            self.goal_callback = cb

    ## @brief Allows users to register a callback to be invoked when a new preempt request is available
    ## @param cb The callback to be invoked
    def register_preempt_callback(self, cb):
        self.preempt_callback = cb

    ## @brief Explicitly start the action server, used it auto_start is set to false
    def start(self):
        self.action_server.start()

    ## @brief Callback for when the ActionServer receives a new goal and passes it on
    def internal_goal_callback(self, goal):
        self.execute_condition.acquire()

        try:
            rospy.logdebug(
                "A new goal %shas been recieved by the single goal action server",
                goal.get_goal_id().id)

            # check that the timestamp is past that of the current goal and the next goal
            if ((not self.current_goal.get_goal() or goal.get_goal_id().stamp
                 >= self.current_goal.get_goal_id().stamp) and
                (not self.next_goal.get_goal() or goal.get_goal_id().stamp >=
                 self.next_goal.get_goal_id().stamp)):
                # if next_goal has not been accepted already... its going to get bumped, but we need to let the client know we're preempting
                if (self.next_goal.get_goal()
                        and (not self.current_goal.get_goal()
                             or self.next_goal != self.current_goal)):
                    self.next_goal.set_canceled(
                        None,
                        "This goal was canceled because another goal was received by the simple action server"
                    )

                self.next_goal = goal
                self.new_goal = True
                self.new_goal_preempt_request = False

                # if the server is active, we'll want to call the preempt callback for the current goal
                if (self.is_active()):
                    self.preempt_request = True
                    # if the user has registered a preempt callback, we'll call it now
                    if (self.preempt_callback):
                        self.preempt_callback()

                # if the user has defined a goal callback, we'll call it now
                if self.goal_callback:
                    self.goal_callback()

                # Trigger runLoop to call execute()
                self.execute_condition.notify()
                self.execute_condition.release()
            else:
                # the goal requested has already been preempted by a different goal, so we're not going to execute it
                goal.set_canceled(
                    None,
                    "This goal was canceled because another goal was received by the simple action server"
                )
                self.execute_condition.release()
        except Exception as e:
            rospy.logerr(
                "SimpleActionServer.internal_goal_callback - exception %s",
                str(e))
            self.execute_condition.release()

    ## @brief Callback for when the ActionServer receives a new preempt and passes it on
    def internal_preempt_callback(self, preempt):
        with self.lock:
            rospy.logdebug(
                "A preempt has been received by the SimpleActionServer")

            #if the preempt is for the current goal, then we'll set the preemptRequest flag and call the user's preempt callback
            if (preempt == self.current_goal):
                rospy.logdebug(
                    "Setting preempt_request bit for the current goal to TRUE and invoking callback"
                )
                self.preempt_request = True

                #if the user has registered a preempt callback, we'll call it now
                if (self.preempt_callback):
                    self.preempt_callback()
            #if the preempt applies to the next goal, we'll set the preempt bit for that
            elif (preempt == self.next_goal):
                rospy.logdebug(
                    "Setting preempt request bit for the next goal to TRUE")
                self.new_goal_preempt_request = True

    ## @brief Called from a separate thread to call blocking execute calls
    def executeLoop(self):
        loop_duration = rospy.Duration.from_sec(.1)

        while (not rospy.is_shutdown()):
            with self.terminate_mutex:
                if (self.need_to_terminate):
                    break

            # the following checks (is_active, is_new_goal_available)
            # are performed without locking
            # the worst thing that might happen in case of a race
            # condition is a warning/error message on the console
            if (self.is_active()):
                rospy.logerr(
                    "Should never reach this code with an active goal")
                return

            if (self.is_new_goal_available()):
                # accept_new_goal() is performing its own locking
                goal = self.accept_new_goal()
                if not self.execute_callback:
                    rospy.logerr(
                        "execute_callback_ must exist. This is a bug in SimpleActionServer"
                    )
                    return

                try:
                    self.execute_callback(goal)

                    if self.is_active():
                        rospy.logwarn(
                            "Your executeCallback did not set the goal to a terminal status.  "
                            +
                            "This is a bug in your ActionServer implementation. Fix your code!  "
                            +
                            "For now, the ActionServer will set this goal to aborted"
                        )
                        self.set_aborted(None, "No terminal state was set.")
                except Exception as ex:
                    rospy.logerr("Exception in your execute callback: %s\n%s",
                                 str(ex), traceback.format_exc())
                    self.set_aborted(
                        None, "Exception in execute callback: %s" % str(ex))

            with self.execute_condition:
                self.execute_condition.wait(loop_duration.to_sec())
예제 #4
0
class ComplexActionServer:
    ## @brief Constructor for a ComplexActionServer
    ## @param name A name for the action server
    ## @param execute_cb Optional callback that gets called in a separate thread whenever
    ## a new goal is received, allowing users to have blocking callbacks.
    ## Adding an execute callback also deactivates the goalCallback.
    ## @param  auto_start A boolean value that tells the ActionServer wheteher or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server.
    def __init__(self, name, ActionSpec, execute_cb = None, auto_start = True):

        self.goals_received_ = 0;
        self.goal_queue_ = six.moves.queue.Queue()

        self.new_goal = False

        self.execute_callback = execute_cb;
        self.goal_callback = None;

        self.need_to_terminate = False
        self.terminate_mutex = threading.RLock();

        # since the internal_goal/preempt_callbacks are invoked from the
        # ActionServer while holding the self.action_server.lock
        # self.lock must always be locked after the action server lock
        # to avoid an inconsistent lock acquisition order
        self.lock = threading.RLock();

        self.execute_condition = threading.Condition(self.lock);

        self.current_goal = ServerGoalHandle();
        self.next_goal = ServerGoalHandle();

        if self.execute_callback:
            self.execute_thread = threading.Thread(None, self.executeLoop);
            self.execute_thread.start();
        else:
            self.execute_thread = None
        

        #create the action server
        self.action_server = ActionServer(name, ActionSpec, self.internal_goal_callback,self.internal_preempt_callback,auto_start);


    def __del__(self):
        if hasattr(self, 'execute_callback') and self.execute_callback:
            with self.terminate_mutex:
                self.need_to_terminate = True;

            assert(self.execute_thread);
            self.execute_thread.join();


    ## @brief Accepts a new goal when one is available The status of this
    ## goal is set to active upon acceptance, 
    def accept_new_goal(self):
        with self.action_server.lock, self.lock:

            rospy.logdebug("Accepting a new goal");

            self.goals_received_ -= 1;

			#get from queue 
            current_goal = self.goal_queue_.get() 

            #set the status of the current goal to be active
            current_goal.set_accepted("This goal has been accepted by the simple action server");

            return current_goal#.get_goal();


    ## @brief Allows  polling implementations to query about the availability of a new goal
    ## @return True if a new goal is available, false otherwise
    def is_new_goal_available(self):
        return self.goals_received_ > 0


    ## @brief Allows  polling implementations to query about the status of the current goal
    ## @return True if a goal is active, false otherwise
    def is_active(self):
       if not self.current_goal.get_goal():
           return False;

       status = self.current_goal.get_goal_status().status;
       return status == actionlib_msgs.msg.GoalStatus.ACTIVE #or status == actionlib_msgs.msg.GoalStatus.PREEMPTING;

    ## @brief Sets the status of the active goal to succeeded
    ## @param  result An optional result to send back to any clients of the goal
    def set_succeeded(self,result=None, text="", goal_handle=None):
      with self.action_server.lock, self.lock:
          if not result:
              result=self.get_default_result();
          #self.current_goal.set_succeeded(result, text);
          goal_handle.set_succeeded(result,text)	

    ## @brief Sets the status of the active goal to aborted
    ## @param  result An optional result to send back to any clients of the goal
    def set_aborted(self, result = None, text="" , goal_handle=None):
        with self.action_server.lock, self.lock:
            if not result:
                result=self.get_default_result();
            #self.current_goal.set_aborted(result, text);
            goal_handle.set_aborted(result,text)

    ## @brief Publishes feedback for a given goal
    ## @param  feedback Shared pointer to the feedback to publish
    def publish_feedback(self,feedback):
        self.current_goal.publish_feedback(feedback);


    def get_default_result(self):
        return self.action_server.ActionResultType();

    ## @brief Allows users to register a callback to be invoked when a new goal is available
    ## @param cb The callback to be invoked
    def register_goal_callback(self,cb):
        if self.execute_callback:
            rospy.logwarn("Cannot call ComplexActionServer.register_goal_callback() because an executeCallback exists. Not going to register it.");
        else:
            self.goal_callback = cb;


    ## @brief Explicitly start the action server, used it auto_start is set to false
    def start(self):
        self.action_server.start();


    ## @brief Callback for when the ActionServer receives a new goal and passes it on
    def internal_goal_callback(self, goal):
          self.execute_condition.acquire();

          try:
              rospy.logdebug("A new goal %shas been recieved by the single goal action server",goal.get_goal_id().id);


              print("got a goal")
              self.next_goal = goal;
              self.new_goal = True;
              self.goals_received_ += 1
				
              #add goal to queue
              self.goal_queue_.put(goal)

                  #Trigger runLoop to call execute()
              self.execute_condition.notify();
              self.execute_condition.release();

          except Exception as e:
              rospy.logerr("ComplexActionServer.internal_goal_callback - exception %s",str(e))
              self.execute_condition.release();

       
    ## @brief Callback for when the ActionServer receives a new preempt and passes it on
    def internal_preempt_callback(self,preempt):
    	return

    ## @brief Called from a separate thread to call blocking execute calls
    def executeLoop(self):
          loop_duration = rospy.Duration.from_sec(.1);

          while (not rospy.is_shutdown()):
              rospy.logdebug("SAS: execute");

              with self.terminate_mutex:
                  if (self.need_to_terminate):
                      break;

              if (self.is_new_goal_available()):
                  # accept_new_goal() is performing its own locking
                  goal_handle = self.accept_new_goal();
                  if not self.execute_callback:
                      rospy.logerr("execute_callback_ must exist. This is a bug in ComplexActionServer");
                      return

                  try:
                  	
                      print("run new executecb")
                      thread = threading.Thread(target=self.run_goal,args=(goal_handle.get_goal(),goal_handle));
                      thread.start()

                  except Exception as ex:
                      rospy.logerr("Exception in your execute callback: %s\n%s", str(ex),
                                   traceback.format_exc())
                      self.set_aborted(None, "Exception in execute callback: %s" % str(ex))

              with self.execute_condition:
                  self.execute_condition.wait(loop_duration.to_sec());
                  
                  
                  
                  
    def run_goal(self,goal, goal_handle):
       print('new thread')
       self.execute_callback(goal,goal_handle);      


            
class QueuedActionServer:
    ## @brief Constructor for a QueuedActionServer
    ## @param name A name for the action server
    ## @param execute_cb Optional callback that gets called in a separate thread whenever
    ## a new goal is received, allowing users to have blocking callbacks.
    ## Adding an execute callback also deactivates the goalCallback.
    ## @param  auto_start A boolean value that tells the ActionServer wheteher or not to start publishing as soon as it comes up. THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and start() should be called after construction of the server.

    def __init__(self, name, ActionSpec, execute_cb=None, auto_start=True):

        self.new_goal = False
        self.preempt_request = False
        self.new_goal_preempt_request = False
        self.maxlen = 5

        #self.goals_buf = deque(maxlen=5)
        self.goals_buf = Queue.Queue(maxsize=self.maxlen)
        self.current_indexA = 0
        self.current_indexP = 0

        #self.maxlen = self.goals_buf.maxlen

        self.execute_callback = execute_cb
        self.goal_callback = None
        self.preempt_callback = None

        self.need_to_terminate = False
        self.terminate_mutex = threading.RLock()
        self.lock = threading.RLock()

        self.execute_condition = threading.Condition(self.lock)

        self.current_goal = ServerGoalHandle()
        self.next_goal = ServerGoalHandle()

        if self.execute_callback:
            self.execute_thread = threading.Thread(None, self.executeLoop)
            self.execute_thread.start()
        else:
            self.execute_thread = None

        #create the action server
        self.action_server = ActionServer(name, ActionSpec,
                                          self.internal_goal_callback,
                                          self.internal_preempt_callback,
                                          auto_start)

    def __del__(self):
        if hasattr(self, 'execute_callback') and self.execute_callback:
            with self.terminate_mutex:
                self.need_to_terminate = True

            assert (self.execute_thread)
            self.execute_thread.join()

## @brief Accepts a new goal when one is available The status of this
## goal is set to active upon acceptance, and the status of any
## previously active goal is set to preempted. Preempts received for the
## new goal between checking if isNewGoalAvailable or invokation of a
## goal callback and the acceptNewGoal call will not trigger a preempt
## callback.  This means, isPreemptReqauested should be called after
## accepting the goal even for callback-based implementations to make
## sure the new goal does not have a pending preempt request.
## @return A shared_ptr to the new goal.

    def accept_new_goal(self):

        with self.lock:
            if not self.new_goal or not self.next_goal.get_goal():
                rospy.logerr(
                    "Attempting to accept the next goal when a new goal is not available"
                )
                return None

            rospy.logdebug("Accepting a new goal")

            #accept the next goal
            self.current_goal = self.next_goal
            self.new_goal = False

            #set preempt to request to equal the preempt state of the new goal
            self.preempt_request = self.new_goal_preempt_request
            self.new_goal_preempt_request = False

            #set the status of the current goal to be active
            self.current_goal.set_accepted(
                "This goal has been accepted by the queued action server")

            return self.current_goal.get_goal()

## @brief Allows  polling implementations to query about the availability of a new goal
## @return True if a new goal is available, false otherwise

    def is_new_goal_available(self):
        return self.new_goal

## @brief Allows  polling implementations to query about preempt requests
## @return True if a preempt is requested, false otherwise

    def is_preempt_requested(self):
        return self.preempt_request

## @brief Allows  polling implementations to query about the status of the current goal
## @return True if a goal is active, false otherwise

    def is_active(self):
        if not self.current_goal.get_goal():
            return False

        status = self.current_goal.get_goal_status().status
        return status == actionlib_msgs.msg.GoalStatus.ACTIVE or status == actionlib_msgs.msg.GoalStatus.PREEMPTING

## @brief Sets the status of the active goal to succeeded
## @param  result An optional result to send back to any clients of the goal

    def set_succeeded(self, result=None, text=""):
        with self.lock:
            if not result:
                result = self.get_default_result()
            self.current_goal.set_succeeded(result, text)

## @brief Sets the status of the active goal to aborted
## @param  result An optional result to send back to any clients of the goal

    def set_aborted(self, result=None, text=""):
        with self.lock:
            if not result:
                result = self.get_default_result()
            self.current_goal.set_aborted(result, text)

## @brief Publishes feedback for a given goal
## @param  feedback Shared pointer to the feedback to publish

    def publish_feedback(self, feedback):
        self.current_goal.publish_feedback(feedback)

    def get_default_result(self):
        return self.action_server.ActionResultType()

## @brief Sets the status of the active goal to preempted
## @param  result An optional result to send back to any clients of the goal

    def set_preempted(self, result=None, text=""):
        if not result:
            result = self.get_default_result()
        with self.lock:
            rospy.logdebug("Setting the current goal as canceled")
            self.current_goal.set_canceled(result, text)

## @brief Allows users to register a callback to be invoked when a new goal is available
## @param cb The callback to be invoked

    def register_goal_callback(self, cb):
        if self.execute_callback:
            rospy.logwarn(
                "Cannot call QueuedActionServer.register_goal_callback() because an executeCallback exists. Not going to register it."
            )
        else:
            self.goal_callback = cb

## @brief Allows users to register a callback to be invoked when a new preempt request is available
## @param cb The callback to be invoked

    def register_preempt_callback(self, cb):
        self.preempt_callback = cb

## @brief Explicitly start the action server, used it auto_start is set to false

    def start(self):
        self.action_server.start()

## @brief Callback for when the ActionServer receives a new goal and passes it on

    def internal_goal_callback(self, goal):

        self.execute_condition.acquire()
        try:

            rospy.logdebug(
                "A new goal %shas been recieved by the Queued goal action server",
                goal.get_goal_id().id)

            if (self.goals_buf.empty()):
                self.new_goal = True
                self.next_goal = goal
                self.goals_buf.put(goal, timeout=1)
            else:

                self.goals_buf.put(goal, timeout=1)

            rospy.loginfo("Queued New Goal")

            if self.goal_callback:
                self.goal_callback()

            #rospy.loginfo("Goals List-----------------------------------------------")

            #for item in self.goals_buf:

            #	rospy.loginfo("Goals Buffer%s" %item.get_goal_status())

            #rospy.loginfo("End of the Goals List-------------------------------------")

    #if the user has defined a goal callback, we'll call it now

    #Trigger runLoop to call execute()
            self.execute_condition.notify()
            self.execute_condition.release()

        except Exception, e:
            rospy.logerr(
                "QueuedActionServer.internal_goal_callback - exception %s",
                str(e))
            self.execute_condition.release()
예제 #6
0
class GitActionServer:

    def __init__(self, name, ActionSpec, execute_cb=None, auto_start=True):

        self.execute_callback = execute_cb
        self.goal_callback = None
        self.preempt_callback = None

        # since the internal_goal/preempt_callbacks are invoked from the
        # ActionServer while holding the self.action_server.lock
        # self.lock must always be locked after the action server lock
        # to avoid an inconsistent lock acquisition order
        #self.lock = threading.RLock()

        #self.execute_condition = threading.Condition(self.lock)

        self.current_goal = ServerGoalHandle()
        self.next_goal = ServerGoalHandle()

        #create the action server
        self.action_server = ActionServer(name, ActionSpec, self.get_next_gh, self.internal_preempt_callback, auto_start)

        ## @brief Explicitly start the action server, used it auto_start is set to false
    def start(self):
        self.action_server.start()

    ## @brief Sets the status of the active goal to succeeded
    ## @param  result An optional result to send back to any clients of the goal
    def set_succeeded(self, gh, result=None, text=""):
        if not result:
            result = self.get_default_result()
        gh.set_succeeded(result, text)

    def get_default_result(self):
        return self.action_server.ActionResultType()

    def git_accept_new_goal(self, goal):
        rospy.loginfo("A new goal %s  has been accepted", str(goal.get_goal()))
        goal.set_accepted("This goal has been accepted by the simple action server -- wait for response")

    def set_preempted(self, goal, result=None, text=""):
        if not result:
            result=self.get_default_result();
        rospy.logdebug("Setting the current goal as canceled");
        goal.set_canceled(result, text);

    #If a new goal is received, put the goalhandle in queue
    def get_next_gh(self, goal):
        #self.execute_condition.acquire()

        try:
            rospy.logdebug("A new goal %s has been recieved by the single goal action server", goal.get_goal_id().id)

            rospy.loginfo("A new goal %s has been recieved by the single goal action server", goal.get_goal_id().id)

            if self.execute_callback:
                #Start new thread on execute()
                rospy.loginfo("New thread about to be launched on execute")
                try:
                    t = threading.Thread(target=self.execute_callback, args=(goal,))
                    t.start()
                except:
                    rospy.logerr("Error: unable to start thread")
            else:
                rospy.logerr("DEFINE an execute callback moron")
            #self.execute_condition.release()

        except Exception as e:
            rospy.logerr("SimpleActionServer.internal_goal_callback - exception %s", str(e))
            #self.execute_condition.release()

    def internal_preempt_callback(self):
        pass