add gripper control

ctrlgui/ctrlgui_node.py

@@ -23,6 +23,7 @@ from interfaces.action import ExecuteBtAction
 from interfaces.msg import SkillCall
 from interfaces.action import Arm
 from interfaces.srv import BtRebuild
+from interfaces.action import JzCmd
 import time
 
 
@@ -56,6 +57,8 @@ class CtrlGuiNode(Node):
             JointState, '/joint_states', self._on_joint_state, 10
         )
 
+        self.gripper_action_client_ = ActionClient(self, JzCmd, '/jz_cmd')
+
         self.file_logger.info('CtrlGuiNode initialized')
         self.file_logger.info(f'Node started at: {self.start_time}')
 
@@ -86,6 +89,44 @@ class CtrlGuiNode(Node):
             return
         self.file_logger.info('BT action goal accepted')
 
+    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.loc = int(loc)
+        gm.speed = int(speed)
+        gm.torque = int(torq)
+        gm.mode = int(mode)
+
+        print("Pack Gripper..")
+        print(f"goal_msg: {gm}")
+
+        self.file_logger.info(f'Sending goal to execute Gripper action')
+        self.file_logger.info(f"goal_msg: {gm}")
+
+        if self.gripper_action_client_.wait_for_server(10.0) == False:
+            print("Gripper Server not available")
+        else:
+            send_goal_future = self.gripper_action_client_.send_goal_async(gm, self.gripper_action_feedback_callback)
+            send_goal_future.add_done_callback(self.gripper_action_response_callback)
+            self.file_logger.info(f'Action Gripper goal sent.')
+            print("Gripper Finish..")
+
+    def gripper_action_response_callback(self, future):
+        goal_handle = future.result()
+        if not goal_handle.accepted:
+            self.file_logger.warning('Gripper action goal rejected')
+            print("error: Gripper action goal rejected")
+            return
+        self.file_logger.info('Gripper action goal accepted')
+        print("Gripper action goal accepted")
+
+    def gripper_action_feedback_callback(self, future):
+        print("gripper action feedback callback")
+        goal_feedback = future.feedback()
+        print("feedback loc: " + str(goal_feedback.loc))
+
     def _setup_file_logging(self):
         """Setup logging to a file with timestamp in filename."""
         # Create logs directory if it doesn't exist
@@ -304,19 +345,6 @@ def build_ui(node: CtrlGuiNode) -> None:
             ui.element().style('height: 40px')
             ui.button('车 -> 滚筒线', on_click=lambda: show_confirm_dialog('把箱子从车上搬到滚筒线上?', rebuild_out)).classes('self-end')
 
-        with ui.column():
-            with ui.row():
-                pass
-            def rebuild_out() -> None:
-                try:
-                    node.file_logger.info('Running rebuild_now...')
-                    rebuild_now("Trigger", "bt_vision_grab", "")
-                except Exception as e:
-                    node.file_logger.error(f'Failed to trigger rebuild: {str(e)}')
-
-            ui.element().style('height: 40px')
-            ui.button('视觉抓取', on_click=lambda: show_confirm_dialog('执行视觉抓取流程?', rebuild_out)).classes('self-end')
-
         with ui.column():
             with ui.row():
                 pass
@@ -349,6 +377,28 @@ def build_ui(node: CtrlGuiNode) -> None:
                 move_home_
             )).classes('self-end')
 
+        with ui.column():
+            # with ui.row():
+            gripper_width = ui.input('width(0~255)', value=0).style('width: 100px')
+            def rebuild_out() -> None:
+                try:
+                    node.file_logger.info('Running rebuild_now...')
+                    rebuild_now("Trigger", "bt_vision_grasp", "")
+                except Exception as e:
+                    node.file_logger.error(f'Failed to trigger rebuild: {str(e)}')
+
+            def cripper_control() -> None:
+                try:
+                    print("Gripper Send Action Start..")
+                    node.gripper_action_send_goal(6, int(gripper_width.value), 50, 50, 2)
+                    print("Gripper Send Action Goal Finish")
+                except Exception as e:
+                    node.file_logger.error(f'Failed to gripper action send goal: {str(e)}')
+
+            # ui.element().style('height: 40px')
+            ui.button('夹爪控制', on_click=lambda: show_confirm_dialog(f'确定控制夹爪? gripper width: {gripper_width.value}', cripper_control)).classes('self-end')
+            ui.button('视觉识别', on_click=lambda: show_confirm_dialog('执行视觉识别流程?', rebuild_out)).classes('self-end')
+
         #move wheel
         with ui.column():
             with ui.row():