#!/usr/bin/env python3 # -*- coding: utf-8 -*- import rclpy from rclpy.node import Node from rclpy.executors import MultiThreadedExecutor from rclpy.callback_groups import ReentrantCallbackGroup from std_srvs.srv import SetBool, Trigger from protocol.srv import AudioExecute, AudioVolumeSet, AudioTextPlay, AudioVolumeGet from protocol.msg import AudioStatus import threading import time class VoiceControlNode(Node): def __init__(self): super().__init__('voice_control_node') # 创建回调组，允许并行处理服务 self.callback_group = ReentrantCallbackGroup() # 当前机器狗状态 self.current_state = "ready" # ready, delivering, unloading self.microphone_enabled = False self.voice_control_enabled = False self.current_volume = 50 # 默认音量 # 创建所有必要的语音服务 self.create_audio_services() # 初始化语音服务状态 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("机器狗语音控制系统已启动") def create_audio_services(self): """创建所有必要的语音服务""" # 音量获取服务 self.volume_get_srv = self.create_service( AudioVolumeGet, 'audio_volume_get', self.volume_get_callback, callback_group=self.callback_group ) # 音量设置服务 self.volume_set_srv = self.create_service( AudioVolumeSet, 'audio_volume_set', self.volume_set_callback, callback_group=self.callback_group ) # 麦克风开关服务 self.microphone_srv = self.create_service( AudioExecute, 'set_audio_state', self.microphone_callback, callback_group=self.callback_group ) # 语音控制开关服务 self.voice_control_srv = self.create_service( SetBool, 'audio_action_set', self.voice_control_callback, callback_group=self.callback_group ) # 语音播放服务 self.play_voice_srv = self.create_service( AudioTextPlay, 'speech_text_play', self.play_voice_callback, callback_group=self.callback_group ) self.get_logger().info("所有语音服务已创建") def volume_get_callback(self, request, response): """音量获取服务回调""" self.get_logger().info("收到音量获取请求") response.volume = self.current_volume return response def volume_set_callback(self, request, response): """音量设置服务回调""" self.get_logger().info(f"收到音量设置请求: {request.volume}") self.current_volume = request.volume response.success = True return response def microphone_callback(self, request, response): """麦克风开关服务回调""" state = "开启" if request.status.state == AudioStatus.AUDIO_STATUS_NORMAL else "关闭" self.get_logger().info(f"收到麦克风开关请求: {state} (来自: {request.client})") # 更新麦克风状态 self.microphone_enabled = (request.status.state == AudioStatus.AUDIO_STATUS_NORMAL) response.result = True return response def voice_control_callback(self, request, response): """语音控制开关服务回调""" state = "开启" if request.data else "关闭" self.get_logger().info(f"收到语音控制开关请求: {state}") # 更新语音控制状态 self.voice_control_enabled = request.data response.success = True response.message = f"语音控制已{state}" return response def play_voice_callback(self, request, response): """语音播放服务回调""" play_type = "在线" if request.is_online else "离线" self.get_logger().info(f"收到语音播放请求: {play_type}, 内容: '{request.text}'") # 模拟播放语音 self.simulate_voice_play(request.text) # 假设播放成功 response.status = 0 return response def simulate_voice_play(self, text): """模拟语音播放""" self.get_logger().info(f"播放中: '{text}'...") time.sleep(2) # 模拟播放时间 self.get_logger().info("播放完成") def enable_microphone(self, enable): """启用或禁用麦克风""" # 由于我们自己实现了服务，直接更新状态 self.microphone_enabled = enable status = "启用" if enable else "禁用" self.get_logger().info(f"麦克风已{status}") def enable_voice_control(self, enable): """启用或禁用语音控制垂域功能""" # 由于我们自己实现了服务，直接更新状态 self.voice_control_enabled = enable status = "启用" if enable else "禁用" self.get_logger().info(f"语音控制垂域功能已{status}") def set_volume(self, volume_level): """设置音量""" self.current_volume = volume_level self.get_logger().info(f"音量设置为: {volume_level}%") def play_voice_message(self, text, is_online=True): """播放语音消息""" # 直接调用播放方法，不需要通过服务 self.get_logger().info(f"播放语音: '{text}'") self.simulate_voice_play(text) def voice_listener(self): """监听语音指令的线程""" self.get_logger().info("语音监听线程已启动") # 模拟语音识别 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"收到语音指令: '{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("配货完成，开始运输") self.get_logger().info("配货完成，开始运输") self.current_state = "transporting" def handle_unloading_complete(self): """处理卸货完成指令""" self.play_voice_message("卸货完成，准备新的配货任务") self.get_logger().info("卸货完成，准备新的配货任务") self.current_state = "ready" 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"状态已更新: {new_state}") # 根据状态播放提示音 if new_state == "delivering": self.play_voice_message("已到达配货库位，请说出'配货完成'确认") elif new_state == "unloading": self.play_voice_message("已到达卸货库位，请说出'卸货完成'确认") else: self.get_logger().error(f"无效状态: {new_state}") def main(args=None): rclpy.init(args=args) try: # 创建节点 node = VoiceControlNode() # 创建多线程执行器 executor = MultiThreadedExecutor() executor.add_node(node) # 在单独的线程中运行执行器 executor_thread = threading.Thread(target=executor.spin, daemon=True) executor_thread.start() # 模拟状态变化 time.sleep(2) node.set_state("delivering") # 主线程显示菜单 while rclpy.ok(): print("\n===== 机器狗语音控制系统 =====") print("1. 进入配货状态") print("2. 进入卸货状态") print("3. 开启麦克风") print("4. 关闭麦克风") print("5. 开启语音控制") print("6. 关闭语音控制") print("7. 设置音量") print("8. 退出") choice = input("请选择操作: ") if choice == '1': node.set_state("delivering") elif choice == '2': node.set_state("unloading") elif choice == '3': node.enable_microphone(True) elif choice == '4': node.enable_microphone(False) elif choice == '5': node.enable_voice_control(True) elif choice == '6': node.enable_voice_control(False) elif choice == '7': volume = int(input("请输入音量大小 (0-100): ")) node.set_volume(volume) elif choice == '8': break else: print("无效选择，请重新输入") time.sleep(0.5) except KeyboardInterrupt: pass except Exception as e: node.get_logger().error(f"发生错误: {str(e)}") finally: rclpy.shutdown() if 'executor_thread' in locals(): executor_thread.join() print("系统已关闭") if __name__ == '__main__': main()