kuavo-humanoid-sdk 1.1.6b1123__20250610205449-py3-none-any.whl

kuavo_humanoid_sdk/kuavo/core/ros/tools.py

@@ -0,0 +1,219 @@
+#! /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
+import rospy
+from kuavo_humanoid_sdk.msg.kuavo_msgs.srv import RepublishTFs
+from kuavo_humanoid_sdk.msg.kuavo_msgs.msg import TFArray
+import tf.transformations as tf_trans
+from std_msgs.msg import Float64
+from nav_msgs.msg import Odometry
+from sensor_msgs.msg import JointState
+from kuavo_humanoid_sdk.msg.kuavo_msgs.msg import sensorsData, lejuClawState, gaitTimeName, dexhandTouchState
+from geometry_msgs.msg import TransformStamped
+from kuavo_humanoid_sdk.interfaces.data_types import (PoseQuaternion, HomogeneousMatrix)
+from kuavo_humanoid_sdk.kuavo.core.ros.param import make_robot_param, EndEffectorType
+
+
+
+class KuavoRobotToolsCore:
+    """Provides core ROS tools for Kuavo humanoid robot transformations.
+    
+    Attributes:
+        tf_service (rospy.ServiceProxy): 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:
+                # 初始化TF2 web republisher服务
+                rospy.wait_for_service('/republish_tfs', timeout=5.0)
+                self.tf_service = rospy.ServiceProxy('/republish_tfs', 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 (rospy.Time, optional): Time of transform. Defaults to latest.
+            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:
+            
+            # 调用服务
+            response = self.tf_service(
+                source_frames=[source_frame],
+                target_frame=target_frame,
+                angular_thres=0.01,  # 角度阈值
+                trans_thres=0.01,    # 平移阈值
+                rate=10.0,           # 更新频率
+                timeout=rospy.Duration(timeout)
+            )
+
+            if response.status == -1:
+                SDKLogger.error(f"{source_frame} or {target_frame} not exist")
+                return None
+            
+            # 检查话题是否发布
+            published_topics = rospy.get_published_topics()
+            if not any(topic_tuple[0] == response.topic_name for topic_tuple in published_topics):
+                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 = rospy.Subscriber(response.topic_name, TFArray, transform_callback)
+            
+            # 等待接收数据
+            start_time = rospy.Time.now()
+            while not transform_received or (rospy.Time.now() - start_time).to_sec() > timeout:
+                rospy.sleep(0.1)
+                
+            # 取消订阅
+            sub.unregister()
+                        
+            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 rospy.ServiceException as e:
+            SDKLogger.error(f"Service call failed: {str(e)}")
+            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 (geometry_msgs/Transform): 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
+        
+        Note:
+            Falls back to pose_quaternion format if invalid return_type specified
+        """
+        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 geometry_msgs/Transform to homogeneous matrix.
+        
+        Args:
+            transform (geometry_msgs/Transform): Input transform message
+        
+        Returns:
+            np.ndarray: 4x4 homogeneous transformation matrix (numpy.float32)
+        
+        Example:
+            >>> matrix = _transform_to_homogeneous(transform_msg)
+            >>> print(matrix.shape)
+            (4, 4)
+        """
+        # 四元数转旋转矩阵
+        rotation = [
+            transform.rotation.x,
+            transform.rotation.y,
+            transform.rotation.z,
+            transform.rotation.w
+        ]
+        rot_matrix = tf_trans.quaternion_matrix(rotation)
+
+        # 设置平移分量
+        translation = [
+            transform.translation.x,
+            transform.translation.y,
+            transform.translation.z
+        ]
+        rot_matrix[:3, 3] = translation
+        
+        return rot_matrix.astype(np.float32)  # 确保矩阵数据类型一致
+
+    def get_link_pose(self, link_name: str, target_frame: str = "base_link") -> Union[PoseQuaternion, None]:
+        """获取指定关节链接的位姿（核心实现）
+        
+        Args:
+            link_name (str): 关节链接名称
+            target_frame (str): 目标坐标系
+            
+        Returns:
+            PoseQuaternion: 包含位置和姿态的四元数表示
+        """
+        return self._get_tf_tree_transform(
+            target_frame,
+            link_name,
+            return_type="pose_quaternion"
+        )
+
+# 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/kuavo/core/ros/vision.py

@@ -0,0 +1,234 @@
+#! /usr/bin/env python3
+# coding: utf-8
+
+import copy
+import time
+from collections import deque
+from typing import Tuple, Optional
+
+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 rospy
+from std_msgs.msg import Float64
+from nav_msgs.msg import Odometry
+from sensor_msgs.msg import JointState
+from apriltag_ros.msg import AprilTagDetectionArray
+from geometry_msgs.msg import TransformStamped, PoseStamped
+
+
+class KuavoRobotVisionCore:
+    """Handles vision-related data processing for Kuavo humanoid robot.
+    
+    Attributes:
+        tf_buffer (tf2_ros.Buffer): TF2 transform buffer
+        tf_listener (tf2_ros.TransformListener): TF2 transform listener
+        tf_broadcaster (tf2_ros.TransformBroadcaster): TF2 transform broadcaster
+    """
+    
+    def __init__(self):
+        """Initializes vision system components including TF and AprilTag subscribers."""
+        if not hasattr(self, '_initialized'):
+
+
+            # FIRST: Initialize data structures before creating subscribers
+            """ data """
+            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 = []
+            )
+            
+            # THEN: Create subscribers after all data structures are initialized
+            self._apriltag_data_camera_sub = rospy.Subscriber('/tag_detections', AprilTagDetectionArray, self._apriltag_data_callback_camera)
+            self._apriltag_data_base_sub = rospy.Subscriber('/robot_tag_info', AprilTagDetectionArray, self._apriltag_data_callback_base)
+            self._apriltag_data_odom_sub = rospy.Subscriber('/robot_tag_info_odom', AprilTagDetectionArray, self._apriltag_data_callback_odom)
+            
+            # Mark as initialized
+            self._initialized = True
+
+    def _apriltag_data_callback_camera(self, data):
+        """Callback for processing AprilTag detections from camera.
+        
+        Args:
+            data (AprilTagDetectionArray): 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)
+            
+            # 添加尺寸 (从size数组中获取)
+            if detection.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)
+            
+            # 添加姿态
+            self._apriltag_data_from_camera.pose.append(AprilTagDetection(
+                position=detection.pose.pose.pose.position,
+                orientation=detection.pose.pose.pose.orientation
+            ))
+        # # debug
+        # rospy.loginfo("Apriltag data from camera: %s", self._apriltag_data_from_camera)
+
+    def _apriltag_data_callback_base(self, data):
+        """Callback for processing AprilTag detections from base link.
+        
+        Args:
+            data (AprilTagDetectionArray): 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)
+            
+            # 添加尺寸 (从size数组中获取)
+            if detection.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)
+            
+            # 添加姿态
+            self._apriltag_data_from_base.pose.append(AprilTagDetection(
+                position=detection.pose.pose.pose.position,
+                orientation=detection.pose.pose.pose.orientation
+            ))
+
+        # # debug
+        # rospy.loginfo("Apriltag data from base: %s", self._apriltag_data_from_base)
+
+    def _apriltag_data_callback_odom(self, data):
+        """Callback for processing AprilTag detections from odom.
+        
+        Args:
+            data (AprilTagDetectionArray): 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)
+            
+            # 添加尺寸 (从size数组中获取)
+            if detection.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)
+
+            # 添加姿态
+            self._apriltag_data_from_odom.pose.append(AprilTagDetection(
+                position=detection.pose.pose.pose.position,
+                orientation=detection.pose.pose.pose.orientation
+            ))
+
+        # # debug
+        # rospy.loginfo("Apriltag data from odom: %s", self._apriltag_data_from_odom)
+        
+    @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
+        
+        Raises:
+            ValueError: If invalid data_source is specified
+        """
+        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:
+            # SDKLogger.error(f"Invalid data source: {data_source}, must be one of {list(data_map.keys())}")
+            return None
+        
+        data = data_map[data_source]
+        
+        # 查找所有匹配的索引
+        indices = [i for i, tag_id in enumerate(data.id) if tag_id == target_id]
+        
+        if not indices:
+            # SDKLogger.debug(f"No data found for tag ID {target_id} in {data_source} source")
+            return None
+        
+        return {
+            "sizes": [data.size[i] for i in indices],
+            "poses": [data.pose[i] for i in indices]
+        }
+
+# if __name__ == "__main__":
+
+#     kuavo_robot_vision_core = KuavoRobotVisionCore()
+#     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()