#!/usr/bin/env python3 # -*- coding: utf-8 -*- import rclpy from rclpy.node import Node from std_srvs.srv import SetBool, Trigger from protocol.srv import AudioExecute, AudioVolumeSet, AudioTextPlay from protocol.msg import AudioStatus import threading import time class VoiceControlNode(Node): def __init__(self): super().__init__('voice_control_node') # 当前机器狗状态 self.current_state = "ready" # ready, delivering, unloading self.microphone_enabled = False self.voice_control_enabled = False # 创建服务客户端 self.enable_mic_client = self.create_client( AudioExecute, 'set_audio_state') self.enable_voice_control_client = self.create_client( SetBool, 'audio_action_set') self.volume_set_client = self.create_client( AudioVolumeSet, 'audio_volume_set') self.play_voice_client = self.create_client( AudioTextPlay, 'speech_text_play') self.trigger_delivery_client = self.create_client( Trigger, 'start_delivery') self.trigger_unloading_client = self.create_client( Trigger, 'complete_unloading') # 确保服务可用 while not self.enable_mic_client.wait_for_service(timeout_sec=1.0): self.get_logger().info('set_audio_state service not available, waiting...') while not self.enable_voice_control_client.wait_for_service(timeout_sec=1.0): self.get_logger().info('audio_action_set service not available, waiting...') # 初始化时开启麦克风和语音控制 self.enable_microphone(True) self.enable_voice_control(True) self.set_volume(50) # 设置音量为50% # 启动语音监听线程 self.listening_thread = threading.Thread(target=self.voice_listener) self.listening_thread.daemon = True self.listening_thread.start() self.get_logger().info("Voice Control Node Initialized") def enable_microphone(self, enable): """启用或禁用麦克风""" req = AudioExecute.Request() req.client = "voice_control" req.status = AudioStatus() req.status.state = AudioStatus.AUDIO_STATUS_NORMAL if enable else AudioStatus.AUDIO_STATUS_OFFMIC future = self.enable_mic_client.call_async(req) rclpy.spin_until_future_complete(self, future) if future.result() is not None and future.result().result: self.microphone_enabled = enable status = "enabled" if enable else "disabled" self.get_logger().info(f"Microphone {status}") else: self.get_logger().error("Failed to set microphone state") def enable_voice_control(self, enable): """启用或禁用语音控制垂域功能""" req = SetBool.Request() req.data = enable future = self.enable_voice_control_client.call_async(req) rclpy.spin_until_future_complete(self, future) if future.result() is not None and future.result().success: self.voice_control_enabled = enable status = "enabled" if enable else "disabled" self.get_logger().info(f"Voice control {status}") else: self.get_logger().error("Failed to set voice control state") def set_volume(self, volume_level): """设置音量""" req = AudioVolumeSet.Request() req.volume = volume_level future = self.volume_set_client.call_async(req) rclpy.spin_until_future_complete(self, future) if future.result() is not None and future.result().success: self.get_logger().info(f"Volume set to {volume_level}%") else: self.get_logger().error("Failed to set volume") def play_voice_message(self, text, is_online=True): """播放语音消息""" req = AudioTextPlay.Request() req.module_name = "voice_control" req.is_online = is_online req.text = text future = self.play_voice_client.call_async(req) rclpy.spin_until_future_complete(self, future) if future.result() is not None: if future.result().status == 0: self.get_logger().info(f"Played message: {text}") else: self.get_logger().error(f"Failed to play message: {text}") def voice_listener(self): """监听语音指令的线程""" while rclpy.ok(): # 在实际应用中，这里应该连接到语音识别服务 # 为了示例，我们使用简单的模拟 time.sleep(2) # 模拟监听间隔 if not self.microphone_enabled or not self.voice_control_enabled: continue # 根据当前状态监听不同的指令 if self.current_state == "delivering": # 模拟听到"配货完成" self.process_voice_command("配货完成") elif self.current_state == "unloading": # 模拟听到"卸货完成" self.process_voice_command("卸货完成") def process_voice_command(self, command): """处理语音命令""" self.get_logger().info(f"Received voice command: {command}") if self.current_state == "delivering" and "配货完成" in command: self.handle_delivery_complete() elif self.current_state == "unloading" and "卸货完成" in command: self.handle_unloading_complete() else: self.play_voice_message("指令无法识别，请重试") def handle_delivery_complete(self): """处理配货完成指令""" self.play_voice_message("配货完成，开始运输") # 触发开始运输 req = Trigger.Request() future = self.trigger_delivery_client.call_async(req) rclpy.spin_until_future_complete(self, future) if future.result() is not None and future.result().success: self.get_logger().info("Delivery started") self.current_state = "transporting" else: self.get_logger().error("Failed to start delivery") def handle_unloading_complete(self): """处理卸货完成指令""" self.play_voice_message("卸货完成，准备新的配货任务") # 触发卸货完成 req = Trigger.Request() future = self.trigger_unloading_client.call_async(req) rclpy.spin_until_future_complete(self, future) if future.result() is not None and future.result().success: self.get_logger().info("Unloading completed") self.current_state = "ready" else: self.get_logger().error("Failed to complete unloading") def set_state(self, new_state): """设置机器狗当前状态""" valid_states = ["ready", "delivering", "unloading", "transporting"] if new_state in valid_states: self.current_state = new_state self.get_logger().info(f"State changed to: {new_state}") # 根据状态播放提示音 if new_state == "delivering": self.play_voice_message("已到达配货库位，请说出'配货完成'确认") elif new_state == "unloading": self.play_voice_message("已到达卸货库位，请说出'卸货完成'确认") else: self.get_logger().error(f"Invalid state: {new_state}") def main(args=None): rclpy.init(args=args) node = VoiceControlNode() # 模拟状态变化 - 在实际应用中应由其他节点设置 # 进入配货状态 time.sleep(5) node.set_state("delivering") # 等待配货完成 time.sleep(10) # 进入卸货状态 node.set_state("unloading") # 等待卸货完成 time.sleep(10) rclpy.spin(node) rclpy.shutdown() if __name__ == '__main__': main()