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

add gripper_status

Browse Source
This commit is contained in:
NuoDaJia02
2026-01-20 10:32:32 +08:00
parent d981bb5d7c
commit 695af8196d
1 changed files with 54 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
ctrlgui/ctrlgui_node.py
View File

@@ -26,6 +26,7 @@ from interfaces.srv import BtRebuild
from interfaces.msg import ArmError
from interfaces.srv import ClearArmError
from interfaces.action import GripperCmd
from interfaces.msg import GripperStatus
import time
@@ -62,6 +63,15 @@ class CtrlGuiNode(Node):
self.gripper0_action_client_ = ActionClient(self, GripperCmd, '/gripper_cmd0')
self.gripper1_action_client_ = ActionClient(self, GripperCmd, '/gripper_cmd1')
# gripper status subscription
self.last_gripper_status = {0: GripperStatus(), 1: GripperStatus()}
self._sub_gripper0_status = self.create_subscription(
GripperStatus, '/gripper0/status', lambda msg: self._on_gripper_status(msg, 0), 10
)
self._sub_gripper1_status = self.create_subscription(
GripperStatus, '/gripper1/status', lambda msg: self._on_gripper_status(msg, 1), 10
)
# arm error subscription
self.last_arm_error = {0: None, 1: None}
self._sub_arm_error = self.create_subscription(
@@ -195,6 +205,11 @@ class CtrlGuiNode(Node):
self.last_arm_error[msg.arm_id] = msg
self.file_logger.error(f'Arm error: {msg}')
def _on_gripper_status(self, msg: GripperStatus, gripper_id: int) -> None:
"""Callback for /gripper_status subscription."""
self.last_gripper_status[gripper_id] = msg
# self.file_logger.debug(f'Gripper {gripper_id} status: {msg}')
async def call_clear_arm_error(self, arm_id: int, joint_num: int):
import asyncio
self.file_logger.info(f'Calling ClearArmError for arm {arm_id}, joint {joint_num}')
@@ -749,6 +764,28 @@ def build_ui(node: CtrlGuiNode) -> None:
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')
# --- Gripper Status ---
ui.label('Gripper Status').classes('text-xl')
with ui.card().classes('w-full card-container'):
with ui.row().classes('w-full justify-around'):
# Gripper 0
with ui.column().classes('items-center'):
ui.label('Gripper 0').classes('font-bold text-lg')
gripper0_pos_label = ui.label('Pos: N/A')
gripper0_speed_label = ui.label('Speed: N/A')
gripper0_torque_label = ui.label('Torque: N/A')
gripper0_state_label = ui.label('State: N/A')
ui.separator().props('vertical')
# Gripper 1
with ui.column().classes('items-center'):
ui.label('Gripper 1').classes('font-bold text-lg')
gripper1_pos_label = ui.label('Pos: N/A')
gripper1_speed_label = ui.label('Speed: N/A')
gripper1_torque_label = ui.label('Torque: N/A')
gripper1_state_label = ui.label('State: N/A')
# Update the labels periodically from the UI thread (safe cross-thread update)
def update_work_info():
msg = node.last_work_info
@@ -819,6 +856,23 @@ def build_ui(node: CtrlGuiNode) -> None:
label_err.set_text('Errors: N/A')
label_brake.set_text('Brakes: N/A')
# Update Gripper Status
for gid, l_pos, l_speed, l_torque, l_state in [
(0, gripper0_pos_label, gripper0_speed_label, gripper0_torque_label, gripper0_state_label),
(1, gripper1_pos_label, gripper1_speed_label, gripper1_torque_label, gripper1_state_label)
]:
g_msg = node.last_gripper_status.get(gid)
if g_msg and hasattr(g_msg, 'loc'):
l_pos.set_text(f'Pos: {g_msg.loc}')
l_speed.set_text(f'Speed: {g_msg.speed}')
l_torque.set_text(f'Torque: {g_msg.torque}')
l_state.set_text(f'State: {g_msg.state}')
else:
l_pos.set_text('Pos: N/A')
l_speed.set_text('Speed: N/A')
l_torque.set_text('Torque: N/A')
l_state.set_text('State: N/A')
ui.timer(0.5, update_work_info)
def main() -> None: