optimize code

ctrlgui/ctrlgui_node.py

@@ -74,25 +74,58 @@ class CtrlGuiNode(Node):
         self.file_logger.info('CtrlGuiNode initialized')
         self.file_logger.info(f'Node started at: {self.start_time}')
 
-    def bt_action_send_goal(self, action_name: str, str_params: SkillCall):
-        """Callback to execute a Behavior Tree action."""
+    async def _wait_for_action_server(self, client: ActionClient, timeout_sec: float = 1.0) -> bool:
+        """Asynchronously wait for an action server to be available."""
+        import asyncio
+        start = time.time()
+        while time.time() - start < timeout_sec:
+            if client.server_is_ready():
+                return True
+            await asyncio.sleep(0.1)
+        return False
+
+    async def call_bt_action(self, action_name: str, str_params: SkillCall):
+        import asyncio
         goal_msg = ExecuteBtAction.Goal()
         goal_msg.action_name = action_name
         goal_msg.calls = [str_params]
         self.exe_bt_action_epoch += 1
         goal_msg.epoch = self.exe_bt_action_epoch
 
-        print(f"goal_msg: {goal_msg}")
-
         self.file_logger.info(f'Sending goal to execute BT action: {action_name}')
         self.file_logger.info(f"goal_msg: {goal_msg}")
+        
+        # Non-blocking wait for server
+        if not await self._wait_for_action_server(self.exe_bt_action_client_, 1.0):
+             self.file_logger.error('BT Action Server not available')
+             ui.notify('BT Action Server not available', color='negative')
+             return
 
-        self.exe_bt_action_client_.wait_for_server(10.0)
         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)
+        
+        while not send_goal_future.done():
+            await asyncio.sleep(0.1)
+            
+        goal_handle = send_goal_future.result()
+        if not goal_handle.accepted:
+            self.file_logger.warning('BT action goal rejected')
+            ui.notify(f'BT action {action_name} rejected', color='negative')
+            return
 
-        self.file_logger.info(f'Action {action_name} goal sent.')
+        self.file_logger.info('BT action goal accepted')
+        ui.notify(f'BT action {action_name} accepted', color='positive')
+
+        get_result_future = goal_handle.get_result_async()
+        
+        while not get_result_future.done():
+            await asyncio.sleep(0.1)
+
+        result = get_result_future.result()
+        bt_result = result.result
+        status = result.status
+        self.file_logger.info(f'BT Action Result: {bt_result}, Status: {status}')
+        ui.notify(f'BT Action {action_name} finished', color='info')
 
     def bt_action_feedback_callback(self, feedback_msg):
         """Callback for BT action feedback."""
@@ -100,79 +133,39 @@ class CtrlGuiNode(Node):
         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()
-        if not goal_handle.accepted:
-            self.file_logger.warning('BT action goal rejected')
-            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...")
+    async def call_gripper_action(self, id, loc, speed, torq, mode):
+        import asyncio
         gm = GripperCmd.Goal()
-        # gm.devid = int(id)
         gm.loc = int(loc)
         gm.speed = int(speed)
         gm.torque = int(torq)
         gm.mode = int(mode)
+        
+        client = self.gripper0_action_client_ if id == 0 else self.gripper1_action_client_
+        
+        if not await self._wait_for_action_server(client, 1.0):
+             ui.notify(f'Gripper {id} server not available', color='negative')
+             return
 
-        print(f"Pack Gripper..{id}")
-        print(f"goal_msg: {gm}")
+        send_goal_future = client.send_goal_async(gm, feedback_callback=self.gripper_action_feedback_callback)
+        
+        while not send_goal_future.done():
+             await asyncio.sleep(0.1)
+             
+        goal_handle = send_goal_future.result()
 
