Update

test/demo_test/rgb_camera_demo.py

@@ -0,0 +1,78 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge
+import cv2
+import time
+from datetime import datetime
+
+class CameraSubscriber(Node):
+    def __init__(self):
+        super().__init__('camera_subscriber')
+        
+        self.bridge = CvBridge()
+        self.screenshot_taken = False
+        self.start_time = time.time()
+        self.wait_time = 10.0  # 最大等待时间
+        
+        # 订阅带诊断功能
+        self.sub_rgb = self.create_subscription(
+            Image, 
+            '/mi_desktop_48_b0_2d_7b_05_1d/image',  # 修改为实际发布的话题
+            self.rgb_callback, 
+            10)
+        
+        # 定时器检查连接状态
+        self.timer = self.create_timer(1.0, self.check_connection)
+        
+        self.get_logger().info("wrong!")
+
+    def check_connection(self):
+        elapsed = time.time() - self.start_time
+        if elapsed > self.wait_time and not any([hasattr(self, f'{cam}_image') 
+                                               for cam in ['left', 'right', 'rgb']]):
+            self.get_logger().error(f"No images received after {self.wait_time} seconds!")
+            self.diagnose_connection()
+
+    def diagnose_connection(self):
+        self.get_logger().info("Running diagnostics...")
+        # 检查话题列表
+        topics = self.get_topic_names_and_types()
+        cam_topics = [t[0] for t in topics if 'image' in t[0].lower()]
+        self.get_logger().info(f"Available image topics: {cam_topics}")
+        
+        # 建议用户检查的命令
+        self.get_logger().info("Suggested checks:")
+
+    def save_image(self, image, name):
+        if image is not None:
+            timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
+            filename = f"{name}_{timestamp}.png"
+            cv2.imwrite(filename, image)
+            self.get_logger().info(f"Saved {filename}")
+
+
+
+    def rgb_callback(self, msg):
+        self.rgb_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
+        self.check_screenshot()
+
+    def check_screenshot(self):
+        if time.time() - self.start_time >= 3.0 and not self.screenshot_taken:
+            self.save_image(self.rgb_image, 'rgb')
+            self.screenshot_taken = True
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = CameraSubscriber()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

tmp/auredemo2.py

@@ -3,8 +3,10 @@
 
 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
+from protocol.srv import AudioExecute, AudioVolumeSet, AudioTextPlay, AudioVolumeGet
 from protocol.msg import AudioStatus
 import threading
 import time
@@ -13,32 +15,19 @@ 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.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')
+        # 创建所有必要的语音服务
+        self.create_audio_services()
         
-        # 确保服务可用
-        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%
@@ -48,74 +37,138 @@ class VoiceControlNode(Node):
         self.listening_thread.daemon = True
         self.listening_thread.start()
         
-        self.get_logger().info("Voice Control Node Initialized")
+        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):
         """启用或禁用麦克风"""
-        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")
+        # 由于我们自己实现了服务，直接更新状态
+        self.microphone_enabled = enable
+        status = "启用" if enable else "禁用"
+        self.get_logger().info(f"麦克风已{status}")
 
     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")
+        # 由于我们自己实现了服务，直接更新状态
+        self.voice_control_enabled = enable
+        status = "启用" if enable else "禁用"
+        self.get_logger().info(f"语音控制垂域功能已{status}")
 
     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")
+        self.current_volume = volume_level
+        self.get_logger().info(f"音量设置为: {volume_level}%")
 
     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}")
+        # 直接调用播放方法，不需要通过服务
+        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:
@@ -131,7 +184,7 @@ class VoiceControlNode(Node):
 
     def process_voice_command(self, command):
         """处理语音命令"""
-        self.get_logger().info(f"Received voice command: {command}")
+        self.get_logger().info(f"收到语音指令: '{command}'")
         
         if self.current_state == "delivering" and "配货完成" in command:
             self.handle_delivery_complete()
@@ -143,39 +196,21 @@ class VoiceControlNode(Node):
     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")
+        self.get_logger().info("配货完成，开始运输")
+        self.current_state = "transporting"
 
     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")
+        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"State changed to: {new_state}")
+            self.get_logger().info(f"状态已更新: {new_state}")
             
             # 根据状态播放提示音
             if new_state == "delivering":
@@ -183,28 +218,72 @@ class VoiceControlNode(Node):
             elif new_state == "unloading":
                 self.play_voice_message("已到达卸货库位，请说出'卸货完成'确认")
         else:
-            self.get_logger().error(f"Invalid state: {new_state}")
+            self.get_logger().error(f"无效状态: {new_state}")
 
 def main(args=None):
     rclpy.init(args=args)
-    node = VoiceControlNode()
     
-    # 模拟状态变化 - 在实际应用中应由其他节点设置
-    # 进入配货状态
-    time.sleep(5)
-    node.set_state("delivering")
+    try:
+        # 创建节点
+        node = VoiceControlNode()
         
-    # 等待配货完成
-    time.sleep(10)
+        # 创建多线程执行器
+        executor = MultiThreadedExecutor()
+        executor.add_node(node)
         
-    # 进入卸货状态
-    node.set_state("unloading")
+        # 在单独的线程中运行执行器
+        executor_thread = threading.Thread(target=executor.spin, daemon=True)
+        executor_thread.start()
         
-    # 等待卸货完成
-    time.sleep(10)
+        # 模拟状态变化
+        time.sleep(2)
+        node.set_state("delivering")
         
-    rclpy.spin(node)
-    rclpy.shutdown()
+        # 主线程显示菜单
+        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()

utils/image_raw.py

@@ -123,6 +123,7 @@ class ImageSubscriber(Node):
         current_time = time.time()
         
         try:
+            print(msg)
             self.cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
             
             if current_time - self.last_save_time >= self.save_interval: