Adding part

ros-navigation · Jun 11, 2024 · dc91869 · dc91869
1 parent 44fb9fc
commit dc91869
Showing 1 changed file with 22 additions and 22 deletions.
diff --git a/nav2_docking/opennav_docking/test/test_docking_server.py b/nav2_docking/opennav_docking/test/test_docking_server.py
@@ -129,26 +129,26 @@ def publish(self):
 
     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)
+        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())
+        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')
         # Force the docking action to run a full recovery loop and then
         # make contact with the dock (based on pose of robot) before
         # we report that the robot is charging
-        # if msg.feedback.num_retries > 0 and \
-        #         msg.feedback.state == msg.feedback.WAIT_FOR_CHARGE:
-        #     self.is_charging = True
+        if msg.feedback.num_retries > 0 and \
+                msg.feedback.state == msg.feedback.WAIT_FOR_CHARGE:
+            self.is_charging = True
         print('action_feedback_callback OK')
 
     def nav_execute_callback(self, goal_handle):
@@ -200,22 +200,22 @@ def test_docking_server(self):
             'navigate_to_pose',
             self.nav_execute_callback)
 
-        # # Spin once so that TF is published
-        # rclpy.spin_once(self.node, timeout_sec=0.1)
+        # Spin once so that TF is published
+        rclpy.spin_once(self.node, timeout_sec=0.1)
 
-        # print('test_docking_server Preample OK')
+        print('test_docking_server Preample OK')
 
-        # # Test docking action
-        # self.action_result = []
-        # self.dock_action_client.wait_for_server(timeout_sec=5.0)
-        # goal = DockRobot.Goal()
-        # goal.use_dock_id = True
-        # 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)
+        # Test docking action
+        self.action_result = []
+        self.dock_action_client.wait_for_server(timeout_sec=5.0)
+        goal = DockRobot.Goal()
+        goal.use_dock_id = True
+        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)
 
-        # print('test_docking_server dock start OK')
+        print('test_docking_server dock start OK')
 
         # # Run for 2 seconds
         # for i in range(20):