diff --git a/rclpy/rclpy/action/server.py b/rclpy/rclpy/action/server.py index 75eafdd38..cc48fcd4d 100644 --- a/rclpy/rclpy/action/server.py +++ b/rclpy/rclpy/action/server.py @@ -197,7 +197,11 @@ def _set_result(self, response: Optional[ResultT]) -> 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) + key = bytes(self.goal_id.uuid) + with self._action_server._goal_registry_lock: + result_future = self._action_server._result_futures.get(key) + if result_future is not None: + result_future.set_result(result_response) def execute( self, @@ -376,6 +380,14 @@ def __init__( # key: UUID in bytes, value: Future self._result_futures: Dict[bytes, Future[GetResultServiceResponse[ResultT]]] = {} + # Serializes mutations and reads of the (`_goal_handles`, `_result_futures`) + # pair so a reader never observes one dict updated without the other. Needed + # because `_execute_goal` runs as its own executor task (see `notify_execute`) + # and can race with the waitable's `_execute_expire_goals` under the + # MultiThreadedExecutor. Only ever held across synchronous sections — never + # across an `await` or a user-callback invocation. + self._goal_registry_lock = threading.Lock() + callback_group.add_entity(self) self._node.add_waitable(self) @@ -418,9 +430,12 @@ async def _execute_goal_request( 'Failed to accept new goal with ID {0}: {1}'.format(goal_uuid.uuid, e)) accepted = False else: - self._goal_handles[bytes(goal_uuid.uuid)] = goal_handle - self._result_futures[bytes(goal_uuid.uuid)] = Future() - self.add_future(self._result_futures[bytes(goal_uuid.uuid)]) + key = bytes(goal_uuid.uuid) + result_future: Future[GetResultServiceResponse[ResultT]] = Future() + with self._goal_registry_lock: + self._goal_handles[key] = goal_handle + self._result_futures[key] = result_future + self.add_future(result_future) # Send response response_msg = self._action_type.Impl.SendGoalService.Response() @@ -478,7 +493,15 @@ async def _execute_goal( result_response = self._action_type.Impl.GetResultService.Response() result_response.status = goal_handle.status result_response.result = execute_result - self._result_futures[bytes(goal_uuid)].set_result(result_response) + with self._goal_registry_lock: + result_future = self._result_futures.get(bytes(goal_uuid)) + if result_future is not None: + result_future.set_result(result_response) + else: + # Goal expired before the callback finished; an expected outcome + # under load with a short result_timeout, so log at debug level. + self._logger.debug( + 'Goal with ID {0} expired before its result could be set'.format(goal_uuid)) async def _execute_cancel_request( self, @@ -495,12 +518,13 @@ async def _execute_cancel_request( for goal_info in cancel_response.goals_canceling: goal_uuid = bytes(goal_info.goal_id.uuid) - if goal_uuid not in self._goal_handles: + with self._goal_registry_lock: + goal_handle = self._goal_handles.get(goal_uuid) + if goal_handle is None: # Possibly the user doesn't care to track the goal handle # Remove from response cancel_response.goals_canceling.remove(goal_info) continue - goal_handle = self._goal_handles[goal_uuid] response = await await_or_execute(self._cancel_callback, goal_handle) if CancelResponse.ACCEPT == response: @@ -536,9 +560,17 @@ async def _execute_get_result_request( self._logger.debug( 'Result request received for goal with ID: {0}'.format(goal_uuid)) + # Atomically check whether the goal is still tracked and grab its result + # future. `_execute_expire_goals` removes the goal handle and result future + # as a pair, so taking the lock here keeps the membership check and the + # future lookup consistent with each other. + key = bytes(goal_uuid) + with self._goal_registry_lock: + result_future = self._result_futures.get(key) if key in self._goal_handles else None + # If no goal with the requested ID exists, then return UNKNOWN status # or the goal with the requested ID has been already expired - if bytes(goal_uuid) not in self._goal_handles: + if result_future is None: self._logger.warning( 'Sending result response for unknown or expired goal ID: {0}'.format(goal_uuid)) result_response = self._action_type.Impl.GetResultService.Response() @@ -549,16 +581,21 @@ async def _execute_get_result_request( # There is an accepted goal matching the goal ID, register a callback to send the # response as soon as it's ready - self._result_futures[bytes(goal_uuid)].add_done_callback( + result_future.add_done_callback( functools.partial(self._send_result_response, request_header)) async def _execute_expire_goals(self, expired_goals: Tuple[GoalInfo, ...]) -> None: for goal in expired_goals: goal_uuid = bytes(goal.goal_id.uuid) - self._goal_handles[goal_uuid].destroy() - del self._goal_handles[goal_uuid] - self.remove_future(self._result_futures[goal_uuid]) - del self._result_futures[goal_uuid] + # Remove the goal handle and result future as a single unit so that + # concurrent readers never see one without the other. + with self._goal_registry_lock: + goal_handle = self._goal_handles.pop(goal_uuid, None) + result_future = self._result_futures.pop(goal_uuid, None) + if goal_handle is not None: + goal_handle.destroy() + if result_future is not None: + self.remove_future(result_future) def _send_result_response( self, @@ -800,11 +837,16 @@ def configure_introspection( def destroy(self) -> None: """Destroy the underlying action server handle.""" - for goal_handle in self._goal_handles.values(): + # Snapshot the goal registry under the lock so teardown does not race + # with `_execute_expire_goals` mutating the dicts. + with self._goal_registry_lock: + goal_handles = list(self._goal_handles.values()) + result_futures = list(self._result_futures.values()) + + for goal_handle in goal_handles: goal_handle.destroy() - """Remove the underlying result future.""" - for result_future in self._result_futures.values(): + for result_future in result_futures: self.remove_future(result_future) self._handle.destroy_when_not_in_use() diff --git a/rclpy/test/test_action_server.py b/rclpy/test/test_action_server.py index 6b9a24bad..5d4bda770 100644 --- a/rclpy/test/test_action_server.py +++ b/rclpy/test/test_action_server.py @@ -706,6 +706,94 @@ def test_expire_goals_multi(self) -> None: self.assertEqual(0, len(action_server._goal_handles)) action_server.destroy() + def test_get_result_request_handles_expired_future(self) -> None: + """ + Regression test for ros2/rclpy#1236. + + ``_execute_get_result_request`` must treat a goal whose result future + has been removed (e.g. by ``_execute_expire_goals`` racing on another + executor thread) as expired and respond with ``STATUS_UNKNOWN`` rather + than raising ``KeyError`` out of the executor. The race is simulated + deterministically here by removing the result future directly. + """ + action_server = ActionServer( + self.node, + Fibonacci, + 'fibonacci', + execute_callback=self.execute_goal_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, self.executor) + goal_handle = goal_future.result() + assert goal_handle + self.assertTrue(goal_handle.accepted) + + # Let the executor run `_execute_goal` so the goal's result future exists. + self.timed_spin(0.5) + + uuid_bytes = bytes(goal_uuid.uuid) + self.assertIn(uuid_bytes, action_server._goal_handles) + self.assertIn(uuid_bytes, action_server._result_futures) + + # Simulate `_execute_expire_goals` having removed the result future + # between the goal-handle check and the future access. + del action_server._result_futures[uuid_bytes] + + # Without the fix this raises KeyError out of `spin_once`; with the fix + # the server detects the missing future and replies STATUS_UNKNOWN. + get_result_future = self.mock_action_client.get_result(goal_uuid) + rclpy.spin_until_future_complete( + self.node, get_result_future, self.executor, timeout_sec=5) + result_response = get_result_future.result() + assert result_response + self.assertEqual(result_response.status, GoalStatus.STATUS_UNKNOWN) + action_server.destroy() + + def test_execute_goal_handles_expired_future(self) -> None: + """ + Regression test for ros2/rclpy#1667. + + If the result future is removed (e.g. by ``_execute_expire_goals`` on + another executor thread) while a goal's execute callback is running, + ``_execute_goal`` must not raise ``KeyError`` when it publishes the + result. + """ + action_server: Optional[ActionServer[Any, Any, Any, Any]] = None + + def execute_callback(goal_handle: ServerGoalHandle[Any, Fibonacci.Result, Any, Any] + ) -> Fibonacci.Result: + # Simulate the result future being expired mid-execution. + assert action_server is not None + del action_server._result_futures[bytes(goal_handle.goal_id.uuid)] + goal_handle.succeed() + return Fibonacci.Result() + + action_server = ActionServer( + self.node, + Fibonacci, + 'fibonacci', + execute_callback=execute_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, self.executor) + accept_response = goal_future.result() + assert accept_response + self.assertTrue(accept_response.accepted) + + # Drive the executor so `_execute_goal` runs to completion. Without the + # fix it raises KeyError out of `spin_once`; with the fix it logs a + # warning and the executor stays alive. + self.timed_spin(1.0) + action_server.destroy() + def test_feedback(self) -> None: def execute_with_feedback(goal_handle: ServerGoalHandle[Fibonacci.Goal, Fibonacci.Result,