kuavo-humanoid-sdk 1.1.3a1239__py3-none-any.whl → 1.1.3a1252__py3-none-any.whl

kuavo_humanoid_sdk/common/global_config.py

@@ -6,6 +6,7 @@ class GlobalConfig:
     use_websocket = False
     websocket_host = '127.0.0.1'
     websocket_port = 9090
+    websocket_timeout = 5.0
 
     def __new__(cls, *args, **kwargs):
         if cls._instance is None:

kuavo_humanoid_sdk/kuavo/core/audio.py

@@ -6,11 +6,15 @@ from typing import Tuple
 from transitions import Machine, State
 
 from kuavo_humanoid_sdk.common.logger import SDKLogger
-from kuavo_humanoid_sdk.kuavo.core.ros.audio import Audio
+from kuavo_humanoid_sdk.common.global_config import GlobalConfig
+from kuavo_humanoid_sdk.kuavo.core.ros.audio import Audio, AudioWebsocket
 
 class KuavoRobotAudioCore:
     def __init__(self):
-        self.robot_audio = Audio()
+        if GlobalConfig.use_websocket:
+            self.robot_audio = AudioWebsocket()
+        else:
+            self.robot_audio = Audio()
 
     def play_audio(self, music_number: str, volume: float = 0.5, speed: float = 1.0) -> bool:
         """

kuavo_humanoid_sdk/kuavo/core/core.py

@@ -26,7 +26,7 @@ from transitions import Machine, State
 
 from kuavo_humanoid_sdk.interfaces.data_types import KuavoArmCtrlMode, KuavoIKParams, KuavoPose
 from kuavo_humanoid_sdk.kuavo.core.ros.control import KuavoRobotControl, KuavoRobotControlWebsocket
-from kuavo_humanoid_sdk.kuavo.core.ros.vision import KuavoRobotVisionCore
+from kuavo_humanoid_sdk.kuavo.core.ros.vision import KuavoRobotVisionCore, KuavoRobotVisionCoreWebsocket
 from kuavo_humanoid_sdk.kuavo.core.ros.tools import KuavoRobotToolsCore
 from kuavo_humanoid_sdk.kuavo.core.ros.state import KuavoRobotStateCore, KuavoRobotStateCoreWebsocket
 from kuavo_humanoid_sdk.kuavo.core.ros.param import make_robot_param
@@ -69,6 +69,7 @@ class KuavoRobotCore:
             if GlobalConfig.use_websocket:
                 self._control = KuavoRobotControlWebsocket()
                 self._rb_state = KuavoRobotStateCoreWebsocket()
+                self._robot_vision = KuavoRobotVisionCoreWebsocket()
             else:
                 self._control = KuavoRobotControl()
                 self._rb_state = KuavoRobotStateCore()

kuavo_humanoid_sdk/kuavo/core/ros/audio.py

@@ -1,15 +1,16 @@
 #!/usr/bin/env python3
 # coding: utf-8
-
 from kuavo_humanoid_sdk.common.logger import SDKLogger
 from kuavo_humanoid_sdk.kuavo.core.core import KuavoRobotCore
+from kuavo_humanoid_sdk.common.websocket_kuavo_sdk import WebSocketKuavoSDK
+import roslibpy
 
-try:
+try:    
     import rospy
     from std_msgs.msg import Bool
     from kuavo_humanoid_sdk.msg.kuavo_msgs.srv import  playmusic, playmusicRequest
     from kuavo_humanoid_sdk.msg.kuavo_msgs.srv import  SpeechSynthesis, SpeechSynthesisRequest
-except ImportError:
+except:
     pass
 
 class Audio:
@@ -91,3 +92,85 @@ class Audio:
         except Exception as e:
             SDKLogger.error(f"[Robot Audio] Failed to play audio text: {str(e)}")
             return False
+
+class AudioWebsocket:
+    """WebSocket-based audio system interface for controlling audio playback functionality of Kuavo humanoid robot.
+    
+    Provides functionality to play music files through WebSocket connection.
+    """
+    
+    def __init__(self):
+        """Initialize the WebSocket audio system."""
+        websocket = WebSocketKuavoSDK()
+        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:
+        """Play the specified audio file through WebSocket.
+        
+        Args:
+            file_name (str): Name of the audio file to play
+            volume (float): Volume level (0.0 to 1.0)
+            speed (float): Playback speed
+                
+        Returns:
+            bool: True if the play request was successfully sent, False otherwise
+        """
+        try:
+            websocket = WebSocketKuavoSDK()
+            service = roslibpy.Service(websocket.client, 'play_music', 'kuavo_msgs/playmusic')
+            
+            volume = min(max(volume, 0), 1.0)
+            request = {
+                "music_number": file_name,
+                "volume": volume,
+                "speed": speed
+            }
+            
+            response = service.call(request)
+            SDKLogger.info(f"[Robot Audio] Requested to play audio file: {file_name}")
+            return True
+        except Exception as e:
+            SDKLogger.error(f"[Robot Audio] Failed to play audio file: {str(e)}")
+            return False
+    
+    def stop_audio(self) -> bool:
+        """Stop the currently playing audio through WebSocket."""
+        try:
+            msg = {
+                "data": True
+            }
+            self._audio_stop_publisher.publish(roslibpy.Message(msg))
+            SDKLogger.info("[Robot Audio] Requested to stop audio playback")
+            return True
+        except Exception as e:
+            SDKLogger.error(f"[Robot Audio] Failed to stop audio playback: {str(e)}")
+            return False
+    
+    def text_to_speech(self, text: str, volume: float = 0.5) -> bool:
+        """Synthesize and play the specified text through WebSocket.
+        
+        Args:
+            text (str): Text to be played
+            volume (float): Volume level (0.0 to 1.0)
+                
+        Returns:
+            bool: True if the play request was successfully sent, False otherwise
+        """
+        try:
+            websocket = WebSocketKuavoSDK()
+            service = roslibpy.Service(websocket.client, 'speech_synthesis', 'kuavo_msgs/SpeechSynthesis')
+            
+            request = {
+                "data": text,
+                "volume": volume
+            }
+            
+            response = service.call(request)
+            SDKLogger.info(f"[Robot Audio] Requested to play audio text: {text}")
+            return True
+        except Exception as e:
+            SDKLogger.error(f"[Robot Audio] Failed to play audio text: {str(e)}")
+            return False
+
+

kuavo_humanoid_sdk/kuavo/core/ros/control.py

@@ -272,9 +272,9 @@ class ControlEndEffectorWebsocket:
                 ]
             }
             response = service.call(request)
-            if not response.get('result', False):
+            if not response.get('success', False):
                 SDKLogger.error(f"Failed to execute gesture '{gestures}': {response.get('message', '')}")
-            return response.get('result', False)
+            return response.get('success', False)
         except Exception as e:
             SDKLogger.error(f"Service call failed: {e}")
             return False

kuavo_humanoid_sdk/kuavo/core/ros/vision.py

@@ -9,6 +9,8 @@ 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)
+import roslibpy
+from kuavo_humanoid_sdk.common.websocket_kuavo_sdk import WebSocketKuavoSDK
 
 try:
     import rospy
@@ -263,6 +265,250 @@ class KuavoRobotVisionCore:
             "poses": [data.pose[i] for i in indices]
         }
 
+class KuavoRobotVisionCoreWebsocket:
+    _instance = None
+
+    def __new__(cls, *args, **kwargs):
+        if not cls._instance:
+            cls._instance = super().__new__(cls)
+        return cls._instance
+
+    def __init__(self):
+        if not hasattr(self, '_initialized'):
+            try:
+                self.websocket = WebSocketKuavoSDK()
+                if not self.websocket.client.is_connected:
+                    SDKLogger.error("Failed to connect to WebSocket server")
+                    raise ConnectionError("Failed to connect to WebSocket server")
+                
+                # Initialize subscribers for vision-related topics
+                self._sub_apriltag_camera = roslibpy.Topic(self.websocket.client, '/tag_detections', 'apriltag_ros/AprilTagDetectionArray')
+                self._sub_apriltag_base = roslibpy.Topic(self.websocket.client, '/robot_tag_info', 'apriltag_ros/AprilTagDetectionArray')
+                
+                # Initialize TF-related topics
+                self._sub_tf = roslibpy.Topic(self.websocket.client, '/tf', 'tf2_msgs/TFMessage')
+                self._sub_tf_static = roslibpy.Topic(self.websocket.client, '/tf_static', 'tf2_msgs/TFMessage')
+                
+                # Subscribe to topics
+                self._sub_apriltag_camera.subscribe(self._apriltag_camera_callback)
+                self._sub_apriltag_base.subscribe(self._apriltag_base_callback)
+                self._sub_tf.subscribe(self._tf_callback)
+                self._sub_tf_static.subscribe(self._tf_static_callback)
+                
+                # 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 = []
+                )
+                
+                # TF buffer for storing transforms
+                self._tf_buffer = {}
+                self._tf_static_buffer = {}
+                
+                self._initialized = True
+            except Exception as e:
+                SDKLogger.error(f"Failed to initialize KuavoRobotVisionCoreWebsocket: {e}")
+                raise
+
+    def _tf_callback(self, msg):
+        """Callback for TF messages."""
+        for transform in msg['transforms']:
+            key = (transform['header']['frame_id'], transform['child_frame_id'])
+            self._tf_buffer[key] = transform
+
+    def _tf_static_callback(self, msg):
+        """Callback for static TF messages."""
+        for transform in msg['transforms']:
+            key = (transform['header']['frame_id'], transform['child_frame_id'])
+            self._tf_static_buffer[key] = transform
+
+    def _get_transform(self, target_frame, source_frame):
+        """Get transform between two frames.
+        
+        Args:
+            target_frame (str): Target frame ID
+            source_frame (str): Source frame ID
+            
+        Returns:
+            dict: Transform data if found, None otherwise
+        """
+        # Check both dynamic and static transforms
+        key = (source_frame, target_frame)
+        if key in self._tf_buffer:
+            return self._tf_buffer[key]
+        if key in self._tf_static_buffer:
+            return self._tf_static_buffer[key]
+        return None
+
+    def _transform_pose(self, pose, transform):
+        """Transform a pose using the given transform.
+        
+        Args:
+            pose (dict): Pose to transform
+            transform (dict): Transform to apply
+            
+        Returns:
+            dict: Transformed pose
+        """
+        # Extract transform components
+        t = transform['transform']
+        translation = t['translation']
+        rotation = t['rotation']
+        
+        # Extract pose components
+        p = pose['position']
+        o = pose['orientation']
+        
+        # TODO: Implement actual pose transformation
+        # This is a placeholder - actual implementation would involve
+        # proper quaternion and vector math
+        transformed_pose = {
+            'position': {
+                'x': p['x'] + translation['x'],
+                'y': p['y'] + translation['y'],
+                'z': p['z'] + translation['z']
+            },
+            'orientation': {
+                'x': o['x'],
+                'y': o['y'],
+                'z': o['z'],
+                'w': o['w']
+            }
+        }
+        
+        return transformed_pose
+
+    def _apriltag_camera_callback(self, msg):
+        """Callback for AprilTag detections in camera frame."""
+        # Clear previous data
+        self._apriltag_data_from_camera.id = []
+        self._apriltag_data_from_camera.size = []
+        self._apriltag_data_from_camera.pose = []
+        
+        # Process each detection
+        for detection in msg['detections']:
+            # Add ID
+            for tag_id in detection['id']:
+                self._apriltag_data_from_camera.id.append(tag_id)
+            
+            # Add size
+            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)
+            
+            # Add pose
+            self._apriltag_data_from_camera.pose.append(detection['pose']['pose']['pose'])
+
+    def _apriltag_base_callback(self, msg):
+        """Callback for AprilTag detections in base frame."""
+        # Clear previous data
+        self._apriltag_data_from_base.id = []
+        self._apriltag_data_from_base.size = []
+        self._apriltag_data_from_base.pose = []
+        
+        # Process each detection
+        for detection in msg['detections']:
+            # Add ID
+            for tag_id in detection['id']:
+                self._apriltag_data_from_base.id.append(tag_id)
+            
+            # Add size
+            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)
+            
+            # Add pose
+            self._apriltag_data_from_base.pose.append(detection['pose']['pose']['pose'])
+        
+        # Transform base data to odom frame
+        self._transform_base_to_odom()
+
+    def _transform_base_to_odom(self):
+        """Transform AprilTag poses from base_link to odom coordinate frame."""
+        # Clear previous odom data
+        self._apriltag_data_from_odom.id = []
+        self._apriltag_data_from_odom.size = []
+        self._apriltag_data_from_odom.pose = []
+        
+        # If no base data, skip transformation
+        if not self._apriltag_data_from_base.id:
+            SDKLogger.warn("No base tag data, skip transform")
+            return
+        
+        # Get transform from base_link to odom
+        transform = self._get_transform("odom", "base_link")
+        if not transform:
+            SDKLogger.warn("Transform from base_link to odom not available")
+            return
+        
+        # Copy ID and size information
+        self._apriltag_data_from_odom.id = self._apriltag_data_from_base.id.copy()
+        self._apriltag_data_from_odom.size = self._apriltag_data_from_base.size.copy()
+        
+        # Transform each pose
+        for pose in self._apriltag_data_from_base.pose:
+            transformed_pose = self._transform_pose(pose, transform)
+            self._apriltag_data_from_odom.pose.append(transformed_pose)
+
+    @property
+    def apriltag_data_from_camera(self):
+        return self._apriltag_data_from_camera
+    
+    @property
+    def apriltag_data_from_base(self):
+        return self._apriltag_data_from_base
+    
+    @property
+    def apriltag_data_from_odom(self):
+        return self._apriltag_data_from_odom
+
+    def get_data_by_id(self, tag_id: int, frame: str = "odom"):
+        """Get AprilTag data for a specific tag ID from the specified frame.
+        
+        Args:
+            tag_id (int): The ID of the AprilTag to get data for
+            frame (str): The frame to get data from ("camera", "base", or "odom")
+            
+        Returns:
+            dict: The AprilTag data for the specified ID and frame, or None if not found
+        """
+        if frame == "camera":
+            data = self._apriltag_data_from_camera
+        elif frame == "base":
+            data = self._apriltag_data_from_base
+        elif frame == "odom":
+            data = self._apriltag_data_from_odom
+        else:
+            SDKLogger.error(f"Invalid frame: {frame}")
+            return None
+            
+        if not data or not data.id:
+            return None
+            
+        # Find all matching indices
+        indices = [i for i, id in enumerate(data.id) if id == tag_id]
+        
+        if not indices:
+            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()

{kuavo_humanoid_sdk-1.1.3a1239.dist-info → kuavo_humanoid_sdk-1.1.3a1252.dist-info}/METADATA

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

{kuavo_humanoid_sdk-1.1.3a1239.dist-info → kuavo_humanoid_sdk-1.1.3a1252.dist-info}/RECORD

@@ -1,5 +1,5 @@
 kuavo_humanoid_sdk/__init__.py,sha256=pdgXhpulXKUcdUPwDVFZCe47AZAkOJdh_YAM8f3g2Ek,141
-kuavo_humanoid_sdk/common/global_config.py,sha256=Tae7-RfXsQk5G3VC-OQoMwB_eB-oud90cmAE7BywmI0,287
+kuavo_humanoid_sdk/common/global_config.py,sha256=2iDGXEwjmfc9MQ4jYOYxFyTB_BnLIxf6HYent5dS4gA,315
 kuavo_humanoid_sdk/common/logger.py,sha256=nSIbsJaW4nDcaQdhZebZkjtji6kmWVR8tYO-sUTz3Gg,1246
 kuavo_humanoid_sdk/common/websocket_kuavo_sdk.py,sha256=lfilfXYdZFTasjRudHb_Xc89ah44jWljpQTozVSABGU,675
 kuavo_humanoid_sdk/interfaces/__init__.py,sha256=_zXDinj2T3X7UH19-Xrotc7uYZWHmZqHaMCWU1FYR5I,100
@@ -18,18 +18,18 @@ kuavo_humanoid_sdk/kuavo/robot_info.py,sha256=etyqcUg22IAA2O59cjBhLTpacQ3DLhT8qZ
 kuavo_humanoid_sdk/kuavo/robot_state.py,sha256=VT2kcOAoc9MtJGVJ6pOwTkFDeFX9mHtRcdpkMxVEetA,11084
 kuavo_humanoid_sdk/kuavo/robot_tool.py,sha256=moCaX46JWO6k9QXIhXPXjL_ywccz4iovkrjS2yH9Dgg,2747
 kuavo_humanoid_sdk/kuavo/robot_vision.py,sha256=AxlD3Ckl2GafsNpPNWYUM89653AN1BvWtMi9stdDALE,3156
-kuavo_humanoid_sdk/kuavo/core/audio.py,sha256=omoUCjZKFSG1yvAuM7fAJ65T9ppQk-kwjM-xllBSJ-I,838
-kuavo_humanoid_sdk/kuavo/core/core.py,sha256=u7PU4LTYOlwJwIdAc31Y9thABa-Im69cXz23BqHC8fo,18576
+kuavo_humanoid_sdk/kuavo/core/audio.py,sha256=lyoYTKkt3ZEnehh1cJenksrkYUrlHBC559r1JmzysR4,1024
+kuavo_humanoid_sdk/kuavo/core/core.py,sha256=VZfyot7BOeUpwJfjHa7_hcKNegdqYqLCEGLBNeb54fc,18676
 kuavo_humanoid_sdk/kuavo/core/dex_hand_control.py,sha256=51iLPIKMFKeOhc5TpRCMWTm7Aw8pt6XG9XUB-_swr4Y,5158
 kuavo_humanoid_sdk/kuavo/core/leju_claw_control.py,sha256=RCrnWTMW-L0fOMa1op7t43aGWSyMVqrOjR0lVtIFzqY,3432
 kuavo_humanoid_sdk/kuavo/core/ros_env.py,sha256=lVhEVUZvdQI43mPkFqRYBld3N-L4nPigRkcSZ-ejL9I,18848
-kuavo_humanoid_sdk/kuavo/core/ros/audio.py,sha256=KbsZMs8XTlGnPixUWAPgSrhrcVZx5MFQuYjVeROdeUQ,3678
-kuavo_humanoid_sdk/kuavo/core/ros/control.py,sha256=KCH5NrcTqjles98GTCtJOMfj5gx5ldeG5dKIdyl86cM,62286
+kuavo_humanoid_sdk/kuavo/core/ros/audio.py,sha256=aJQ9i7df8mK55XCtNM4MDLcwz8CnEk-K3P93QrtWwXQ,6850
+kuavo_humanoid_sdk/kuavo/core/ros/control.py,sha256=YnANyRmeXXdcV6lyeAmbpu6VQyiUp8RHgp_qbUni3F4,62288
 kuavo_humanoid_sdk/kuavo/core/ros/param.py,sha256=r8Xve8zsfFBBZQnEbLOCNC1YQhv_h-Xv1hHx6gHkQag,11745
 kuavo_humanoid_sdk/kuavo/core/ros/sat_utils.py,sha256=AhBNM-s9nVwRiLZ8gtqC2RvPImkT9uulONSLXk3KooI,3317
 kuavo_humanoid_sdk/kuavo/core/ros/state.py,sha256=OkoGUWj0llDSYWFE-unKKHeiPKik1vSSU6mtVP4Mwwg,34658
 kuavo_humanoid_sdk/kuavo/core/ros/tools.py,sha256=RF9PVvHEfYBuNYyKRbgtLrFrR15kfPTjtcXmZG17Gk4,6142
-kuavo_humanoid_sdk/kuavo/core/ros/vision.py,sha256=v7G12z7Ncj94kEa5ff85D1cDNlbfGVSZGmCEbSGu8qk,10810
+kuavo_humanoid_sdk/kuavo/core/ros/vision.py,sha256=0sVTsfvaaLyHJIos7aUpHi_DkxCD0LDw90ZedK8YFGg,20173
 kuavo_humanoid_sdk/msg/__init__.py,sha256=8TElMWrXsBCGxhJBe3mfgtiAlX4LzgQep66TSwMcaDQ,19
 kuavo_humanoid_sdk/msg/kuavo_msgs/__init__.py,sha256=wk-pfWFvAOl475vxWFVTwbJGejP6UKgivUApv8RvK_8,216
 kuavo_humanoid_sdk/msg/kuavo_msgs/msg/_AprilTagDetection.py,sha256=2Iz0kN1w5IqvXFmK-DN14i-9hrtZNSRSWvxr6tSPIlA,11655
@@ -106,7 +106,7 @@ kuavo_humanoid_sdk/msg/ocs2_msgs/msg/_mpc_target_trajectories.py,sha256=c8l2__Lu
 kuavo_humanoid_sdk/msg/ocs2_msgs/msg/_multiplier.py,sha256=UY0klrLwiuQeEOxBXGrLdivCOhI3HsHUbi5uX7nsT8g,4895
 kuavo_humanoid_sdk/msg/ocs2_msgs/srv/__init__.py,sha256=LaBHLkXuxdf1Z6BCqgWQC2F9yUNSDYC_iQp6NMYAnjQ,22
 kuavo_humanoid_sdk/msg/ocs2_msgs/srv/_reset.py,sha256=KtnylmS0VOjVfa3lldprbhZl04wU3ng5v94jghu3dYU,13379
-kuavo_humanoid_sdk-1.1.3a1239.dist-info/METADATA,sha256=U4mW2wfjkpSrWT7XJ23Y5uT8FKT4YbxSEwFTNcuxCC8,10831
-kuavo_humanoid_sdk-1.1.3a1239.dist-info/WHEEL,sha256=g4nMs7d-Xl9-xC9XovUrsDHGXt-FT0E17Yqo92DEfvY,92
-kuavo_humanoid_sdk-1.1.3a1239.dist-info/top_level.txt,sha256=9uSyZoiOEykRMOf0Gm-N9mkyQQg9yOSimfsb632G99c,19
-kuavo_humanoid_sdk-1.1.3a1239.dist-info/RECORD,,
+kuavo_humanoid_sdk-1.1.3a1252.dist-info/METADATA,sha256=njSY4oZBP93mFsnwGqGn7LTY-4h2RRbQ0pllUQhvS6c,10831
+kuavo_humanoid_sdk-1.1.3a1252.dist-info/WHEEL,sha256=g4nMs7d-Xl9-xC9XovUrsDHGXt-FT0E17Yqo92DEfvY,92
+kuavo_humanoid_sdk-1.1.3a1252.dist-info/top_level.txt,sha256=9uSyZoiOEykRMOf0Gm-N9mkyQQg9yOSimfsb632G99c,19
+kuavo_humanoid_sdk-1.1.3a1252.dist-info/RECORD,,