# PlannerNode2 机器人决策系统

机器人智能决策系统，基于 ROS2 构建，包含配置管理、环境感知、导航模拟和大模型服务等核心节点。

---

## 目录结构

```
PlannerNode2/
├── config_node/          # 配置管理节点
├── environment_node/     # 环境感知节点
├── nav_simulator/        # 导航模拟节点
├── largemodel/           # 大模型服务节点
├── README.md             # 本文档
```

---

## 节点概述

### 1. config_node (配置管理节点)

**功能说明**

- 从 MySQL 数据库读取配置（当前版本使用模拟数据）
- 定时刷新配置并发布到 `/ai/config` topic
- 支持 YAML 文件作为 fallback 配置

**发布话题**

| 话题名称 | 类型 | 说明 |
|---------|------|------|
| `/ai/config` | `std_msgs/String` | 配置数据 (JSON 格式) |

**配置内容**

- ASR 语音识别配置
- Action Service 动作服务配置
- Model Service 模型服务配置
- 大模型接口配置（阿里百炼、百度 TTS 等）
- Topic 配置
- Environment 节点配置

**启动方式**

```bash
ros2 launch config_node config_node.launch.py
```

**参数说明**

| 参数名 | 类型 | 默认值 | 说明 |
|-------|------|--------|------|
| `config_file` | string | `config/database.yaml` | 数据库配置文件路径 |
| `refresh_interval` | double | `1.0` | 配置刷新间隔 (秒) |
| `use_mock_data` | bool | `True` | 是否使用模拟数据 |
| `use_yaml_fallback` | bool | `True` | 是否使用 YAML fallback |
| `topic_name` | string | `/ai/config` | 配置发布话题 |

---

### 2. environment_node (环境感知节点)

**功能说明**

- 订阅 `config_node` 获取配置
- 统一发布机器人环境相关数据
- 支持电池、温度、天气、地图导航点等数据
- 使用模拟数据进行测试

**订阅话题**

| 话题名称 | 类型 | 说明 |
|---------|------|------|
| `/ai/config` | `std_msgs/String` | 配置数据 (来自 config_node) |

**发布话题**

| 话题名称 | 类型 | 说明 |
|---------|------|------|
| `/ai/env` | `std_msgs/String` | 环境数据 (JSON 格式) |

**环境数据结构**

```json
{
  "timestamp": "2026-05-12T15:02:26",
  "battery": {
    "level": 85,
    "voltage": 12.6,
    "is_charging": false,
    "capacity_wh": 5000,
    "current_ma": -500
  },
  "temperature": {
    "indoor": 25.5,
    "outdoor": 28.0,
    "unit": "celsius",
    "humidity": 60
  },
  "weather": {
    "condition": "sunny",
    "description": "晴",
    "temperature": 28,
    "humidity": 65,
    "wind_speed": 3.5
  },
  "map": {
    "points": [
      {"id": "A", "name": "办公室", "position": {"x": 1.633, "y": 3.490, "z": 0.0}},
      {"id": "B", "name": "酒店大堂", "position": {"x": 2.436, "y": -0.574, "z": 0.0}},
      {"id": "C", "name": "园区", "position": {"x": 0.024, "y": -1.820, "z": 0.0}}
    ]
  }
}
```

**启动方式**

```bash
ros2 launch environment_node environment.launch.py
```

**参数说明**

| 参数名 | 类型 | 默认值 | 说明 |
|-------|------|--------|------|
| `config_topic` | string | `/ai/config` | 配置订阅话题 |
| `environment_topic` | string | `/ai/env` | 环境发布话题 |
| `use_mock_data` | bool | `True` | 是否使用模拟数据 |

---

### 3. nav_simulator (导航模拟节点)

**功能说明**

- 模拟 TF 变换 (map → base_footprint)
- 提供 NavigateToPose Action Server
- 模拟机器人移动过程
- 加载 `map_mapping.yaml` 中的导航点配置
- 用于在没有真实机器人/定位系统时测试导航功能

**发布话题**

| 话题名称 | 类型 | 说明 |
|---------|------|------|
| `/tf` | `tf2_msgs/TFMessage` | TF 变换 |

**服务/动作**

| 名称 | 类型 | 说明 |
|------|------|------|
| `/navigate_to_pose` | `nav2_msgs/NavigateToPose` | 导航到目标点 |

**动作请求格式**

使用 `geometry_msgs/PoseStamped` 指定目标位置和朝向。

**启动方式**

```bash
ros2 launch nav_simulator nav_simulator.launch.py
```

**参数说明**

| 参数名 | 类型 | 默认值 | 说明 |
|-------|------|--------|------|
| `map_mapping_file` | string | `largemodel/config/map_mapping.yaml` | 地图导航点配置文件 |
| `initial_x` | double | `0.0` | 初始 X 坐标 |
| `initial_y` | double | `0.0` | 初始 Y 坐标 |
| `initial_z` | double | `0.0` | 初始 Z 坐标 |
| `initial_yaw` | double | `0.0` | 初始朝向角 (弧度) |
| `move_speed` | double | `0.5` | 移动速度 (m/s) |
| `tf_publish_rate` | double | `10.0` | TF 发布频率 (Hz) |
| `simulate_move` | bool | `True` | 是否模拟移动过程 |

