update docs and add tests

Author: gonzalocasas

docs/reference/index.rst

@@ -8,6 +8,6 @@ API Reference
     from roslibpy import *
 
 .. automodule:: roslibpy
-.. automodule:: roslibpy.actionlib
 .. automodule:: roslibpy.tf
+.. automodule:: roslibpy.ros1.actionlib
 .. automodule:: roslibpy.ros2

src/roslibpy/__init__.py

@@ -82,8 +82,8 @@ class and are passed around via :class:`Topics <Topic>` using a **publish/subscr
 .. autoclass:: ServiceResponse
    :members:
 
-Actions
---------
+Actions (ROS 2)
+---------------
 
 An Action client for ROS 2 Actions can be used by managing goal/feedback/result
 messages via :class:`ActionClient <ActionClient>`.

src/roslibpy/comm/comm.py

@@ -174,7 +174,7 @@ def _handle_action_result(self, message):
 
         LOGGER.debug("Received Action result with status: %s", status)
 
-        results = {"status": ActionGoalStatus(status).name, "values": message.get("values", {})}
+        results = {"status": ActionGoalStatus(status), "values": message.get("values", {})}
 
         if "result" in message and message["result"] is False:
             if errback:

src/roslibpy/core.py

@@ -3,6 +3,7 @@
 import json
 import logging
 import time
+import threading
 from enum import Enum
 
 # Python 2/3 compatibility import list
@@ -553,6 +554,7 @@ def __init__(self, ros, name, action_type, reconnect_on_close=True):
         self._service_callback = None
         self._is_advertised = False
         self.reconnect_on_close = reconnect_on_close
+        self.wait_results = {}
 
     def send_goal(self, goal, resultback, feedback, errback):
         """Start a call to an action.
@@ -572,6 +574,7 @@ def send_goal(self, goal, resultback, feedback, errback):
             return
 
         action_goal_id = "send_action_goal:%s:%d" % (self.name, self.ros.id_counter)
+        self.wait_results[action_goal_id] = threading.Event()
 
         message = Message(
             {
@@ -584,7 +587,19 @@ def send_goal(self, goal, resultback, feedback, errback):
             }
         )
 
-        self.ros.call_async_action(message,  resultback, feedback, errback)
+        def _internal_resultback(result):
+            wait_result_event = self.wait_results.get(action_goal_id)
+            if wait_result_event is not None:
+                wait_result_event.set()
+            resultback(result)
+
+        def _internal_errback(err):
+            wait_result_event = self.wait_results.get(action_goal_id)
+            if wait_result_event is not None:
+                wait_result_event.set()
+            errback(err)
+
+        self.ros.call_async_action(message,  _internal_resultback, feedback, _internal_errback)
         return action_goal_id
 
     def cancel_goal(self, goal_id):
@@ -603,9 +618,28 @@ def cancel_goal(self, goal_id):
             }
         )
         self.ros.send_on_ready(message)
+
         # Remove message_id from RosBridgeProtocol._pending_action_requests in comms.py?
         # Not needed since an action result is returned upon cancelation.
 
+    def wait_goal(self, goal_id, timeout=None):
+        """Block until the result is available.
+
+        If ``timeout`` is ``None``, it will wait indefinitely.
+
+        Args:
+            goal_id: Goal ID returned from ``send_goal()``
+            timeout (:obj:`int`): Timeout to wait for the result expressed in seconds.
+        """
+
+        wait_result_event = self.wait_results.get(goal_id)
+        if wait_result_event is None:
+            raise ValueError("Unknown goal ID")
+
+        if not wait_result_event.wait(timeout):
+            raise RosTimeoutError("Goal failed to receive result")
+
+        self.wait_results.pop(goal_id, None)
 
 class Param(object):
     """A ROS parameter.

src/roslibpy/ros1/actionlib.py

@@ -1,6 +1,6 @@
 """
-Actionlib
-=========
+Actionlib (ROS 1)
+=================
 
 Another way to interact with ROS is through the **actionlib** stack. Actions in
 ROS allow to execute preemptable tasks, i.e. tasks that can be interrupted by the client.

tests/ros1/test_actionlib.py

@@ -3,7 +3,7 @@
 import time
 
 from roslibpy import Ros
-from roslibpy.actionlib import ActionClient, Goal, GoalStatus, SimpleActionServer
+from roslibpy.ros1.actionlib import ActionClient, Goal, GoalStatus, SimpleActionServer
 
 
 def test_action_success():

tests/ros2/test_actions.py

@@ -0,0 +1,74 @@
+from __future__ import print_function
+
+import time
+
+from roslibpy import Ros
+from roslibpy import ActionClient, ActionGoal, ActionGoalStatus
+
+def test_fibonacci():
+    ros = Ros("127.0.0.1", 9090)
+    ros.run()
+
+    action = ActionClient(
+        ros,
+        '/fibonacci',
+        'example_interfaces/action/Fibonacci'
+    )
+
+    results = {}
+
+    def on_result(result):
+        print(f"Result: {result}")
+        results["result"] = result
+
+    def on_feedback(feedback):
+        print(f"Feedback: {feedback}")
+
+    def on_error(error):
+        print(f"Error: {error}")
+
+    goal = ActionGoal({"order": 4})
+    goal_id = action.send_goal(goal, on_result, on_feedback, on_error)
+    action.wait_goal(goal_id)
+    time.sleep(0.2)
+    
+    assert results["result"]["values"]["sequence"] == [0, 1, 1, 2, 3]
+    assert results["result"]["status"] == ActionGoalStatus.SUCCEEDED
+
+    ros.close()
+
+def test_cancel():
+    ros = Ros("127.0.0.1", 9090)
+    ros.run()
+
+    action = ActionClient(
+        ros,
+        '/fibonacci',
+        'example_interfaces/action/Fibonacci'
+    )
+
+    results = {}
+
+    def on_result(result):
+        print(f"Result: {result}")
+        results["result"] = result
+
+    def on_feedback(feedback):
+        print(f"Feedback: {feedback}")
+
+    def on_error(error):
+        print(f"Error: {error}")
+
+    goal = ActionGoal({"order": 10})
+    goal_id = action.send_goal(goal, on_result, on_feedback, on_error)
+    time.sleep(2)
+    action.cancel_goal(goal_id)
+    action.wait_goal(goal_id)
+    time.sleep(0.2)
+
+    assert results["result"]["status"] == ActionGoalStatus.CANCELED
+
+    ros.close()
+
+if __name__ == "__main__":
+    test_cancel()