kuavo-humanoid-sdk 1.2.1b3279__tar.gz → 1.2.1b3290__tar.gz

{kuavo_humanoid_sdk-1.2.1b3279 → kuavo_humanoid_sdk-1.2.1b3290}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: kuavo_humanoid_sdk
-Version: 1.2.1b3279
+Version: 1.2.1b3290
 Summary: A Python SDK for kuavo humanoid robot.
 Home-page: https://gitee.com/leju-robot/kuavo-ros-opensource/
 Author: ['lejurobot']

kuavo_humanoid_sdk-1.2.1b3290/kuavo_humanoid_sdk/kuavo/core/ros/camera.py

@@ -0,0 +1,105 @@
+#!/usr/bin/env python3
+# coding: utf-8
+import rospy
+from std_msgs.msg import Bool, Int16MultiArray
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+from kuavo_humanoid_sdk.kuavo.core.core import KuavoRobotCore
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge, CvBridgeError
+from kuavo_msgs.msg import yoloOutputData, yoloDetection
+import threading
+
+class CameraROSInterface:
+    def __init__(self):
+        self.bridge = CvBridge()
+        
+        self.cameras = {
+            'head': {
+                'topic': '/camera/color/image_raw',
+                'pub_topic': '/model_output_data',
+                'publisher': None
+            },
+            'chest': {
+                'topic': '/chest_cam',
+                'pub_topic': '/chest_detection_data',
+                'publisher': None
+            }
+        }
+
+        self.cv_image_shape = None
+
+    
+    def init_ros_node(self):
+        if not rospy.get_node_uri():
+            rospy.init_node('YOLO_detection', anonymous=True)
+        
+        for camera in self.cameras.values():
+            camera['publisher'] = rospy.Publisher(
+                camera['pub_topic'], 
+                yoloDetection, 
+                queue_size=1, 
+                tcp_nodelay=True
+            )
+
+    def get_camera_image(self, camera):
+        if camera not in self.cameras:
+            rospy.logerr(f"Unknown camera: {camera}")
+            return None
+            
+        try:
+            # 使用rospy.wait_for_message()获取一帧图像
+            msg = rospy.wait_for_message(self.cameras[camera]['topic'], Image, timeout=1.0)
+            cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
+            self.cv_image_shape = cv_image.shape
+            return cv_image
+        except rospy.ROSException as e:
+            print(f"Timeout waiting for {camera} camera image: {e}")
+            return None
+        except CvBridgeError as e:
+            rospy.logerr(f"{camera.capitalize()} Camera CvBridge Error: {e}")
+            return None
+
+    def node_spin(self):
+        rospy.spin()
+
+    def node_is_shutdown(self):
+        return rospy.is_shutdown()
+
+    def tensor_to_msg(self, results):
+        if not results:
+            return None
+        else:
+            yolo_detections = yoloDetection()
+            for result in results:
+                boxes = result.boxes.cpu().numpy()
+
+                xywh = boxes.xywh  # center-x(i,0), center-y(i,1), width(i,2), height(i,3)
+                class_ids = result.boxes.cls.int().cpu().numpy()
+                class_names = [result.names[cls.item()] for cls in result.boxes.cls.int()]
+                confs = boxes.conf
+
+                for i in range(len(xywh)):
+                    yolo_output_data = yoloOutputData(
+                        class_name=class_names[i],
+                        class_id=int(class_ids[i]),
+                        confidence=confs[i], 
+                        x_pos=xywh[i][0], 
+                        y_pos=xywh[i][1], 
+                        height=xywh[i][2], 
+                        width=xywh[i][3]
+                    )
+                    yolo_detections.data.append(yolo_output_data)
+            return yolo_detections
+
+    def publish_results(self, results, camera):
+        if not results:
+            rospy.loginfo(f"No results to publish for {camera} camera.")
+            return
+            
+        if camera not in self.cameras:
+            rospy.logerr(f"Unknown camera: {camera}")
+            return
+            
+        yolo_detections = self.tensor_to_msg(results)
+        if yolo_detections:
+            self.cameras[camera]['publisher'].publish(yolo_detections)

