Ask Your Question

How to queue goals with actionlib

asked 2016-11-29 04:53:32 -0500

highWaters gravatar image


As I have understood, with the simple action server, when a new goal comes in, it preempts old ones. From this post it seems that it is possible to put such goals into queues, however it is not clear to me how. Is there any tutorial, or anything providing a guideline for how to implement this?

Thank you for your time:)

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2016-12-03 09:34:13 -0500

highWaters gravatar image

Hello, After playing a bit around with action-server and simple-action-server, I came up with the below simple solution (it is far from being complete of course), in case someone else might find it useful or as a start point. Also, if you see some pitfall, I'd be glad to hear it:) First I create a custom simple server as follows:

#!/usr/bin/env python
import sys
from gitagent.msg import *
import random
import roslib
roslib.load_manifest('package name')
import rospy
from actionlib import ActionServer
from actionlib.server_goal_handle import ServerGoalHandle
import traceback
import time
import threading

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

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

        #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):

    ## @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 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")

    #If a new goal is received, start new thread on execute
    def get_next_gh(self, goal):

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

            rospy.loginfo("A new goal %shas 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")
                    t = threading.Thread(target=self.execute_callback, args=(goal,))
                    rospy.logerr("Error: unable to start thread")
                rospy.logerr("DEFINE an execute callback moron")

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

    def internal_preempt_callback(self):

Then, each thread running execute puts the goalhandle in a queue. theLoop treats each item of the queue one after the other.

#!/usr/bin/env python
import sys
from gitagent.msg import *
from threading import Lock
import random
import roslib
roslib.load_manifest('package name')
import rospy
import actionlib
import Queue
import action_server
import traceback
import time
import threading

class Server:
    def __init__(self):
        rospy.init_node('server', anonymous=True)
        self.queueGoalHandles = Queue.Queue()
        self.server = action_server.ActionServer('server', doMeFavorAction, self.execute, False)
        self.keep_track_threads = []
        self.lock = Lock()

    #Needs to be used similarly as handle_serve, in order to block threads respectively.
    def execute(self, goalhandle):
        print 'I got a request'

        # Add tag to identify current thread
        # Task status: -1 ~ FAIL, 0 ~ PENDING, 1 ~ SUCCESS, 10 ~ no thread active
        index = -1

        goal = goalhandle.get_goal()
        print 'extracted goal content: ' + str(goal.sender)
        self.keep_track_threads.append({'senderId':int(goal.sender), 'task_status ...
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools



Asked: 2016-11-29 04:53:32 -0500

Seen: 1,354 times

Last updated: Dec 03 '16