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