diff --git a/rclpy/rclpy/action/server.py b/rclpy/rclpy/action/server.py index 6f877f416..22a314aae 100644 --- a/rclpy/rclpy/action/server.py +++ b/rclpy/rclpy/action/server.py @@ -120,12 +120,22 @@ def _update_state(self, event): if not self._goal_handle.is_active(): self._action_server.notify_goal_done() + def _set_result(self, response): + # 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 execute(self, execute_callback=None): # It's possible that there has been a request to cancel the goal prior to executing. # In this case we want to avoid the illegal state transition to EXECUTING # but still call the users execute callback to let them handle canceling the goal. if not self.is_cancel_requested: - self._update_state(_rclpy.GoalEvent.EXECUTE) + self.executing() self._action_server.notify_execute(self, execute_callback) def publish_feedback(self, feedback): @@ -146,14 +156,20 @@ 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) + self._set_result(response) - def abort(self): + def abort(self, response=None): self._update_state(_rclpy.GoalEvent.ABORT) + self._set_result(response) - def canceled(self): + def canceled(self, response=None): self._update_state(_rclpy.GoalEvent.CANCELED) + self._set_result(response) def destroy(self): with self._lock: @@ -186,7 +202,7 @@ def __init__( node, action_type, action_name, - execute_callback, + execute_callback=None, *, callback_group=None, goal_callback=default_goal_callback, @@ -233,7 +249,11 @@ 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()