Fix action cancellation by passing status from JSON

.pre-commit-config.yaml

@@ -1,7 +1,7 @@
 # See https://pre-commit.com for more information
 repos:
   - repo: https://github.com/pre-commit/pre-commit-hooks
-    rev: v4.0.1
+    rev: v4.6.0
     hooks:
       - id: check-added-large-files
       - id: check-case-conflict
@@ -16,27 +16,27 @@ repos:
       - id: trailing-whitespace
 
   - repo: https://github.com/pycqa/isort
-    rev: 5.12.0
+    rev: 5.13.2
     hooks:
       - id: isort
 
   - repo: https://github.com/psf/black
-    rev: 22.3.0
+    rev: 24.8.0
     hooks:
       - id: black
 
   - repo: https://github.com/PyCQA/flake8
-    rev: 3.9.2
+    rev: 7.1.1
     hooks:
       - id: flake8
 
   - repo: https://github.com/PyCQA/bandit
-    rev: 1.7.0
+    rev: 1.7.10
     hooks:
       - id: bandit
         args: ["--skip", "B101,B110,B311"]
 
   - repo: https://github.com/codespell-project/codespell
-    rev: v2.1.0
+    rev: v2.3.0
     hooks:
       - id: codespell

ROSBRIDGE_PROTOCOL.md

@@ -560,15 +560,15 @@ A result for a ROS action.
   "id": <string>,
   "action": <string>,
   "values": <json>,
-  "result": <boolean>
+  "result": <int>
 }
 ```
 
  * **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.
+ * **result** - 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.
 
 ---
 

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
@@ -77,14 +79,20 @@ async def execute_callback(self, goal: Any) -> Any:
         # generate a unique ID
         goal_id = f"action_goal:{self.action_name}:{self.next_id()}"
 
-        def done_callback(fut: rclpy.task.Future()) -> None:
+        def done_callback(fut: rclpy.task.Future) -> None:
             if fut.cancelled():
                 goal.abort()
                 self.protocol.log("info", f"Aborted goal {goal_id}")
                 # 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/advertise_service.py

@@ -31,7 +31,7 @@ def next_id(self):
 
     async def handle_request(self, req, res):
         # generate a unique ID
-        request_id = f"service_request:{self.service_name }:{self.next_id()}"
+        request_id = f"service_request:{self.service_name}:{self.next_id()}"
 
         future = rclpy.task.Future()
         self.request_futures[request_id] = future

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/capability.py

@@ -79,7 +79,7 @@ def basic_type_check(self, msg, types_info):
         """Performs basic typechecking on fields in msg.
 
         Keyword arguments:
-        msg        -- a message, deserialized into a dictoinary
+        msg        -- a message, deserialized into a dictionary
         types_info -- a list of tuples (mandatory, fieldname, fieldtype) where
                 mandatory - boolean, is the field mandatory
                 fieldname - the name of the field in the message

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()
@@ -182,10 +185,11 @@ def send_goal(
         return json_response
 
     def cancel_goal(self) -> None:
+        print("\n\n CANCELING GOAL \n\n")
         if self.goal_handle is 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/src/rosbridge_library/protocol.py

@@ -311,7 +311,6 @@ def serialize(self, msg, cid=None):
             return None
 
     def deserialize(self, msg, cid=None):
-
         """Turns the wire-level representation into a dictionary of values
 
         Default behaviour assumes JSON. Override to use a different container.

rosbridge_library/test/capabilities/test_action_capabilities.py

@@ -1,10 +1,12 @@
 #!/usr/bin/env python
+import os
 import time
 import unittest
 from json import dumps, loads
 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
@@ -96,9 +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"
-    )
+    @unittest.skipIf(os.environ.get("ROS_DISTRO") == "iron", "This test fails on Iron")
     def test_execute_advertised_action(self):
         # Advertise the action
         action_path = "/fibonacci_action_2"
@@ -184,6 +184,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,16 +196,15 @@ 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 > 30000:
                 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"
-    )
+    @unittest.skipIf(os.environ.get("ROS_DISTRO") == "iron", "This test fails on Iron")
     def test_cancel_advertised_action(self):
         # Advertise the action
         action_path = "/fibonacci_action_3"
@@ -267,9 +267,36 @@ def test_cancel_advertised_action(self):
             if loop_iterations > 3:
                 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 > 30000:
+                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):

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/experimental/complex_srv+tcp/test_non-ros_service_client_complex-srv.py

@@ -35,7 +35,7 @@ def request_service():
                 "position": {"y": 0.0, "x": 0.0, "z": 0.0},
                 "orientation": {"y": 0.0, "x": 0.0, "z": 0.0, "w": 0.0},
             }
-        }
+        },
         # "count" : request_byte_count           # count is the parameter for send_bytes as defined in srv-file (always put into args field!)
     }
     service_request = json.dumps(service_request_object)

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",
@@ -109,7 +109,7 @@ def test_publish(self):
         """Make sure that publishing works"""
         topic = "/test_publish"
         msg_type = "std_msgs/String"
-        msg = {"data": "why halo thar"}
+        msg = {"data": "why hello there"}
 
         received = {"msg": None}
 
@@ -133,7 +133,7 @@ def test_publish_twice(self):
         """Make sure that publishing works"""
         topic = "/test_publish_twice"
         msg_type = "std_msgs/String"
-        msg = {"data": "why halo thar"}
+        msg = {"data": "why hello there"}
 
         received = {"msg": None}
 

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",