From 523c403c60c12dc304564f2e681bf1d6dd637b9d Mon Sep 17 00:00:00 2001
From: Jacob Perron <jacob@openrobotics.org>
Date: Thu, 24 Jan 2019 14:16:35 -0800
Subject: [PATCH] Implement synchronous action client methods

---
 rclpy/rclpy/action_client.py | 61 +++++++++++++++++++++++++++++++-----
 1 file changed, 54 insertions(+), 7 deletions(-)

diff --git a/rclpy/rclpy/action_client.py b/rclpy/rclpy/action_client.py
index 6fe35c688..fa08467a1 100644
--- a/rclpy/rclpy/action_client.py
+++ b/rclpy/rclpy/action_client.py
@@ -12,8 +12,8 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-# import threading
 import time
+import threading
 import uuid
 
 from action_msgs.msg import GoalStatus
@@ -273,15 +273,36 @@ def send_goal(self, goal, **kwargs):
 
         See :meth:`send_goal_async` for more info about keyword arguments.
 
+        Unlike :meth:`send_goal_async`, this method returns the final result of the
+        action (not a goal handle).
+
         :param goal: The goal request.
         :type goal: action_type.Goal
         :return: The result response.
         :rtype: action_type.Result
         """
-        future = self.send_goal_async(goal, kwargs)
-        while self.node.context.ok() and not future.done():
-            time.sleep(0.1)
-        return future.result()
+        event = threading.Event()
+
+        def unblock(future):
+            nonlocal event
+            event.set()
+
+        print("sending goal {} with args {}".format(goal, kwargs))
+        send_goal_future = self.send_goal_async(goal, kwargs)
+        send_goal_future.add_done_callback(unblock)
+
+        print('waiting for goal reponse...')
+        event.wait()
+        print('goal reponse received')
+        if send_goal_future.exception() is not None:
+            raise send_goal_future.exception()
+
+        goal_handle = send_goal_future.result()
+
+        print('waiting for get result')
+        result = self.get_result(goal_handle)
+
+        return result
 
     def send_goal_async(self, goal, feedback_callback=None, goal_uuid=None):
         """
@@ -301,6 +322,7 @@ def send_goal_async(self, goal, feedback_callback=None, goal_uuid=None):
             has been accepted or rejected.
         :rtype: :class:`rclpy.task.Future` instance
         """
+        print("sending goal")
         goal.action_goal_id = self._generate_random_uuid() if goal_uuid is None else goal_uuid
         sequence_number = _rclpy_action.rclpy_action_send_goal_request(self.client_handle, goal)
         if sequence_number in self._pending_goal_requests:
@@ -316,6 +338,7 @@ def send_goal_async(self, goal, feedback_callback=None, goal_uuid=None):
         self._pending_goal_requests[sequence_number] = future
         self._sequence_number_to_goal_id[sequence_number] = goal.action_goal_id
         future.add_done_callback(self._remove_pending_goal_request)
+        print("goal sent")
 
         return future
 
@@ -329,7 +352,19 @@ def cancel_goal(self, goal_handle):
         :type goal_handle: :class:`ClientGoalHandle`
         :return: The cancel response.
         """
-        pass
+        event = threading.Event()
+
+        def unblock(future):
+            nonlocal event
+            event.set()
+
+        future = self.cancel_goal_async(goal_handle)
+        future.add_done_callback(unblock)
+
+        event.wait()
+        if future.exception() is not None:
+            raise future.exception()
+        return future.result()
 
     def cancel_goal_async(self, goal_handle):
         """
@@ -369,7 +404,19 @@ def get_result(self, goal_handle):
         :type goal_handle: :class:`ClientGoalHandle`
         :return: The result response.
         """
-        pass
+        event = threading.Event()
+
+        def unblock(future):
+            nonlocal event
+            event.set()
+
+        future = self.get_result_async(goal_handle)
+        future.add_done_callback(unblock)
+
+        event.wait()
+        if future.exception() is not None:
+            raise future.exception()
+        return future.result()
 
     def get_result_async(self, goal_handle):
         """