diff --git a/ctrlgui/ctrlgui_node.py b/ctrlgui/ctrlgui_node.py index ad5d828..2637244 100644 --- a/ctrlgui/ctrlgui_node.py +++ b/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."""