kuavo-humanoid-sdk-ws 1.1.6b858__tar.gz → 1.1.6b1125__tar.gz

{kuavo_humanoid_sdk_ws-1.1.6b858 → kuavo_humanoid_sdk_ws-1.1.6b1125}/PKG-INFO

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

{kuavo_humanoid_sdk_ws-1.1.6b858 → kuavo_humanoid_sdk_ws-1.1.6b1125}/kuavo_humanoid_sdk/interfaces/data_types.py

@@ -269,3 +269,25 @@ class KuavoTwist:
     """
     linear: Tuple[float, float, float]
     angular: Tuple[float, float, float]
+
+
+
+@dataclass
+class AprilTagDetection:
+    """表示AprilTag检测结果的数据类"""
+    
+    @dataclass
+    class Point:
+        x: float
+        y: float
+        z: float
+
+    @dataclass
+    class Quaternion:
+        x: float
+        y: float
+        z: float
+        w: float
+
+    position: Point
+    orientation: Quaternion

{kuavo_humanoid_sdk_ws-1.1.6b858 → kuavo_humanoid_sdk_ws-1.1.6b1125}/kuavo_humanoid_sdk/kuavo/__init__.py

@@ -1,8 +1,8 @@
 from .robot import KuavoSDK, KuavoRobot  
 from .robot_info import KuavoRobotInfo
 from .robot_state import KuavoRobotState
-# from .robot_vision import KuavoRobotVision
-# from .robot_tool import KuavoRobotTools
+from .robot_vision import KuavoRobotVision
+from .robot_tool import KuavoRobotTools
 from .robot_arm import KuavoRobotArm
 from .robot_head import KuavoRobotHead
 from .robot_audio import KuavoRobotAudio

{kuavo_humanoid_sdk_ws-1.1.6b858 → kuavo_humanoid_sdk_ws-1.1.6b1125}/kuavo_humanoid_sdk/kuavo/core/audio.py

@@ -12,7 +12,7 @@ class KuavoRobotAudioCore:
     def __init__(self):
         self.robot_audio = AudioWebsocket()
 
-    def play_audio(self, music_number: str, volume: float = 0.5, speed: float = 1.0) -> bool:
+    def play_audio(self, music_number: str, volume: int = 100, speed: float = 1.0) -> bool:
         """
         play music
         """

{kuavo_humanoid_sdk_ws-1.1.6b858 → kuavo_humanoid_sdk_ws-1.1.6b1125}/kuavo_humanoid_sdk/kuavo/core/ros/audio.py

@@ -16,7 +16,7 @@ class AudioWebsocket:
         self._audio_stop_publisher = roslibpy.Topic(websocket.client, 'stop_music', 'std_msgs/Bool')
         self._audio_stop_publisher.advertise()
 
-    def play_audio(self, file_name: str, volume: float = 0.5, speed: float = 1.0) -> bool:
+    def play_audio(self, file_name: str, volume: int = 100, speed: float = 1.0) -> bool:
         """Play the specified audio file through WebSocket.
         
         Args:
@@ -31,11 +31,10 @@ class AudioWebsocket:
             websocket = WebSocketKuavoSDK()
             service = roslibpy.Service(websocket.client, 'play_music', 'kuavo_msgs/playmusic')
             
-            volume = min(max(volume, 0), 1.0)
+            volume = min(max(volume, 0), 100)
             request = {
                 "music_number": file_name,
-                "volume": volume,
-                "speed": speed
+                "volume": volume
             }
             
             response = service.call(request)

{kuavo_humanoid_sdk_ws-1.1.6b858 → kuavo_humanoid_sdk_ws-1.1.6b1125}/kuavo_humanoid_sdk/kuavo/core/ros/state.py