kuavo_humanoid_sdk-1.2.1b3290/kuavo_humanoid_sdk/kuavo/core/sdk_deprecated.py

@@ -0,0 +1,41 @@
+import warnings
+from deprecated import deprecated
+
+def custom_formatwarning(message, category, filename, lineno, file=None, line=None):
+    return f"\033[33mWarning: {filename}:{lineno} - {message}\033[0m\n"
+
+warnings.formatwarning = custom_formatwarning
+
+def sdk_deprecated(reason=None, version=None, replacement=None, removed_in=None, remove_date=None):
+    """
+    SDK专用的弃用装饰器，支持函数和类
+    
+    Args:
+        reason (str): 弃用原因
+        version (str): 弃用版本
+        replacement (str): 替代函数/类名，如果为None则自动生成
+        removed_in (str): 将在哪个版本移除
+        remove_date (str): 将在哪个日期移除 (格式: YYYY-MM-DD)
+    """
+    # 构建reason字符串，包含replacement、removed_in和remove_date信息
+    if reason is None:
+        reason = ""
+    if replacement:
+        if reason:
+            reason += f"，建议使用 {replacement} 替代"
+        else:
+            reason = f"建议使用 {replacement} 替代"
+    if removed_in:
+        if reason:
+            reason += f"，将在版本 {removed_in} 移除"
+        else:
+            reason = f"将在版本 {removed_in} 移除"
+    if remove_date:
+        if reason:
+            reason += f"，预计移除日期: {remove_date}"
+        else:
+            reason = f"预计移除日期: {remove_date}"
+    
+    # 使用标准的deprecated装饰器
+    return deprecated(version=version, reason=reason)
+

{kuavo_humanoid_sdk-1.2.1b3279 → kuavo_humanoid_sdk-1.2.1b3290}/kuavo_humanoid_sdk/kuavo/robot_arm.py

@@ -5,38 +5,56 @@ import math
 from typing import Tuple
 from kuavo_humanoid_sdk.interfaces.data_types import KuavoArmCtrlMode, KuavoIKParams, KuavoPose, KuavoManipulationMpcFrame, KuavoManipulationMpcCtrlMode, KuavoManipulationMpcControlFlow
 from kuavo_humanoid_sdk.kuavo.core.core import KuavoRobotCore
+from kuavo_humanoid_sdk.kuavo.core.sdk_deprecated import sdk_deprecated
 from kuavo_humanoid_sdk.kuavo.robot_info import KuavoRobotInfo
 
+@sdk_deprecated(reason="接口废弃", version="1.2.2", replacement="KuavoRobot", remove_date="2026-06-30")
 class KuavoRobotArm:
     """Kuavo机器人手臂控制类。
     
     提供了控制机器人手臂的各种接口,包括关节位置控制、轨迹控制、末端执行器姿态控制等。
+    
+    .. warning:: 
+        此类已过期废弃，将在 2026-06-30 移除。
+        请使用 KuavoRobot 类替代。
     """
     def __init__(self):
         self._kuavo_core = KuavoRobotCore()
         self._robot_info = KuavoRobotInfo(robot_type="kuavo")
     
+    @sdk_deprecated(reason="接口废弃", version="1.2.2", replacement="KuavoRobot.arm_reset", remove_date="2026-06-30")
     def arm_reset(self)-> bool:
         """重置机器人手臂。
         
+        .. warning:: 
+            此接口已过期废弃，将在 2026-06-30 移除。
+            请使用 KuavoRobot.arm_reset() 替代。
+        
         Returns:
             bool: 重置成功返回True,否则返回False。
         """
         return self._kuavo_core.robot_arm_reset()
     
+    @sdk_deprecated(reason="接口废弃", version="1.2.2", replacement="KuavoRobot.manipulation_mpc_reset", remove_date="2026-06-30")
     def manipulation_mpc_reset(self)-> bool:
         """重置机器人 Manipulation MPC 控制器。
         
+        .. warning:: 
+            此接口已过期废弃，将在 2026-06-30 移除。
+            请使用 KuavoRobot.manipulation_mpc_reset() 替代。
+        
         Returns:
             bool: 重置成功返回True,否则返回False。
         """
         return self._kuavo_core.robot_manipulation_mpc_reset()
 
+    @sdk_deprecated(reason="接口废弃", version="1.2.2", replacement="KuavoRobot.control_arm_joint_trajectory", remove_date="2026-06-30")
     def control_arm_target_poses(self, times: list, q_frames: list) -> bool:
         """控制机器人手臂目标姿态（已废弃）。
         
-        .. deprecated:: 
-            请使用 :meth:`control_arm_joint_trajectory` 替代此函数。
+        .. warning:: 
+            此接口已过期废弃，将在 2026-06-30 移除。
+            请使用 KuavoRobot.control_arm_joint_trajectory() 替代。
         
         Args:
             times (list): 时间间隔列表，单位秒
@@ -63,9 +81,14 @@ class KuavoRobotArm:
             q_degs.append([(p * 180.0 / math.pi) for p in q])
 
         return self._kuavo_core.control_robot_arm_target_poses(times=times, joint_q=q_degs)
+    @sdk_deprecated(reason="接口废弃", version="1.2.2", replacement="KuavoRobot.control_arm_joint_positions", remove_date="2026-06-30")
     def control_arm_joint_positions(self, joint_position:list)->bool:
         """控制机器人手臂关节位置。
 
+        .. warning:: 
+            此接口已过期废弃，将在 2026-06-30 移除。
+            请使用 KuavoRobot.control_arm_joint_positions() 替代。
+
         Args:
             joint_position (list): 关节位置列表,单位为弧度
 
@@ -87,9 +110,14 @@ class KuavoRobotArm:
 
         return self._kuavo_core.control_robot_arm_joint_positions(joint_data=joint_position)
 
+    @sdk_deprecated(reason="接口废弃", version="1.2.2", replacement="KuavoRobot.control_arm_joint_trajectory", remove_date="2026-06-30")
     def control_arm_joint_trajectory(self, times:list, joint_q:list)->bool:
         """控制机器人手臂关节轨迹。
 
+        .. warning:: 
+            此接口已过期废弃，将在 2026-06-30 移除。
+            请使用 KuavoRobot.control_arm_joint_trajectory() 替代。
+
         Args:
             times (list): 时间间隔列表,单位为秒
             joint_q (list): 关节位置列表,单位为弧度
@@ -118,9 +146,14 @@ class KuavoRobotArm:
 
         return self._kuavo_core.control_robot_arm_joint_trajectory(times=times, joint_q=q_degs)
 
+    @sdk_deprecated(reason="接口废弃", version="1.2.2", replacement="KuavoRobot.control_robot_end_effector_pose", remove_date="2026-06-30")
     def control_robot_end_effector_pose(self, left_pose: KuavoPose, right_pose: KuavoPose, frame: KuavoManipulationMpcFrame)->bool:
         """控制机器人末端执行器姿态。
 
+        .. warning:: 
+            此接口已过期废弃，将在 2026-06-30 移除。
+            请使用 KuavoRobot.control_robot_end_effector_pose() 替代。
+
         Args:
             left_pose (KuavoPose): 左手臂姿态,包含xyz位置和四元数方向
             right_pose (KuavoPose): 右手臂姿态,包含xyz位置和四元数方向
@@ -131,22 +164,33 @@ class KuavoRobotArm:
         """
         return self._kuavo_core.control_robot_end_effector_pose(left_pose, right_pose, frame)
 
+    @sdk_deprecated(reason="接口废弃", version="1.2.2", replacement="KuavoRobot.set_fixed_arm_mode", remove_date="2026-06-30")
     def set_fixed_arm_mode(self) -> bool:
         """固定/冻结机器人手臂。
 
+        .. warning:: 
+            此接口已过期废弃，将在 2026-06-30 移除。
+            请使用 KuavoRobot.set_fixed_arm_mode() 替代。
+
         Returns:
             bool: 固定/冻结成功返回True,否则返回False
         """
         return self._kuavo_core.change_robot_arm_ctrl_mode(KuavoArmCtrlMode.ArmFixed)
 
+    @sdk_deprecated(reason="接口废弃", version="1.2.2", replacement="KuavoRobot.set_auto_swing_arm_mode", remove_date="2026-06-30")
     def set_auto_swing_arm_mode(self) -> bool:
         """设置手臂自动摆动模式。
 
+        .. warning:: 
+            此接口已过期废弃，将在 2026-06-30 移除。
+            请使用 KuavoRobot.set_auto_swing_arm_mode() 替代。
+
         Returns:
             bool: 设置成功返回True,否则返回False
         """
         return self._kuavo_core.change_robot_arm_ctrl_mode(KuavoArmCtrlMode.AutoSwing)
 
+    @sdk_deprecated(reason="接口废弃", version="1.2.2", replacement="KuavoRobot.arm_ik_free", remove_date="2026-06-30")
     def arm_ik_free(self,
                     left_pose: KuavoPose,
                     right_pose: KuavoPose,
@@ -154,41 +198,68 @@ class KuavoRobotArm:
                     right_elbow_pos_xyz: list = [0.0, 0.0, 0.0],
                     arm_q0: list = None,
                     params: KuavoIKParams=None) -> list:
+        """机器人手臂自由空间逆向运动学求解
+        
+        .. warning:: 
+            此接口已过期废弃，将在 2026-06-30 移除。
+            请使用 KuavoRobot.arm_ik_free() 替代。
+        """
         return self._kuavo_core.arm_ik_free(left_pose, right_pose, left_elbow_pos_xyz, right_elbow_pos_xyz, arm_q0, params)
     
+    @sdk_deprecated(reason="接口废弃", version="1.2.2", replacement="KuavoRobot.set_external_control_arm_mode", remove_date="2026-06-30")
     def set_external_control_arm_mode(self) -> bool:
         """设置手臂外部控制模式。
 
+        .. warning:: 
+            此接口已过期废弃，将在 2026-06-30 移除。
+            请使用 KuavoRobot.set_external_control_arm_mode() 替代。
+
         Returns:
             bool: 设置成功返回True,否则返回False
         """
         return self._kuavo_core.change_robot_arm_ctrl_mode(KuavoArmCtrlMode.ExternalControl)
 
+    @sdk_deprecated(reason="接口废弃", version="1.2.2", replacement="KuavoRobot.set_manipulation_mpc_mode", remove_date="2026-06-30")
     def set_manipulation_mpc_mode(self, ctrl_mode: KuavoManipulationMpcCtrlMode) -> bool:
         """设置 Manipulation MPC 控制模式。
 
+        .. warning:: 
+            此接口已过期废弃，将在 2026-06-30 移除。
+            请使用 KuavoRobot.set_manipulation_mpc_mode() 替代。
+
         Returns:
             bool: 设置成功返回True,否则返回False
         """
         return self._kuavo_core.change_manipulation_mpc_ctrl_mode(ctrl_mode)
     
+    @sdk_deprecated(reason="接口废弃", version="1.2.2", replacement="KuavoRobot.set_manipulation_mpc_control_flow", remove_date="2026-06-30")
     def set_manipulation_mpc_control_flow(self, control_flow: KuavoManipulationMpcControlFlow) -> bool:
         """设置 Manipulation MPC 控制流。
 
+        .. warning:: 
+            此接口已过期废弃，将在 2026-06-30 移除。
+            请使用 KuavoRobot.set_manipulation_mpc_control_flow() 替代。
+
         Returns:
             bool: 设置成功返回True,否则返回False
         """
         return self._kuavo_core.change_manipulation_mpc_control_flow(control_flow)
     
+    @sdk_deprecated(reason="接口废弃", version="1.2.2", replacement="KuavoRobot.set_manipulation_mpc_frame", remove_date="2026-06-30")
     def set_manipulation_mpc_frame(self, frame: KuavoManipulationMpcFrame) -> bool:
         """设置 Manipulation MPC 坐标系。
 
+        .. warning:: 
+            此接口已过期废弃，将在 2026-06-30 移除。
+            请使用 KuavoRobot.set_manipulation_mpc_frame() 替代。
+
         Returns:
             bool: 设置成功返回True,否则返回False
         """
         return self._kuavo_core.change_manipulation_mpc_frame(frame)
     
     """ 手臂正向运动学和逆向运动学 """
+    @sdk_deprecated(reason="接口废弃", version="1.2.2", replacement="KuavoRobot.arm_ik", remove_date="2026-06-30")
     def arm_ik(self, 
                left_pose: KuavoPose, 
                right_pose: KuavoPose,
@@ -198,6 +269,10 @@ class KuavoRobotArm:
                params: KuavoIKParams=None) -> list:
         """机器人手臂逆向运动学求解
         
+        .. warning:: 
+            此接口已过期废弃，将在 2026-06-30 移除。
+            请使用 KuavoRobot.arm_ik() 替代。
+        
         Args:
             left_pose (KuavoPose): 左手臂目标姿态,包含xyz位置和四元数方向
             right_pose (KuavoPose): 右手臂目标姿态,包含xyz位置和四元数方向
@@ -221,6 +296,7 @@ class KuavoRobotArm:
         """
         return self._kuavo_core.arm_ik(left_pose, right_pose, left_elbow_pos_xyz, right_elbow_pos_xyz, arm_q0, params)
 
+    @sdk_deprecated(reason="接口废弃", version="1.2.2", replacement="KuavoRobot.arm_ik_free", remove_date="2026-06-30")
     def arm_ik_free(self, 
                 left_pose: KuavoPose, 
                 right_pose: KuavoPose, 
@@ -228,11 +304,22 @@ class KuavoRobotArm:
                 right_elbow_pos_xyz: list = [0.0, 0.0, 0.0],
                 arm_q0: list = None,
                 params: KuavoIKParams=None) -> list:
+        """机器人手臂自由空间逆向运动学求解（重复定义）
+        
+        .. warning:: 
+            此接口已过期废弃，将在 2026-06-30 移除。
+            请使用 KuavoRobot.arm_ik_free() 替代。
+        """
         return self._kuavo_core.arm_ik_free(left_pose, right_pose, left_elbow_pos_xyz, right_elbow_pos_xyz, arm_q0, params)
 
+    @sdk_deprecated(reason="接口废弃", version="1.2.2", replacement="KuavoRobot.arm_fk", remove_date="2026-06-30")
     def arm_fk(self, q: list) -> Tuple[KuavoPose, KuavoPose]:
         """机器人手臂正向运动学求解
         
+        .. warning:: 
+            此接口已过期废弃，将在 2026-06-30 移除。
+            请使用 KuavoRobot.arm_fk() 替代。
+        
         Args:
             q (list): 关节位置列表,单位为弧度
             
@@ -251,9 +338,14 @@ class KuavoRobotArm:
             return None, None
         return result
 
+    @sdk_deprecated(reason="接口废弃", version="1.2.2", replacement="KuavoRobot.control_hand_wrench", remove_date="2026-06-30")
     def control_hand_wrench(self, left_wrench: list, right_wrench: list) -> bool:
         """控制机器人末端力/力矩
         
+        .. warning:: 
+            此接口已过期废弃，将在 2026-06-30 移除。
+            请使用 KuavoRobot.control_hand_wrench() 替代。
+        
         Args:
             left_wrench (list): 左手臂6维力控指令 [Fx, Fy, Fz, Tx, Ty, Tz]
             right_wrench (list): 右手臂6维力控指令 [Fx, Fy, Fz, Tx, Ty, Tz]
@@ -266,26 +358,46 @@ class KuavoRobotArm:
         """
         return self._kuavo_core.control_hand_wrench(left_wrench, right_wrench)
 
