# Q 路点选择器 使用手册

## 1. 概述

`q_waypoint_selector.py` 是一个独立的 ROS 2 Python 节点，用于比赛场景下的**自动有序路点导航**。它从 JSON 文件中加载多路线、多候选的路点坐标，结合全局 costmap 实时判断候选点是否可通行，并通过 Nav2 的 `NavigateToPose` action 下发导航目标，依次逐个完成所有路点。

### 核心能力

- **多路线冗余**：同一个路点序号可配置多个候选坐标（来自不同 JSON 文件），一个被障碍物占据时自动切换
- **costmap 感知**：下发目标前先检查该坐标在全局 costmap 上是否被占用，避免朝障碍物导航
- **路线偏好记忆**：成功后记住当前路线编号，后续 Q 优先选择同一条路线
- **超时自动切换**：单个目标超时后自动取消，尝试下一个候选

---

## 2. 路点 JSON 文件格式

### 目录约定

所有 JSON 文件放在同一目录下，文件名按字母序排列。节点加载时：

```
json_dir/
├── test1.json   # 路线 0
├── test2.json   # 路线 1
├── test3.json   # 路线 2
└── ...
```

每个 JSON 文件代表一条**完整路线**，包含有序的路点列表。

### JSON 结构

```json
{
  "points": [
    {
      "name": "goal_001",
      "yaw_degrees": 4.39,
      "odom": {
        "pose": {
          "pose": {
            "position": { "x": 1.48, "y": 0.18, "z": 0.0 },
            "orientation": { "x": 0.0, "y": 0.0, "z": 0.03, "w": 0.99 }
          }
        }
      }
    },
    {
      "name": "goal_002",
      "yaw_degrees": 15.10,
      "odom": {
        "pose": {
          "pose": {
            "position": { "x": 3.24, "y": 0.23, "z": 0.0 },
            "orientation": { "x": 0.0, "y": 0.0, "z": 0.13, "w": 0.99 }
          }
        }
      }
    }
  ]
}
```

### 字段说明

| 字段 | 类型 | 必需 | 说明 |
|------|------|------|------|
| `points` | array | 是 | 有序路点数组，按索引 0, 1, 2... 依次导航 |
| `points[i].name` | string | 否 | 路点名称（日志显示用，缺省为 `{文件名}_{索引}`） |
| `points[i].yaw_degrees` | number | 否 | 目标朝向角（度），优先级高于 orientation 四元数 |
| `points[i].odom.pose.pose.position` | object | 是 | 路点坐标 `{x, y, z}` |
| `points[i].odom.pose.pose.orientation` | object | 否 | 目标朝向四元数 `{x, y, z, w}`，仅在 `yaw_degrees` 缺失时使用 |

> **注意**：`position.x` 和 `position.y` 必须与 `goal_frame` 参数指定的坐标系一致（如 `odom` 或 `odom_combined`）。

---

## 3. Q 路点组概念

### 路点编组规则

假设有 2 个 JSON 文件，经过编组后：

```
test1.json: [p0, p1, p2]        (路线 0)
test2.json: [p0, p1, p2]        (路线 1)
           ↓ 转置编组
Q_0 = [test1[0], test2[0]]     ← 两个候选路点，指向同一目标位置的不同路线
Q_1 = [test1[1], test2[1]]
Q_2 = [test1[2], test2[2]]
```

**路径数不同时的处理**：编组时以最长路线为准，较短路线在对应索引上缺失的点被跳过，不会影响其他路线的候选。

### 候选选择策略

节点在每个 Q 组内按以下优先级选择候选：

1. **偏好路线优先**（`preferred_route_index`）：上次导航成功的路线编号
2. **循环顺序**：若偏好路线不可用，按路线编号循环尝试
3. **costmap 过滤**：被 costmap 判定为占用的候选直接跳过
4. **已拒绝列表**：本次 Q 内已失败/被拒绝的候选不再重试

---

## 4. 运行流程

```
启动节点
  │
  ├─ 加载 json_dir 下所有 .json → 编组为 Q_0, Q_1, ...
  ├─ 等待 NavigateToPose action server 就绪
  ├─ 等待 costmap 话题有数据
  │
  └─ 定时器循环 (selection_period_sec)
       │
       ├─ 所有 Q 完成? → 退出
       │
       ├─ 有活跃 goal ?
       │   ├─ 进行中 → 等待
       │   └─ 超时 → 取消 goal，拒绝当前候选，下一轮切换到下一个候选
       │
       ├─ 从 Q_current 选择候选
       │   ├─ 有可用候选 → 通过 NavigateToPose 下发
       │   └─ 全部被占用 → 跳过当前 Q，前进到 Q_{current+1}
       │
       └─ goal 结果回调
           ├─ status=4 (SUCCEEDED) → 记录路线偏好，前进到下一 Q
           └─ 其他 (失败/拒绝)  → 拒绝当前候选，下次 tick 换候选
```

---

## 5. 参数说明

### 启动参数

| 参数 | 类型 | 默认值 | 说明 |
|------|------|--------|------|
| `json_dir` | string | `obstacle_nav2/config/json` | JSON 路点文件目录 |
| `goal_frame` | string | `odom` | 下发导航目标使用的坐标帧，实车应设为 `odom_combined` |
| `costmap_topic` | string | `/global_costmap/costmap` | 全局 costmap 话题，用于避占检测 |
| `navigate_action` | string | `/navigate_to_pose` | Nav2 的 NavigateToPose action 名称 |
| `occupied_threshold` | int | `50` | costmap 占用阈值 (0-100)，栅格值 >= 此值视为被占据 |
| `treat_unknown_as_occupied` | bool | `true` | 是否将未知区域 (costmap 值 = -1) 视为不可通行 |
| `goal_timeout_sec` | float | `180.0` | 单个导航目标超时时间（秒），取值范围 [1.0, ∞) |
| `selection_period_sec` | float | `0.5` | 选择器主循环周期（秒），取值范围 [0.1, ∞) |
| `use_sim_time` | bool | `false` | 是否使用仿真时间 |

### 参数调优建议

| 场景 | 建议设置 |
|------|---------|
| 路点间距大（>10m） | `goal_timeout_sec` 适当增大（如 300s） |
| 障碍物密集区域 | `occupied_threshold` 降低（如 30），更保守地避让 |
| 动态障碍物多 | `selection_period_sec` 设短（如 0.2s），更快响应 costmap 变化 |
| costmap 覆盖不全 | `treat_unknown_as_occupied` 视情况关闭，允许向未知区域导航 |

---

## 6. 启动方式

### 方式 A：独立启动（我使用的是这个，测试成功

```bash
python3 q_waypoint_selector.py \
    --ros-args \
    -p json_dir:=/home/user/waypoints \
    -p goal_frame:=odom_combined \
    -p costmap_topic:=/global_costmap/costmap \
    -p goal_timeout_sec:=120.0
```

### 方式 B：通过 launch 文件启动

```bash
ros2 launch obstacle_nav2 obstacle_nav2_q_waypoints.launch.py \
    use_sim_time:=false \
    global_frame:=odom_combined \
    json_dir:=/home/user/waypoints \
    goal_frame:=odom_combined \
    goal_timeout_sec:=120.0 \
    occupied_threshold:=50
```

这个 launch 文件会**同时启动**：底盘驱动、激光雷达、obstacle_scanner、Nav2 导航栈 和 Q 路点选择器。

### 方式 C：在已有导航栈上单独启动

```bash
ros2 run obstacle_nav2 q_waypoint_selector.py \
    --ros-args \
    -p json_dir:=/home/user/waypoints \
    -p goal_frame:=odom_combined
```

---

## 7. 运行日志解读

### 正常日志

```
[INFO] loaded Q waypoint groups json_dir=/... groups=5 routes=3 goal_frame=odom
```
启动成功，加载了 5 个 Q 组，每条 Q 最多 3 个候选路线。

```
[INFO] send Q_0 candidate route=0 file=test1.json name=goal_001 pose=(1.480,0.180,0.076)
```
下发了 Q_0 的第一个候选路点。

```
[INFO] reached Q_0 route=0 file=test1.json name=goal_001; advance to Q_1
```
到达目标，记录路线偏好并进入下一路点。

### 异常日志及处理

| 日志 | 含义 | 处理方式 |
|------|------|---------|
| `waiting for NavigateToPose action server` | Nav2 未启动或未就绪 | 等待 nav2_bringup 完成启动 |
| `waiting for costmap; publisher_count=0` | costmap 话题无发布者 | 检查 global_costmap 节点是否启动 |
| `skip occupied candidate ... reason=occupied cost=100` | 候选路点栅格被障碍物占据 | 自动尝试下一个候选路线 |
| `skip occupied candidate ... reason=outside_costmap` | 候选路点超出当前 costmap 范围 | 增大 costmap 尺寸或调整路点坐标 |
| `active goal timed out; cancelling` | 导航目标超时 | 自动取消并切换候选，可能需要增大 `goal_timeout_sec` |
| `all candidates in Q_N are occupied; skipping` | 该 Q 所有候选均不可用 | 自动跳过到下一个 Q |
| `all Q waypoint groups completed` | 所有路点已完成 | 正常退出信号 |

---

## 8. Nav2 goal 状态码参考

节点根据 `NavigateToPose` action 的结果状态码决定下一步行为：

| status | 含义 | 节点行为 |
|--------|------|---------|
| 0 | UNKNOWN | 拒绝当前候选，换下一个 |
| 1 | ACCEPTED | (中间状态，不触发回调) |
| 2 | EXECUTING | (中间状态，不触发回调) |
| 3 | CANCELING | (中间状态，不触发回调) |
| 4 | **SUCCEEDED** | 记录路线偏好，前进到下一 Q |
| 5 | CANCELED | 拒绝当前候选，换下一个 |
| 6 | ABORTED | 拒绝当前候选，换下一个 |

---

## 9. 常见问题

**Q: 节点启动后一直等待 costmap，不发送目标？**

检查 `costmap_topic` 参数是否与实际话题一致（默认 `/global_costmap/costmap`）：
```bash
ros2 topic list | grep costmap
```

**Q: 所有候选路点都被判定为占用？**

1. 确认路点坐标与 `goal_frame` 坐标系一致
2. 检查 `occupied_threshold` 是否过低（costmap 中膨胀区域可能为 50-100）
3. 尝试设置 `treat_unknown_as_occupied:=false` 如果 costmap 覆盖不全

**Q: 机器人到达路点后没有立即进入下一个？**

节点的定时器周期为 `selection_period_sec`（默认 0.5s），最坏情况下需要等待一个周期。如果 goal 结果回调已触发，下一个 tick 就会下发新目标。

**Q: 如何录制路点 JSON 文件？**

路点 JSON 可以通过监听 `/odom_combined` (实车) 或 `/odom` (仿真) 话题并手动记录坐标生成。JSON 格式参见第 2 节。
