kuavo-humanoid-sdk 1.2.1b3295__20250916004502-py3-none-any.whl

kuavo_humanoid_sdk/kuavo/core/microphone.py

@@ -0,0 +1,192 @@
+import os
+import numpy as np
+import rospy
+from kuavo_humanoid_sdk.kuavo.core.ros.microphone import Microphone
+import contextlib, sys
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+
+from funasr import AutoModel
+
+
+
+class RobotMicrophoneCore:
+    """
+    The core logic for handling wake-up word detection using audio data provided by ROS nodes.
+    """
+    def __init__(self, subscribe_topic="/micphone_data"):
+        self.microphone = Microphone(subscribe_topic)
+    
+        # 创建VAD和ASR两个模型
+        self.vad_model = AutoModel(model="fsmn-vad", model_revision="v2.0.4", disable_update=True)
+        self.asr_model = AutoModel(model="paraformer-zh-streaming", model_revision="v2.0.4",disable_update=True)
+        
+        # 配置参数
+        self.CHUNK = 1024  # 每个缓冲区的帧数
+        self.target_sample_rate = 16000  # ASR模型的目标采样率
+        self.target_mic_keywords = ['夸父', '鲁班',]  # 识别麦克风设备的关键词
+
+        # 流式识别相关参数
+        self.audio_buffer = []  # 音频缓冲区
+        self.vad_cache = {}  # VAD模型缓存
+        self.asr_cache = {}  # ASR模型缓存
+        self.silence_frames = 0  # 静音帧计数
+        self.max_silence_frames = int(1.0 * self.target_sample_rate / self.CHUNK)  # 最大静音帧数（1秒）
+
+
+        # 语音段状态管理
+        self.speech_segment_buffer = []  # 语音段缓冲区
+        self.is_in_speech = False  # 是否在语音段中
+        self.speech_start_time = 0  # 语音开始时间
+        self.consecutive_speech_frames = 0  # 连续语音帧计数
+        self.min_speech_frames = 3  # 最小连续语音帧数，防止误触发
+
+        # VAD配置
+        self.vad_chunk_size = 300  # 毫秒，VAD模型的块大小
+        self.vad_chunk_stride = int(self.vad_chunk_size * self.target_sample_rate / 1000)  # 3200 采样点
+
+        # ASR配置
+        self.asr_chunk_size = [0, 10, 5]  # [0, 10, 5] 表示600ms实时出字粒度
+        self.asr_encoder_chunk_look_back = 4  # 编码器回看的块数
+        self.asr_decoder_chunk_look_back = 1  # 解码器回看的块数
+        self.asr_chunk_stride = self.asr_chunk_size[1] * 960  # 9600 采样点 (600ms * 16kHz)
+
+        SDKLogger.debug("The audio processor node is ready.")
+
+    def wait_for_wake_word(self, timeout_sec=60, wake_word='鲁班鲁班'):
+        """
+        Actively pull audio data, process it and wait for wake-up word detection.
+        Returns True if a wake-up word is detected within the timeout period, otherwise returns False.
+        """
+        hot_word = [wake_word] + self.target_mic_keywords
+        start_time = rospy.get_time()
+        
+        # 重置所有状态
+        self.audio_buffer = []
+        self.vad_cache = {}
+        self.asr_cache = {}
+        self.speech_segment_buffer = []
+        self.is_in_speech = False
+        self.silence_frames = 0
+        
+        while not rospy.is_shutdown():
+            if rospy.get_time() - start_time > timeout_sec:
+                SDKLogger.debug("Timeout has been reached. No wake-up word was detected.")
+                return False
+
+            new_data = self.microphone.get_data()
+            if new_data:
+
+                audio_np = np.frombuffer(new_data, dtype=np.int16).astype(np.float32) / 32768.0
+
+                self.audio_buffer.extend(audio_np)
+                # 检测是否有声音（简单的音量检测）
+                current_volume = np.sqrt(np.mean(audio_np**2))
+                
+                # 调试：打印音量信息
+                # print(f"current_volume: {current_volume:.6f}")
+                
+                # 调整音量阈值 - 对于归一化到[-1,1]的数据，静音应该在0.01以下
+                is_speaking = current_volume > 0.01
+                
+                if is_speaking:
+                    self.silence_frames = 0
+                else:
+                    self.silence_frames += 1
+                
+                # print(f"is_speaking: {is_speaking}")
+                # 当缓冲区有足够的数据时进行VAD检测
+                if len(self.audio_buffer) >= self.vad_chunk_stride:
+
+                    # 提取一个VAD块的数据
+                    vad_chunk = np.array(self.audio_buffer[:self.vad_chunk_stride], dtype=np.float64)
+                    
+                    # 判断是否为最后一个块（静音超过阈值）
+                    is_final = self.silence_frames >= self.max_silence_frames
+                    
+                    try:
+                        # 使用VAD检测语音活动
+                        with self.suppress_output():
+                            vad_res = self.vad_model.generate(input=vad_chunk, cache=self.vad_cache, is_final=is_final, chunk_size=self.vad_chunk_size)
+                        
+                        # print(vad_res)
+                        # 检查VAD结果
+                        has_speech = False
+                        if len(vad_res) > 0 and "value" in vad_res[0] and vad_res[0]["value"]:
+                            has_speech = True
+                        
+                        # 语音段管理
+                        if has_speech and not self.is_in_speech:
+                            # 开始新的语音段
+                            self.is_in_speech = True
+                            self.speech_segment_buffer = []
+                            SDKLogger.debug("🎤 检测到语音开始")
+                        
+                        if self.is_in_speech:
+                            # 在语音段中，累积音频数据
+                            self.speech_segment_buffer.extend(vad_chunk)
+                        
+                        if is_final and self.is_in_speech:
+                            # 语音段结束，进行ASR识别
+                            SDKLogger.debug("🔍 语音段结束，开始识别...")
+                            
+                            if len(self.speech_segment_buffer) > 0:
+                                # 将语音段数据转换为ASR格式
+                                speech_segment = np.array(self.speech_segment_buffer, dtype=np.float64)
+                                
+                                # 使用ASR进行识别
+                                with self.suppress_output():
+                                    asr_res = self.asr_model.generate(input=speech_segment, cache=self.asr_cache, is_final=True, 
+                                                            chunk_size=self.asr_chunk_size, 
+                                                            encoder_chunk_look_back=self.asr_encoder_chunk_look_back, 
+                                                            decoder_chunk_look_back=self.asr_decoder_chunk_look_back, hotword=hot_word)
+                                
+                                # 检查ASR结果
+                                if len(asr_res) > 0:
+                                    # 检查不同的结果字段
+                                    recognized_text = ""
+                                    if "value" in asr_res[0] and asr_res[0]["value"]:
+                                        recognized_text = " ".join(asr_res[0]["value"])
+                                    elif "text" in asr_res[0] and asr_res[0]["text"]:
+                                        recognized_text = asr_res[0]["text"]
+                                    
+                                    if recognized_text:
+                                        SDKLogger.debug(f"📝 识别结果: {recognized_text}")
+                                        if wake_word in recognized_text:
+                                            return True
+                                    else:
+                                        SDKLogger.debug("❌ 未能识别出内容")
+                                else:
+                                    SDKLogger.debug("❌ ASR识别失败")
+                            
+                            # 重置语音段状态
+                            self.is_in_speech = False
+                            self.speech_segment_buffer = []
+                            self.vad_cache = {}
+                            self.asr_cache = {}
+                            SDKLogger.debug("--- 语音段处理完成 ---")
+                    
+                        # 移除已处理的数据，保留一些重叠
+                        # overlap = self.vad_chunk_stride // 2  # 50% 重叠
+                        self.audio_buffer = self.audio_buffer[self.vad_chunk_stride:]
+                            
+                    except Exception as e:
+                        SDKLogger.debug(f"处理错误: {e}")
+            rospy.sleep(0.01)  # 减少睡眠时间以提高响应性
+
+    # 创建输出抑制上下文管理器
+    @contextlib.contextmanager
+    def suppress_output(self):
+        """临时抑制所有标准输出和错误输出"""
+        # 保存原始的标准输出和错误输出
+        old_stdout = sys.stdout
+        old_stderr = sys.stderr
+        # 重定向到空设备
+        with open(os.devnull, 'w') as devnull:
+            sys.stdout = devnull
+            sys.stderr = devnull
+            try:
+                yield
+            finally:
+                # 恢复原始输出
+                sys.stdout = old_stdout
+                sys.stderr = old_stderr