+    @sdk_deprecated(reason="接口废弃", version="1.2.2", replacement="KuavoRobot.is_arm_collision", remove_date="2026-06-30")
     def is_arm_collision(self)->bool:
         """判断当前是否发生碰撞
         
+        .. warning:: 
+            此接口已过期废弃，将在 2026-06-30 移除。
+            请使用 KuavoRobot.is_arm_collision() 替代。
+        
         Returns:
             bool: 发生碰撞返回True,否则返回False
         """
         return self._kuavo_core.is_arm_collision()
     
+    @sdk_deprecated(reason="接口废弃", version="1.2.2", replacement="KuavoRobot.release_arm_collision_mode", remove_date="2026-06-30")
     def release_arm_collision_mode(self):
         """释放碰撞模式
+        
+        .. warning:: 
+            此接口已过期废弃，将在 2026-06-30 移除。
+            请使用 KuavoRobot.release_arm_collision_mode() 替代。
         """
         self._kuavo_core.release_arm_collision_mode()
 
+    @sdk_deprecated(reason="接口废弃", version="1.2.2", replacement="KuavoRobot.wait_arm_collision_complete", remove_date="2026-06-30")
     def wait_arm_collision_complete(self):
         """等待碰撞完成
+        
+        .. warning:: 
+            此接口已过期废弃，将在 2026-06-30 移除。
+            请使用 KuavoRobot.wait_arm_collision_complete() 替代。
         """
         self._kuavo_core.wait_arm_collision_complete()
 
+    @sdk_deprecated(reason="接口废弃", version="1.2.2", replacement="KuavoRobot.set_arm_collision_mode", remove_date="2026-06-30")
     def set_arm_collision_mode(self, enable: bool):
         """设置碰撞模式
+        
+        .. warning:: 
+            此接口已过期废弃，将在 2026-06-30 移除。
+            请使用 KuavoRobot.set_arm_collision_mode() 替代。
         """
         self._kuavo_core.set_arm_collision_mode(enable)
 

{kuavo_humanoid_sdk-1.2.1b3279 → kuavo_humanoid_sdk-1.2.1b3290}/kuavo_humanoid_sdk/kuavo/robot_blockly.py

@@ -28,9 +28,8 @@ from geometry_msgs.msg import Pose, Point, Quaternion
 from std_msgs.msg import Float64MultiArray
 
 import inspect
-from model_utils import *
-import model_utils as model_utils
-
+from kuavo_humanoid_sdk.kuavo.core.model_utils.model_utils import *
+from kuavo_humanoid_sdk.kuavo.core.model_utils import model_utils as model_utils
 
 from enum import IntEnum
 # Global flag for handling Ctrl+C
@@ -45,24 +44,6 @@ else:
     INIT_ARM_POS = [22.91831, 0, 0, -45.83662, 22.91831, 0, 0, -45.83662] # task.info: shoudler_center: 0.4rad, elbow_center: -0.8rad
 
 
-
-global ACTION_FILE_FOLDER, package_path, THRESHOLD_HEAD_CONTROL_COUNT, FLOAT_MAX
-THRESHOLD_HEAD_CONTROL_COUNT = 30
-FLOAT_MAX = float('inf')
-
-package_name = 'planarmwebsocketservice'
-package_path = rospkg.RosPack().get_path(package_name)
-sudo_user = os.environ.get("SUDO_USER")
-if sudo_user:
-    user_info = pwd.getpwnam(sudo_user)
-    home_path = user_info.pw_dir
-else:
-    home_path = os.path.expanduser("~")
-ACTION_FILE_FOLDER = os.path.join(home_path, '.config', 'lejuconfig', 'action_files')
-plan_arm_state_progress = 0
-plan_arm_state_status = False
-
-
 def signal_handler(sig, frame):
     """Signal handler for catching Ctrl+C and safely stopping the robot.
 
@@ -287,16 +268,29 @@ class RobotControlBlockly:
         g_robot_type = "ocs2"
         self.loop = None
         self.init_ros_publishers()
+        self.THRESHOLD_HEAD_CONTROL_COUNT = 30
+        self.FLOAT_MAX = float('inf')
         self.to_stance()
 
+        package_name = 'planarmwebsocketservice'
+        self.package_path = rospkg.RosPack().get_path(package_name)
+        sudo_user = os.environ.get("SUDO_USER")
+        if sudo_user:
+            user_info = pwd.getpwnam(sudo_user)
+            home_path = user_info.pw_dir
+        else:
+            home_path = os.path.expanduser("~")
+        self.ACTION_FILE_FOLDER = os.path.join(home_path, '.config', 'lejuconfig', 'action_files')
+        self.plan_arm_state_progress = 0
+        self.plan_arm_state_status = False
+
     def set_action_file_path(self, proj_name: str):
         """Set the action file path for a specific project.
 
         Args:
             proj_name (str): Project name.
         """
-        global ACTION_FILE_FOLDER
-        ACTION_FILE_FOLDER = package_path + "/upload_files/" + proj_name + "/action_files"
+        self.ACTION_FILE_FOLDER = self.package_path + "/upload_files/" + proj_name + "/action_files"
 
     def init_ros_publishers(self):
         """Initialize ROS publishers and subscribers for arm, hand, head control topics, sensor data, and trajectory state."""
@@ -318,7 +312,7 @@ class RobotControlBlockly:
         Only publishes when robot type is 'ocs2' and trajectory is not finished.
         """
         global kuavo_arm_traj_pub, control_hand_pub, control_head_pub, g_robot_type
-        if g_robot_type == "ocs2" and len(ocs2_joint_state.position) > 0 and plan_arm_state_status is False:
+        if g_robot_type == "ocs2" and len(ocs2_joint_state.position) > 0 and self.plan_arm_state_status is False:
             kuavo_arm_traj_pub.publish(ocs2_joint_state)
             control_hand_pub.publish(ocs2_hand_state)
             # control_head_pub.publish(ocs2_head_state)
@@ -349,9 +343,8 @@ class RobotControlBlockly:
         Args:
             msg (planArmState): Arm trajectory state message.
         """
-        global plan_arm_state_progress, plan_arm_state_status
-        plan_arm_state_progress = msg.progress
-        plan_arm_state_status = msg.is_finished
+        self.plan_arm_state_progress = msg.progress
+        self.plan_arm_state_status = msg.is_finished
 
     def traj_callback(self, msg):
         """Callback for trajectory messages, parses trajectory points and updates global arm, hand, and head state variables.
@@ -597,14 +590,13 @@ class RobotControlBlockly:
         """
         global g_robot_type
         g_robot_type = "ocs2"
-        global plan_arm_state_status
-        plan_arm_state_status = False
+        self.plan_arm_state_status = False
 
         action_filename = action_file + ".tact"
         if proj_name is None:
-            action_file_path = os.path.expanduser(f"{ACTION_FILE_FOLDER}/{action_filename}")
+            action_file_path = os.path.expanduser(f"{self.ACTION_FILE_FOLDER}/{action_filename}")
         else:
-            action_file_path = package_path + "/upload_files/" + proj_name + "/action_files/" + action_filename
+            action_file_path = self.package_path + "/upload_files/" + proj_name + "/action_files/" + action_filename
         if not os.path.exists(action_file_path):
             print(f"Action file not found: {action_file_path}")
             return
@@ -623,7 +615,7 @@ class RobotControlBlockly:
             self.play_music(music_file)
         self.plan_arm_trajectory_bezier_curve_client(req)
         time.sleep(0.5)
-        while plan_arm_state_status is False:
+        while self.plan_arm_state_status is False:
             time.sleep(0.01)
         print("action file executed")
 
@@ -635,13 +627,12 @@ class RobotControlBlockly:
             pitch (float): Pitch angle in degrees.
         """
         try:
-            global THRESHOLD_HEAD_CONTROL_COUNT, FLOAT_MAX
             self.robot.stance()
             yaw = math.radians(yaw)
             pitch = math.radians(pitch)
             self.robot.control_head(yaw, pitch)
-            diff_yaw_min = FLOAT_MAX
-            diff_pitch_min = FLOAT_MAX
+            diff_yaw_min = self.FLOAT_MAX
+            diff_pitch_min = self.FLOAT_MAX
             times_count = 0
             while True:
                 time.sleep(0.01)
@@ -657,7 +648,7 @@ class RobotControlBlockly:
                     diff_pitch_min = diff_pitch
                     continue
                 times_count += 1
-                if times_count > THRESHOLD_HEAD_CONTROL_COUNT:
+                if times_count > self.THRESHOLD_HEAD_CONTROL_COUNT:
                     break
 
         except Exception as e:
@@ -670,12 +661,11 @@ class RobotControlBlockly:
             yaw (float): Yaw angle in degrees.
         """
         try:
-            global THRESHOLD_HEAD_CONTROL_COUNT, FLOAT_MAX
             self.robot.stance()
             yaw = math.radians(yaw)
             self.robot.control_head(yaw, current_head_joint_state[1])
             times_count = 0
-            diff_yaw_min = FLOAT_MAX
+            diff_yaw_min = self.FLOAT_MAX
             while True:
                 time.sleep(0.01)
                 diff_yaw = abs(current_head_joint_state[0] - yaw)
@@ -684,7 +674,7 @@ class RobotControlBlockly:
                     diff_yaw_min = diff_yaw
                     continue
                 times_count += 1
-                if times_count > THRESHOLD_HEAD_CONTROL_COUNT:
+                if times_count > self.THRESHOLD_HEAD_CONTROL_COUNT:
                     break
         except Exception as e:
             print(f"Robot head control failed: {str(e)}")
@@ -696,12 +686,11 @@ class RobotControlBlockly:
             pitch (float): Pitch angle in degrees.
         """
         try:
-            global THRESHOLD_HEAD_CONTROL_COUNT, FLOAT_MAX
             self.robot.stance()
             pitch = math.radians(pitch)
             self.robot.control_head(current_head_joint_state[0], pitch)
             times_count = 0
-            diff_pitch_min = FLOAT_MAX
+            diff_pitch_min = self.FLOAT_MAX
             while True:
                 time.sleep(0.01)
                 diff_pitch = abs(current_head_joint_state[1] - pitch)
@@ -710,7 +699,7 @@ class RobotControlBlockly:
                     diff_pitch_min = diff_pitch
                     continue
                 times_count += 1
-                if times_count > THRESHOLD_HEAD_CONTROL_COUNT:
+                if times_count > self.THRESHOLD_HEAD_CONTROL_COUNT:
                     break
         except Exception as e:
             print(f"Robot head control failed: {str(e)}")
@@ -1086,14 +1075,18 @@ class RobotControlBlockly:
             if  not is_yolo_init:
                 yolo_detection = YOLO_detection()
                 yolo_detection.init_ros_node()
-                caller_file_path = inspect.stack()[1].filename
+                caller_file_path = self.package_path + "/upload_files/"
+                print(f"caller_file_path: {caller_file_path}")
                 model_path = os.path.join(os.path.dirname(caller_file_path), 'best.pt')
                 # print(f"函数被文件调用: {model_path}")
                 model = yolo_detection.load_model(model_path)
+                result = yolo_detection.camera_interface.get_camera_image("head")
+                if result is None:
+                    print("No head camera image...")
+                    return
 
-
-            IMAGE_CENTER_X = yolo_detection.cv_image_shape[1] / 2.0
-            IMAGE_CENTER_Y = yolo_detection.cv_image_shape[0] / 2.0
+            IMAGE_CENTER_X = yolo_detection.camera_interface.cv_image_shape[1] / 2.0
+            IMAGE_CENTER_Y = yolo_detection.camera_interface.cv_image_shape[0] / 2.0
             # rospy.loginfo(f"IMAGE_CENTER_X: {IMAGE_CENTER_X}")
             # rospy.loginfo(f"IMAGE_CENTER_Y: {IMAGE_CENTER_Y}")
             MOVE_SPEED_Y = 0.05  # Y方向移动速度
@@ -1102,7 +1095,6 @@ class RobotControlBlockly:
             while not rospy.is_shutdown():
 
                 results = yolo_detection.get_detections("head", model)
-
                 if not results:
                     continue