Packed library into a ROS package

CMakeLists.txt

@@ -0,0 +1,48 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(actionlib_enhanced)
+
+find_package(catkin REQUIRED COMPONENTS
+  actionlib
+  actionlib_msgs
+  message_generation
+  message_runtime
+  rospy
+)
+
+catkin_python_setup()
+
+add_action_files(
+  FILES
+  BasicCom.action
+)
+generate_messages(
+  DEPENDENCIES
+  actionlib_msgs
+  std_msgs
+)
+
+catkin_package(
+  LIBRARIES actionlib_enhanced
+  CATKIN_DEPENDS actionlib actionlib_msgs message_runtime std_msgs
+  DEPENDS Boost
+)
+
+
+###########
+## Build ##
+###########
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+
+#############
+## Install ##
+#############
+
+install(PROGRAMS
+  examples/client.py
+  examples/server.py
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/examples
+)

README.md

@@ -0,0 +1,12 @@
+# actionlib_enhanced
+
+## Overview
+This is a custom actionlib client-server implementation.
+It can be used as one can do with the SimpleActionLib. However, it can handle multiple client for the same server, as many goal at any frequency from the same client (multi-threaded requests) without missing tracks of any goal. This means the preemptive state is useless here, as you can choose to handle the goal one after the other, or all at the same time.
+
+## Examples
+Examples of how this package can be used are in the directory examples.
+The server can handle multiple clients without any issue.
+
+## Installation
+`pip install actionlib_enhanced` should work for most users.

action/BasicCom.action

@@ -0,0 +1,4 @@
+int32 numRequest
+---
+int32 numReceived
+---

examples/client.py

@@ -0,0 +1,70 @@
+#!/usr/bin/env python
+# coding: utf-8
+
+import random
+import rospy
+import actionlib
+from actionlib_enhanced.msg import BasicComAction, BasicComGoal
+from cv_bridge import CvBridge
+from actionlib_enhanced import ActionClientCustom
+
+
+class ActionClient(object):
+
+    TAG = "TestActionlibClient"
+
+    def __init__(self):
+        rospy.init_node("test_actionlib_client", anonymous=True)
+        self.ns = rospy.get_param("~namespace", "test")
+        rospy.on_shutdown(self.onShutdown)
+        self.bridge = CvBridge()
+
+        rospy.set_param('actionlib_client_sub_queue_size', 10)
+        self.goal = BasicComGoal()
+        # Simple Actionlib client
+        self.actionClient = actionlib.SimpleActionClient("/{}/basic_com".format(self.ns), BasicComAction)
+        if not self.actionClient.wait_for_server(timeout=rospy.Duration(10)):
+            rospy.signal_shutdown("Simple server not started")
+        # Custom Actionlib client
+        self.actionClientCustom = ActionClientCustom("/{}/enhanced_com".format(self.ns), BasicComAction)
+        if not self.actionClientCustom.wait_for_server(timeout=rospy.Duration(10)):
+            rospy.signal_shutdown("Server custom not started")
+
+        rospy.loginfo("{} initialized".format(self.TAG))
+
+    def onShutdown(self):
+        pass
+
+    def sendRequest(self, count):
+        """
+        :param count: number corresponding to the BasicComGoal message
+        """
+        self.goal.numRequest = count
+        self.actionClient.send_goal(self.goal, done_cb=lambda x, y: self.doneCallback(x, y))
+
+    def sendRequest2(self, count):
+        """
+        :param count: number corresponding to the BasicComGoal message
+        """
+        self.goal.numRequest = count
+
+        # the ID returned from send_goal can be compared to the doneCallback 3rd arg
+        _ = self.actionClientCustom.send_goal(self.goal, done_cb=lambda x, y, z: self.doneCallback(x, y, z))
+
+    def doneCallback(self, goalStatus, data, name=None):
+        """
+        :param goalStatus: Status of the received goal
+        :param data: BasicComResult message
+        :param name: ID of the goal, useful for multithreaded requests
+        """
+        if goalStatus == 3:  # SUCCESS
+            rospy.loginfo("received {}".format(data.numReceived))
+
+
+if __name__ == "__main__":
+    actionClient = ActionClient()
+    for i in range(1, 21):
+        rospy.loginfo("send {}".format(i))
+        actionClient.sendRequest2(i)
+        # rospy.sleep(random.randint(1, 20))
+    rospy.spin()

examples/server.py

@@ -0,0 +1,63 @@
+#!/usr/bin/env python
+# coding: utf-8
+
+import rospy
+import actionlib
+from actionlib_enhanced import ActionServerCustom
+from actionlib_enhanced.msg import BasicComAction, BasicComResult
+
+class ActionServer():
+
+    TAG = "TestActionlibServer"
+
+    def __init__(self):
+        rospy.init_node("test_actionlib_server")
+        self.ns = rospy.get_param("~namespace", "test")
+        # Actionlib
+        self.actionServer = actionlib.SimpleActionServer("/{}/basic_com".format(self.ns),
+                                                         BasicComAction,
+                                                         self.onRequest,
+                                                         auto_start=False)
+        self.actionServer.start()
+        # ActionlibCustom
+        self.actionServerCustom = ActionServerCustom("/{}/enhanced_com".format(self.ns),
+                                                     BasicComAction,
+                                                     self.onRequest2,
+                                                     auto_start=False)
+        self.actionServerCustom.start()
+
+        rospy.on_shutdown(self.onShutdown)
+        rospy.loginfo("{} initialized".format(self.TAG))
+
+    def onShutdown(self):
+        pass
+
+    def onRequest(self, goal):
+        """
+        send back the goal it receive
+        :param goal: BasicComGoal
+        """
+        result = BasicComResult(numReceived=goal.numRequest)
+        rospy.sleep(self.delay)
+        self.actionServer.set_succeeded(result)
+
+    def onRequest2(self, goalHandle):  # multithreaded on requests
+        """
+        send back the goal it receive
+        :param goalHandle: goalHandle containing the BasicComGoal
+        """
+        # ID info if needed (/!\ : ch3.1 in http://wiki.ros.org/actionlib/DetailedDescription)
+        who, nbrRequest, stamp = goalHandle.get_goal_id().id.split("-")
+
+        goal = goalHandle.get_goal()
+        result = BasicComResult(numReceived=goal.numRequest)
+        rospy.sleep(rospy.Duration(5))
+        rate = rospy.Rate(100)
+        while not self.actionServerCustom.isElected(goalHandle):
+            rate.sleep()
+        self.actionServerCustom.set_succeeded(goalHandle, result)
+
+
+if __name__ == "__main__":
+    actionServer = ActionServer()
+    rospy.spin()

package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>actionlib_enhanced</name>
+  <version>0.0.0</version>
+  <description>The actionlib_enhanced package</description>
+  <maintainer email="[email protected]">Fabrice Poirier</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>message_runtime</build_depend>
+  <exec_depend>message_runtime</exec_depend>
+
+  <depend>actionlib_msgs</depend>
+  <depend>actionlib</depend>
+  <depend>boost</depend>
+  <depend>rospy</depend>
+  <depend>std_msgs</depend>
+</package>

setup.py

@@ -0,0 +1,12 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+
+d = generate_distutils_setup(
+    packages=['actionlib_enhanced'],
+    package_dir={'': 'src'}
+)
+
+setup(**d)

src/actionlib_enhanced/__init__.py

@@ -0,0 +1,2 @@
+from actionlib_enhanced.action_client import *
+from actionlib_enhanced.action_server import *

src/actionlib_enhanced/action_client.py

@@ -0,0 +1,110 @@
+#!/usr/bin/env python
+# coding: utf-8
+
+import threading
+import rospy
+from actionlib.action_client import ActionClient, CommState
+
+
+class GoalState(object):
+    PENDING = 0
+    ACTIVE = 1
+    DONE = 2
+    name = {0: "PENDING", 1: "ACTIVE", 2: "DONE"}
+
+
+class ActionClientCustom(object):
+
+    def __init__(self, ns, actionMsg):
+        self.action_client = ActionClient(ns, actionMsg)
+        self.ghDict = dict()
+        self.lock = threading.Lock()
+
+    def wait_for_server(self, timeout=rospy.Duration()):
+        """
+        :param timeout: rospy duration to wait for server
+        """
+        return self.action_client.wait_for_server(timeout)
+
+    def send_goal(self, goal, done_cb=None, active_cb=None, feedback_cb=None):
+        """
+        :param goal: goal for the server
+        :param done_cb: callback function for this goal when done
+        :param active_cb: callback function for this goal when goal is set to active
+        :param feedback_cb: callback function for this goal when feedback from server
+        :return name: ID of the goal if necessary
+        """
+        self.lock.acquire()
+        newGh = self.action_client.send_goal(goal, self._handle_transition, self._handle_feedback)
+        name = newGh.comm_state_machine.action_goal.goal_id.id
+        self.ghDict[name] = [newGh,
+                             GoalState.PENDING,
+                             done_cb,
+                             active_cb,
+                             feedback_cb]
+        self.lock.release()
+        return name
+
+    def removeDone(self, ID):
+        """
+        :param ID: remove a goal when Done
+        """
+        del self.ghDict[ID]
+
+    def _handle_transition(self, curGh):
+        """
+        :param curGh: handle transition for a GoalHandler
+        """
+        comm_state = curGh.get_comm_state()
+        name = curGh.comm_state_machine.action_goal.goal_id.id
+        if name in self.ghDict.keys():
+            oldStatus = self.ghDict[name][1]
+            done_cb = self.ghDict[name][2]
+            active_cb = self.ghDict[name][3]
+
+            error_msg = "Received comm state {} when in state {}".format(comm_state,
+                                                                         GoalState.name[oldStatus])
+
+            if comm_state == CommState.ACTIVE:
+                if oldStatus == GoalState.PENDING:
+                    self._set_state(GoalState.ACTIVE, name)
+                    if active_cb:
+                        active_cb()
+                elif oldStatus == GoalState.DONE:
+                    rospy.logerr(error_msg)
+            elif comm_state == CommState.RECALLING:
+                if oldStatus != GoalState.PENDING:
+                    rospy.logerr(error_msg)
+            elif comm_state == CommState.PREEMPTING:
+                if oldStatus == GoalState.PENDING:
+                    self._set_state(GoalState.ACTIVE, name)
+                    if active_cb:
+                        active_cb()
+                elif oldStatus == GoalState.DONE:
+                    rospy.logerr(error_msg)
+
+            elif comm_state == CommState.DONE:
+                if oldStatus in [GoalState.PENDING, GoalState.ACTIVE]:
+                    self._set_state(GoalState.DONE, name)
+                    if done_cb:
+                        done_cb(curGh.get_goal_status(), curGh.get_result(), name)
+                        self.removeDone(name)
+                elif oldStatus == GoalState.DONE:
+                    rospy.logerr("ActionClientCustom received DONE twice")
+
+    def _handle_feedback(self, curGh, feedback):
+        """
+        :param curGh: GoalHandler
+        :param feedback: feedback message received from server
+        """
+        name = curGh.comm_state_machine.action_goal.goal_id.id
+        if name in self.ghDict.keys():
+            feedback_cb = self.ghDict[name][4]
+            feedback_cb(feedback)
+
+    def _set_state(self, state, name):
+        """
+        :param state: new state received
+        :param name: name of the goal to change
+        """
+        self.ghDict[name][1] = state