-        self.file_logger.info(f'Sending goal to execute Gripper action')
-        self.file_logger.info(f"goal_msg: {gm}")
-
-        if id == 0:
-            if self.gripper0_action_client_.wait_for_server(10.0) == False:
-                print("Gripper0 Server not available")
-            else:
-                send_goal_future = self.gripper0_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 Gripper0 goal sent.')
-                print("Gripper0 Finish..")
-        elif id == 1:
-            if self.gripper1_action_client_.wait_for_server(10.0) == False:
-                print("Gripper1 Server not available")
-            else:
-                send_goal_future = self.gripper1_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 Gripper1 goal sent.')
-                print("Gripper1 Finish..")
-        else:
-            print(f"Gripper id error: {id}")
-
-    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")
-        # 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_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}')
-        if hasattr(result, 'state'):
-             print(f"Gripper Result State: {result.state}")
-        if hasattr(result, 'result'):
-             print(f"Gripper Result Code: {result.result}")
+             ui.notify(f'Gripper {id} action rejected', color='negative')
+             return
+             
+        ui.notify(f'Gripper {id} action accepted', color='positive')
+        
+        result_future = goal_handle.get_result_async()
+        while not result_future.done():
+            await asyncio.sleep(0.1)
+            
+        result = result_future.result()
+        ui.notify(f'Gripper {id} action finished', color='info')
 
     def gripper_action_feedback_callback(self, feedback_msg):
         print("gripper action feedback callback")
@@ -288,7 +281,36 @@ class CtrlGuiNode(Node):
         self.publisher_.publish(msg)
         self.messages_sent += 1
         self.file_logger.info(f"Published message #{self.messages_sent} to /led_cmd: {str(text)}")
+    async def call_rebuild_now(self, type: str, config: str, param: str):
+        import asyncio
+        self.rebuild_requests += 1
+        self.file_logger.info(f'Rebuild BehaviorTree now. Total requests: {self.rebuild_requests}')
 
+        request = BtRebuild.Request()
+        request.type = type 
+        request.config = config 
+        request.param = param 
+
+        if not self.run_trigger_.service_is_ready():
+             ui.notify('Rebuild service not ready', color='negative')
+             return
+
+        future = self.run_trigger_.call_async(request)
+        
+        while not future.done():
+            await asyncio.sleep(0.1)
+            
+        try:
+            response = future.result()
+            if response.success:
+                self.file_logger.info('Rebuild request successful')
+                ui.notify(f'Rebuild {config} success', color='positive')
+            else:
+                 self.file_logger.warning(f'Rebuild request failed: {response.message}')
+                 ui.notify(f'Rebuild failed: {response.message}', color='negative')
+        except Exception as e:
+             self.file_logger.error(f'Rebuild request exception: {str(e)}')
+             ui.notify(f'Rebuild error: {str(e)}', color='negative')
     # def rebuild_now(self, type: str, config: str, param: str) -> None:
     #     self.rebuild_requests += 1
     #     self.file_logger.info('Rebuild BehaviorTree now')
