Lock the goal-handle / result-future registry in ActionServer

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()

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,