@@ -133,14 +133,14 @@ class KuavoRobotStateCoreWebsocket:
                     angular = (0.0, 0.0, 0.0)
                 )
                 self._eef_state = (EndEffectorState(
-                    position = [0.0] * 12 if kuavo_info['end_effector_type'].startswith('qiangnao') else [0.0] * 2,
-                    velocity = [0.0] * 12 if kuavo_info['end_effector_type'].startswith('qiangnao') else [0.0] * 2,
-                    effort = [0.0] * 12 if kuavo_info['end_effector_type'].startswith('qiangnao') else [0.0] * 2,
+                    position = [0.0] * 12 if kuavo_info['end_effector_type'] is not None and kuavo_info['end_effector_type'].startswith('qiangnao') else [0.0] * 2,
+                    velocity = [0.0] * 12 if kuavo_info['end_effector_type'] is not None and  kuavo_info['end_effector_type'].startswith('qiangnao') else [0.0] * 2,
+                    effort = [0.0] * 12 if kuavo_info['end_effector_type'] is not None and  kuavo_info['end_effector_type'].startswith('qiangnao') else [0.0] * 2,
                     state=EndEffectorState.GraspingState.UNKNOWN
                 ), EndEffectorState(
-                    position = [0.0] * 12 if kuavo_info['end_effector_type'].startswith('qiangnao') else [0.0] * 2,
-                    velocity = [0.0] * 12 if kuavo_info['end_effector_type'].startswith('qiangnao') else [0.0] * 2,
-                    effort = [0.0] * 12 if kuavo_info['end_effector_type'].startswith('qiangnao') else [0.0] * 2,
+                    position = [0.0] * 12 if kuavo_info['end_effector_type'] is not None and  kuavo_info['end_effector_type'].startswith('qiangnao') else [0.0] * 2,
+                    velocity = [0.0] * 12 if kuavo_info['end_effector_type'] is not None and  kuavo_info['end_effector_type'].startswith('qiangnao') else [0.0] * 2,
+                    effort = [0.0] * 12 if kuavo_info['end_effector_type'] is not None and  kuavo_info['end_effector_type'].startswith('qiangnao') else [0.0] * 2,
                     state=EndEffectorState.GraspingState.UNKNOWN
                 ))
                                 

kuavo_humanoid_sdk_ws-1.1.6b1125/kuavo_humanoid_sdk/kuavo/core/ros/tools.py