@@ -350,10 +372,16 @@ def build_ui(node: CtrlGuiNode) -> None:
             message: 要显示的确认消息
             confirm_callback: 点击确认按钮后执行的函数
         """
+        async def on_ok():
+            dialog.close()
+            res = confirm_callback()
+            if res is not None and hasattr(res, '__await__'):
+                await res
+
         with ui.dialog() as dialog, ui.card():
             ui.label(message)
             with ui.row():
-                ui.button('OK', on_click=lambda: (dialog.close(), confirm_callback())).classes('text-white bg-blue-500')
+                ui.button('OK', on_click=on_ok).classes('text-white bg-blue-500')
                 ui.button('Cancel', on_click=dialog.close).classes('text-white bg-red-500')
         dialog.open()
 
@@ -363,7 +391,7 @@ def build_ui(node: CtrlGuiNode) -> None:
     with ui.row():
 
          # send action package
-        def send_action_package(action_name: str, params: str) -> None:
+        async def send_action_package(action_name: str, params: str) -> None:
             """Send an action package to the robot."""
             try:            
                 msg = SkillCall()
@@ -372,10 +400,9 @@ def build_ui(node: CtrlGuiNode) -> None:
                 msg.call_kind = "action"
                 msg.topic = ""
                 msg.payload_yaml = params
-                node.bt_action_send_goal(action_name, msg)
-                ui.notify(f'Sent action: {action_name} with params: {params}')
+                await node.call_bt_action(action_name, msg)
             except Exception as e:
-                node.file_logger.error(f'Failed to trigger bt_action_send_goal: {str(e)}')
+                node.file_logger.error(f'Failed to trigger bt_action: {str(e)}')
                 ui.notify(f'Failed to send action: {str(e)}', color='negative')
 
         # def _on_rebuild_response(future):
@@ -392,61 +419,51 @@ def build_ui(node: CtrlGuiNode) -> None:
         #         node.file_logger.error(f'Rebuild request exception: {str(e)}')
         #         ui.notify(f'Rebuild request exception: {str(e)}', color='negative')
 
-        def rebuild_now(type: str, config: str, param: str) -> None:
+        async def rebuild_now(type: str, config: str, param: str) -> None:
             try:
-                node.rebuild_requests += 1
-                node.file_logger.info('Rebuild BehaviorTree now')
-                node.file_logger.info(f'Total rebuild requests: {str(node.rebuild_requests)}')
-
-                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 = node.run_trigger_.call_async(request)
-                future.add_done_callback(node._on_rebuild_response)
-                ui.notify(f'rebuild_now: type={type}, config={config}, param={param}')
+                await node.call_rebuild_now(type, config, param)
             except Exception as e:
                 node.file_logger.error(f'Failed to rebuild_now: type={type}, config={config}, {str(e)}')
                 ui.notify(f'Failed to rebuild_now: type={type}, config={config}, {str(e)}', color='negative')
+
               
         # rebuild button
         with ui.column():
             with ui.row():
                 pass
-            def rebuild_in() -> None:
+            async def rebuild_in() -> None:
                 try:
                     node.file_logger.info('Running rebuild_now...')
-                    rebuild_now("Trigger", "bt_carry_boxes_sch1", "")
+                    await rebuild_now("Trigger", "bt_carry_boxes_sch1", "")
                 except Exception as e:
                     node.file_logger.error(f'Failed to trigger rebuild: {str(e)}')
-            def rebuild_out() -> None:
+            async def rebuild_out_default() -> None:
                 try:
                     node.file_logger.info('Running rebuild_now...')
-                    rebuild_now("Trigger", "bt_carry_boxes_sch2", "")
+                    await rebuild_now("Trigger", "bt_carry_boxes_sch2", "")
                 except Exception as e:
                     node.file_logger.error(f'Failed to trigger rebuild: {str(e)}')
 
-            def rebuild_out(action_name) -> None:
+            async def rebuild_out(action_name) -> None:
                 try:
                     node.file_logger.info('Running rebuild_now...')
-                    rebuild_now("Trigger", action_name, "")
+                    await rebuild_now("Trigger", action_name, "")
                 except Exception as e:
                     node.file_logger.error(f'Failed to trigger rebuild: {str(e)}')
 
-            def rebuild_cancel() -> None:
+            async def rebuild_cancel() -> None:
                 try:
                     node.file_logger.info('Running rebuild_now...')
                     print("CancelBTTask")
-                    rebuild_now("Trigger", "CancelBTTask", "")
+                    await rebuild_now("Trigger", "CancelBTTask", "")
                 except Exception as e:
                     node.file_logger.error(f'Failed to trigger rebuild: {str(e)}')
                     print(f"CancelBTTask: {str(e)}")
-            ui.element().style('height: 40px')
+            # ui.element().style('height: 40px')
             # ui.button('滚筒线 -> 车', on_click=lambda: show_confirm_dialog('把箱子从滚筒线上搬到车上?', rebuild_in)).classes('self-end')
             # ui.button('车 -> 滚筒线', on_click=lambda: show_confirm_dialog('把箱子从车上搬到滚筒线上?', rebuild_out)).classes('self-end')
-            ui.button('右臂视觉抓取', on_click=lambda: show_confirm_dialog('执行右臂视觉抓取流程?', lambda: rebuild_out("bt_vision_grasp_right_arm"))).classes('self-end')
             ui.button('左臂视觉抓取', on_click=lambda: show_confirm_dialog('执行左臂视觉抓取流程?', lambda: rebuild_out("bt_vision_grasp_left_arm"))).classes('self-end')
+            ui.button('右臂视觉抓取', on_click=lambda: show_confirm_dialog('执行右臂视觉抓取流程?', lambda: rebuild_out("bt_vision_grasp_right_arm"))).classes('self-end')
             ui.button('双臂视觉抓取', on_click=lambda: show_confirm_dialog('执行双臂视觉抓取流程?', lambda: rebuild_out("bt_vision_grasp_dual_arm"))).classes('self-end')
             ui.button('取消任务', on_click=lambda: show_confirm_dialog('取消任务?', rebuild_cancel)).classes('self-end')
 
@@ -455,12 +472,12 @@ def build_ui(node: CtrlGuiNode) -> None:
             with ui.row():
                 camera_id = ui.select(['top', 'left', 'right'], value='top').style('width: 100px')
 
-            def vision_recg_(id) -> None:
+            async def vision_recg_(id) -> None:
                 node.file_logger.info('Running vision_recg_...')
                 action_name = "VisionObjectRecognition"
                 text = f"camera_position: {id}\n"
                 # send_action_package(action_name, text)
-                rebuild_now("Remote", action_name, text)
+                await rebuild_now("Remote", action_name, text)
 
             ui.button('Vision Recg', on_click=lambda: show_confirm_dialog(
                 f'Confirm Vision Recognition operation?\nCamera ID: {camera_id.value}',
@@ -471,12 +488,12 @@ def build_ui(node: CtrlGuiNode) -> None:
         # with ui.column():
         #     with ui.row():
         #         pass
-            def move_home_() -> None:
+            async def move_home_() -> None:
                 node.file_logger.info('Running move_home_...')
                 action_name = "MoveHome"
                 text = ""
                 # send_action_package(action_name, text)
-                rebuild_now("Remote", action_name, text)
+                await rebuild_now("Remote", action_name, text)
 
             # ui.element().style('height: 40px')
             ui.button('Move Home', on_click=lambda: show_confirm_dialog(
@@ -487,10 +504,10 @@ def build_ui(node: CtrlGuiNode) -> None:
         with ui.column():
             # with ui.row():
             gripper_width = ui.input('width(0~255)', value=0).style('width: 100px')
-            def gripper_control(id) -> None:
+            async def gripper_control(id) -> None:
                 try:
                     print(f"Gripper {id} Send Action Start..")
-                    node.gripper_action_send_goal(id, int(gripper_width.value), 50, 50, 2)
+                    await node.call_gripper_action(id, int(gripper_width.value), 50, 50, 2)
                     print(f"Gripper {id} Send Action Goal Finish")
                 except Exception as e:
                     node.file_logger.error(f'Failed to gripper action send goal: {str(e)}')
@@ -507,12 +524,12 @@ def build_ui(node: CtrlGuiNode) -> None:
 
             # TODO check input data
 
-            def move_wheel_() -> None:
+            async def move_wheel_() -> None:
                 node.file_logger.info('Running move_wheel_...')
                 action_name = "MoveWheel"
                 text = f"move_distance: {move_wheel_input_move_forward.value}\nmove_angle: {move_wheel_input_move_turn.value}\n"
                 # send_action_package(action_name, text)
-                rebuild_now("Remote", action_name, text)
+                await rebuild_now("Remote", action_name, text)
 
             ui.button('Move Wheel', on_click=lambda: show_confirm_dialog(
                 f'Confirm Move Wheel operation?\nmove_distance: {move_wheel_input_move_forward.value}\nmove_angle: {move_wheel_input_move_turn.value}',
@@ -524,56 +541,29 @@ def build_ui(node: CtrlGuiNode) -> None:
             with ui.row():
                 pass
 
-            def move_wheel_joy_(distance, angle) -> None:
-                node.file_logger.info('Running move_wheel_...')
+            async def perform_move_wheel(distance: float, angle: float) -> None:
+                node.file_logger.info('Running move_wheel (joy)...')
                 action_name = "MoveWheel"
                 text = f"move_distance: {distance}\nmove_angle: {angle}\n"
-                # send_action_package(action_name, text)
                 print("MoveWheel: " + text)
-                rebuild_now("Remote", action_name, text)
+                await rebuild_now("Remote", action_name, text)
 
-            ui.button('左转1度', on_click=lambda: move_wheel_joy_(0, 1))
-            ui.button('左转3度', on_click=lambda: move_wheel_joy_(0, 3))
-            ui.button('右转1度', on_click=lambda: move_wheel_joy_(0, -1))
-            ui.button('右转3度', on_click=lambda: move_wheel_joy_(0, -3))
-
-
-        ####
-        with ui.column():
+            ui.label('微调-转向')
             with ui.row():
-                pass
+                ui.button('左转1度', on_click=lambda: perform_move_wheel(0, 1))
+                ui.button('左转3度', on_click=lambda: perform_move_wheel(0, 3))
+                ui.button('右转1度', on_click=lambda: perform_move_wheel(0, -1))
+                ui.button('右转3度', on_click=lambda: perform_move_wheel(0, -3))
 
-            def move_wheel_joy_(distance, angle) -> None:
-                node.file_logger.info('Running move_wheel_...')
-                action_name = "MoveWheel"
-                text = f"move_distance: {distance}\nmove_angle: {angle}\n"
-                # send_action_package(action_name, text)
-                print("MoveWheel: " + text)
-                rebuild_now("Remote", action_name, text)
+            ui.label('微调-进退')
+            with ui.row():
+                ui.button('往前0.1', on_click=lambda: perform_move_wheel(0.1, 0))
+                ui.button('往前0.3', on_click=lambda: perform_move_wheel(0.3, 0))
+                ui.button('往后0.1', on_click=lambda: perform_move_wheel(-0.1, 0))
+                ui.button('往后0.3', on_click=lambda: perform_move_wheel(-0.3, 0))
 
-            ui.button('往前0.1', on_click=lambda: move_wheel_joy_(0.1, 0))
-            ui.button('往前0.3', on_click=lambda: move_wheel_joy_(0.3, 0))
-            ui.button('往后0.1', on_click=lambda: move_wheel_joy_(-0.1, 0))
-            ui.button('往后0.3', on_click=lambda: move_wheel_joy_(-0.3, 0))
-
-        #move leg
+        #move leg (commented out)
         # with ui.column():
-        #     with ui.row():
-        #         move_leg_input_move_distance = ui.input('distance', value='0').style('width: 100px')
-
-        #     # TODO check input data
-
-        #     def move_leg_() -> None:
-        #         node.file_logger.info('Running move_leg_...')
-        #         action_name = "MoveLeg"
-        #         text = f"move_up_distance: {move_leg_input_move_distance.value}\n"
-        #         # send_action_package(action_name, text)
-        #         rebuild_now("Remote", action_name, text)
-
-        #     ui.button('Move Leg', on_click=lambda: show_confirm_dialog(
-        #         f'Confirm Move Leg operation?\nmove_up_distance: {move_leg_input_move_distance.value}',
-        #         move_leg_
-        #     )).classes('self-end')
 
         #move waist
         with ui.column():
@@ -583,12 +573,12 @@ def build_ui(node: CtrlGuiNode) -> None:
 
             # TODO check input data
 
-            def move_waist_() -> None:
+            async def move_waist_() -> 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)
-                rebuild_now("Remote", action_name, text)
+                await 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}',
@@ -646,8 +636,8 @@ def build_ui(node: CtrlGuiNode) -> None:
             # Update inputs when select value changes
             arm_mode_select.on('update:model-value', lambda e: update_arm_inputs())
 
-        with ui.column():
-            def move_arm_() -> None:
+        # with ui.column():
+            async def move_arm_() -> None:
                 node.file_logger.info('Running move_arm_...')
                 action_name = "Arm"
 
@@ -667,7 +657,7 @@ def build_ui(node: CtrlGuiNode) -> None:
 
                 text = f"body_id: {left_right_select}\ndata_type: {data_type}\ndata_length: {data_length}\ncommand_id: 0\nframe_time_stamp: {node.get_clock().now().to_msg().sec}\ndata_array: [{arm_inputs['left'].value}, {arm_inputs['right'].value}]\n"
                 # send_action_package(action_name, text)
-                rebuild_now("Remote", action_name, text)
+                await rebuild_now("Remote", action_name, text)
 
             ui.button('Arm Control', on_click=lambda: show_confirm_dialog(
                 f'Confirm Move {arm_left_right_select.value} operation?\nMove: {arm_mode_select.value}\nleft_arm_params: {arm_inputs["left"].value}\nright_arm_params: {arm_inputs["right"].value}\n',