Guard against unexpected action responses (#474)

Fixes ros2/demos#417

If multiple action servers are running with the same action name, then it is possible that both
servers will reply to requests from the action client. In the event that this happens, we avoid
crashing the code by ignoring the unexpected response and logging a warning for the user.
We have similar behavior for the action client implementation in rclcpp.

Signed-off-by: Jacob Perron <[email protected]>

rclpy/rclpy/action/client.py

@@ -148,6 +148,7 @@ def __init__(
         check_for_type_support(action_type)
         self._node = node
         self._action_type = action_type
+        self._action_name = action_name
         with node.handle as node_capsule:
             self._client_handle = _rclpy_action.rclpy_action_create_client(
                 node_capsule,
@@ -278,27 +279,45 @@ async def execute(self, taken_data):
         """
         if 'goal' in taken_data:
             sequence_number, goal_response = taken_data['goal']
-            goal_handle = ClientGoalHandle(
-                self,
-                self._sequence_number_to_goal_id[sequence_number],
-                goal_response)
-
-            if goal_handle.accepted:
-                goal_uuid = bytes(goal_handle.goal_id.uuid)
-                if goal_uuid in self._goal_handles:
-                    raise RuntimeError(
-                        'Two goals were accepted with the same ID ({})'.format(goal_handle))
-                self._goal_handles[goal_uuid] = weakref.ref(goal_handle)
-
-            self._pending_goal_requests[sequence_number].set_result(goal_handle)
+            if sequence_number in self._sequence_number_to_goal_id:
+                goal_handle = ClientGoalHandle(
+                    self,
+                    self._sequence_number_to_goal_id[sequence_number],
+                    goal_response)
+
+                if goal_handle.accepted:
+                    goal_uuid = bytes(goal_handle.goal_id.uuid)
+                    if goal_uuid in self._goal_handles:
+                        raise RuntimeError(
+                            'Two goals were accepted with the same ID ({})'.format(goal_handle))
+                    self._goal_handles[goal_uuid] = weakref.ref(goal_handle)
+
+                self._pending_goal_requests[sequence_number].set_result(goal_handle)
+            else:
+                self._node.get_logger().warning(
+                    'Ignoring unexpected goal response. There may be more than '
+                    f"one action server for the action '{self._action_name}'"
+                )
 
         if 'cancel' in taken_data:
             sequence_number, cancel_response = taken_data['cancel']
-            self._pending_cancel_requests[sequence_number].set_result(cancel_response)
+            if sequence_number in self._pending_cancel_requests:
+                self._pending_cancel_requests[sequence_number].set_result(cancel_response)
+            else:
+                self._node.get_logger().warning(
+                    'Ignoring unexpected cancel response. There may be more than '
+                    f"one action server for the action '{self._action_name}'"
+                )
 
         if 'result' in taken_data:
             sequence_number, result_response = taken_data['result']
-            self._pending_result_requests[sequence_number].set_result(result_response)
+            if sequence_number in self._pending_result_requests:
+                self._pending_result_requests[sequence_number].set_result(result_response)
+            else:
+                self._node.get_logger().warning(
+                    'Ignoring unexpected result response. There may be more than '
+                    f"one action server for the action '{self._action_name}'"
+                )
 
         if 'feedback' in taken_data:
             feedback_msg = taken_data['feedback']