|
|
@@ -0,0 +1,476 @@
|
|
|
+"""
|
|
|
+MQTT 模拟器节点
|
|
|
+模拟外部模块响应大模型发出的 MQTT 命令
|
|
|
+
|
|
|
+功能：
|
|
|
+- 订阅 /ai/agent/command 话题（MQTT），接收大模型的命令
|
|
|
+- 根据命令类型模拟处理延迟
|
|
|
+- 向 /ai/module/status 和 /ai/module/event 话题（MQTT）发布模拟回复
|
|
|
+
|
|
|
+Author: sunrise
|
|
|
+"""
|
|
|
+
|
|
|
+import rclpy
|
|
|
+from rclpy.node import Node
|
|
|
+import paho.mqtt.client as mqtt
|
|
|
+import json
|
|
|
+import time
|
|
|
+import random
|
|
|
+import threading
|
|
|
+from ament_index_python.packages import get_package_share_directory
|
|
|
+import os
|
|
|
+import yaml
|
|
|
+
|
|
|
+
|
|
|
+class MQTTSimulatorNode(Node):
|
|
|
+ """
|
|
|
+ MQTT 模拟器节点
|
|
|
+
|
|
|
+ 订阅大模型发出的 MQTT 命令，模拟外部模块的响应行为
|
|
|
+ """
|
|
|
+
|
|
|
+ def __init__(self):
|
|
|
+ super().__init__('mqtt_simulator_node')
|
|
|
+
|
|
|
+ # ========== 声明参数 ==========
|
|
|
+ self.declare_parameter('config_file', '')
|
|
|
+ self.declare_parameter('mqtt.broker_host', '127.0.0.1')
|
|
|
+ self.declare_parameter('mqtt.broker_port', 1883)
|
|
|
+ self.declare_parameter('mqtt.client_id', 'mqtt_simulator')
|
|
|
+ self.declare_parameter('mqtt.username', '')
|
|
|
+ self.declare_parameter('mqtt.password', '')
|
|
|
+ self.declare_parameter('mqtt.keepalive', 60)
|
|
|
+ self.declare_parameter('topics.command_topic', '/ai/agent/command')
|
|
|
+ self.declare_parameter('topics.status_topic', '/ai/module/status')
|
|
|
+ self.declare_parameter('topics.event_topic', '/ai/module/event')
|
|
|
+ self.declare_parameter('simulator.enable', True)
|
|
|
+ self.declare_parameter('simulator.log_all_messages', True)
|
|
|
+ self.declare_parameter('simulator.enable_random_delay', True)
|
|
|
+ self.declare_parameter('delay.page_load', [1.0, 2.0])
|
|
|
+ self.declare_parameter('delay.face_recognition', [2.0, 4.0])
|
|
|
+ self.declare_parameter('delay.navigation', [1.0, 2.0])
|
|
|
+ self.declare_parameter('delay.customer_service', [1.0, 2.0])
|
|
|
+ self.declare_parameter('delay.visitor_register', [2.0, 3.0])
|
|
|
+ self.declare_parameter('results.face_recognition_success_rate', 1.0)
|
|
|
+ self.declare_parameter('results.always_success', True)
|
|
|
+
|
|
|
+ # ========== 加载配置文件 ==========
|
|
|
+ self._load_config()
|
|
|
+
|
|
|
+ # MQTT 客户端
|
|
|
+ self.mqtt_client = None
|
|
|
+ self.mqtt_connected = False
|
|
|
+
|
|
|
+ # 日志输出
|
|
|
+ self.get_logger().info('MQTT Simulator Node 初始化完成')
|
|
|
+ self.get_logger().info(f'Broker: {self.broker_host}:{self.broker_port}')
|
|
|
+ self.get_logger().info(f'订阅话题: {self.command_topic}')
|
|
|
+ self.get_logger().info(f'发布话题: {self.status_topic}, {self.event_topic}')
|
|
|
+
|
|
|
+ # 如果启用，初始化 MQTT 连接
|
|
|
+ if self.enable:
|
|
|
+ self.init_mqtt()
|
|
|
+ else:
|
|
|
+ self.get_logger().warn('模拟器已禁用')
|
|
|
+
|
|
|
+ def _load_config(self):
|
|
|
+ """
|
|
|
+ 从 YAML 配置文件加载参数
|
|
|
+ """
|
|
|
+ config_file = self.get_parameter('config_file').value
|
|
|
+ if not config_file:
|
|
|
+ pkg_share = get_package_share_directory('mqtt_simulator')
|
|
|
+ config_file = os.path.join(pkg_share, 'config', 'simulator.yaml')
|
|
|
+
|
|
|
+ self.get_logger().info(f'加载配置文件: {config_file}')
|
|
|
+
|
|
|
+ if os.path.exists(config_file):
|
|
|
+ try:
|
|
|
+ with open(config_file, 'r') as f:
|
|
|
+ yaml_config = yaml.safe_load(f)
|
|
|
+
|
|
|
+ mqtt_cfg = yaml_config.get('mqtt', {})
|
|
|
+ topics_cfg = yaml_config.get('topics', {})
|
|
|
+ sim_cfg = yaml_config.get('simulator', {})
|
|
|
+ delay_cfg = yaml_config.get('delay', {})
|
|
|
+ results_cfg = yaml_config.get('results', {})
|
|
|
+
|
|
|
+ self.broker_host = mqtt_cfg.get('broker_host', '127.0.0.1')
|
|
|
+ self.broker_port = mqtt_cfg.get('broker_port', 1883)
|
|
|
+ self.client_id = mqtt_cfg.get('client_id', 'mqtt_simulator')
|
|
|
+ self.username = mqtt_cfg.get('username', '')
|
|
|
+ self.password = mqtt_cfg.get('password', '')
|
|
|
+ self.keepalive = mqtt_cfg.get('keepalive', 60)
|
|
|
+ self.command_topic = topics_cfg.get('command_topic', '/ai/agent/command')
|
|
|
+ self.status_topic = topics_cfg.get('status_topic', '/ai/module/status')
|
|
|
+ self.event_topic = topics_cfg.get('event_topic', '/ai/module/event')
|
|
|
+ self.enable = sim_cfg.get('enable', True)
|
|
|
+ self.log_all_messages = sim_cfg.get('log_all_messages', True)
|
|
|
+ self.enable_random_delay = sim_cfg.get('enable_random_delay', True)
|
|
|
+ self.page_load_delay = delay_cfg.get('page_load', [1.0, 2.0])
|
|
|
+ self.face_recognition_delay = delay_cfg.get('face_recognition', [2.0, 4.0])
|
|
|
+ self.navigation_delay = delay_cfg.get('navigation', [1.0, 2.0])
|
|
|
+ self.customer_service_delay = delay_cfg.get('customer_service', [1.0, 2.0])
|
|
|
+ self.visitor_register_delay = delay_cfg.get('visitor_register', [2.0, 3.0])
|
|
|
+ self.face_success_rate = results_cfg.get('face_recognition_success_rate', 1.0)
|
|
|
+ self.always_success = results_cfg.get('always_success', True)
|
|
|
+
|
|
|
+ self.get_logger().info('配置文件加载成功')
|
|
|
+
|
|
|
+ except Exception as e:
|
|
|
+ self.get_logger().warn(f'配置文件加载失败: {e}，使用默认值')
|
|
|
+ self._use_default_config()
|
|
|
+ else:
|
|
|
+ self.get_logger().warn(f'配置文件不存在: {config_file}，使用默认值')
|
|
|
+ self._use_default_config()
|
|
|
+
|
|
|
+ def _use_default_config(self):
|
|
|
+ """
|
|
|
+ 使用默认配置
|
|
|
+ """
|
|
|
+ self.broker_host = self.get_parameter('mqtt.broker_host').value
|
|
|
+ self.broker_port = self.get_parameter('mqtt.broker_port').value
|
|
|
+ self.client_id = self.get_parameter('mqtt.client_id').value
|
|
|
+ self.username = self.get_parameter('mqtt.username').value
|
|
|
+ self.password = self.get_parameter('mqtt.password').value
|
|
|
+ self.keepalive = self.get_parameter('mqtt.keepalive').value
|
|
|
+ self.command_topic = self.get_parameter('topics.command_topic').value
|
|
|
+ self.status_topic = self.get_parameter('topics.status_topic').value
|
|
|
+ self.event_topic = self.get_parameter('topics.event_topic').value
|
|
|
+ self.enable = self.get_parameter('simulator.enable').value
|
|
|
+ self.log_all_messages = self.get_parameter('simulator.log_all_messages').value
|
|
|
+ self.enable_random_delay = self.get_parameter('simulator.enable_random_delay').value
|
|
|
+ self.page_load_delay = self.get_parameter('delay.page_load').value
|
|
|
+ self.face_recognition_delay = self.get_parameter('delay.face_recognition').value
|
|
|
+ self.navigation_delay = self.get_parameter('delay.navigation').value
|
|
|
+ self.customer_service_delay = self.get_parameter('delay.customer_service').value
|
|
|
+ self.visitor_register_delay = self.get_parameter('delay.visitor_register').value
|
|
|
+ self.face_success_rate = self.get_parameter('results.face_recognition_success_rate').value
|
|
|
+ self.always_success = self.get_parameter('results.always_success').value
|
|
|
+
|
|
|
+ def init_mqtt(self):
|
|
|
+ """
|
|
|
+ 初始化 MQTT 连接
|
|
|
+ """
|
|
|
+ try:
|
|
|
+ self.mqtt_client = mqtt.Client(client_id=self.client_id)
|
|
|
+
|
|
|
+ if self.username:
|
|
|
+ self.mqtt_client.username_pw_set(self.username, self.password)
|
|
|
+
|
|
|
+ self.mqtt_client.on_connect = self._on_connect
|
|
|
+ self.mqtt_client.on_disconnect = self._on_disconnect
|
|
|
+ self.mqtt_client.on_message = self._on_mqtt_message
|
|
|
+
|
|
|
+ self.get_logger().info(f'正在连接到 MQTT Broker: {self.broker_host}:{self.broker_port}')
|
|
|
+ self.mqtt_client.connect(self.broker_host, self.broker_port, self.keepalive)
|
|
|
+ self.mqtt_client.loop_start()
|
|
|
+
|
|
|
+ except Exception as e:
|
|
|
+ self.get_logger().error(f'MQTT 初始化失败: {e}')
|
|
|
+
|
|
|
+ def _on_connect(self, client, userdata, flags, rc):
|
|
|
+ """
|
|
|
+ MQTT 连接回调
|
|
|
+ """
|
|
|
+ if rc == 0:
|
|
|
+ self.mqtt_connected = True
|
|
|
+ self.get_logger().info('MQTT 连接成功')
|
|
|
+
|
|
|
+ try:
|
|
|
+ client.subscribe(self.command_topic)
|
|
|
+ self.get_logger().info(f'已订阅话题: {self.command_topic}')
|
|
|
+ except Exception as e:
|
|
|
+ self.get_logger().error(f'订阅话题失败: {e}')
|
|
|
+ else:
|
|
|
+ self.mqtt_connected = False
|
|
|
+ self.get_logger().error(f'MQTT 连接失败, rc={rc}')
|
|
|
+
|
|
|
+ def _on_disconnect(self, client, userdata, rc):
|
|
|
+ """
|
|
|
+ MQTT 断开连接回调
|
|
|
+ """
|
|
|
+ self.mqtt_connected = False
|
|
|
+ if rc != 0:
|
|
|
+ self.get_logger().warn(f'MQTT 意外断开, rc={rc}')
|
|
|
+ else:
|
|
|
+ self.get_logger().info('MQTT 连接已断开')
|
|
|
+
|
|
|
+ def _on_mqtt_message(self, client, userdata, msg):
|
|
|
+ """
|
|
|
+ MQTT 消息回调
|
|
|
+ """
|
|
|
+ try:
|
|
|
+ topic = msg.topic
|
|
|
+ payload_str = msg.payload.decode('utf-8')
|
|
|
+
|
|
|
+ if self.log_all_messages:
|
|
|
+ self.get_logger().info(f'收到 MQTT 消息 [{topic}]: {payload_str}')
|
|
|
+
|
|
|
+ data = json.loads(payload_str)
|
|
|
+
|
|
|
+ thread = threading.Thread(target=self._handle_command, args=(data,))
|
|
|
+ thread.daemon = True
|
|
|
+ thread.start()
|
|
|
+
|
|
|
+ except json.JSONDecodeError as e:
|
|
|
+ self.get_logger().warn(f'JSON 解析失败: {e}')
|
|
|
+ except Exception as e:
|
|
|
+ self.get_logger().error(f'处理消息异常: {e}')
|
|
|
+
|
|
|
+ def _get_delay(self, delay_config):
|
|
|
+ """
|
|
|
+ 获取延迟时间
|
|
|
+
|
|
|
+ Args:
|
|
|
+ delay_config: 延迟配置 [min, max]
|
|
|
+
|
|
|
+ Returns:
|
|
|
+ float: 延迟时间（秒）
|
|
|
+ """
|
|
|
+ if self.enable_random_delay and isinstance(delay_config, list) and len(delay_config) == 2:
|
|
|
+ return random.uniform(delay_config[0], delay_config[1])
|
|
|
+ elif isinstance(delay_config, list) and len(delay_config) == 2:
|
|
|
+ return (delay_config[0] + delay_config[1]) / 2
|
|
|
+ else:
|
|
|
+ return float(delay_config)
|
|
|
+
|
|
|
+ def _handle_command(self, data):
|
|
|
+ """
|
|
|
+ 处理命令（在新线程中执行）
|
|
|
+
|
|
|
+ Args:
|
|
|
+ data: 命令数据字典
|
|
|
+ """
|
|
|
+ try:
|
|
|
+ target = data.get('target', '')
|
|
|
+ module = data.get('module', '')
|
|
|
+ action = data.get('action', '')
|
|
|
+ page = data.get('page', '')
|
|
|
+ original_msg_id = data.get('msg_id', '')
|
|
|
+
|
|
|
+ self.get_logger().info(f'处理命令: target={target}, module={module}, action={action}, page={page}')
|
|
|
+
|
|
|
+ if action == 'open_page':
|
|
|
+ self._handle_open_page(target, module, page, original_msg_id)
|
|
|
+ elif action == 'start':
|
|
|
+ self._handle_start(target, module, original_msg_id, data.get('payload', {}))
|
|
|
+ elif action == 'pause':
|
|
|
+ self._handle_pause(original_msg_id)
|
|
|
+ elif action == 'navigate':
|
|
|
+ self._handle_navigate(target, original_msg_id, data.get('payload', {}))
|
|
|
+ elif action == 'call':
|
|
|
+ self._handle_call_service(original_msg_id)
|
|
|
+ elif action == 'register':
|
|
|
+ self._handle_visitor_register(original_msg_id)
|
|
|
+ else:
|
|
|
+ self.get_logger().warn(f'未知动作类型: {action}')
|
|
|
+
|
|
|
+ except Exception as e:
|
|
|
+ self.get_logger().error(f'处理命令异常: {e}')
|
|
|
+
|
|
|
+ def _handle_open_page(self, target, module, page, original_msg_id):
|
|
|
+ """
|
|
|
+ 处理打开页面命令
|
|
|
+ """
|
|
|
+ delay = self._get_delay(self.page_load_delay)
|
|
|
+ self.get_logger().info(f'模拟页面加载，延迟 {delay:.2f} 秒')
|
|
|
+ time.sleep(delay)
|
|
|
+
|
|
|
+ status_msg = {
|
|
|
+ 'msg_id': f'sim_{int(time.time() * 1000)}',
|
|
|
+ 'source': 'simulator',
|
|
|
+ 'target': 'agent',
|
|
|
+ 'module': module,
|
|
|
+ 'status': 'page_opened',
|
|
|
+ 'page': page,
|
|
|
+ 'original_msg_id': original_msg_id,
|
|
|
+ 'timestamp': int(time.time())
|
|
|
+ }
|
|
|
+
|
|
|
+ self._publish_mqtt(self.status_topic, status_msg)
|
|
|
+ self.get_logger().info(f'已回复页面打开状态: {page}')
|
|
|
+
|
|
|
+ def _handle_start(self, target, module, original_msg_id, payload):
|
|
|
+ """
|
|
|
+ 处理启动命令（主要用于人脸识别）
|
|
|
+ """
|
|
|
+ if target == 'face':
|
|
|
+ self._handle_face_recognition(original_msg_id, payload)
|
|
|
+ else:
|
|
|
+ self.get_logger().warn(f'未知启动目标: {target}')
|
|
|
+
|
|
|
+ def _handle_face_recognition(self, original_msg_id, payload):
|
|
|
+ """
|
|
|
+ 处理人脸识别
|
|
|
+ """
|
|
|
+ delay = self._get_delay(self.face_recognition_delay)
|
|
|
+ self.get_logger().info(f'模拟人脸识别，延迟 {delay:.2f} 秒')
|
|
|
+ time.sleep(delay)
|
|
|
+
|
|
|
+ if self.always_success:
|
|
|
+ success = True
|
|
|
+ else:
|
|
|
+ success = random.random() < self.face_success_rate
|
|
|
+
|
|
|
+ if success:
|
|
|
+ event_data = {
|
|
|
+ 'msg_id': f'sim_{int(time.time() * 1000)}',
|
|
|
+ 'source': 'simulator',
|
|
|
+ 'event': 'face_recognition_result',
|
|
|
+ 'original_msg_id': original_msg_id,
|
|
|
+ 'data': {
|
|
|
+ 'success': True,
|
|
|
+ 'person_id': 'visitor_001',
|
|
|
+ 'person_name': '测试用户',
|
|
|
+ 'confidence': round(random.uniform(0.85, 0.99), 2)
|
|
|
+ },
|
|
|
+ 'timestamp': int(time.time())
|
|
|
+ }
|
|
|
+ else:
|
|
|
+ event_data = {
|
|
|
+ 'msg_id': f'sim_{int(time.time() * 1000)}',
|
|
|
+ 'source': 'simulator',
|
|
|
+ 'event': 'face_recognition_result',
|
|
|
+ 'original_msg_id': original_msg_id,
|
|
|
+ 'data': {
|
|
|
+ 'success': False,
|
|
|
+ 'reason': 'not_found',
|
|
|
+ 'confidence': 0.0
|
|
|
+ },
|
|
|
+ 'timestamp': int(time.time())
|
|
|
+ }
|
|
|
+
|
|
|
+ self._publish_mqtt(self.event_topic, event_data)
|
|
|
+ self.get_logger().info(f'已回复人脸识别结果: success={success}')
|
|
|
+
|
|
|
+ def _handle_pause(self, original_msg_id):
|
|
|
+ """
|
|
|
+ 处理暂停命令
|
|
|
+ """
|
|
|
+ self.get_logger().info('收到全局暂停命令，已记录')
|
|
|
+
|
|
|
+ def _handle_navigate(self, target, original_msg_id, payload):
|
|
|
+ """
|
|
|
+ 处理导航命令
|
|
|
+ """
|
|
|
+ delay = self._get_delay(self.navigation_delay)
|
|
|
+ self.get_logger().info(f'模拟导航操作，延迟 {delay:.2f} 秒')
|
|
|
+ time.sleep(delay)
|
|
|
+
|
|
|
+ status_msg = {
|
|
|
+ 'msg_id': f'sim_{int(time.time() * 1000)}',
|
|
|
+ 'source': 'simulator',
|
|
|
+ 'target': 'agent',
|
|
|
+ 'module': 'nav',
|
|
|
+ 'status': 'navigation_completed',
|
|
|
+ 'original_msg_id': original_msg_id,
|
|
|
+ 'timestamp': int(time.time())
|
|
|
+ }
|
|
|
+
|
|
|
+ self._publish_mqtt(self.status_topic, status_msg)
|
|
|
+ self.get_logger().info('已回复导航完成状态')
|
|
|
+
|
|
|
+ def _handle_call_service(self, original_msg_id):
|
|
|
+ """
|
|
|
+ 处理呼叫客服命令
|
|
|
+ """
|
|
|
+ delay = self._get_delay(self.customer_service_delay)
|
|
|
+ self.get_logger().info(f'模拟呼叫客服，延迟 {delay:.2f} 秒')
|
|
|
+ time.sleep(delay)
|
|
|
+
|
|
|
+ status_msg = {
|
|
|
+ 'msg_id': f'sim_{int(time.time() * 1000)}',
|
|
|
+ 'source': 'simulator',
|
|
|
+ 'target': 'agent',
|
|
|
+ 'module': 'customer_service',
|
|
|
+ 'status': 'call_connected',
|
|
|
+ 'original_msg_id': original_msg_id,
|
|
|
+ 'timestamp': int(time.time())
|
|
|
+ }
|
|
|
+
|
|
|
+ self._publish_mqtt(self.status_topic, status_msg)
|
|
|
+ self.get_logger().info('已回复客服呼叫成功')
|
|
|
+
|
|
|
+ def _handle_visitor_register(self, original_msg_id):
|
|
|
+ """
|
|
|
+ 处理访客登记命令
|
|
|
+ """
|
|
|
+ delay = self._get_delay(self.visitor_register_delay)
|
|
|
+ self.get_logger().info(f'模拟访客登记，延迟 {delay:.2f} 秒')
|
|
|
+ time.sleep(delay)
|
|
|
+
|
|
|
+ status_msg = {
|
|
|
+ 'msg_id': f'sim_{int(time.time() * 1000)}',
|
|
|
+ 'source': 'simulator',
|
|
|
+ 'target': 'agent',
|
|
|
+ 'module': 'visitor',
|
|
|
+ 'status': 'registration_completed',
|
|
|
+ 'original_msg_id': original_msg_id,
|
|
|
+ 'data': {
|
|
|
+ 'visitor_id': f'V{int(time.time())}',
|
|
|
+ 'visitor_name': '访客'
|
|
|
+ },
|
|
|
+ 'timestamp': int(time.time())
|
|
|
+ }
|
|
|
+
|
|
|
+ self._publish_mqtt(self.status_topic, status_msg)
|
|
|
+ self.get_logger().info('已回复访客登记完成')
|
|
|
+
|
|
|
+ def _publish_mqtt(self, topic, data):
|
|
|
+ """
|
|
|
+ 通过 MQTT 发布消息
|
|
|
+
|
|
|
+ Args:
|
|
|
+ topic: MQTT topic
|
|
|
+ data: 要发布的数据字典
|
|
|
+ """
|
|
|
+ if self.mqtt_client is None or not self.mqtt_connected:
|
|
|
+ self.get_logger().warn(f'MQTT 未连接，跳过发布: topic={topic}')
|
|
|
+ return
|
|
|
+
|
|
|
+ try:
|
|
|
+ payload = json.dumps(data, ensure_ascii=False)
|
|
|
+ result = self.mqtt_client.publish(topic, payload)
|
|
|
+
|
|
|
+ if result.rc == mqtt.MQTT_ERR_SUCCESS:
|
|
|
+ if self.log_all_messages:
|
|
|
+ self.get_logger().info(f'[MQTT发布] topic={topic}, payload={payload}')
|
|
|
+ else:
|
|
|
+ self.get_logger().error(f'[MQTT发布] 失败, rc={result.rc}')
|
|
|
+
|
|
|
+ except Exception as e:
|
|
|
+ self.get_logger().error(f'[MQTT发布] 异常: {e}')
|
|
|
+
|
|
|
+ def destroy_node(self):
|
|
|
+ """
|
|
|
+ 销毁节点
|
|
|
+ """
|
|
|
+ if self.mqtt_client is not None:
|
|
|
+ try:
|
|
|
+ self.mqtt_client.loop_stop()
|
|
|
+ self.mqtt_client.disconnect()
|
|
|
+ self.get_logger().info('MQTT 连接已关闭')
|
|
|
+ except Exception as e:
|
|
|
+ self.get_logger().warn(f'关闭 MQTT 连接时出错: {e}')
|
|
|
+
|
|
|
+ super().destroy_node()
|
|
|
+
|
|
|
+
|
|
|
+def main(args=None):
|
|
|
+ """
|
|
|
+ 入口函数
|
|
|
+ """
|
|
|
+ rclpy.init(args=args)
|
|
|
+
|
|
|
+ try:
|
|
|
+ simulator_node = MQTTSimulatorNode()
|
|
|
+ rclpy.spin(simulator_node)
|
|
|
+ except KeyboardInterrupt:
|
|
|
+ pass
|
|
|
+ finally:
|
|
|
+ rclpy.shutdown()
|
|
|
+
|
|
|
+
|
|
|
+if __name__ == '__main__':
|
|
|
+ main()
|
