Add cancel goal capability

rosbridge_library/src/rosbridge_library/capabilities/send_action_goal.py

@@ -46,8 +46,10 @@ class SendActionGoal(Capability):
         (False, "fragment_size", (int, type(None))),
         (False, "compression", str),
     ]
+    cancel_action_goal_msg_fields = [(True, "action", str)]
 
     actions_glob = None
+    client_handler_list = {}
 
     def __init__(self, protocol):
         # Call superclass constructor
@@ -71,6 +73,8 @@ def __init__(self, protocol):
             protocol.node_handle.get_logger().info("Sending action goal in existing thread")
             protocol.register_operation("send_action_goal", self.send_action_goal)
 
+        protocol.register_operation("cancel_action_goal", self.cancel_action_goal)
+
     def send_action_goal(self, message):
         # Pull out the ID
         cid = message.get("id", None)
@@ -117,9 +121,37 @@ def send_action_goal(self, message):
             f_cb = None
 
         # Run action client handler in the same thread.
-        ActionClientHandler(
+        client_handler = ActionClientHandler(
             trim_action_name(action), action_type, args, s_cb, e_cb, f_cb, self.protocol.node_handle
-        ).run()
+        )
+        self.client_handler_list[cid] = client_handler
+        client_handler.run()
+        del self.client_handler_list[cid]
+
+    def cancel_action_goal(self, message):
+        # Extract the args
+        cid = message.get("id", None)
+        action = message["action"]
+
+        # Pull out the ID
+        # Check for deprecated action ID, eg. /rosbridge/topics#33
+        cid = extract_id(action, cid)
+
+        # Cancel the action
+        if cid in self.client_handler_list:
+            client_handler = self.client_handler_list[cid]
+            if client_handler.send_goal_helper is not None:
+                client_handler.send_goal_helper.cancel_goal()
+
+                outgoing_message = {
+                    "op": "action_result",
+                    "action": action,
+                    "result": False,
+                }
+                if cid is not None:
+                    outgoing_message["id"] = cid
+                # TODO: fragmentation, compression
+                self.protocol.send(outgoing_message)
 
     def _success(self, cid, action, fragment_size, compression, message):
         outgoing_message = {

rosbridge_library/src/rosbridge_library/internal/actions.py

@@ -67,6 +67,7 @@ def __init__(
 
         Keyword arguments:
         action           -- the name of the action to execute.
+        action_type      -- the type of the action to execute.
         args             -- arguments to pass to the action. Can be an
         ordered list, or a dict of name-value pairs. Anything else will be
         treated as though no arguments were provided (which is still valid for
@@ -86,12 +87,14 @@ def __init__(
         self.error = error_callback
         self.feedback = feedback_callback
         self.node_handle = node_handle
+        self.send_goal_helper = None
 
     def run(self):
         try:
             # Call the service and pass the result to the success handler
+            self.send_goal_helper = SendGoal()
             self.success(
-                SendGoal().send_goal(
+                self.send_goal_helper.send_goal(
                     self.node_handle,
                     self.action,
                     self.action_type,
@@ -123,20 +126,24 @@ def args_to_action_goal_instance(action, inst, args):
 
 
 class SendGoal:
+    """Helper class to send action goals."""
+
+    def __init__(self, sleep_time=0.001):
+        self.sleep_time = sleep_time
+        self.goal_handle = None
+
     def get_result_cb(self, future):
         self.result = future.result()
 
     def goal_response_cb(self, future):
-        goal_handle = future.result()
-        if not goal_handle.accepted:
+        self.goal_handle = future.result()
+        if not self.goal_handle.accepted:
             raise Exception("Action goal was rejected")
-        result_future = goal_handle.get_result_async()
+        result_future = self.goal_handle.get_result_async()
         result_future.add_done_callback(self.get_result_cb)
 
-    def send_goal(
-        self, node_handle, action, action_type, args=None, feedback_cb=None, sleep_time=0.001
-    ):
-        # Given the action nam and type, fetch a request instance
+    def send_goal(self, node_handle, action, action_type, args=None, feedback_cb=None):
+        # Given the action name and type, fetch a request instance
         action_name = expand_topic_name(action, node_handle.get_name(), node_handle.get_namespace())
         action_class = get_action_class(action_type)
         inst = get_action_goal_instance(action_type)
@@ -150,7 +157,7 @@ def send_goal(
         send_goal_future.add_done_callback(self.goal_response_cb)
 
         while self.result is None:
-            time.sleep(sleep_time)
+            time.sleep(self.sleep_time)
 
         client.destroy()
         if self.result is not None:
@@ -160,3 +167,11 @@ def send_goal(
             raise Exception(self.result)
 
         return json_response
+
+    def cancel_goal(self):
+        if self.goal_handle is None:
+            return
+
+        cancel_goal_future = self.goal_handle.cancel_goal_async()
+        while not cancel_goal_future.done():
+            time.sleep(self.sleep_time)

rosbridge_library/test/capabilities/test_action_capabilities.py

@@ -198,10 +198,76 @@ def test_execute_advertised_action(self):
         self.assertEqual(self.received_message["op"], "action_result")
         self.assertEqual(self.received_message["values"]["result"]["sequence"], [1, 1, 2, 3, 5])
 
+    def test_cancel_advertised_action(self):
+        # Advertise the action
+        action_path = "/fibonacci_3"
+        advertise_msg = loads(
+            dumps(
+                {
+                    "op": "advertise_action",
+                    "type": "example_interfaces/Fibonacci",
+                    "action": action_path,
+                }
+            )
+        )
+        self.advertise.advertise_action(advertise_msg)
+        time.sleep(0.1)
+
+        # Send a goal to the advertised action using rosbridge
+        self.received_message = None
+        goal_msg = loads(
+            dumps(
+                {
+                    "op": "call_service",
+                    "id": "foo",
+                    "action": action_path,
+                    "action_type": "example_interfaces/Fibonacci",
+                    "args": {"order": 5},
+                }
+            )
+        )
+        Thread(target=self.send_goal.send_action_goal, args=(goal_msg,)).start()
+
+        loop_iterations = 0
+        while self.received_message is None:
+            time.sleep(0.5)
+            loop_iterations += 1
+            if loop_iterations > 3:
+                self.fail("Timed out waiting for action goal message.")
+
+        self.assertIsNotNone(self.received_message)
+        self.assertTrue("op" in self.received_message)
+        self.assertEqual(self.received_message["op"], "send_action_goal")
+        self.assertTrue("id" in self.received_message)
+
+        # Now cancel the goal
+        cancel_msg = loads(
+            dumps(
+                {
+                    "op": "cancel_action_goal",
+                    "action": action_path,
+                    "id": "foo",
+                }
+            )
+        )
+        self.received_message = None
+        self.send_goal.cancel_action_goal(cancel_msg)
+
+        loop_iterations = 0
+        while self.received_message is None:
+            time.sleep(0.5)
+            loop_iterations += 1
+            if loop_iterations > 3:
+                self.fail("Timed out waiting for action result message.")
+
+        self.assertIsNotNone(self.received_message)
+        self.assertEqual(self.received_message["op"], "action_result")
+        self.assertFalse(self.received_message["result"])
+
     @unittest.skip("Currently raises an exception not catchable by unittest, need to fix this")
     def test_unadvertise_action(self):
         # Advertise the action
-        action_path = "/fibonacci_3"
+        action_path = "/fibonacci_4"
         advertise_msg = loads(
             dumps(
                 {