Fix action cancelling/aborting (#1013)

* Wait for goal handle before canceling action goal

* Abort action goal if action unadvertised

* Fix failing action unadvertise test

* Speed up action unit tests

* Fix mypy error

Author: bjsowa

rosbridge_library/src/rosbridge_library/capabilities/advertise_action.py

@@ -86,6 +86,10 @@ def done_callback(fut: Future) -> None:
                 # Send an empty result to avoid stack traces
                 fut.set_result(get_action_class(self.action_type).Result())
             else:
+                if goal_id not in self.goal_statuses:
+                    goal.abort()
+                    return
+
                 status = self.goal_statuses[goal_id]
                 if status == GoalStatus.STATUS_SUCCEEDED:
                     goal.succeed()

rosbridge_library/src/rosbridge_library/internal/actions.py

@@ -185,9 +185,15 @@ def send_goal(
         return json_response
 
     def cancel_goal(self) -> None:
-        if self.goal_handle is None:
+        while self.goal_handle is None and self.result is None:
+            time.sleep(self.sleep_time)
+
+        if self.result is not None:
+            # The action has already completed
             return
 
+        assert self.goal_handle is not None
+
         cancel_goal_future = self.goal_handle.cancel_goal_async()
         cancel_goal_future.add_done_callback(self.goal_cancel_cb)
         while not cancel_goal_future.done():

rosbridge_library/test/capabilities/test_action_capabilities.py

@@ -127,11 +127,10 @@ def test_execute_advertised_action(self):
         )
         Thread(target=self.send_goal.send_action_goal, args=(goal_msg,)).start()
 
-        loop_iterations = 0
+        start_time = time.monotonic()
         while self.received_message is None:
-            time.sleep(0.5)
-            loop_iterations += 1
-            if loop_iterations > 5:
+            time.sleep(0.1)
+            if time.monotonic() - start_time > 1.0:
                 self.fail("Timed out waiting for action goal message.")
 
         self.assertIsNotNone(self.received_message)
@@ -164,14 +163,20 @@ def test_execute_advertised_action(self):
             )
         )
         self.feedback.action_feedback(feedback_msg)
-        loop_iterations = 0
-        while self.latest_feedback is None:
-            time.sleep(0.5)
-            loop_iterations += 1
-            if loop_iterations > 5:
+
+        start_time = time.monotonic()
+        while self.received_message is None:
+            # self.executor.spin_once(timeout_sec=0.1)
+            time.sleep(0.1)
+            if time.monotonic() - start_time > 1.0:
                 self.fail("Timed out waiting for action feedback message.")
 
-        self.assertIsNotNone(self.latest_feedback)
+        start_time = time.monotonic()
+        while self.latest_feedback is None:
+            time.sleep(0.1)
+            if time.monotonic() - start_time > 1.0:
+                self.fail("Timed out waiting for action feedback callback.")
+
         self.assertEqual(list(self.latest_feedback.feedback.sequence), [0, 1, 1])
 
         # Now send the result
@@ -190,11 +195,10 @@ def test_execute_advertised_action(self):
         self.received_message = None
         self.result.action_result(result_msg)
 
-        loop_iterations = 0
+        start_time = time.monotonic()
         while self.received_message is None:
-            time.sleep(0.5)
-            loop_iterations += 1
-            if loop_iterations > 5:
+            time.sleep(0.1)
+            if time.monotonic() - start_time > 1.0:
                 self.fail("Timed out waiting for action result message.")
 
         self.assertIsNotNone(self.received_message)
@@ -232,11 +236,10 @@ def test_cancel_advertised_action(self):
         )
         Thread(target=self.send_goal.send_action_goal, args=(goal_msg,)).start()
 
-        loop_iterations = 0
+        start_time = time.monotonic()
         while self.received_message is None:
-            time.sleep(0.5)
-            loop_iterations += 1
-            if loop_iterations > 5:
+            time.sleep(0.1)
+            if time.monotonic() - start_time > 1.0:
                 self.fail("Timed out waiting for action goal message.")
 
         self.assertIsNotNone(self.received_message)
@@ -257,12 +260,11 @@ def test_cancel_advertised_action(self):
         self.received_message = None
         self.send_goal.cancel_action_goal(cancel_msg)
 
-        loop_iterations = 0
+        start_time = time.monotonic()
         while self.received_message is None:
-            time.sleep(0.5)
-            loop_iterations += 1
-            if loop_iterations > 5:
-                self.fail("Timed out waiting for action result message.")
+            time.sleep(0.1)
+            if time.monotonic() - start_time > 1.0:
+                self.fail("Timed out waiting for cancel action message.")
 
         self.assertIsNotNone(self.received_message)
         self.assertEqual(self.received_message["op"], "cancel_action_goal")
@@ -283,11 +285,10 @@ def test_cancel_advertised_action(self):
         self.received_message = None
         self.result.action_result(result_msg)
 
-        loop_iterations = 0
+        start_time = time.monotonic()
         while self.received_message is None:
-            time.sleep(0.5)
-            loop_iterations += 1
-            if loop_iterations > 5:
+            time.sleep(0.1)
+            if time.monotonic() - start_time > 1.0:
                 self.fail("Timed out waiting for action result message.")
 
         self.assertIsNotNone(self.received_message)
@@ -326,11 +327,10 @@ def test_unadvertise_action(self):
         )
         Thread(target=self.send_goal.send_action_goal, args=(goal_msg,)).start()
 
-        loop_iterations = 0
+        start_time = time.monotonic()
         while self.received_message is None:
-            time.sleep(0.5)
-            loop_iterations += 1
-            if loop_iterations > 5:
+            time.sleep(0.1)
+            if time.monotonic() - start_time > 1.0:
                 self.fail("Timed out waiting for action goal message.")
 
         self.assertIsNotNone(self.received_message)
@@ -343,12 +343,10 @@ def test_unadvertise_action(self):
         self.received_message = None
         self.unadvertise.unadvertise_action(unadvertise_msg)
 
-        loop_iterations = 0
+        start_time = time.monotonic()
         while self.received_message is None:
-            rclpy.spin_once(self.node, timeout_sec=0.1)
-            time.sleep(0.5)
-            loop_iterations += 1
-            if loop_iterations > 5:
+            time.sleep(0.1)
+            if time.monotonic() - start_time > 1.0:
                 self.fail("Timed out waiting for unadvertise action message.")