use service for control

ctrlgui/ctrlgui_node.py

@@ -20,6 +20,8 @@ import datetime
 import logging
 from interfaces.action import ExecuteBtAction
 from interfaces.msg import SkillCall
+from interfaces.action import Arm
+from interfaces.srv import BtRebuild
 
 class CtrlGuiNode(Node):
     """ROS2 node that publishes String messages to /led_cmd."""
@@ -37,7 +39,7 @@ class CtrlGuiNode(Node):
         
         # publisher on /led_cmd
         self.publisher_ = self.create_publisher(String, '/led_cmd', 10)
-        self.run_trigger_ = self.create_client(Trigger, '/cerebrum/rebuild_now')
+        self.run_trigger_ = self.create_client(BtRebuild, '/cerebrum/rebuild_now')
         # subscribe to /robot_work_info to display incoming messages in the UI
         self.last_work_info: RobotWorkInfo = RobotWorkInfo()
         self._sub_work_info = self.create_subscription(
@@ -137,11 +139,17 @@ class CtrlGuiNode(Node):
         self.messages_sent += 1
         self.file_logger.info(f"Published message #{self.messages_sent} to /led_cmd: {str(text)}")
 
-    def rebuild_now(self) -> None:
+    def rebuild_now(self, type: str, config: str, param: str) -> None:
         self.rebuild_requests += 1
         self.file_logger.info('Rebuild BehaviorTree now')
         self.file_logger.info(f'Total rebuild requests: {str(self.rebuild_requests)}')
-        future = self.run_trigger_.call_async(Trigger.Request())
+
+        request = BtRebuild.Request()
+        request.type = type #"Remote"
+        request.config = config #"MoveWaist"
+        request.param = param #f"move_pitch_degree: 10\nmove_yaw_degree: 10\n"
+
+        future = self.run_trigger_.call_async(request)
         future.add_done_callback(self._on_rebuild_response)
         
         # Log to file
@@ -179,8 +187,10 @@ def build_ui(node: CtrlGuiNode) -> None:
     
     # --- build UI ---
     ui.label('HiveCore Robot Control Node').classes('text-2xl')
-    
-    # 公共确认对话框函数
+
+    ui.separator()
+    ui.label('Control Panel').classes('text-xl')
+    # confirm dialog function
     def show_confirm_dialog(message: str, confirm_callback) -> None:
         """显示确认对话框
         
@@ -203,11 +213,10 @@ def build_ui(node: CtrlGuiNode) -> None:
             def rebuild() -> None:
                 try:
                     node.file_logger.info('Running rebuild_now...')
-                    node.rebuild_now()
+                    node.rebuild_now("Trigger", "SwitchSchedule", "")
                 except Exception as e:
                     node.file_logger.error(f'Failed to trigger rebuild: {str(e)}')
 
-            # 使用明确的高度来对齐按钮
             ui.element().style('height: 40px')
             ui.button('Switch Schedule', on_click=lambda: show_confirm_dialog('Confirm Switch Schedule?', rebuild)).classes('self-end')
 
@@ -227,6 +236,28 @@ def build_ui(node: CtrlGuiNode) -> None:
                 node.file_logger.error(f'Failed to trigger bt_action_send_goal: {str(e)}')
                 ui.notify(f'Failed to send action: {str(e)}', color='negative')
 
+        #move arm
+        with ui.column():
+            with ui.row():
+                left_arm_position_pose = ui.input('LEFT_ARM_POSE: x, y, z, rx, ry, rz, w', value='0, 0, 0, 0, 0, 0, 0').style('width: 550px')
+
+            with ui.row():
+                right_arm_position_pose = ui.input('RIGHT_ARM_POSE: x, y, z, rx, ry, rz, w', value='0, 0, 0, 0, 0, 0, 0').style('width: 550px')
+
+            # TODO check input data
+
+            def move_arm_() -> None:
+                node.file_logger.info('Running move_arm_...')
+                action_name = "Arm"
+                text = f"body_id: 0\ndata_type: 3\ndata_length: 14\ncommand_id: 0\nframe_time_stamp: {node.get_clock().now().to_msg().sec}\ndata_array: [{left_arm_position_pose.value}, {right_arm_position_pose.value}]\n"
+                # send_action_package(action_name, text)
+                node.rebuild_now("Remote", action_name, text)
+
+            ui.button('Arm Control', on_click=lambda: show_confirm_dialog(
+                f'Confirm Move Arm operation?\nleft_arm_pose: {left_arm_position_pose.value}\nright_arm_pose: {right_arm_position_pose.value}\n',
+                move_arm_
+            )).classes('self-end')
+
         #move waist
         with ui.column():
             with ui.row():
@@ -239,7 +270,8 @@ def build_ui(node: CtrlGuiNode) -> None:
                 node.file_logger.info('Running move_waist_...')
                 action_name = "MoveWaist"
                 text = f"move_pitch_degree: {move_waist_input_move_pitch_degree.value}\nmove_yaw_degree: {move_waist_input_move_yaw_degree.value}\n"
-                send_action_package(action_name, text)
+                # send_action_package(action_name, text)
+                node.rebuild_now("Remote", action_name, text)
 
             ui.button('Move Waist', on_click=lambda: show_confirm_dialog(
                 f'Confirm Move Waist operation?\nmove_pitch_degree: {move_waist_input_move_pitch_degree.value}\nmove_yaw_degree: {move_waist_input_move_yaw_degree.value}',