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

add action feedback

Browse Source
This commit is contained in:
david
2025-11-25 17:55:46 +08:00
parent fa86790ab1
commit dbc1173470
1 changed files with 32 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
ctrlgui/ctrlgui_node.py
View File

@@ -57,7 +57,7 @@ class CtrlGuiNode(Node):
JointState, '/joint_states', self._on_joint_state, 10
)
self.gripper_action_client_ = ActionClient(self, JzCmd, '/jz_cmd')
self.gripper_action_client_ = ActionClient(self, JzCmd, '/jz_cmd0')
self.file_logger.info('CtrlGuiNode initialized')
self.file_logger.info(f'Node started at: {self.start_time}')
@@ -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,11 +96,21 @@ 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...")
gm = JzCmd.Goal()
gm.devid = int(id)
# gm.devid = int(id)
gm.loc = int(loc)
gm.speed = int(speed)
gm.torque = int(torq)
@@ -121,11 +138,20 @@ class CtrlGuiNode(Node):
return
self.file_logger.info('Gripper action goal accepted')
print("Gripper action goal accepted")
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_feedback_callback(self, future):
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."""