diff --git a/ROSBRIDGE_PROTOCOL.md b/ROSBRIDGE_PROTOCOL.md index e58850d61..a937afe6b 100644 --- a/ROSBRIDGE_PROTOCOL.md +++ b/ROSBRIDGE_PROTOCOL.md @@ -560,15 +560,17 @@ A result for a ROS action. "id": , "action": , "values": , + "status": , "result": } ``` * **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. --- diff --git a/rosbridge_library/package.xml b/rosbridge_library/package.xml index 706a9fa6e..10c88807b 100644 --- a/rosbridge_library/package.xml +++ b/rosbridge_library/package.xml @@ -30,7 +30,7 @@ python3-bson rosbridge_test_msgs - actionlib_msgs + action_msgs ament_cmake_pytest builtin_interfaces diagnostic_msgs diff --git a/rosbridge_library/src/rosbridge_library/capabilities/action_result.py b/rosbridge_library/src/rosbridge_library/capabilities/action_result.py index 6dfc5aafe..fde481c0c 100644 --- a/rosbridge_library/src/rosbridge_library/capabilities/action_result.py +++ b/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) diff --git a/rosbridge_library/src/rosbridge_library/capabilities/advertise_action.py b/rosbridge_library/src/rosbridge_library/capabilities/advertise_action.py index 1478a3b24..2699414b1 100644 --- a/rosbridge_library/src/rosbridge_library/capabilities/advertise_action.py +++ b/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}") diff --git a/rosbridge_library/src/rosbridge_library/capabilities/send_action_goal.py b/rosbridge_library/src/rosbridge_library/capabilities/send_action_goal.py index ba39f50d5..089d84562 100644 --- a/rosbridge_library/src/rosbridge_library/capabilities/send_action_goal.py +++ b/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: diff --git a/rosbridge_library/src/rosbridge_library/internal/actions.py b/rosbridge_library/src/rosbridge_library/internal/actions.py index 0421c31e8..efc9211cc 100644 --- a/rosbridge_library/src/rosbridge_library/internal/actions.py +++ b/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 diff --git a/rosbridge_library/test/capabilities/test_action_capabilities.py b/rosbridge_library/test/capabilities/test_action_capabilities.py index a3aea51fb..61804f34e 100755 --- a/rosbridge_library/test/capabilities/test_action_capabilities.py +++ b/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.") diff --git a/rosbridge_library/test/capabilities/test_advertise.py b/rosbridge_library/test/capabilities/test_advertise.py index 4e172aba9..acff712e2 100755 --- a/rosbridge_library/test/capabilities/test_advertise.py +++ b/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", diff --git a/rosbridge_library/test/internal/publishers/test_multi_publisher.py b/rosbridge_library/test/internal/publishers/test_multi_publisher.py index 01b1f8295..51e6478d1 100755 --- a/rosbridge_library/test/internal/publishers/test_multi_publisher.py +++ b/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", diff --git a/rosbridge_library/test/internal/subscribers/test_multi_subscriber.py b/rosbridge_library/test/internal/subscribers/test_multi_subscriber.py index 76f084b5f..aa6a51233 100755 --- a/rosbridge_library/test/internal/subscribers/test_multi_subscriber.py +++ b/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", diff --git a/rosbridge_library/test/internal/test_message_conversion.py b/rosbridge_library/test/internal/test_message_conversion.py index f837e6275..e955535f5 100755 --- a/rosbridge_library/test/internal/test_message_conversion.py +++ b/rosbridge_library/test/internal/test_message_conversion.py @@ -228,7 +228,7 @@ def test_header_msg(self): def test_assorted_msgs(self): assortedmsgs = [ "geometry_msgs/Pose", - "actionlib_msgs/GoalStatus", + "action_msgs/GoalStatus", "geometry_msgs/WrenchStamped", "stereo_msgs/DisparityImage", "nav_msgs/OccupancyGrid", diff --git a/rosbridge_library/test/internal/test_ros_loader.py b/rosbridge_library/test/internal/test_ros_loader.py index 69d7677ca..06f2e3ba8 100755 --- a/rosbridge_library/test/internal/test_ros_loader.py +++ b/rosbridge_library/test/internal/test_ros_loader.py @@ -130,7 +130,7 @@ def test_msg_cache(self): def test_assorted_msg_names(self): assortedmsgs = [ "geometry_msgs/Pose", - "actionlib_msgs/GoalStatus", + "action_msgs/GoalStatus", "geometry_msgs/WrenchStamped", "stereo_msgs/DisparityImage", "nav_msgs/OccupancyGrid", diff --git a/rosbridge_server/test/websocket/advertise_action.test.py b/rosbridge_server/test/websocket/advertise_action.test.py index 00a16a83f..5021aa635 100644 --- a/rosbridge_server/test/websocket/advertise_action.test.py +++ b/rosbridge_server/test/websocket/advertise_action.test.py @@ -3,6 +3,7 @@ import sys import unittest +from action_msgs.msg import GoalStatus from example_interfaces.action import Fibonacci from rclpy.action import ActionClient from rclpy.node import Node @@ -68,6 +69,7 @@ async def test_two_concurrent_calls(self, node: Node, make_client): "op": "action_result", "action": "/test_fibonacci_action", "values": {"sequence": [0, 1, 1, 2]}, + "status": GoalStatus.STATUS_SUCCEEDED, "id": requests[0]["id"], "result": True, } @@ -82,6 +84,7 @@ async def test_two_concurrent_calls(self, node: Node, make_client): "op": "action_result", "action": "/test_fibonacci_action", "values": {"sequence": [0, 1, 1, 2, 3, 5]}, + "status": GoalStatus.STATUS_SUCCEEDED, "id": requests[1]["id"], "result": True, } diff --git a/rosbridge_server/test/websocket/advertise_action_feedback.test.py b/rosbridge_server/test/websocket/advertise_action_feedback.test.py index 2c1880853..69b2c6864 100644 --- a/rosbridge_server/test/websocket/advertise_action_feedback.test.py +++ b/rosbridge_server/test/websocket/advertise_action_feedback.test.py @@ -3,6 +3,7 @@ import sys import unittest +from action_msgs.msg import GoalStatus from example_interfaces.action import Fibonacci from rclpy.action import ActionClient from rclpy.node import Node @@ -74,6 +75,7 @@ async def test_feedback(self, node: Node, make_client): "op": "action_result", "action": "/test_fibonacci_action", "values": {"sequence": [0, 1, 1, 2, 3, 5]}, + "status": GoalStatus.STATUS_SUCCEEDED, "id": requests[0]["id"], "result": True, } diff --git a/rosbridge_server/test/websocket/send_action_goal.test.py b/rosbridge_server/test/websocket/send_action_goal.test.py index 8a6dace0b..1db33ab53 100644 --- a/rosbridge_server/test/websocket/send_action_goal.test.py +++ b/rosbridge_server/test/websocket/send_action_goal.test.py @@ -4,8 +4,9 @@ import time import unittest +from action_msgs.msg import GoalStatus from example_interfaces.action import Fibonacci -from rclpy.action import ActionServer +from rclpy.action import ActionServer, CancelResponse from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node from twisted.python import log @@ -21,19 +22,24 @@ class TestSendActionGoal(unittest.TestCase): + def cancel_callback(self, _): + return CancelResponse.ACCEPT + def execute_callback(self, goal): feedback_msg = Fibonacci.Feedback() feedback_msg.sequence = [0, 1] for i in range(1, goal.request.order): + if goal.is_cancel_requested: + goal.canceled() + return Fibonacci.Result() + feedback_msg.sequence.append(feedback_msg.sequence[i] + feedback_msg.sequence[i - 1]) goal.publish_feedback(feedback_msg) time.sleep(0.5) goal.succeed() - result = Fibonacci.Result() - result.sequence = feedback_msg.sequence - return result + return Fibonacci.Result(sequence=feedback_msg.sequence) @websocket_test async def test_one_call(self, node: Node, make_client): @@ -41,7 +47,8 @@ async def test_one_call(self, node: Node, make_client): node, Fibonacci, "/test_fibonacci_action", - self.execute_callback, + execute_callback=self.execute_callback, + cancel_callback=self.cancel_callback, callback_group=ReentrantCallbackGroup(), ) @@ -71,7 +78,8 @@ async def test_one_call(self, node: Node, make_client): self.assertEqual(responses[-1]["op"], "action_result") self.assertEqual(responses[-1]["action"], "/test_fibonacci_action") - self.assertEqual(responses[-1]["values"]["result"]["sequence"], expected_result) + self.assertEqual(responses[-1]["values"]["sequence"], expected_result) + self.assertEqual(responses[-1]["status"], GoalStatus.STATUS_SUCCEEDED) self.assertEqual(responses[-1]["result"], True) action_server.destroy() diff --git a/rosbridge_test_msgs/package.xml b/rosbridge_test_msgs/package.xml index bf5cf1ae7..7c8232bde 100644 --- a/rosbridge_test_msgs/package.xml +++ b/rosbridge_test_msgs/package.xml @@ -29,7 +29,7 @@ geometry_msgs rosidl_default_runtime - actionlib_msgs + action_msgs ament_cmake_pytest builtin_interfaces diagnostic_msgs