Change action goal sending to async and add rosbridge_server test

Author: sea-bass

rosbridge_library/src/rosbridge_library/capabilities/advertise_action.py

@@ -31,7 +31,6 @@
 # POSSIBILITY OF SUCH DAMAGE.
 
 import fnmatch
-import time
 
 import rclpy
 from rclpy.action import ActionServer
@@ -67,11 +66,12 @@ def next_id(self):
         self.id_counter += 1
         return id
 
-    def execute_callback(self, goal):
+    async def execute_callback(self, goal):
         # generate a unique ID
         goal_id = f"action_goal:{self.action_name}:{self.next_id()}"
 
         future = rclpy.task.Future()
+        future.add_done_callback(lambda _: goal.succeed())
         self.goal_handles[goal_id] = goal
         self.goal_futures[goal_id] = future
 
@@ -85,13 +85,11 @@ def execute_callback(self, goal):
         }
         self.protocol.send(goal_message)
 
-        while not future.done():
-            time.sleep(self.sleep_time)
-        result = future.result()
-        goal.succeed()
-        del self.goal_futures[goal_id]
-        del self.goal_handles[goal_id]
-        return result
+        try:
+            return await future
+        finally:
+            del self.goal_futures[goal_id]
+            del self.goal_handles[goal_id]
 
     def handle_feedback(self, goal_id, feedback):
         """

rosbridge_library/src/rosbridge_library/capabilities/send_action_goal.py

@@ -160,7 +160,7 @@ def _failure(self, cid, action, exc):
         # send response with result: false
         outgoing_message = {
             "op": "action_result",
-            "service": action,
+            "action": action,
             "values": str(exc),
             "result": False,
         }

rosbridge_server/CMakeLists.txt

@@ -23,6 +23,7 @@ install(FILES
 
 if(BUILD_TESTING)
   find_package(launch_testing_ament_cmake REQUIRED)
+  add_launch_test(test/websocket/advertise_action.test.py)
   add_launch_test(test/websocket/advertise_service.test.py)
   add_launch_test(test/websocket/call_service.test.py)
   add_launch_test(test/websocket/smoke.test.py)

rosbridge_server/package.xml

@@ -24,6 +24,7 @@
   <exec_depend>rosbridge_msgs</exec_depend>
   <exec_depend>rosapi</exec_depend>
 
+  <test_depend>example_interfaces</test_depend>
   <test_depend>python3-autobahn</test_depend>
   <test_depend>launch</test_depend>
   <test_depend>launch_ros</test_depend>

rosbridge_server/test/websocket/advertise_action.test.py

@@ -0,0 +1,95 @@
+#!/usr/bin/env python
+import os
+import sys
+import unittest
+
+from example_interfaces.action import Fibonacci
+from rclpy.action import ActionClient
+from rclpy.node import Node
+from twisted.python import log
+
+sys.path.append(os.path.dirname(__file__))  # enable importing from common.py in this directory
+
+import common  # noqa: E402
+from common import expect_messages, websocket_test  # noqa: E402
+
+log.startLogging(sys.stderr)
+
+generate_test_description = common.generate_test_description
+
+
+class TestAdvertiseAction(unittest.TestCase):
+    def goal1_response_callback(self, future):
+        goal_handle = future.result()
+        if not goal_handle.accepted:
+            return
+        self.goal1_result_future = goal_handle.get_result_async()
+
+    def goal2_response_callback(self, future):
+        goal_handle = future.result()
+        if not goal_handle.accepted:
+            return
+        self.goal2_result_future = goal_handle.get_result_async()
+
+    @websocket_test
+    async def test_two_concurrent_calls(self, node: Node, make_client):
+        ws_client = await make_client()
+        ws_client.sendJson(
+            {
+                "op": "advertise_action",
+                "action": "/test_fibonacci_action",
+                "type": "example_interfaces/Fibonacci",
+            }
+        )
+        client = ActionClient(node, Fibonacci, "/test_fibonacci_action")
+        client.wait_for_server()
+
+        requests_future, ws_client.message_handler = expect_messages(
+            2, "WebSocket", node.get_logger()
+        )
+        requests_future.add_done_callback(lambda _: node.executor.wake())
+
+        self.goal1_result_future = None
+        goal1_future = client.send_goal_async(Fibonacci.Goal(order=3))
+        goal1_future.add_done_callback(self.goal1_response_callback)
+
+        self.goal2_result_future = None
+        goal2_future = client.send_goal_async(Fibonacci.Goal(order=5))
+        goal2_future.add_done_callback(self.goal2_response_callback)
+
+        requests = await requests_future
+
+        self.assertEqual(requests[0]["op"], "send_action_goal")
+        self.assertEqual(requests[0]["action"], "/test_fibonacci_action")
+        self.assertEqual(requests[0]["action_type"], "example_interfaces/Fibonacci")
+        self.assertEqual(requests[0]["args"], {"order": 3})
+        ws_client.sendJson(
+            {
+                "op": "action_result",
+                "action": "/test_fibonacci_action",
+                "values": {"sequence": [1, 1, 2]},
+                "id": requests[0]["id"],
+                "result": True,
+            }
+        )
+
+        self.assertEqual(requests[1]["op"], "send_action_goal")
+        self.assertEqual(requests[1]["action"], "/test_fibonacci_action")
+        self.assertEqual(requests[1]["action_type"], "example_interfaces/Fibonacci")
+        self.assertEqual(requests[1]["args"], {"order": 5})
+        ws_client.sendJson(
+            {
+                "op": "action_result",
+                "action": "/test_fibonacci_action",
+                "values": {"sequence": [1, 1, 2, 3, 5]},
+                "id": requests[1]["id"],
+                "result": True,
+            }
+        )
+
+        result1 = await self.goal1_result_future
+        self.assertEqual(result1.result, Fibonacci.Result(sequence=[1, 1, 2]))
+        result2 = await self.goal2_result_future
+        self.assertEqual(result2.result, Fibonacci.Result(sequence=[1, 1, 2, 3, 5]))
+
+        node.destroy_client(client)