kuavo_humanoid_sdk/kuavo/core/navigation.py

@@ -0,0 +1,70 @@
+import time
+import math
+import rospy
+import threading
+import numpy as np
+from typing import Tuple
+from transitions import Machine, State
+from geometry_msgs.msg import Pose
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+from kuavo_humanoid_sdk.kuavo.core.ros.navigation import Navigation, NavigationStatus
+
+class KuavoRobotNavigationCore:
+    def __init__(self):
+        self.robot_navigation = Navigation()
+
+    def stop_navigation(self) -> bool:
+        """
+        stop navigation
+        """
+        return self.robot_navigation.pub_stop_navigation()
+
+    def get_current_status(self) -> NavigationStatus:
+        """
+        get current status
+        """
+        return self.robot_navigation.get_current_status()
+
+    def load_map(self, map_name: str) -> bool:
+        """
+        load map
+        """
+        return self.robot_navigation.srv_load_map(map_name) 
+
+    def get_current_map(self) -> str:
+        """
+        get current map
+        """
+        return self.robot_navigation.srv_get_current_map()
+
+    def get_all_maps(self) -> list:
+        """
+        get all maps
+        """
+        return self.robot_navigation.srv_get_all_maps()
+    
+    def navigate_to_task_point(self, task_point_name: str) -> bool:
+        """
+        navigate to task point
+        """
+        return self.robot_navigation.srv_navigate_to_task_point(task_point_name)
+    
+    def init_localization_by_pose(self, pose: Pose) -> bool:
+        """
+        initialize localization by pose
+        """
+        return self.robot_navigation.pub_init_localization_by_pose(pose)
+    
+    def init_localization_by_task_point(self, task_point_name: str) -> bool:
+        """
+        initialize localization by task point
+        """
+        return self.robot_navigation.srv_init_localization_by_task_point(task_point_name)
+    
+    def navigate_to_goal(self, goal: Pose) -> bool:
+        """
+        navigate to goal
+        """
+        return self.robot_navigation.pub_navigate_to_goal(goal)
+    
+    

