HiveCoreRD/hivecore_robot_ctrl_gui
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add arm error page

Browse Source
This commit is contained in:
NuoDaJia02
2026-01-14 17:42:30 +08:00
parent 5578cc312e
commit 59bc92e4f3
1 changed files with 76 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

84
ctrlgui/ctrlgui_node.py
View File

@@ -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: