Added ROS2 ActionClient support

AUTHORS.rst

@@ -11,3 +11,4 @@ Authors
 * Pedro Pereira `@MisterOwlPT <https://github.com/MisterOwlPT>`_
 * Domenic Rodriguez `@DomenicP <https://github.com/DomenicP>`_
 * Ilia Baranov `@iliabaranov <https://github.com/iliabaranov>`_
+* Dani Martinez `@danmartzla <https://github.com/danmartzla>`_

CHANGELOG.rst

@@ -11,6 +11,7 @@ Unreleased
 ----------
 
 **Added**
+* Added ROS2 action client object with limited capabilities ``roslibpy.ActionClient``.
 
 **Changed**
 

docs/examples.rst

@@ -246,6 +246,13 @@ This example is very simplified and uses the :meth:`roslibpy.actionlib.Goal.wait
 function to make the code easier to read as an example. A more robust way to handle
 results is to hook up to the ``result`` event with a callback.
 
+For action clients to deal with ROS2 action servers, check the following example: 
+
+.. literalinclude :: files/ros2-action-client.py
+   :language: python
+
+* :download:`ros2-action-client.py <files/ros2-action-client.py>`
+
 Querying ROS API
 ----------------
 

docs/files/ros2-action-client.py

@@ -0,0 +1,34 @@
+from __future__ import print_function
+import roslibpy
+import time
+
+
+global result
+
+client = roslibpy.Ros(host='localhost', port=9090)
+client.run()
+
+action_client = roslibpy.ActionClient(client,
+                                      '/fibonacci',
+                                      'custom_action_interfaces/action/Fibonacci')
+result = None
+
+def result_callback(msg):
+    global result
+    result = msg['result']
+
+def feedback_callback(msg):
+    print('Action feedback:',msg['partial_sequence'])
+
+def fail_callback(msg):
+    print('Action failed:',msg)
+
+goal_id = action_client.send_goal(roslibpy.ActionGoal({'order': 8}),
+                                  result_callback,
+                                  feedback_callback,
+                                  fail_callback)
+
+while result == None:
+    time.sleep(1)
+
+print('Action result: {}'.format(result['sequence']))

src/roslibpy/__init__.py

@@ -82,6 +82,20 @@ class and are passed around via :class:`Topics <Topic>` using a **publish/subscr
 .. autoclass:: ServiceResponse
    :members:
 
+Actions
+--------
+
+An Action client for ROS2 Actions can be used by managing goal/feedback/result
+messages via :class:`ActionClient <ActionClient>`.
+
+.. autoclass:: ActionClient
+   :members:
+.. autoclass:: ActionGoal
+   :members:
+.. autoclass:: ActionFeedback
+   :members:
+.. autoclass:: ActionResult
+   :members:
 
 Parameter server
 ----------------
@@ -114,6 +128,10 @@ class and are passed around via :class:`Topics <Topic>` using a **publish/subscr
     __version__,
 )
 from .core import (
+    ActionClient,
+    ActionFeedback,
+    ActionGoal,
+    ActionResult,
     Header,
     Message,
     Param,
@@ -140,6 +158,10 @@ class and are passed around via :class:`Topics <Topic>` using a **publish/subscr
     "Service",
     "ServiceRequest",
     "ServiceResponse",
+    "ActionClient",
+    "ActionGoal",
+    "ActionFeedback",
+    "ActionResult",
     "Time",
     "Topic",
     "set_rosapi_timeout",

src/roslibpy/comm/comm.py

@@ -3,7 +3,13 @@
 import json
 import logging
 
-from roslibpy.core import Message, MessageEncoder, ServiceResponse
+from roslibpy.core import (
+    ActionFeedback,
+    ActionResult,
+    Message,
+    MessageEncoder,
+    ServiceResponse,
+)
 
 LOGGER = logging.getLogger("roslibpy")
 
@@ -22,19 +28,23 @@ def __init__(self, *args, **kwargs):
         super(RosBridgeProtocol, self).__init__(*args, **kwargs)
         self.factory = None
         self._pending_service_requests = {}
+        self._pending_action_requests = {}
         self._message_handlers = {
             "publish": self._handle_publish,
             "service_response": self._handle_service_response,
             "call_service": self._handle_service_request,
+            "send_action_goal": self._handle_action_request,  # TODO: action server
+            "cancel_action_goal": self._handle_action_cancel,  # TODO: action server
+            "action_feedback": self._handle_action_feedback,
+            "action_result": self._handle_action_result,
+            "status": None,  # TODO: add handlers for op: status
         }
-        # TODO: add handlers for op: status
 
     def on_message(self, payload):
         message = Message(json.loads(payload.decode("utf8")))
         handler = self._message_handlers.get(message["op"], None)
         if not handler:
             raise RosBridgeException('No handler registered for operation "%s"' % message["op"])
-
         handler(message)
 
     def send_ros_message(self, message):
@@ -106,3 +116,55 @@ def _handle_service_request(self, message):
             raise ValueError("Expected service name missing in service request")
 
         self.factory.emit(message["service"], message)
+
+    def send_ros_action_goal(self, message, resultback, feedback, errback):
+        """Initiate a ROS action request by sending a goal through the ROS Bridge.
+
+        Args:
+        message (:class:`.Message`): ROS Bridge Message containing the action request.
+        callback: Callback invoked on receiving result.
+        feedback: Callback invoked when receiving feedback from action server.
+        errback: Callback invoked on error.
+        """
+        request_id = message["id"]
+        self._pending_action_requests[request_id] = (resultback, feedback, errback)
+
+        json_message = json.dumps(dict(message), cls=MessageEncoder).encode("utf8")
+        LOGGER.debug("Sending ROS action goal request: %s", json_message)
+
+        self.send_message(json_message)
+
+    def _handle_action_request(self, message):
+        if "action" not in message:
+            raise ValueError("Expected action name missing in action request")
+        raise RosBridgeException('Action server capabilities not yet implemented')
+
+    def _handle_action_cancel(self, message):
+        if "action" not in message:
+            raise ValueError("Expected action name missing in action request")
+        raise RosBridgeException('Action server capabilities not yet implemented')
+
+    def _handle_action_feedback(self, message):
+        if "action" not in message:
+            raise ValueError("Expected action name missing in action feedback")
+
+        request_id = message["id"]
+        _, feedback, _ = self._pending_action_requests.get(request_id, None)
+        feedback(ActionFeedback(message["values"]))
+
+    def _handle_action_result(self, message):
+        request_id = message["id"]
+        action_handlers = self._pending_action_requests.get(request_id, None)
+
+        if not action_handlers:
+            raise RosBridgeException('No handler registered for action request ID: "%s"' % request_id)
+
+        resultback, _ , errback = action_handlers
+        del self._pending_action_requests[request_id]
+
+        if "result" in message and message["result"] is False:
+            if errback:
+                errback(message["values"])
+        else:
+            if resultback:
+                resultback(ActionResult(message["values"]))

src/roslibpy/core.py

@@ -20,6 +20,9 @@
     "Service",
     "ServiceRequest",
     "ServiceResponse",
+    "ActionGoal",
+    "ActionFeedback",
+    "ActionResult",
     "Time",
     "Topic",
 ]
@@ -131,6 +134,33 @@ def __init__(self, values=None):
             self.update(values)
 
 
+class ActionResult(UserDict):
+    """Result returned from a action call."""
+
+    def __init__(self, values=None):
+        self.data = {}
+        if values is not None:
+            self.update(values)
+
+
+class ActionFeedback(UserDict):
+    """Feedback returned from a action call."""
+
+    def __init__(self, values=None):
+        self.data = {}
+        if values is not None:
+            self.update(values)
+
+
+class ActionGoal(UserDict):
+    """Action Goal for an action call."""
+
+    def __init__(self, values=None):
+        self.data = {}
+        if values is not None:
+            self.update(values)
+
+
 class MessageEncoder(json.JSONEncoder):
     """Internal class to serialize some of the core data types into json."""
 
@@ -491,6 +521,76 @@ def _service_response_handler(self, request):
         self.ros.send_on_ready(call)
 
 
+class ActionClient(object):
+    """Action Client of ROS2 actions.
+
+    Args:
+        ros (:class:`.Ros`): Instance of the ROS connection.
+        name (:obj:`str`): Service name, e.g. ``/fibonacci``.
+        action_type (:obj:`str`): Action type, e.g. ``rospy_tutorials/fibonacci``.
+    """
+
+    def __init__(self, ros, name, action_type, reconnect_on_close=True):
+        self.ros = ros
+        self.name = name
+        self.action_type = action_type
+
+        self._service_callback = None
+        self._is_advertised = False
+        self.reconnect_on_close = reconnect_on_close
+
+    def send_goal(self, goal, resultback, feedback, errback):
+        """ Start a service call.
+
+            Note:
+            The action client is non-blocking.
+
+            Args:
+            request (:class:`.ServiceRequest`): Service request.
+            resultback: Callback invoked on receiving action result.
+            feedback: Callback invoked on receiving action feedback.
+            errback: Callback invoked on error.
+
+            Returns:
+            object: goal ID if successfull, otherwise ``None``.
+        """
+        if self._is_advertised:
+            return
+
+        action_goal_id = "send_action_goal:%s:%d" % (self.name, self.ros.id_counter)
+
+        message = Message(
+            {
+                "op": "send_action_goal",
+                "id": action_goal_id,
+                "action": self.name,
+                "action_type": self.action_type,
+                "args": dict(goal),
+                "feedback": True,
+            }
+        )
+
+        self.ros.call_async_action(message,  resultback, feedback, errback)
+        return action_goal_id
+
+    def cancel_goal(self, goal_id):
+        """ Cancel an ongoing action.
+            NOTE: Async cancelation is not yet supported on rosbridge (rosbridge_suite issue #909)
+
+            Args:
+            goal_id: Goal ID returned from "send_goal()"
+        """
+        message = Message(
+            {
+                "op": "cancel_action_goal",
+                "id": goal_id,
+                "action": self.name,
+            }
+        )
+        self.ros.send_on_ready(message)
+        # Remove message_id from RosBridgeProtocol._pending_action_requests in comms.py?
+
+
 class Param(object):
     """A ROS parameter.
 

src/roslibpy/ros.py

@@ -272,6 +272,24 @@ def _send_internal(proto):
 
         self.factory.on_ready(_send_internal)
 
+    def call_async_action(self, message, resultback, feedback, errback):
+        """Send a action request to ROS once the connection is established.
+
+        If a connection to ROS is already available, the request is sent immediately.
+
+        Args:
+            message (:class:`.Message`): ROS Bridge Message containing the request.
+            resultback: Callback invoked on successful execution.
+            feedback: Callback invoked on receiving action feedback.
+            errback: Callback invoked on error.
+        """
+
+        def _send_internal(proto):
+            proto.send_ros_action_goal(message, resultback, feedback, errback)
+            return proto
+
+        self.factory.on_ready(_send_internal)
+
     def set_status_level(self, level, identifier):
         level_message = Message({"op": "set_level", "level": level, "id": identifier})