@@ -0,0 +1,196 @@
+#! /usr/bin/env python3
+# coding: utf-8
+
+import copy
+import time
+import numpy as np
+from typing import Tuple, Union
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+from transforms3d import euler, quaternions
+import roslibpy
+from kuavo_humanoid_sdk.common.websocket_kuavo_sdk import WebSocketKuavoSDK
+from kuavo_humanoid_sdk.interfaces.data_types import (PoseQuaternion, HomogeneousMatrix)
+from kuavo_humanoid_sdk.kuavo.core.ros.param import make_robot_param, EndEffectorType
+
+
+class KuavoRobotToolsCoreWebsocket:
+    """Provides core ROS tools for Kuavo humanoid robot transformations.
+    
+    Attributes:
+        tf_service (roslibpy.Service): Service proxy for tf2_web_republisher
+        _transform_cache (dict): Cache for storing recent transforms
+    """
+    
+    def __init__(self):
+        """Initializes TF2 web republisher service proxy."""
+        if not hasattr(self, '_initialized'):
+            try:
+                # Initialize WebSocket connection
+                self.websocket = WebSocketKuavoSDK()
+                
+                # Initialize TF2 web republisher service
+                self.tf_service = roslibpy.Service(
+                    self.websocket.client,
+                    '/republish_tfs',
+                    'kuavo_msgs/RepublishTFs'
+                )
+                self._transform_cache = {}
+                self._initialized = True
+            except Exception as e:
+                SDKLogger.error(f"Failed to initialize tf2_web_republisher: {str(e)}")
+                SDKLogger.error("Please make sure tf2_web_republisher node is running")
+                raise
+
+    def _get_tf_tree_transform(self, target_frame: str, source_frame: str, 
+                     time_=0.0, timeout=5.0,
+                     return_type: str = "pose_quaternion") -> Union[PoseQuaternion, HomogeneousMatrix, None]:
+        """Gets transform between coordinate frames using tf2_web_republisher.
+        
+        Args:
+            target_frame (str): Target coordinate frame name
+            source_frame (str): Source coordinate frame name
+            time (float, optional): Time of transform. Defaults to 0.0.
+            timeout (float, optional): Wait timeout in seconds. Defaults to 5.0.
+            return_type (str, optional): Return data format. Options: 
+                "pose_quaternion", "homogeneous". Defaults to "pose_quaternion".
+        
+        Returns:
+            Union[PoseQuaternion, HomogeneousMatrix, None]: Requested transform data
+                or None if failed
+        """
+        try:
+                       
+            # 调用服务
+            request = {
+                'source_frames': [source_frame],
+                'target_frame': target_frame,
+                'angular_thres': 0.01,
+                'trans_thres': 0.01,
+                'rate': 10.0,
+                'timeout': {'secs': int(timeout), 'nsecs': int((timeout % 1) * 1e9)}
+            }
+            
+            response = self.tf_service.call(request)
+            if response['status'] == -1:
+                SDKLogger.error(f"{source_frame} or {target_frame} not exist")
+                return None
+            
+            # 检查话题是否发布
+            topic_list = self.websocket.client.get_topics()
+            if response['topic_name'] not in topic_list:
+                SDKLogger.error(f"Topic {response['topic_name']} not published")
+                return None
+                
+            # 创建订阅者
+            transform_received = False
+            transform_data = None
+            
+            def transform_callback(msg):
+                nonlocal transform_received, transform_data
+                transform_received = True
+                transform_data = msg
+                
+            sub = roslibpy.Topic(self.websocket.client, response['topic_name'], 'kuavo_msgs/TFArray')
+            sub.subscribe(transform_callback)
+            
+            # 等待接收数据
+            start_time = time.time()
+            while not transform_received and (time.time() - start_time) < timeout:
+                time.sleep(0.1)
+                
+            # 取消订阅
+            sub.unsubscribe()
+            
+            if not transform_received:
+                SDKLogger.error("No transform data received")
+                return None
+                
+            # 从TFArray中获取对应的变换
+            for tf_msg in transform_data['transforms']:
+                if tf_msg['header']['frame_id'] == target_frame and tf_msg['child_frame_id'] == source_frame:
+                    return self._parse_transform(tf_msg['transform'], return_type)
+                    
+            SDKLogger.error(f"No matching transform found in TFArray")
+            return None
+            
+        except Exception as e:
+            SDKLogger.error(f"Transform error: {str(e)}")
+            return None
+
+    def _parse_transform(self, transform, return_type: str) -> Union[PoseQuaternion, HomogeneousMatrix, None]:
+        """Parses transform data to specified format.
+        
+        Args:
+            transform (dict): Input transform data
+            return_type (str): Output format type. Valid values: 
+                "pose_quaternion", "homogeneous"
+        
+        Returns:
+            Union[PoseQuaternion, HomogeneousMatrix, None]: Parsed transform data
+                in requested format, or None if invalid input
+        """
+        if return_type == "pose_quaternion":
+            return PoseQuaternion(
+                position=(transform['translation']['x'], 
+                         transform['translation']['y'],
+                         transform['translation']['z']),
+                orientation=(transform['rotation']['x'],
+                            transform['rotation']['y'],
+                            transform['rotation']['z'],
+                            transform['rotation']['w'])
+            )
+        elif return_type == "homogeneous":
+            return HomogeneousMatrix(
+                matrix=self._transform_to_homogeneous(transform)
+            )
+        else:
+            SDKLogger.warn(f"Invalid return_type: {return_type}, using default(pose_quaternion)")
+            return self._parse_transform(transform, "pose_quaternion")
+
+    def _transform_to_homogeneous(self, transform) -> np.ndarray:
+        """Converts transform dict to homogeneous matrix.
+        
+        Args:
+            transform (dict): Input transform message
+        
+        Returns:
+            np.ndarray: 4x4 homogeneous transformation matrix (numpy.float32)
+        """
+        # 四元数转旋转矩阵
+        rotation = [
+            transform['rotation']['x'],
+            transform['rotation']['y'],
+            transform['rotation']['z'],
+            transform['rotation']['w']
+        ]
+        
+        # Convert quaternion to rotation matrix using transforms3d
+        rot_matrix = np.eye(4)
+        rot_matrix[:3,:3] = quaternions.quat2mat([rotation[3], rotation[0], rotation[1], rotation[2]])
+
+        # 设置平移分量
+        translation = [
+            transform['translation']['x'],
+            transform['translation']['y'],
+            transform['translation']['z']
+        ]
+        rot_matrix[:3, 3] = translation
+        
+        return rot_matrix.astype(np.float32)  # 确保矩阵数据类型一致
+
+# if __name__ == "__main__":
+#     robot_tools = KuavoRobotToolsCore()
+#     time.sleep(0.1)
+#     # 获取位姿信息
+#     pose = robot_tools._get_tf_tree_transform("odom", "base_link", return_type="pose_quaternion")
+#     print(f"Position: {pose.position}")
+#     print(f"Orientation: {pose.orientation}")
+
+#     # 获取齐次矩阵
+#     homogeneous = robot_tools._get_tf_tree_transform("odom", "base_link", return_type="homogeneous")
+#     print(f"Transformation matrix:\n{homogeneous.matrix}")
+
+#     # 矩阵运算示例
+#     transform_matrix = homogeneous.matrix
+#     inverse_matrix = np.linalg.inv(transform_matrix)  # 求逆变换
+#     print(f"Inverse matrix:\n{inverse_matrix}")

