289 lines
10 KiB
Python
289 lines
10 KiB
Python
#!/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() |