feat: demo

main.py

@@ -51,7 +51,7 @@ def main():
         info("Recovery stand", "info")
         Ctrl.base_msg.stand_up()
         print('yuyin')
-        speak('nihao')
+        # speak('nihao')
         # Ctrl.base_msg.stop()  # BUG 垃圾指令 for eat
 
         # time.sleep(100) # TEST,
@@ -60,10 +60,10 @@ def main():
         # run_task_1(Ctrl, msg, time_sleep=TIME_SLEEP)
 
         # arrow_direction = run_task_2(Ctrl, msg, xy_flag=False)
-        # # arrow_direction = 'right' # TEST
+        # arrow_direction = 'right' # TEST
 
         # info(f"识别到箭头方向: {arrow_direction}", "info")
-        # run_task_2_5(Ctrl, msg, direction=arrow_direction)
+        #run_task_2_5(Ctrl, msg, direction='right')
 
         # if arrow_direction == 'left':
         #     run_task_4(Ctrl, msg)
@@ -72,11 +72,11 @@ def main():
 
         # turn_degree_v2(Ctrl, msg, degree=90, absolute=True)
         # run_task_5(Ctrl, msg, direction=arrow_direction, time_sleep=TIME_SLEEP)
-
-        # if arrow_direction == 'left': 
-        #     run_task_3_back(Ctrl, msg)
-        # else:
-        #     run_task_4_back(Ctrl, msg)
+        arrow_direction = 'left'
+        if arrow_direction == 'left': 
+            run_task_3_back(Ctrl, msg)
+        else:
+            run_task_4_back(Ctrl, msg)
 
         # run_task_2_5_back(Ctrl, msg, direction=arrow_direction)
 

test/demo_test/auredemo.py

@@ -0,0 +1,181 @@
+普在#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String, Bool
+from protocol.srv import AudioTextPlay, AudioVolumeGet, AudioVolumeSet
+from protocol.msg import AudioPlay
+import time
+import threading
+
+class VoiceInteractionSystem(Node):
+    def __init__(self):
+        super().__init__('voice_interaction_system')
+        
+        # 机器狗状态：'preparing_pickup'（准备配货）或 'preparing_delivery'（准备卸货）
+        self.current_state = 'preparing_pickup'  # 初始状态设为准备配货
+        
+        # 创建服务客户端
+        self.audio_play_client = self.create_client(AudioTextPlay, 'speech_text_play')
+        self.volume_get_client = self.create_client(AudioVolumeGet, 'audio_volume_get')
+        self.volume_set_client = self.create_client(AudioVolumeSet, 'audio_volume_set')
+        
+        # 等待服务可用
+        while not self.audio_play_client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('语音播放服务不可用，等待中...')
+        while not self.volume_get_client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('音量获取服务不可用，等待中...')
+        while not self.volume_set_client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('音量设置服务不可用，等待中...')
+        
+        # 创建订阅器，监听语音指令
+        self.voice_command_sub = self.create_subscription(
+            String,
+            'asr_text',  # 语音识别结果的主题
+            self.voice_command_callback,
+            10
+        )
+        
+        # 创建状态发布器
+        self.state_pub = self.create_publisher(String, 'task_state', 10)
+        
+        # 设置初始音量（确保语音清晰可听）
+        self.set_volume(50)
+        
+        self.get_logger().info("语音交互系统已启动，当前状态：准备配货")
+        
+        # 播放欢迎提示
+        self.play_voice_prompt("准备就绪，请开始配货任务")
+    
+    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() and future.result().success:
+            self.get_logger().info(f"音量已设置为: {volume_level}")
+        else:
+            self.get_logger().warn("音量设置失败")
+    
+    def play_voice_prompt(self, text, online=True):
+        self.get_logger().info('语音播放tishi...')
+        """播放语音提示"""
+        req = AudioTextPlay.Request()
+        req.module_name = "task_system"
+        req.is_online = online
+        req.text = text if online else ""
+        
+        # 如果是离线语音，设置对应的语音ID
+        if not online:
+            req.speech = AudioPlay()
+            # 这里使用示例ID，实际应根据协议定义设置
+            req.speech.play_id = 101 if "配货" in text else 102
+        
+        future = self.audio_play_client.call_async(req)
+        rclpy.spin_until_future_complete(self, future)
+        
+        if future.result() and future.result().status == 0:
+            self.get_logger().info(f"语音播放成功: '{text}'")
+        else:
+            self.get_logger().warn("语音播放失败")
+    
+    def voice_command_callback(self, msg):
+        """处理语音指令"""
+        self.get_logger().info('语音chuli...')
+        command = msg.data.lower()
+        self.get_logger().info(f"收到语音指令: {command}")
+        
+        if "完成" in command or "结束" in command:
+            if "配货" in command and self.current_state == 'preparing_pickup':
+                self.handle_pickup_complete()
+            elif "卸货" in command and self.current_state == 'preparing_delivery':
+                self.handle_delivery_complete()
+            else:
+                self.play_voice_prompt("当前状态不支持此操作")
+        elif "状态" in command:
+            self.report_current_state()
+        else:
+            self.play_voice_prompt("未识别的指令，请重试")
+    
+    def handle_pickup_complete(self):
+        """处理配货完成"""
+        self.play_voice_prompt("配货完成，开始运输")
+        self.get_logger().info("配货完成，开始运输")
+        
+        # 更新状态
+        self.current_state = 'in_transit'
+        self.publish_state("in_transit")
+        
+        # 模拟运输过程
+        threading.Thread(target=self.simulate_transport).start()
+    
+    def handle_delivery_complete(self):
+        """处理卸货完成"""
+        self.play_voice_prompt("卸货完成，准备返回")
+        self.get_logger().info("卸货完成，准备返回")
+        
+        # 更新状态
+        self.current_state = 'returning'
+        self.publish_state("returning")
+        
+        # 模拟返回过程
+        threading.Thread(target=self.simulate_return).start()
+    
+    def simulate_transport(self):
+        """模拟运输过程"""
+        time.sleep(5)  # 模拟运输时间
+        
+        # 到达卸货点
+        self.play_voice_prompt("已到达卸货点，请开始卸货")
+        self.get_logger().info("已到达卸货点")
+        
+        # 更新状态
+        self.current_state = 'preparing_delivery'
+        self.publish_state("preparing_delivery")
+    
+    def simulate_return(self):
+        """模拟返回过程"""
+        time.sleep(5)  # 模拟返回时间
+        
+        # 返回配货点
+        self.play_voice_prompt("已返回配货点，准备新的配货任务")
+        self.get_logger().info("已返回配货点")
+        
+        # 更新状态
+        self.current_state = 'preparing_pickup'
+        self.publish_state("preparing_pickup")
+    
+    def report_current_state(self):
+        """报告当前状态"""
+        states = {
+            'preparing_pickup': "准备配货",
+            'in_transit': "运输中",
+            'preparing_delivery': "准备卸货",
+            'returning': "返回中"
+        }
+        state_name = states.get(self.current_state, "未知状态")
+        self.play_voice_prompt(f"当前状态: {state_name}")
+    
+    def publish_state(self, state):
+        """发布当前状态"""
+        msg = String()
+        msg.data = state
+        self.state_pub.publish(msg)
+        self.get_logger().info(f"状态更新: {state}")
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = VoiceInteractionSystem()
+    
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

tmp/auredemo2.py

@@ -0,0 +1,210 @@
+#!/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()