kuavo_humanoid_sdk_ws-1.1.6b1125/kuavo_humanoid_sdk/kuavo/core/ros/vision.py

@@ -0,0 +1,280 @@
+#! /usr/bin/env python3
+# coding: utf-8
+
+import copy
+import time
+import numpy as np
+from collections import deque
+from typing import Tuple, Optional
+from transforms3d import quaternions, euler
+
+from kuavo_humanoid_sdk.kuavo.core.ros.param import make_robot_param, EndEffectorType
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+from kuavo_humanoid_sdk.interfaces.data_types import (AprilTagData, AprilTagDetection)
+import roslibpy
+from kuavo_humanoid_sdk.common.websocket_kuavo_sdk import WebSocketKuavoSDK
+
+class KuavoRobotVisionCoreWebsocket:
+    """Handles vision-related data processing for Kuavo humanoid robot.
+    
+    Attributes:
+        tf_client (roslibpy.Topic): TF client for coordinate transformations
+        tf_publisher (roslibpy.Topic): TF publisher for broadcasting transforms
+    """
+    
+    def __init__(self):
+        """Initializes vision system components including TF and AprilTag subscribers."""
+        if not hasattr(self, '_initialized'):
+            # Initialize WebSocket connection
+            self.websocket = WebSocketKuavoSDK()
+            
+            # Initialize AprilTag subscribers
+            self._apriltag_data_camera_sub = roslibpy.Topic(
+                self.websocket.client,
+                '/tag_detections',
+                'apriltag_ros/AprilTagDetectionArray'
+            )
+            self._apriltag_data_camera_sub.subscribe(self._apriltag_data_callback_camera)
+            
+            self._apriltag_data_base_sub = roslibpy.Topic(
+                self.websocket.client,
+                '/robot_tag_info',
+                'apriltag_ros/AprilTagDetectionArray'
+            )
+            self._apriltag_data_base_sub.subscribe(self._apriltag_data_callback_base)
+
+            self._apriltag_data_odom_sub = roslibpy.Topic(
+                self.websocket.client,
+                '/robot_tag_info_odom',
+                'apriltag_ros/AprilTagDetectionArray'
+            )
+            self._apriltag_data_odom_sub.subscribe(self._apriltag_data_callback_odom)
+
+            # Initialize data structures
+            self._apriltag_data_from_camera = AprilTagData(
+                id = [],
+                size = [],
+                pose = []
+            )
+
+            self._apriltag_data_from_base = AprilTagData(
+                id = [],
+                size = [],
+                pose = []
+            )
+
+            self._apriltag_data_from_odom = AprilTagData(
+                id = [],
+                size = [],
+                pose = []
+            )
+            
+            self._initialized = True
+
+    def _tf_callback(self, msg):
+        """Callback for TF messages.
+        
+        Args:
+            msg: TF message containing transforms
+        """
+        for transform in msg['transforms']:
+            key = (transform['header']['frame_id'], transform['child_frame_id'])
+            self._transforms[key] = transform
+
+    def _apriltag_data_callback_camera(self, data):
+        """Callback for processing AprilTag detections from camera.
+        
+        Args:
+            data (dict): Raw detection data from camera
+        """
+        # 清空之前的数据
+        self._apriltag_data_from_camera.id = []
+        self._apriltag_data_from_camera.size = []
+        self._apriltag_data_from_camera.pose = []
+        
+        # 处理每个标签检测
+        for detection in data['detections']:
+            # 添加ID
+            for id in detection['id']:
+                self._apriltag_data_from_camera.id.append(id)
+            
+            # 添加尺寸
+            if detection.get('size') and len(detection['size']) > 0:
+                self._apriltag_data_from_camera.size.append(detection['size'][0])
+            else:
+                self._apriltag_data_from_camera.size.append(0.0)
+            
+            
+            # Convert pose dict to AprilTagDetection
+            pose_dict = detection['pose']['pose']['pose']
+
+            tag_detection = AprilTagDetection(
+                position=AprilTagDetection.Point(
+                    x=float(pose_dict['position']['x']), 
+                    y=float(pose_dict['position']['y']), 
+                    z=float(pose_dict['position']['z'])
+                    ), 
+                orientation=AprilTagDetection.Quaternion(
+                    x=float(pose_dict['orientation']['x']), 
+                    y=float(pose_dict['orientation']['y']), 
+                    z=float(pose_dict['orientation']['z']), 
+                    w=float(pose_dict['orientation']['w'])
+                ))
+            # 添加姿态
+            self._apriltag_data_from_camera.pose.append(tag_detection)
+            
+    def _apriltag_data_callback_base(self, data):
+        """Callback for processing AprilTag detections from base link.
+        
+        Args:
+            data (dict): Raw detection data from base frame
+        """
+        # 清空之前的数据
+        self._apriltag_data_from_base.id = []
+        self._apriltag_data_from_base.size = []
+        self._apriltag_data_from_base.pose = []
+        
+        # 处理每个标签检测
+        for detection in data['detections']:
+            # 添加ID
+            for id in detection['id']:
+                self._apriltag_data_from_base.id.append(id)
+            
+            # 添加尺寸
+            if detection.get('size') and len(detection['size']) > 0:
+                self._apriltag_data_from_base.size.append(detection['size'][0])
+            else:
+                self._apriltag_data_from_base.size.append(0.0)
+            
+                        # Convert pose dict to AprilTagDetection
+            pose_dict = detection['pose']['pose']['pose']
+            
+            tag_detection = AprilTagDetection(
+                position=AprilTagDetection.Point(
+                    x=float(pose_dict['position']['x']), 
+                    y=float(pose_dict['position']['y']), 
+                    z=float(pose_dict['position']['z'])
+                    ), 
+                orientation=AprilTagDetection.Quaternion(
+                    x=float(pose_dict['orientation']['x']), 
+                    y=float(pose_dict['orientation']['y']), 
+                    z=float(pose_dict['orientation']['z']), 
+                    w=float(pose_dict['orientation']['w'])
+                ))
+            
+            # 添加姿态
+            self._apriltag_data_from_base.pose.append(tag_detection)
+ 
+    def _apriltag_data_callback_odom(self, data):
+        """Callback for processing AprilTag detections from odom frame.
+        
+        Args:
+            data (dict): Raw detection data from odom frame
+        """
+        # 清空之前的数据
+        self._apriltag_data_from_odom.id = []
+        self._apriltag_data_from_odom.size = []
+        self._apriltag_data_from_odom.pose = []
+        
+        # 处理每个标签检测
+        for detection in data['detections']:
+            # 添加ID
+            for id in detection['id']:
+                self._apriltag_data_from_odom.id.append(id)
+            
+            # 添加尺寸
+            if detection.get('size') and len(detection['size']) > 0:
+                self._apriltag_data_from_odom.size.append(detection['size'][0])
+            else:
+                self._apriltag_data_from_odom.size.append(0.0)
+            
+            # Convert pose dict to AprilTagDetection
+            pose_dict = detection['pose']['pose']['pose']
+            
+            tag_detection = AprilTagDetection(
+                position=AprilTagDetection.Point(
+                    x=float(pose_dict['position']['x']), 
+                    y=float(pose_dict['position']['y']), 
+                    z=float(pose_dict['position']['z'])
+                    ), 
+                orientation=AprilTagDetection.Quaternion(
+                    x=float(pose_dict['orientation']['x']), 
+                    y=float(pose_dict['orientation']['y']), 
+                    z=float(pose_dict['orientation']['z']), 
+                    w=float(pose_dict['orientation']['w'])
+                ))
+            
+            # 添加姿态
+            self._apriltag_data_from_odom.pose.append(tag_detection)
+
+    @property 
+    def apriltag_data_from_camera(self) -> AprilTagData:
+        """AprilTag detection data in camera coordinate frame.
+        
+        Returns:
+            AprilTagData: Contains lists of tag IDs, sizes and poses
+        """
+        return self._apriltag_data_from_camera
+    
+    @property
+    def apriltag_data_from_base(self) -> AprilTagData:
+        """AprilTag detection data in base_link coordinate frame.
+        
+        Returns:
+            AprilTagData: Contains lists of tag IDs, sizes and poses
+        """
+        return self._apriltag_data_from_base
+    
+    @property
+    def apriltag_data_from_odom(self) -> AprilTagData:
+        """AprilTag detection data in odom coordinate frame.
+        
+        Returns:
+            AprilTagData: Contains lists of tag IDs, sizes and transformed poses
+        """
+        return self._apriltag_data_from_odom
+
+    def _get_data_by_id(self, target_id: int, data_source: str = "base") -> Optional[dict]:
+        """Retrieves AprilTag data by specific ID from selected source.
+        
+        Args:
+            target_id (int): AprilTag ID to search for
+            data_source (str): Data source selector, valid options: 
+                "camera", "base", "odom"
+        
+        Returns:
+            Optional[dict]: Dictionary containing 'sizes' and 'poses' lists if found,
+                None if no matching data
+        """
+        data_map = {
+            "camera": self._apriltag_data_from_camera,
+            "base": self._apriltag_data_from_base,
+            "odom": self._apriltag_data_from_odom
+        }
+        
+        if data_source not in data_map:
+            raise ValueError(f"Invalid data source: {data_source}")
+            
+        data = data_map[data_source]
+        
+        # 查找匹配的ID
+        try:
+            idx = data.id.index(target_id)
+            return {
+                'sizes': [data.size[idx]],
+                'poses': [data.pose[idx]]
+            }
+        except ValueError:
+            return None
+
+# if __name__ == "__main__":
+
+#     kuavo_robot_vision_core = KuavoRobotVisionCoreWebsocket()
+#     time.sleep(5)
+#     print("apriltag_data_from_camera:")
+#     print(kuavo_robot_vision_core.apriltag_data_from_camera)
+#     print("apriltag_data_from_base:")
+#     print(kuavo_robot_vision_core.apriltag_data_from_base)
+#     print("apriltag_data_from_odom:")
+#     print(kuavo_robot_vision_core.apriltag_data_from_odom)
+#     rospy.spin()