kuavo_humanoid_sdk/kuavo/core/ros/audio.py

@@ -0,0 +1,110 @@
+#!/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 kuavo_humanoid_sdk.msg.kuavo_msgs.srv import  playmusic, playmusicRequest
+from kuavo_humanoid_sdk.msg.kuavo_msgs.srv import  SpeechSynthesis, SpeechSynthesisRequest
+class Audio:
+    """Audio system interface for controlling audio playback functionality of Kuavo humanoid robot.
+    
+    Provides functionality to play music files.
+    """
+    
+    def __init__(self):
+        """Initialize the audio system."""
+        self._audio_stop_publisher = rospy.Publisher('stop_music', Bool, queue_size=10)
+        self.audio_data_publisher = rospy.Publisher('audio_data', Int16MultiArray, queue_size=10)
+        rospy.sleep(0.5)  # Wait for publisher initialization
+    def play_audio(self, file_name: str, volume: int = 100, speed: float = 1.0) -> bool:
+        """Play the specified audio file.
+        
+        Args:
+            file_name (str): Name of the audio file to play
+                
+        Returns:
+            bool: True if the play request was successfully sent, False otherwise
+        """
+        try:
+            # Wait for service availability
+            rospy.wait_for_service('play_music', timeout=2.0)
+            # Create service client
+            play_music_service = rospy.ServiceProxy('play_music', playmusic)
+            # Call service
+            request = playmusicRequest()
+            request.music_number = file_name
+            volume = min(max(volume , 0), 100)
+            request.volume = volume
+            response = play_music_service(request)
+            SDKLogger.info(f"[Robot Audio] Requested to play audio file: {file_name}")
+            return True
+        except rospy.ROSException as e:
+            SDKLogger.error(f"[Robot Audio] Audio playback service unavailable: {str(e)}")
+            return False
+        except Exception as e:
+            SDKLogger.error(f"[Robot Audio] Failed to play audio file: {str(e)}")
+            return False
+    
+    def stop_audio(self):
+        """Stop the currently playing audio."""
+        try:
+            msg = Bool()
+            msg.data = True
+            self._audio_stop_publisher.publish(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.
+        
+        Args:
+            text (str): Text to be played
+                
+        Returns:
+            bool: True if the play request was successfully sent, False otherwise
+        """
+        try:
+            # Wait for service availability
+            rospy.wait_for_service('play_music', timeout=2.0)
+            # Create service client
+            play_music_service = rospy.ServiceProxy('speech_synthesis', SpeechSynthesis)
+            # Call service
+            request = SpeechSynthesisRequest()
+            request.data = text
+            request.volume = volume
+            response = play_music_service(request)
+            SDKLogger.info(f"[Robot Audio] Requested to play audio text: {text}")
+            return True
+        except rospy.ROSException as e:
+            SDKLogger.error(f"[Robot Audio] Audio playback service unavailable: {str(e)}")
+            return False
+        except Exception as e:
+            SDKLogger.error(f"[Robot Audio] Failed to play audio text: {str(e)}")
+            return False
+
+    def publish_audio_chunk(self, audio_chunk, gain: int = 1):
+        """Publish a single audio chunk to the topic, for real-time audio streaming"""
+        try:
+            if not audio_chunk:
+                return False
+                
+            # 应用增益
+            amplified_chunk = [int(sample * gain) for sample in audio_chunk]
+            
+            # 创建并发布消息
+            msg = Int16MultiArray()
+            msg.data = amplified_chunk
+            
+            self.audio_data_publisher.publish(msg)
+            # SDKLogger.debug(f"[Robot Audio] 发布音频块，大小: {len(amplified_chunk)}")
+            
+            return True
+            
+        except Exception as e:
+            SDKLogger.error(f"[Robot Audio] 发布音频块时出错: {e}")
+            return False
+            

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)