More attempts to replicate cpp implementation

Author: jmblixt3

rclpy/rclpy/action/client.py

@@ -182,11 +182,10 @@ def __init__(
         self._node.add_waitable(self)
         self._logger = self._node.get_logger().get_child('action_client')
 
-        self._lock_goal_request = threading.Lock()
-        self._lock_cancel_request = threading.Lock()
-        self._lock_result_request = threading.Lock()
-        self._lock_feedback = threading.Lock()
-        self._lock_status = threading.Lock()
+        self._lock_goal_handles = threading.Lock()
+        self._lock_goal_requests = threading.Lock()
+        self._lock_cancel_requests = threading.Lock()
+        self._lock_result_requests = threading.Lock()
 
     def _generate_random_uuid(self):
         return UUID(uuid=list(uuid.uuid4().bytes))
@@ -216,21 +215,24 @@ def _remove_pending_request(self, future, pending_requests):
         return None
 
     def _remove_pending_goal_request(self, future):
-        seq = self._remove_pending_request(future, self._pending_goal_requests)
-        if seq in self._goal_sequence_number_to_goal_id:
-            del self._goal_sequence_number_to_goal_id[seq]
+        with self._lock_goal_requests:
+            seq = self._remove_pending_request(future, self._pending_goal_requests)
+            if seq in self._goal_sequence_number_to_goal_id:
+                del self._goal_sequence_number_to_goal_id[seq]
 
     def _remove_pending_cancel_request(self, future):
-        self._remove_pending_request(future, self._pending_cancel_requests)
+        with self._lock_cancel_requests:
+            self._remove_pending_request(future, self._pending_cancel_requests)
 
     def _remove_pending_result_request(self, future):
-        seq = self._remove_pending_request(future, self._pending_result_requests)
-        if seq in self._result_sequence_number_to_goal_id:
-            goal_uuid = bytes(self._result_sequence_number_to_goal_id[seq].uuid)
-            del self._result_sequence_number_to_goal_id[seq]
-            # remove feeback_callback if user is aware of result and it's been received
-            if goal_uuid in self._feedback_callbacks:
-                del self._feedback_callbacks[goal_uuid]
+        with self._lock_result_requests:
+            seq = self._remove_pending_request(future, self._pending_result_requests)
+            if seq in self._result_sequence_number_to_goal_id:
+                goal_uuid = bytes(self._result_sequence_number_to_goal_id[seq].uuid)
+                del self._result_sequence_number_to_goal_id[seq]
+                # remove feeback_callback if user is aware of result and it's been received
+                if goal_uuid in self._feedback_callbacks:
+                    del self._feedback_callbacks[goal_uuid]
 
     # Start Waitable API
     def is_ready(self, wait_set):
@@ -247,39 +249,39 @@ def take_data(self):
         """Take stuff from lower level so the wait set doesn't immediately wake again."""
         data = {}
         if self._is_goal_response_ready:
-            with self._lock_goal_request:
+            with self._node.handle:
                 taken_data = self._client_handle.take_goal_response(
                     self._action_type.Impl.SendGoalService.Response)
             # If take fails, then we get (None, None)
             if all(taken_data):
                 data['goal'] = taken_data
 
         if self._is_cancel_response_ready:
-            with self._lock_cancel_request:
+            with self._node.handle:
                 taken_data = self._client_handle.take_cancel_response(
                     self._action_type.Impl.CancelGoalService.Response)
             # If take fails, then we get (None, None)
             if all(taken_data):
                 data['cancel'] = taken_data
 
         if self._is_result_response_ready:
-            with self._lock_result_request:
+            with self._node.handle:
                 taken_data = self._client_handle.take_result_response(
                     self._action_type.Impl.GetResultService.Response)
             # If take fails, then we get (None, None)
             if all(taken_data):
                 data['result'] = taken_data
 
         if self._is_feedback_ready:
-            with self._lock_feedback:
+            with self._node.handle:
                 taken_data = self._client_handle.take_feedback(
                     self._action_type.Impl.FeedbackMessage)
             # If take fails, then we get None
             if taken_data is not None:
                 data['feedback'] = taken_data
 
         if self._is_status_ready:
-            with self._lock_status:
+            with self._node.handle:
                 taken_data = self._client_handle.take_status(
                     self._action_type.Impl.GoalStatusMessage)
             # If take fails, then we get None
@@ -296,46 +298,50 @@ async def execute(self, taken_data):
         call any user-defined callbacks (e.g. feedback).
         """
         if 'goal' in taken_data:
-            sequence_number, goal_response = taken_data['goal']
-            if sequence_number in self._goal_sequence_number_to_goal_id:
-                goal_handle = ClientGoalHandle(
-                    self,
-                    self._goal_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._logger.warning(
-                    'Ignoring unexpected goal response. There may be more than '
-                    f"one action server for the action '{self._action_name}'"
-                )
+            with self._lock_goal_requests:
+                sequence_number, goal_response = taken_data['goal']
+                if sequence_number in self._goal_sequence_number_to_goal_id:
+                    goal_handle = ClientGoalHandle(
+                        self,
+                        self._goal_sequence_number_to_goal_id[sequence_number],
+                        goal_response)
+
+                    if goal_handle.accepted:
+                        goal_uuid = bytes(goal_handle.goal_id.uuid)
+                        with self._lock_goal_handles:
+                            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._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']
-            if sequence_number in self._pending_cancel_requests:
-                self._pending_cancel_requests[sequence_number].set_result(cancel_response)
-            else:
-                self._logger.warning(
-                    'Ignoring unexpected cancel response. There may be more than '
-                    f"one action server for the action '{self._action_name}'"
-                )
+            with self._lock_cancel_requests:
+                sequence_number, cancel_response = taken_data['cancel']
+                if sequence_number in self._pending_cancel_requests:
+                    self._pending_cancel_requests[sequence_number].set_result(cancel_response)
+                else:
+                    self._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']
-            if sequence_number in self._pending_result_requests:
-                self._pending_result_requests[sequence_number].set_result(result_response)
-            else:
-                self._logger.warning(
-                    'Ignoring unexpected result response. There may be more than '
-                    f"one action server for the action '{self._action_name}'"
-                )
+            with self._lock_result_requests:
+                sequence_number, result_response = taken_data['result']
+                if sequence_number in self._pending_result_requests:
+                    self._pending_result_requests[sequence_number].set_result(result_response)
+                else:
+                    self._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']
@@ -349,19 +355,19 @@ async def execute(self, taken_data):
             for status_msg in taken_data['status'].status_list:
                 goal_uuid = bytes(status_msg.goal_info.goal_id.uuid)
                 status = status_msg.status
-
-                if goal_uuid in self._goal_handles:
-                    goal_handle = self._goal_handles[goal_uuid]()
-                    if goal_handle is not None:
-                        goal_handle._status = status
-                        # Remove "done" goals from the list
-                        if (GoalStatus.STATUS_SUCCEEDED == status or
-                                GoalStatus.STATUS_CANCELED == status or
-                                GoalStatus.STATUS_ABORTED == status):
+                with self._lock_goal_handles:
+                    if goal_uuid in self._goal_handles:
+                        goal_handle = self._goal_handles[goal_uuid]()
+                        if goal_handle is not None:
+                            goal_handle._status = status
+                            # Remove "done" goals from the list
+                            if (GoalStatus.STATUS_SUCCEEDED == status or
+                                    GoalStatus.STATUS_CANCELED == status or
+                                    GoalStatus.STATUS_ABORTED == status):
+                                del self._goal_handles[goal_uuid]
+                        else:
+                            # Weak reference is None
                             del self._goal_handles[goal_uuid]
-                    else:
-                        # Weak reference is None
-                        del self._goal_handles[goal_uuid]
 
     def get_num_entities(self):
         """Return number of each type of entity used in the wait set."""
@@ -370,7 +376,8 @@ def get_num_entities(self):
 
     def add_to_wait_set(self, wait_set):
         """Add entities to wait set."""
-        self._client_handle.add_to_waitset(wait_set)
+        with self._node.handle:
+            self._client_handle.add_to_waitset(wait_set)
 
     def __enter__(self):
         return self._client_handle.__enter__()
@@ -449,23 +456,22 @@ def send_goal_async(self, goal, feedback_callback=None, goal_uuid=None):
         request.goal_id = self._generate_random_uuid() if goal_uuid is None else goal_uuid
         request.goal = goal
         future = Future()
-        with self._lock_goal_request:
+        with self._lock_goal_requests:
             sequence_number = self._client_handle.send_goal_request(request)
             if sequence_number in self._pending_goal_requests:
                 raise RuntimeError(
                     'Sequence ({}) conflicts with pending goal request'.format(sequence_number))
             self._pending_goal_requests[sequence_number] = future
             self._goal_sequence_number_to_goal_id[sequence_number] = request.goal_id
+            future.add_done_callback(self._remove_pending_goal_request)
+            # Add future so executor is aware
+            self.add_future(future)
 
         if feedback_callback is not None:
             # TODO(jacobperron): Move conversion function to a general-use package
             goal_uuid = bytes(request.goal_id.uuid)
             self._feedback_callbacks[goal_uuid] = feedback_callback
 
-        future.add_done_callback(self._remove_pending_goal_request)
-        # Add future so executor is aware
-        self.add_future(future)
-
         return future
 
     def _cancel_goal(self, goal_handle):
@@ -508,16 +514,15 @@ def _cancel_goal_async(self, goal_handle):
         cancel_request = CancelGoal.Request()
         cancel_request.goal_info.goal_id = goal_handle.goal_id
         future = Future()
-        with self._lock_cancel_request:
+        with self._lock_cancel_requests:
             sequence_number = self._client_handle.send_cancel_request(cancel_request)
             if sequence_number in self._pending_cancel_requests:
                 raise RuntimeError(
                     'Sequence ({}) conflicts with pending cancel request'.format(sequence_number))
             self._pending_cancel_requests[sequence_number] = future
-
-        future.add_done_callback(self._remove_pending_cancel_request)
-        # Add future so executor is aware
-        self.add_future(future)
+            future.add_done_callback(self._remove_pending_cancel_request)
+            # Add future so executor is aware
+            self.add_future(future)
 
         return future
 
@@ -561,17 +566,18 @@ def _get_result_async(self, goal_handle):
         result_request = self._action_type.Impl.GetResultService.Request()
         result_request.goal_id = goal_handle.goal_id
         future = Future()
-        with self._lock_result_request:
+        with self._lock_result_requests:
             sequence_number = self._client_handle.send_result_request(result_request)
             if sequence_number in self._pending_result_requests:
                 raise RuntimeError(
                     'Sequence ({}) conflicts with pending result request'.format(sequence_number))
+            
             self._pending_result_requests[sequence_number] = future
             self._result_sequence_number_to_goal_id[sequence_number] = result_request.goal_id
 
-        future.add_done_callback(self._remove_pending_result_request)
-        # Add future so executor is aware
-        self.add_future(future)
+            future.add_done_callback(self._remove_pending_result_request)
+            # Add future so executor is aware
+            self.add_future(future)
 
         return future