From d2c14fe668e59e74d03b1341a1351c37d9848cc3 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 10 Jun 2024 19:26:25 -0700 Subject: [PATCH] try actions without done callbacks --- .../test/test_docking_server.py | 46 +++++++++++-------- 1 file changed, 27 insertions(+), 19 deletions(-) diff --git a/nav2_docking/opennav_docking/test/test_docking_server.py b/nav2_docking/opennav_docking/test/test_docking_server.py index 1fcfcb93032..3b67cbbdff7 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server.py +++ b/nav2_docking/opennav_docking/test/test_docking_server.py @@ -127,19 +127,19 @@ def publish(self): self.battery_state_pub.publish(b) print('publish OK') - def action_goal_callback(self, future): - print('action_goal_callback Start') - self.goal_handle = future.result() - assert self.goal_handle.accepted - result_future = self.goal_handle.get_result_async() - result_future.add_done_callback(self.action_result_callback) - print('action_goal_callback OK') - - def action_result_callback(self, future): - print('action_result_callback Start') - self.action_result.append(future.result()) - print(future.result()) - print('action_result_callback OK') + # def action_goal_callback(self, future): + # print('action_goal_callback Start') + # self.goal_handle = future.result() + # assert self.goal_handle.accepted + # result_future = self.goal_handle.get_result_async() + # result_future.add_done_callback(self.action_result_callback) + # print('action_goal_callback OK') + + # def action_result_callback(self, future): + # print('action_result_callback Start') + # self.action_result.append(future.result()) + # print(future.result()) + # print('action_result_callback OK') def action_feedback_callback(self, msg): print('action_feedback_callback Start') @@ -202,7 +202,6 @@ def test_docking_server(self): # Spin once so that TF is published rclpy.spin_once(self.node, timeout_sec=0.1) - rclpy.spin_once(self.node, timeout_sec=0.1) print('test_docking_server Preample OK') @@ -214,7 +213,10 @@ def test_docking_server(self): goal.dock_id = 'test_dock' future = self.dock_action_client.send_goal_async( goal, feedback_callback=self.action_feedback_callback) - future.add_done_callback(self.action_goal_callback) + rclpy.spin_until_future_complete(self.node, future) + self.goal_handle = future.result() + assert goal_handle.accepted + # future.add_done_callback(self.action_goal_callback) print('test_docking_server dock start OK') @@ -228,14 +230,20 @@ def test_docking_server(self): # Send another goal to preempt the first future = self.dock_action_client.send_goal_async( goal, feedback_callback=self.action_feedback_callback) - future.add_done_callback(self.action_goal_callback) + rclpy.spin_until_future_complete(self.node, future) + self.goal_handle = future.result() + assert self.goal_handle.accepted + result_future = self.goal_handle.get_result_async() + # future.add_done_callback(self.action_goal_callback) + rclpy.spin_until_future_complete(self.node, result_future) + self.action_result.append(result_future.result()) print('test_docking_server dock preempt OK') # Wait until we get the dock preemption - while len(self.action_result) < 1: - rclpy.spin_once(self.node, timeout_sec=0.1) - time.sleep(0.1) + # while len(self.action_result) == 0: + # rclpy.spin_once(self.node, timeout_sec=0.1) + # time.sleep(0.1) print('test_docking_server dock prempt spin OK')