Added ROS 2 ActionClient support (#125)

Co-authored-by: Dani Martinez <[email protected]>

Author: danmartzla

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 ROS 2 action client support.
 
 **Changed**
 
@@ -55,7 +56,7 @@ Unreleased
 
 **Added**
 
-* Added a ROS2-compatible header class in ``roslibpy.ros2.Header``.
+* Added a ROS 2 compatible header class in ``roslibpy.ros2.Header``.
 
 **Changed**
 

README.rst

@@ -44,7 +44,7 @@ local ROS environment, allowing usage from platforms other than Linux.
 
 The API of **roslibpy** is modeled to closely match that of `roslibjs`_.
 
-ROS1 is fully supported. ROS2 support is still in progress.
+ROS 1 is fully supported. ROS 2 support is still in progress.
 
 
 Main features

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 ROS 2 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,74 @@
+from __future__ import print_function
+import roslibpy
+import time
+
+
+global result
+
+def result_callback(msg):
+    global result
+    result = msg
+    print("Action result:", msg)
+
+def feedback_callback(msg):
+    print(f"Action feedback: {msg['partial_sequence']}")
+
+def fail_callback(msg):
+    print(f"Action failed: {msg}")
+
+
+def test_action_success(action_client):
+    """ This test function sends a action goal to an Action server.
+    """
+    global result
+    result = None
+    
+    action_client.send_goal(roslibpy.ActionGoal({"order": 8}),
+                                result_callback,
+                                feedback_callback,
+                                fail_callback)
+
+    while result == None:
+        time.sleep(1)
+
+    print("-----------------------------------------------")
+    print("Action status:", result["status"])
+    print("Action result: {}".format(result["values"]["sequence"]))
+
+
+def test_action_cancel(action_client):
+    """ This test function sends a cancel request to an Action server.
+        NOTE: Make sure to start the "rosbridge_server" node with the parameter
+              "send_action_goals_in_new_thread" set to "true".
+    """
+    global result
+    result = None
+
+    goal_id = action_client.send_goal(roslibpy.ActionGoal({"order": 8}),
+                                    result_callback,
+                                    feedback_callback,
+                                    fail_callback)
+    time.sleep(3)
+    print("Sending action goal cancel request...")
+    action_client.cancel_goal(goal_id)
+
+    while result == None:
+        time.sleep(1)
+
+    print("-----------------------------------------------")
+    print("Action status:", result["status"])
+    print("Action result: {}".format(result["values"]["sequence"]))
+
+
+if __name__ == "__main__":
+    client = roslibpy.Ros(host="localhost", port=9090)
+    client.run()
+
+    action_client = roslibpy.ActionClient(client,
+                                        "/fibonacci",
+                                      "custom_action_interfaces/action/Fibonacci")
+    print("\n** Starting action client test **")
+    test_action_success(action_client)
+
+    print("\n** Starting action goal cancelation test **")
+    test_action_cancel(action_client)

docs/index.rst

@@ -44,7 +44,7 @@ local ROS environment, allowing usage from platforms other than Linux.
 
 The API of **roslibpy** is modeled to closely match that of `roslibjs <http://wiki.ros.org/roslibjs>`_.
 
-ROS1 is fully supported. ROS2 support is still in progress.
+ROS 1 is fully supported. ROS 2 support is still in progress.
 
 ========
 Contents

src/roslibpy/__init__.py

@@ -41,13 +41,13 @@
 Main ROS concepts
 =================
 
-ROS1 vs ROS2
+ROS 1 vs ROS 2
 ------------
 
-This library has been tested to work with ROS1. ROS2 should work, but it is still
+This library has been tested to work with ROS 1. ROS 2 should work, but it is still
 in the works.
 
-One area in which ROS1 and ROS2 differ is in the header interface. To use ROS2, use
+One area in which ROS 1 and ROS 2 differ is in the header interface. To use ROS 2, use
 the header defined in the `roslibpy.ros2` module.
 
 .. autoclass:: roslibpy.ros2.Header
@@ -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 ROS 2 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,11 @@ class and are passed around via :class:`Topics <Topic>` using a **publish/subscr
     __version__,
 )
 from .core import (
+    ActionClient,
+    ActionFeedback,
+    ActionGoal,
+    ActionGoalStatus,
+    ActionResult,
     Header,
     Message,
     Param,
@@ -140,6 +159,11 @@ class and are passed around via :class:`Topics <Topic>` using a **publish/subscr
     "Service",
     "ServiceRequest",
     "ServiceResponse",
+    "ActionClient",
+    "ActionGoal",
+    "ActionGoalStatus",
+    "ActionFeedback",
+    "ActionResult",
     "Time",
     "Topic",
     "set_rosapi_timeout",

src/roslibpy/comm/comm.py

@@ -3,7 +3,14 @@
 import json
 import logging
 
-from roslibpy.core import Message, MessageEncoder, ServiceResponse
+from roslibpy.core import (
+    ActionFeedback,
+    ActionGoalStatus,
+    ActionResult,
+    Message,
+    MessageEncoder,
+    ServiceResponse,
+)
 
 LOGGER = logging.getLogger("roslibpy")
 
@@ -22,19 +29,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 +117,59 @@ 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]
+
+        LOGGER.debug("Received Action result with status: %s", message["status"])
+
+        results = {"status": ActionGoalStatus(message["status"]).name, "values": message["values"]}
+
+        if "result" in message and message["result"] is False:
+            if errback:
+                errback(results)
+        else:
+            if resultback:
+                resultback(ActionResult(results))