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

add arm joint state sub

Browse Source
This commit is contained in:
david
2025-10-28 16:24:38 +08:00
parent 1d99d5a4f6
commit f55ce895ee
1 changed files with 29 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
ctrlgui/ctrlgui_node.py
View File

@@ -13,6 +13,7 @@ from rclpy.node import Node
from rclpy.action.client import ActionClient
from std_msgs.msg import String
from std_srvs.srv import Trigger
from sensor_msgs.msg import JointState
from nicegui import ui
from interfaces.msg import RobotWorkInfo
from starlette.staticfiles import StaticFiles
@@ -51,6 +52,10 @@ class CtrlGuiNode(Node):
self.exe_bt_action_client_ = ActionClient(self, ExecuteBtAction, '/execute_bt_action')
self.exe_bt_action_epoch = 0
self.joint_state_sub_ = self.create_subscription(
JointState, '/joint_states', self._on_joint_state, 10
)
self.file_logger.info('CtrlGuiNode initialized')
self.file_logger.info(f'Node started at: {self.start_time}')
@@ -134,6 +139,16 @@ class CtrlGuiNode(Node):
except Exception as e:
self.file_logger.error(f'Rebuild request exception: {str(e)}')
def _on_joint_state(self, msg: JointState) -> None:
"""Callback for /joint_states subscription: store latest joint states."""
self.last_joint_state = msg
print("Joint states received, Joint positions: ")
for i, position in enumerate(self.last_joint_state.position):
print(f" Joint {i}: {position}")
self.file_logger.info(f"/joint_states positions: {str(self.last_joint_state.position)}")
def publish_info(self, text: str) -> None:
msg = String()
msg.data = text
@@ -449,6 +464,7 @@ def build_ui(node: CtrlGuiNode) -> None:
last_task_label = ui.label('Last Task: ')
current_task_label = ui.label('Current Task: ')
next_task_label = ui.label('Next Task: ')
arm_joint_label = ui.label('Arm Joint: ').classes('col-span-2')
ui.label('Skill Information').classes('text-lg font-bold')
with ui.grid(columns=2).classes('w-full'):
@@ -494,6 +510,19 @@ def build_ui(node: CtrlGuiNode) -> None:
last_task_label.set_text('Last Task: N/A')
next_task_label.set_text('Next Task: N/A')
current_task_label.set_text('Current Task: N/A')
if hasattr(node, 'last_joint_state') and hasattr(node.last_joint_state, 'position'):
positions = list(node.last_joint_state.position)
if len(positions) >= 21:
left_arm_subset = positions[9:15] # Subset of joint positions
right_arm_subset = positions[15:21]
formatted_left = [f"{p * 180.0 / 3.14:.2f}" for p in left_arm_subset]
formatted_right = [f"{p * 180.0 / 3.14:.2f}" for p in right_arm_subset]
arm_joint_label.set_text(f'Arm Joint: [LEFT] {", ".join(formatted_left)} [RIGHT] {", ".join(formatted_right)}')
else:
arm_joint_label.set_text('Arm Joint: N/A (not enough joints)')
else:
arm_joint_label.set_text('Arm Joint: N/A')
# Skill Information
skill_label.set_text(f'Skill: {str(msg.skill)}')