{kuavo_humanoid_sdk_ws-1.1.6b858 → kuavo_humanoid_sdk_ws-1.1.6b1125}/kuavo_humanoid_sdk/kuavo/robot_audio.py

@@ -12,7 +12,7 @@ class KuavoRobotAudio:
         """Initialize the audio system."""
         self.audio = KuavoRobotAudioCore()
 
-    def play_audio(self, file_name: str, volume: float = 0.5, speed: float = 1.0) -> bool:
+    def play_audio(self, file_name: str, volume: int = 100, speed: float = 1.0) -> bool:
         """Play the specified audio file.
         
         Args:

kuavo_humanoid_sdk_ws-1.1.6b1125/kuavo_humanoid_sdk/kuavo/robot_tool.py

@@ -0,0 +1,58 @@
+#!/usr/bin/env python3
+# coding: utf-8
+import math
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+from kuavo_humanoid_sdk.interfaces.data_types import (PoseQuaternion, HomogeneousMatrix)
+from kuavo_humanoid_sdk.kuavo.core.ros.tools import KuavoRobotToolsCoreWebsocket
+from typing import Union
+
+class KuavoRobotTools:
+    """机器人工具类,提供坐标系转换接口。
+    
+    该类封装了不同机器人坐标系之间的坐标变换查询功能,支持多种返回数据格式。
+    """
+    
+    def __init__(self):
+        self.tools_core = KuavoRobotToolsCoreWebsocket()
+
+    def get_tf_transform(self, target_frame: str, source_frame: str, 
+                       return_type: str = "pose_quaternion") -> Union[PoseQuaternion, HomogeneousMatrix, None]:
+        """获取指定坐标系之间的变换。
+        
+        Args:
+            target_frame (str): 目标坐标系名称
+            source_frame (str): 源坐标系名称
+            return_type (str, optional): 返回数据格式类型。有效值: \n
+                - "pose_quaternion" : 四元数姿态格式,  \n
+                - "homogeneous" : 齐次矩阵格式。默认为"pose_quaternion"。\n
+        
+        Returns:
+            Union[PoseQuaternion, HomogeneousMatrix, None]: 
+                指定格式的变换数据,如果失败则返回None
+        
+        Raises:
+            ValueError: 如果提供了无效的 return_type
+        """
+        return self.tools_core._get_tf_tree_transform(target_frame, source_frame, return_type=return_type)
+
+    def get_base_to_odom(self, return_type: str = "pose_quaternion") -> Union[PoseQuaternion, HomogeneousMatrix, None]:
+        """获取从base_link到odom坐标系的变换。
+        
+        Args:
+            return_type (str, optional): 返回格式类型。与get_tf_transform相同，默认为"pose_quaternion"。
+        
+        Returns:
+            Union[PoseQuaternion, HomogeneousMatrix, None]: 变换数据或 None
+        """
+        return self.get_tf_transform("odom", "base_link", return_type)
+
+    def get_camera_to_base(self, return_type: str = "homogeneous") -> Union[PoseQuaternion, HomogeneousMatrix, None]:
+        """获取从camera_link到base_link坐标系的变换。
+        
+        Args:
+            return_type (str, optional): 返回格式类型。与get_tf_transform相同，默认为"homogeneous"。
+        
+        Returns:
+            Union[PoseQuaternion, HomogeneousMatrix, None]: 变换数据或None
+        """
+        return self.get_tf_transform("base_link", "camera_link", return_type)

kuavo_humanoid_sdk_ws-1.1.6b1125/kuavo_humanoid_sdk/kuavo/robot_vision.py

@@ -0,0 +1,82 @@
+#!/usr/bin/env python3
+# coding: utf-8
+import math
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+from kuavo_humanoid_sdk.interfaces.data_types import (AprilTagData)
+from kuavo_humanoid_sdk.kuavo.core.ros.vision import KuavoRobotVisionCoreWebsocket
+
+
+class KuavoRobotVision:
+    """Kuavo机器人视觉系统接口。
+    
+    提供从不同坐标系获取AprilTag检测数据的接口。
+    """
+    
+    def __init__(self, robot_type: str = "kuavo"):
+        """初始化视觉系统。
+        
+        Args:
+            robot_type (str, optional): 机器人类型标识符。默认为"kuavo"
+        """
+        if not hasattr(self, '_initialized'):
+            self._vision_core = KuavoRobotVisionCoreWebsocket()
+
+    def get_data_by_id(self, target_id: int, data_source: str = "base") -> dict:
+        """获取指定ID的AprilTag检测数据。
+        
+        Args:
+            target_id (int): 要检索的AprilTag ID
+            data_source (str, optional): 数据源坐标系。可以是"base"、"camera"或"odom"。默认为"base"。
+                
+        Returns:
+            dict: 包含位置、方向和元数据的检测数据
+        """
+        return self._vision_core._get_data_by_id(target_id, data_source)
+    
+    def get_data_by_id_from_camera(self, target_id: int) -> dict:
+        """从相机坐标系获取AprilTag数据。
+        
+        Args:
+            target_id (int): 要检索的AprilTag ID
+            
+        Returns:
+            dict: 包含位置、方向和元数据的检测数据。参见 :meth:`get_data_by_id` 的返回格式说明。
+        """
+        return self._vision_core._get_data_by_id(target_id, "camera")
+    
+    def get_data_by_id_from_base(self, target_id: int) -> dict:
+        """从基座坐标系获取AprilTag数据。
+        
+        Args:
+            target_id (int): 要检索的AprilTag ID
+            
+        Returns:
+            dict: 包含位置、方向和元数据的检测数据。参见 :meth:`get_data_by_id` 的返回格式说明。
+        """
+        return self._vision_core._get_data_by_id(target_id, "base")
+    
+    def get_data_by_id_from_odom(self, target_id: int) -> dict:
+        """从里程计坐标系获取AprilTag数据。
+        
+        Args:
+            target_id (int): 要检索的AprilTag ID
+            
+        Returns:
+            dict: 包含位置、方向和元数据的检测数据。参见 :meth:`get_data_by_id` 的返回格式说明。
+        """
+        return self._vision_core._get_data_by_id(target_id, "odom")
+    
+    @property
+    def apriltag_data_from_camera(self) -> AprilTagData:
+        """AprilTagData: 相机坐标系中检测到的所有AprilTag（属性）"""
+        return self._vision_core.apriltag_data_from_camera
+    
+    @property
+    def apriltag_data_from_base(self) -> AprilTagData:
+        """AprilTagData: 基座坐标系中检测到的所有AprilTag（属性）"""
+        return self._vision_core.apriltag_data_from_base
+    
+    @property
+    def apriltag_data_from_odom(self) -> AprilTagData:
+        """AprilTagData: 里程计坐标系中检测到的所有AprilTag（属性）"""
+        return self._vision_core.apriltag_data_from_odom

{kuavo_humanoid_sdk_ws-1.1.6b858 → kuavo_humanoid_sdk_ws-1.1.6b1125}/kuavo_humanoid_sdk_ws.egg-info/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: kuavo-humanoid-sdk-ws
-Version: 1.1.6b858
+Version: 1.1.6b1125
 Summary: A Python SDK for kuavo humanoid robot.
 Home-page: https://gitee.com/leju-robot/kuavo-ros-opensource/
 Author: ['lejurobot']

{kuavo_humanoid_sdk_ws-1.1.6b858 → kuavo_humanoid_sdk_ws-1.1.6b1125}/kuavo_humanoid_sdk_ws.egg-info/SOURCES.txt

@@ -18,6 +18,8 @@ kuavo_humanoid_sdk/kuavo/robot_head.py
 kuavo_humanoid_sdk/kuavo/robot_info.py
 kuavo_humanoid_sdk/kuavo/robot_observation.py
 kuavo_humanoid_sdk/kuavo/robot_state.py
+kuavo_humanoid_sdk/kuavo/robot_tool.py
+kuavo_humanoid_sdk/kuavo/robot_vision.py
 kuavo_humanoid_sdk/kuavo/core/audio.py
 kuavo_humanoid_sdk/kuavo/core/core.py
 kuavo_humanoid_sdk/kuavo/core/dex_hand_control.py
@@ -29,6 +31,8 @@ kuavo_humanoid_sdk/kuavo/core/ros/observation.py
 kuavo_humanoid_sdk/kuavo/core/ros/param.py
 kuavo_humanoid_sdk/kuavo/core/ros/sat_utils.py
 kuavo_humanoid_sdk/kuavo/core/ros/state.py
+kuavo_humanoid_sdk/kuavo/core/ros/tools.py
+kuavo_humanoid_sdk/kuavo/core/ros/vision.py
 kuavo_humanoid_sdk/kuavo_strategy/__init__.py
 kuavo_humanoid_sdk/kuavo_strategy/kuavo_strategy.py
 kuavo_humanoid_sdk/kuavo_strategy/grasp_box/grasp_box_strategy.py

{kuavo_humanoid_sdk_ws-1.1.6b858 → kuavo_humanoid_sdk_ws-1.1.6b1125}/setup.py

@@ -49,7 +49,7 @@ setup(
     install_requires=[
         "numpy", 
         "transitions",
-        "roslibpy"
+        "roslibpy",
     ],
     python_requires=">=3.8",
     classifiers=[