diff --git a/rclpy/rclpy/action/server.py b/rclpy/rclpy/action/server.py index 64a2d0320..325c57ba6 100644 --- a/rclpy/rclpy/action/server.py +++ b/rclpy/rclpy/action/server.py @@ -146,9 +146,21 @@ def publish_feedback(self, feedback): # Publish self._action_server._handle.publish_feedback(feedback_message) - def succeed(self): + def executing(self): + self._update_state(_rclpy.GoalEvent.EXECUTE) + + def succeed(self, response=None): self._update_state(_rclpy.GoalEvent.SUCCEED) + # Set result + result_response = self._action_server._action_type.Impl.GetResultService.Response() + result_response.status = self.status + if response is not None: + result_response.result = response + else: + result_response.result = self._action_server._action_type.Result() + self._action_server._result_futures[bytes(self.goal_id.uuid)].set_result(result_response) + def abort(self): self._update_state(_rclpy.GoalEvent.ABORT) @@ -186,7 +198,7 @@ def __init__( node, action_type, action_name, - execute_callback, + execute_callback=None, *, callback_group=None, goal_callback=default_goal_callback, @@ -233,7 +245,10 @@ def __init__( self.register_handle_accepted_callback(handle_accepted_callback) self.register_goal_callback(goal_callback) self.register_cancel_callback(cancel_callback) - self.register_execute_callback(execute_callback) + if execute_callback: + self.register_execute_callback(execute_callback) + elif handle_accepted_callback is default_handle_accepted_callback: + self._logger.warning("Not handling nor executing the goal, this server will do nothing") # Import the typesupport for the action module if not already done check_for_type_support(action_type) diff --git a/rclpy/test/test_action_server.py b/rclpy/test/test_action_server.py index fa2af1e74..977b134eb 100644 --- a/rclpy/test/test_action_server.py +++ b/rclpy/test/test_action_server.py @@ -21,6 +21,7 @@ import rclpy from rclpy.action import ActionServer, CancelResponse, GoalResponse +from rclpy.action.server import ServerGoalHandle from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor @@ -419,7 +420,7 @@ def execute_callback(gh): # Execute the goal server_goal_handle.execute() - # Get the result and exepect it to have canceled status + # Get the result and expect it to have canceled status get_result_future = self.mock_action_client.get_result(goal_uuid) rclpy.spin_until_future_complete(self.node, get_result_future, self.executor) result = get_result_future.result() @@ -684,6 +685,53 @@ def execute_with_feedback(goal_handle): finally: action_server.destroy() + def test_without_execute_cb(self): + # Just like test_execute_succeed, but without an execute callback + # Goal handle is stored and succeeded from outside the execute callback + stored_goal_handle = None + + def handle_accepted_callback(goal_handle: ServerGoalHandle): + goal_handle.executing() + nonlocal stored_goal_handle + stored_goal_handle = goal_handle + + executor = MultiThreadedExecutor(context=self.context) + + action_server = ActionServer( + self.node, + Fibonacci, + 'fibonacci', + handle_accepted_callback=handle_accepted_callback, + ) + + goal_uuid = UUID(uuid=list(uuid.uuid4().bytes)) + goal_msg = Fibonacci.Impl.SendGoalService.Request() + goal_msg.goal_id = goal_uuid + goal_future = self.mock_action_client.send_goal(goal_msg) + rclpy.spin_until_future_complete(self.node, goal_future, executor) + goal_handle = goal_future.result() + self.assertTrue(goal_handle.accepted) + + get_result_future = self.mock_action_client.get_result(goal_uuid) + self.assertFalse(get_result_future.done()) + + result = Fibonacci.Result() + result.sequence.extend([1, 1, 2, 3, 5]) + stored_goal_handle.succeed(result) + + # Handle all callbacks + for _ in range(5): + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.01) + if get_result_future.done(): + break + self.assertTrue(get_result_future.done()) + + result_response = get_result_future.result() + + self.assertEqual(result_response.status, GoalStatus.STATUS_SUCCEEDED) + self.assertEqual(result_response.result.sequence.tolist(), [1, 1, 2, 3, 5]) + action_server.destroy() + if __name__ == '__main__': unittest.main()