diff --git a/ctrlgui/ctrlgui_node.py b/ctrlgui/ctrlgui_node.py index 96fc565..a0aa299 100644 --- a/ctrlgui/ctrlgui_node.py +++ b/ctrlgui/ctrlgui_node.py @@ -23,6 +23,8 @@ from interfaces.action import ExecuteBtAction from interfaces.msg import SkillCall from interfaces.action import Arm from interfaces.srv import BtRebuild +from interfaces.msg import ArmError +from interfaces.srv import ClearArmError from interfaces.action import GripperCmd import time @@ -60,6 +62,15 @@ class CtrlGuiNode(Node): self.gripper0_action_client_ = ActionClient(self, GripperCmd, '/gripper_cmd0') self.gripper1_action_client_ = ActionClient(self, GripperCmd, '/gripper_cmd1') + # arm error subscription + self.last_arm_error = {0: None, 1: None} + self._sub_arm_error = self.create_subscription( + ArmError, '/arm_errors', self._on_arm_error, 10 + ) + + # clear arm error service client + self.clear_arm_err_client_ = self.create_client(ClearArmError, 'clear_arm_error') + self.file_logger.info('CtrlGuiNode initialized') self.file_logger.info(f'Node started at: {self.start_time}') @@ -175,6 +186,37 @@ class CtrlGuiNode(Node): if hasattr(goal_feedback, 'torque'): print(f"Feedback Torque: {goal_feedback.torque}") + def _on_arm_error(self, msg: ArmError) -> None: + """Callback for /arm_errors subscription.""" + self.last_arm_error[msg.arm_id] = msg + self.file_logger.error(f'Arm error: {msg}') + + def call_clear_arm_error(self, arm_id: int, joint_num: int): + self.file_logger.info(f'Calling ClearArmError for arm {arm_id}, joint {joint_num}') + if not self.clear_arm_err_client_.service_is_ready(): + self.file_logger.error('Clear arm error service not available') + ui.notify('Service not available', color='negative') + return + + req = ClearArmError.Request() + req.arm_id = int(arm_id) + req.joint_num = int(joint_num) + + future = self.clear_arm_err_client_.call_async(req) + def done_callback(f): + try: + res = f.result() + if res.success: + self.file_logger.info(f'Clear arm error success: {res.message}') + ui.notify(f'Arm {arm_id} clear success', color='positive') + else: + self.file_logger.warning(f'Clear arm error failed: {res.message}') + ui.notify(f'Arm {arm_id} clear failed: {res.message}', color='negative') + except Exception as e: + self.file_logger.error(f'Clear arm error exception: {str(e)}') + ui.notify(f'Error: {str(e)}', color='negative') + future.add_done_callback(done_callback) + def _setup_file_logging(self): """Setup logging to a file with timestamp in filename.""" # Create logs directory if it doesn't exist @@ -629,15 +671,7 @@ def build_ui(node: CtrlGuiNode) -> None: f'Confirm Move {arm_left_right_select.value} operation?\nMove: {arm_mode_select.value}\nleft_arm_params: {arm_inputs["left"].value}\nright_arm_params: {arm_inputs["right"].value}\n', move_arm_ )).classes('self-end') - - # Add statistics display - # ui.separator() - # ui.label('Node Statistics').classes('text-xl') - # with ui.card().classes('w-full card-container'): - # stats_label = ui.label() - # Display area for incoming /robot_work_info messages - # ui.separator() ui.label('Work Information').classes('text-xl') # Create labels for all RobotWorkInfo fields @@ -669,6 +703,30 @@ def build_ui(node: CtrlGuiNode) -> None: bt_node_status_label = ui.label() smacc_state_label = ui.label() + # --- Arm Diagnostics --- + ui.label('Arm Diagnostics').classes('text-xl') + with ui.card().classes('w-full card-container'): + with ui.row().classes('w-full justify-around'): + # Left Arm + with ui.column().classes('items-center'): + ui.label('Left Arm (ID:0)').classes('font-bold text-lg') + left_arm_err_label = ui.label('Errors: N/A') + left_arm_brake_label = ui.label('Brakes: N/A') + with ui.row().classes('items-center'): + left_joint_num = ui.select([0, 1, 2, 3, 4, 5, 6, 7], label='Joint', value=0).style('width: 80px') + ui.button('Clear', on_click=lambda: node.call_clear_arm_error(0, left_joint_num.value)).classes('bg-blue-500 text-white') + + ui.separator().props('vertical') + + # Right Arm + with ui.column().classes('items-center'): + ui.label('Right Arm (ID:1)').classes('font-bold text-lg') + right_arm_err_label = ui.label('Errors: N/A') + right_arm_brake_label = ui.label('Brakes: N/A') + with ui.row().classes('items-center'): + right_joint_num = ui.select([0, 1, 2, 3, 4, 5, 6, 7], label='Joint', value=0).style('width: 80px') + ui.button('Clear', on_click=lambda: node.call_clear_arm_error(1, right_joint_num.value)).classes('bg-blue-500 text-white') + # Update the labels periodically from the UI thread (safe cross-thread update) def update_work_info(): msg = node.last_work_info @@ -729,6 +787,16 @@ def build_ui(node: CtrlGuiNode) -> None: bt_node_status_label.set_text(f'BT Node Status: {str(msg.bt_node_status)}') smacc_state_label.set_text(f'SMACC State: {str(msg.smacc_state)}') + # Update Arm Diagnostics + for arm_id, label_err, label_brake in [(0, left_arm_err_label, left_arm_brake_label), (1, right_arm_err_label, right_arm_brake_label)]: + arm_msg = node.last_arm_error.get(arm_id) + if arm_msg: + label_err.set_text(f'Errors: {list(arm_msg.err_flag)}') + label_brake.set_text(f'Brakes: {list(arm_msg.brake_state)} (1=Locked, 0=Released)') + else: + label_err.set_text('Errors: N/A') + label_brake.set_text('Brakes: N/A') + ui.timer(0.5, update_work_info) def main() -> None: