From 857f310b65ce90e34a660a95a24a10bcbeadc1f5 Mon Sep 17 00:00:00 2001 From: hav <123@q.com> Date: Tue, 19 Aug 2025 14:47:19 +0800 Subject: [PATCH] Update --- test/demo_test/rgb_camera_demo.py | 78 ++++++++ tmp/auredemo2.py | 309 +++++++++++++++++++----------- utils/image_raw.py | 1 + 3 files changed, 273 insertions(+), 115 deletions(-) create mode 100644 test/demo_test/rgb_camera_demo.py diff --git a/test/demo_test/rgb_camera_demo.py b/test/demo_test/rgb_camera_demo.py new file mode 100644 index 0000000..c0a2390 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/tmp/auredemo2.py b/tmp/auredemo2.py index 326ef69..6db76ee 100644 --- a/tmp/auredemo2.py +++ b/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") - - # 等待配货完成 - time.sleep(10) - - # 进入卸货状态 - node.set_state("unloading") - - # 等待卸货完成 - time.sleep(10) - - rclpy.spin(node) - rclpy.shutdown() + 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() \ No newline at end of file diff --git a/utils/image_raw.py b/utils/image_raw.py index f9539ed..93c5e76 100755 --- a/utils/image_raw.py +++ b/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: