add action feedback

ctrlgui/ctrlgui_node.py

@@ -76,11 +76,18 @@ class CtrlGuiNode(Node):
         self.file_logger.info(f"goal_msg: {goal_msg}")
 
         self.exe_bt_action_client_.wait_for_server(10.0)
-        send_goal_future = self.exe_bt_action_client_.send_goal_async(goal_msg)
+        send_goal_future = self.exe_bt_action_client_.send_goal_async(
+            goal_msg, feedback_callback=self.bt_action_feedback_callback)
         send_goal_future.add_done_callback(self.bt_action_response_callback)
 
         self.file_logger.info(f'Action {action_name} goal sent.')
 
+    def bt_action_feedback_callback(self, feedback_msg):
+        """Callback for BT action feedback."""
+        feedback = feedback_msg.feedback
+        self.file_logger.info(f'BT Action Feedback: {feedback}')
+        print(f'BT Action Feedback: {feedback}')
+
     def bt_action_response_callback(self, future):
         """Callback for BT action response."""
         goal_handle = future.result()
@@ -89,6 +96,16 @@ class CtrlGuiNode(Node):
             return
         self.file_logger.info('BT action goal accepted')
 
+        self._get_result_future = goal_handle.get_result_async()
+        self._get_result_future.add_done_callback(self.bt_action_result_callback)
+
+    def bt_action_result_callback(self, future):
+        """Callback for BT action result."""
+        result = future.result().result
+        status = future.result().status
+        self.file_logger.info(f'BT Action Result: {result}, Status: {status}')
+        print(f'BT Action Result: {result}, Status: {status}')
+
     def gripper_action_send_goal(self, id, loc, speed, torq, mode):
         """Callback to execute a Gripper action."""
         print("Start Gripper...")
@@ -122,10 +139,20 @@ class CtrlGuiNode(Node):
         self.file_logger.info('Gripper action goal accepted')
         print("Gripper action goal accepted")
 
-    def gripper_action_feedback_callback(self, future):
+        self._get_gripper_result_future = goal_handle.get_result_async()
+        self._get_gripper_result_future.add_done_callback(self.gripper_action_result_callback)
+
+    def gripper_action_result_callback(self, future):
+        result = future.result().result
+        status = future.result().status
+        self.file_logger.info(f'Gripper Action Result: {result}, Status: {status}')
+        print(f'Gripper Action Result: {result}, Status: {status}')
+
+    def gripper_action_feedback_callback(self, feedback_msg):
         print("gripper action feedback callback")
-        goal_feedback = future.feedback()
-        print("feedback loc: " + str(goal_feedback.loc))
+        goal_feedback = feedback_msg.feedback
+        self.file_logger.info(f'Gripper action feedback: {goal_feedback}')
+        print("feedback callback" + str(goal_feedback))
 
     def _setup_file_logging(self):
         """Setup logging to a file with timestamp in filename."""