Fix action cancellation by passing status from JSON (#953)

Author: sea-bass

ROSBRIDGE_PROTOCOL.md

@@ -560,15 +560,17 @@ A result for a ROS action.
   "id": <string>,
   "action": <string>,
   "values": <json>,
+  "status": <int>,
   "result": <boolean>
 }
 ```
 
  * **action** – the name of the action that was executed
+ * **id** – if an ID was provided to the action goal, then the action result will contain the ID
  * **values** – the result values. If the service had no return values, then
     this field can be omitted (and will be by the rosbridge server)
- * **id** – if an ID was provided to the action goal, then the action result will contain the ID
- * **result** - return value of the action. true means success, false failure.
+ * **status** - return status of the action. This matches the enumeration in the [`action_msgs/msg/GoalStatus`](https://docs.ros2.org/latest/api/action_msgs/msg/GoalStatus.html) ROS message.
+ * **result** - return value of action. True means success, false failure.
 
 ---
 

rosbridge_library/package.xml

@@ -30,7 +30,7 @@
   <exec_depend>python3-bson</exec_depend>
 
   <test_depend>rosbridge_test_msgs</test_depend>
-  <test_depend>actionlib_msgs</test_depend>
+  <test_depend>action_msgs</test_depend>
   <test_depend>ament_cmake_pytest</test_depend>
   <test_depend>builtin_interfaces</test_depend>
   <test_depend>diagnostic_msgs</test_depend>

rosbridge_library/src/rosbridge_library/capabilities/action_result.py

@@ -41,6 +41,7 @@ class ActionResult(Capability):
         (True, "action", str),
         (False, "id", str),
         (False, "values", dict),
+        (True, "status", int),
         (True, "result", bool),
     ]
 
@@ -63,11 +64,12 @@ def action_result(self, message: dict) -> None:
             if message["result"]:
                 # parse the message
                 values = message["values"]
+                status = message["status"]
                 # create a message instance
                 result = ros_loader.get_action_result_instance(action_handler.action_type)
                 message_conversion.populate_instance(values, result)
-                # pass along the result
-                action_handler.handle_result(goal_id, result)
+                # pass along the result and status
+                action_handler.handle_result(goal_id, result, status)
             else:
                 # Abort the goal
                 action_handler.handle_abort(goal_id)

rosbridge_library/src/rosbridge_library/capabilities/advertise_action.py

@@ -34,6 +34,7 @@
 from typing import Any
 
 import rclpy
+from action_msgs.msg import GoalStatus
 from rclpy.action import ActionServer
 from rclpy.action.server import CancelResponse, ServerGoalHandle
 from rclpy.callback_groups import ReentrantCallbackGroup
@@ -52,6 +53,7 @@ def __init__(
     ) -> None:
         self.goal_futures = {}
         self.goal_handles = {}
+        self.goal_statuses = {}
 
         self.action_name = action_name
         self.action_type = action_type
@@ -84,7 +86,13 @@ def done_callback(fut: rclpy.task.Future) -> None:
                 # Send an empty result to avoid stack traces
                 fut.set_result(get_action_class(self.action_type).Result())
             else:
-                goal.succeed()
+                status = self.goal_statuses[goal_id]
+                if status == GoalStatus.STATUS_SUCCEEDED:
+                    goal.succeed()
+                elif status == GoalStatus.STATUS_CANCELED:
+                    goal.canceled()
+                else:
+                    goal.abort()
 
         future = rclpy.task.Future()
         future.add_done_callback(done_callback)
@@ -113,7 +121,6 @@ def cancel_callback(self, cancel_request: ServerGoalHandle) -> CancelResponse:
         for goal_id, goal_handle in self.goal_handles.items():
             if cancel_request.goal_id == goal_handle.goal_id:
                 self.protocol.log("warning", f"Canceling action {goal_id}")
-                self.goal_futures[goal_id].cancel()
                 cancel_message = {
                     "op": "cancel_action_goal",
                     "id": goal_id,
@@ -131,11 +138,12 @@ def handle_feedback(self, goal_id: str, feedback: Any) -> None:
         else:
             self.protocol.log("warning", f"Received action feedback for unrecognized id: {goal_id}")
 
-    def handle_result(self, goal_id: str, result: Any) -> None:
+    def handle_result(self, goal_id: str, result: dict, status: int) -> None:
         """
         Called by the ActionResult capability to handle a successful action result from the external client.
         """
         if goal_id in self.goal_futures:
+            self.goal_statuses[goal_id] = status
             self.goal_futures[goal_id].set_result(result)
         else:
             self.protocol.log("warning", f"Received action result for unrecognized id: {goal_id}")

rosbridge_library/src/rosbridge_library/capabilities/send_action_goal.py

@@ -34,6 +34,7 @@
 from functools import partial
 from threading import Thread
 
+from action_msgs.msg import GoalStatus
 from rosbridge_library.capability import Capability
 from rosbridge_library.internal.actions import ActionClientHandler
 from rosbridge_library.internal.message_conversion import extract_values
@@ -75,7 +76,11 @@ def __init__(self, protocol: Protocol) -> None:
             protocol.node_handle.get_logger().info("Sending action goals in existing thread")
             protocol.register_operation("send_action_goal", self.send_action_goal)
 
-        protocol.register_operation("cancel_action_goal", self.cancel_action_goal)
+        # Always register goal canceling in a new thread.
+        protocol.register_operation(
+            "cancel_action_goal",
+            lambda msg: Thread(target=self.cancel_action_goal, args=(msg,)).start(),
+        )
 
     def send_action_goal(self, message: dict) -> None:
         # Pull out the ID
@@ -154,7 +159,8 @@ def _success(
         outgoing_message = {
             "op": "action_result",
             "action": action,
-            "values": message,
+            "values": message["result"],
+            "status": message["status"],
             "result": True,
         }
         if cid is not None:
@@ -169,6 +175,7 @@ def _failure(self, cid: str, action: str, exc: Exception) -> None:
             "op": "action_result",
             "action": action,
             "values": str(exc),
+            "status": GoalStatus.STATUS_UNKNOWN,
             "result": False,
         }
         if cid is not None:

rosbridge_library/src/rosbridge_library/internal/actions.py

@@ -147,6 +147,9 @@ def goal_response_cb(self, future: Future) -> None:
         result_future = self.goal_handle.get_result_async()
         result_future.add_done_callback(self.get_result_cb)
 
+    def goal_cancel_cb(self, _: Future) -> None:
+        self.goal_canceled = True
+
     def send_goal(
         self,
         node_handle: Node,
@@ -169,7 +172,7 @@ def send_goal(
         send_goal_future = client.send_goal_async(inst, feedback_callback=feedback_cb)
         send_goal_future.add_done_callback(self.goal_response_cb)
 
-        while self.result is None and not self.goal_canceled:
+        while self.result is None:
             time.sleep(self.sleep_time)
 
         client.destroy()
@@ -186,6 +189,6 @@ def cancel_goal(self) -> None:
             return
 
         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():
             time.sleep(self.sleep_time)
-        self.goal_canceled = True

rosbridge_library/test/capabilities/test_action_capabilities.py

@@ -5,6 +5,7 @@
 from threading import Thread
 
 import rclpy
+from action_msgs.msg import GoalStatus
 from example_interfaces.action._fibonacci import Fibonacci_FeedbackMessage
 from rclpy.executors import SingleThreadedExecutor
 from rclpy.node import Node
@@ -97,7 +98,7 @@ def test_advertise_action(self):
         self.advertise.advertise_action(advertise_msg)
 
     @unittest.skip(
-        reason="Currently fails in Iron/Rolling due to https://github.com/ros2/rclpy/issues/1195, need to fix this"
+        reason="Currently fails in Iron due to https://github.com/ros2/rclpy/issues/1195. Unskip when Iron is EOL in Nov 2024."
     )
     def test_execute_advertised_action(self):
         # Advertise the action
@@ -133,7 +134,7 @@ def test_execute_advertised_action(self):
         while self.received_message is None:
             time.sleep(0.5)
             loop_iterations += 1
-            if loop_iterations > 3:
+            if loop_iterations > 5:
                 self.fail("Timed out waiting for action goal message.")
 
         self.assertIsNotNone(self.received_message)
@@ -170,7 +171,7 @@ def test_execute_advertised_action(self):
         while self.latest_feedback is None:
             time.sleep(0.5)
             loop_iterations += 1
-            if loop_iterations > 3:
+            if loop_iterations > 5:
                 self.fail("Timed out waiting for action feedback message.")
 
         self.assertIsNotNone(self.latest_feedback)
@@ -184,6 +185,7 @@ def test_execute_advertised_action(self):
                     "action": action_path,
                     "id": self.received_message["id"],
                     "values": {"sequence": [0, 1, 1, 2, 3, 5]},
+                    "status": GoalStatus.STATUS_SUCCEEDED,
                     "result": True,
                 }
             )
@@ -195,15 +197,16 @@ def test_execute_advertised_action(self):
         while self.received_message is None:
             time.sleep(0.5)
             loop_iterations += 1
-            if loop_iterations > 3:
+            if loop_iterations > 5:
                 self.fail("Timed out waiting for action result message.")
 
         self.assertIsNotNone(self.received_message)
         self.assertEqual(self.received_message["op"], "action_result")
-        self.assertEqual(self.received_message["values"]["result"]["sequence"], [0, 1, 1, 2, 3, 5])
+        self.assertEqual(self.received_message["values"]["sequence"], [0, 1, 1, 2, 3, 5])
+        self.assertEqual(self.received_message["status"], GoalStatus.STATUS_SUCCEEDED)
 
     @unittest.skip(
-        reason="Currently fails in Iron/Rolling due to https://github.com/ros2/rclpy/issues/1195, need to fix this"
+        reason="Currently fails in due to https://github.com/ros2/rclpy/issues/1195, need to fix this"
     )
     def test_cancel_advertised_action(self):
         # Advertise the action
@@ -239,7 +242,7 @@ def test_cancel_advertised_action(self):
         while self.received_message is None:
             time.sleep(0.5)
             loop_iterations += 1
-            if loop_iterations > 3:
+            if loop_iterations > 5:
                 self.fail("Timed out waiting for action goal message.")
 
         self.assertIsNotNone(self.received_message)
@@ -264,12 +267,39 @@ def test_cancel_advertised_action(self):
         while self.received_message is None:
             time.sleep(0.5)
             loop_iterations += 1
-            if loop_iterations > 3:
+            if loop_iterations > 5:
+                self.fail("Timed out waiting for action result message.")
+
+        self.assertIsNotNone(self.received_message)
+        self.assertEqual(self.received_message["op"], "cancel_action_goal")
+
+        # Now send the cancel result
+        result_msg = loads(
+            dumps(
+                {
+                    "op": "action_result",
+                    "action": action_path,
+                    "id": self.received_message["id"],
+                    "values": {"sequence": []},
+                    "status": GoalStatus.STATUS_CANCELED,
+                    "result": True,
+                }
+            )
+        )
+        self.received_message = None
+        self.result.action_result(result_msg)
+
+        loop_iterations = 0
+        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.")
 
         self.assertIsNotNone(self.received_message)
         self.assertEqual(self.received_message["op"], "action_result")
-        self.assertFalse(self.received_message["result"])
+        self.assertEqual(self.received_message["values"]["sequence"], [])
+        self.assertEqual(self.received_message["status"], GoalStatus.STATUS_CANCELED)
 
     @unittest.skip("Currently raises an exception not catchable by unittest, need to fix this")
     def test_unadvertise_action(self):
@@ -307,7 +337,7 @@ def test_unadvertise_action(self):
         while self.received_message is None:
             time.sleep(0.5)
             loop_iterations += 1
-            if loop_iterations > 3:
+            if loop_iterations > 5:
                 self.fail("Timed out waiting for action goal message.")
 
         self.assertIsNotNone(self.received_message)
@@ -327,7 +357,7 @@ def test_unadvertise_action(self):
             rclpy.spin_once(self.node, timeout_sec=0.1)
             time.sleep(0.5)
             loop_iterations += 1
-            if loop_iterations > 3:
+            if loop_iterations > 5:
                 self.fail("Timed out waiting for unadvertise action message.")
 
 

rosbridge_library/test/capabilities/test_advertise.py

@@ -137,7 +137,7 @@ def test_invalid_msg_classes(self):
     def test_valid_msg_classes(self):
         assortedmsgs = [
             "geometry_msgs/Pose",
-            "actionlib_msgs/GoalStatus",
+            "action_msgs/GoalStatus",
             "geometry_msgs/WrenchStamped",
             "stereo_msgs/DisparityImage",
             "nav_msgs/OccupancyGrid",

rosbridge_library/test/internal/publishers/test_multi_publisher.py

@@ -88,7 +88,7 @@ def test_verify_type(self):
         msg_type = "std_msgs/String"
         othertypes = [
             "geometry_msgs/Pose",
-            "actionlib_msgs/GoalStatus",
+            "action_msgs/GoalStatus",
             "geometry_msgs/WrenchStamped",
             "stereo_msgs/DisparityImage",
             "nav_msgs/OccupancyGrid",

rosbridge_library/test/internal/subscribers/test_multi_subscriber.py

@@ -58,7 +58,7 @@ def test_verify_type(self):
         msg_type = "std_msgs/String"
         othertypes = [
             "geometry_msgs/Pose",
-            "actionlib_msgs/GoalStatus",
+            "action_msgs/GoalStatus",
             "geometry_msgs/WrenchStamped",
             "stereo_msgs/DisparityImage",
             "nav_msgs/OccupancyGrid",