**地图配置文件格式**

```yaml
A:
  position:
    x: 1.633
    y: 3.490
    z: 0.0
  orientation:
    yaw: 0.0

B:
  position:
    x: 2.436
    y: -0.574
    z: 0.0
  orientation:
    yaw: 1.57
```

---

### 4. largemodel (大模型服务节点)

**功能说明**

- 对接大模型 API（阿里百炼等）
- 订阅 ASR 语音识别结果
- 发布动作指令
- 支持对话聊天和动作任务执行

**相关文档**

详见 `largemodel/README.md`

---

## 快速启动

### 一键启动所有节点

```bash
# 启动 config_node
ros2 launch config_node config_node.launch.py

# 启动 environment_node
ros2 launch environment_node environment.launch.py

# 启动 nav_simulator
ros2 launch nav_simulator nav_simulator.launch.py

# 启动 largemodel
ros2 launch largemodel largemodel_control.launch.py 
```

### 测试环境数据发布

```bash
# 查看发布的环境数据
ros2 topic echo /ai/env
```

### 测试配置发布

```bash
# 查看发布的配置
ros2 topic echo /ai/config
```

### 测试导航模拟

```bash
# 使用命令行发送导航目标
ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "{pose: {header: {frame_id: 'map'}, pose: {position: {x: 1.633, y: 3.490, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}}"
```

---

## 依赖说明

- ROS2 (Humble 或更高版本)
- Python 3.8+
- rclpy
- std_msgs
- nav2_msgs
- geometry_msgs
- tf2_ros
- ament_index_python

---

## 作者

sunrise

---

## 更新日志

| 日期 | 版本 | 更新内容 |
|------|------|---------|
| 2026-05-12 | 1.0.0 | 初始版本，包含 config_node、environment_node、nav_simulator |
| 2026-05-20 | 1.1.0 | 新增 MQTT 客户端模块及业务函数，支持与外部模块通信 |

---

## MQTT 通信协议

### 概述

MQTT（Message Queuing Telemetry Transport）是轻量级的发布/订阅消息传输协议，本系统使用 MQTT 实现智能体与外部业务模块（屏幕、机械臂、人脸识别等）之间的通信。

**依赖安装**

```bash
pip install paho-mqtt
```

### MQTT 客户端

**文件位置**: `largemodel/utils/mqttclient.py`

**类名**: `MQTTClient`

**初始化方式**

```python
from utils.mqttclient import MQTTClient

mqtt_client = MQTTClient(logger=self.get_logger())
mqtt_client.init(mqtt_config)
```

**MQTT 配置格式**（来自 `/ai/config`）

```json
{
  "mqtt": {
    "enable": true,
    "broker_host": "192.168.0.30",
    "broker_port": 1883,
    "username": "",
    "password": "",
    "client_id": "rdk_agent",
    "keepalive": 60
  }
}
```

**配置参数说明**

| 参数 | 类型 | 默认值 | 说明 |
|------|------|--------|------|
| `enable` | bool | `false` | 是否启用 MQTT |
| `broker_host` | string | `"127.0.0.1"` | MQTT Broker 地址 |
| `broker_port` | int | `1883` | MQTT Broker 端口 |
| `username` | string | `""` | 用户名（可选） |
| `password` | string | `""` | 密码（可选） |
| `client_id` | string | `"rdk_agent"` | 客户端唯一标识 |
| `keepalive` | int | `60` | 保活时间（秒） |

---

### 消息格式

**命令消息结构**

所有通过 `publish_command()` 发送的消息统一使用以下 JSON 格式：

```json
{
  "msg_id": "agent_<timestamp_ms>",
  "source": "agent",
  "target": "<目标模块>",
  "module": "<模块类型>",
  "action": "<动作名称>",
  "page": "<页面标识>",
  "payload": {
    // 额外参数
  },
  "timestamp": <unix_timestamp>
}
```

**字段说明**

| 字段 | 类型 | 必填 | 说明 |
|------|------|------|------|
| `msg_id` | string | 是 | 消息唯一 ID，格式：`agent_<毫秒时间戳>` |
| `source` | string | 是 | 消息来源，固定为 `"agent"` |
| `target` | string | 是 | 目标模块（见下方模块列表） |
| `module` | string | 是 | 模块类型 |
| `action` | string | 是 | 动作名称 |
| `page` | string | 否 | 页面标识，用于 UI 相关操作 |
| `payload` | object | 否 | 额外参数 |
| `timestamp` | int | 是 | Unix 时间戳（秒） |

---

### 目标模块 (target)

| target 值 | 说明 | 接收方 |
|-----------|------|--------|
| `all` | 所有模块（广播） | 全系统 |
| `screen` | 屏幕/UI 模块 | 屏幕控制系统 |
| `face` | 人脸识别模块 | 人脸识别服务 |
| `nav` | 导航模块 | 导航控制系统 |
| `visitor` | 访客管理模块 | 访客登记系统 |
| `customer_service` | 客服模块 | 客服呼叫系统 |
| `advertisement` | 广告模块 | 广告播放系统 |
| `arm` | 机械臂模块 | 机械臂控制系统 |

---

### 已实现的业务函数

#### 1. global_pause - 全局暂停

通知所有外部模块暂停当前业务。

**使用场景**
- 机器人被唤醒时
- 进入语音交互前
- 欢迎语播放前
- 业务页面切换时

**函数定义**

```python
def global_pause(self, reason="agent_wakeup"):
```

**MQTT 消息**

```json
{
  "msg_id": "agent_1747734123000",
  "source": "agent",
  "target": "all",
  "module": "system",
  "action": "pause",
  "page": "",
  "payload": {
    "reason": "wakeup",
    "source_behavior": "agent_wakeup"
  },
  "timestamp": 1747734123
}
```

---

#### 2. open_visitor_register_page - 访客登记页面

唤醒访客登记页面。

**MQTT 消息**

```json
{
  "target": "screen",
  "module": "visitor",
  "action": "open_page",
  "page": "visitor_register",
  "payload": {
    "title": "访客登记",
    "input_mode": "touch_and_voice"
  }
}
```

---

#### 3. open_appointment_confirm_page - 预约确认页面

唤醒预约确认页面。

**MQTT 消息**

```json
{
  "target": "screen",
  "module": "visitor",
  "action": "open_page",
  "page": "appointment_confirm",
  "payload": {
    "title": "预约确认",
    "input_mode": "touch"
  }
}
```

---

#### 4. open_navigation_page - 导航页面

唤醒导航页面。

**MQTT 消息**

```json
{
  "target": "screen",
  "module": "nav",
  "action": "open_page",
  "page": "navigation",
  "payload": {
    "title": "导航服务",
    "mode": "select_destination"
  }
}
```

---

#### 5. open_customer_service_page - 呼叫客服页面

唤醒呼叫客服页面。

**MQTT 消息**

```json
{
  "target": "screen",
  "module": "customer_service",
  "action": "open_page",
  "page": "customer_service",
  "payload": {
    "title": "呼叫客服",
    "mode": "call_service"
  }
}
```

---

#### 6. open_face_recognition_page - 人脸识别页面

唤醒人脸识别页面并启动识别。此函数发送两条 MQTT 消息。

**第一条 - 打开页面**

```json
{
  "target": "screen",
  "module": "face",
  "action": "open_page",
  "page": "face_recognition",
  "payload": {
    "title": "人脸识别",
    "tips": "请面向摄像头",
    "mode": "whitelist"
  }
}
```

**第二条 - 启动识别**

```json
{
  "target": "face",
  "module": "face",
  "action": "start",
  "page": "",
  "payload": {
    "mode": "whitelist",
    "timeout_ms": 10000,
    "threshold": 0.65
  }
}
```

---

### 订阅话题 (status/event)

**状态话题** (`/ai/module/status`)

接收外部模块的状态更新。

**事件话题** (`/ai/module/event`)

接收外部模块的事件通知（如人脸识别结果）。

**回调注册方式**

```python
mqtt_client.on_status_callback = self.on_status_received
mqtt_client.on_event_callback = self.on_event_received
```

---

### 注意事项

1. **MQTT 初始化时机**: MQTT 客户端在收到 `/ai/config` 配置后初始化，仅初始化一次，后续配置更新不会重新初始化。

2. **发布失败不影响主流程**: 所有业务函数内部已做异常捕获，MQTT 发布失败只会打印 warning 日志，不影响原有 ROS2 逻辑。

3. **日志前缀**: 所有 MQTT 业务日志使用 `[MQTT业务]` 前缀，方便排查。

4. **唤醒时自动暂停**: `wakeup_callback` 收到唤醒信号后会自动调用 `global_pause("wakeup")`，通知全车暂停。

---

### 扩展新的 MQTT 业务函数

在 `CustomActionServer` 类中新增成员函数即可被大模型调用（通过 `hasattr` 判断）。

**模板**

```python
def open_xxx_page(self):
    """
    打开XXX页面
    """
    try:
        ok = self.mqtt_client.publish_command(
            target="screen",
            module="xxx",
            action="open_page",
            page="xxx_page",
            payload={
                "title": "XXX",
                "mode": "xxx"
            }
        )
        self.get_logger().info(
            f"[MQTT业务] open_xxx_page sent, ok={ok}"
        )
    except Exception as e:
        self.get_logger().warn(
            f"[MQTT业务] open_xxx_page failed: {e}"
        )
```

**后续调用方式**

大模型返回以下 JSON 格式即可触发：

```json
{
  "action": ["open_xxx_page()"],
  "response": "好的，已为您打开XXX页面。"
}
```