diff --git a/rosbridge_library/src/rosbridge_library/capabilities/advertise_action.py b/rosbridge_library/src/rosbridge_library/capabilities/advertise_action.py index ef119bb4..0fc963ae 100644 --- a/rosbridge_library/src/rosbridge_library/capabilities/advertise_action.py +++ b/rosbridge_library/src/rosbridge_library/capabilities/advertise_action.py @@ -103,10 +103,6 @@ def done_callback(fut: Future[ROSActionResultT]) -> 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( - cast("ROSActionResultT", get_action_class(self.action_type).Result()) - ) else: if goal_id not in self.goal_statuses: goal.abort() @@ -138,7 +134,9 @@ def done_callback(fut: Future[ROSActionResultT]) -> None: try: result = await future - assert result is not None, "Action result cannot be None" + if result is None: + # Return empty result when cancelled/aborted + return cast("ROSActionResultT", get_action_class(self.action_type).Result()) return result finally: del self.goal_futures[goal_id] diff --git a/rosbridge_library/src/rosbridge_library/internal/actions.py b/rosbridge_library/src/rosbridge_library/internal/actions.py index ce3a133a..d45e7bd8 100644 --- a/rosbridge_library/src/rosbridge_library/internal/actions.py +++ b/rosbridge_library/src/rosbridge_library/internal/actions.py @@ -146,7 +146,7 @@ def args_to_action_goal_instance(inst: ROSMessage, args: list | dict[str, Any] | class SendGoal(Generic[ROSActionGoalT, ROSActionResultT, ROSActionFeedbackT]): """Helper class to send action goals.""" - result: GetResultServiceResponse[ROSActionResultT] | None = None + result: GetResultServiceResponse[ROSActionResultT] | Exception | None = None def __init__(self, server_timeout_time: float = 1.0, sleep_time: float = 0.001) -> None: self.server_timeout_time = server_timeout_time @@ -166,7 +166,8 @@ def goal_response_cb( assert self.goal_handle is not None if not self.goal_handle.accepted: msg = "Action goal was rejected" - raise Exception(msg) + self.result = Exception(msg) + return result_future: Future[GetResultServiceResponse[ROSActionResultT]] = ( self.goal_handle.get_result_async() ) @@ -195,7 +196,9 @@ def send_goal( client: ActionClient[ROSActionGoalT, ROSActionResultT, ROSActionFeedbackT] = ActionClient( node_handle, action_class, action_name ) - client.wait_for_server(timeout_sec=self.server_timeout_time) + if not client.wait_for_server(timeout_sec=self.server_timeout_time): + msg = "No action server available" + raise Exception(msg) send_goal_future = client.send_goal_async(inst, feedback_callback=feedback_cb) # type: ignore[arg-type] send_goal_future.add_done_callback(self.goal_response_cb) @@ -203,6 +206,10 @@ def send_goal( time.sleep(self.sleep_time) client.destroy() + + if isinstance(self.result, Exception): + raise self.result + if self.result is not None: # Turn the response into JSON and pass to the callback json_response = extract_values(self.result)