# 四角 LiDAR 外部真值定位模板

这个目录定义真实车间外部真值定位的代码模板。目标场景是：

```text
车间四角固定 LiDAR
  -> 观测车端标定球
  -> 算法求解车辆 base_link 在车间坐标系下的位姿
  -> 发布 ExternalLocalizationTelemetry
  -> external_localization_service 做 readiness、质量校验和 report 回填
```

## 边界

算法实现者主要修改：

- `four_lidar_ball_algorithm.py`

模板维护者主要维护：

- `workshop_external_lidar_localization_node.py`
- `workshop_external_lidar_common.py`
- `replay_four_lidar_smoke.py`
- `config/four_lidar_ball_template.yaml`
- 与总控、external service、report 的接口约定

车端 agent 不实现这套外部真值定位算法。车端如果需要 external pose，只接收车间电脑侧已经求出的位姿。

## 算法输入

`FourLidarBallLocalizationInput` 已固定这些输入：

- `frames`：四角 LiDAR 最近一帧数据，点坐标保持在各自 LiDAR frame 下。
- `lidar_mounts`：每台 LiDAR 的 topic、frame_id 和车间安装位姿。
- `ball_targets`：车端标定球在 `base_link` 下的安装位置和半径。
- `max_frame_age_ms`：允许参与融合的最大数据年龄。
- `reference_source_name`：外部真值源名称。
- `workcell_zone_id`：工位区域 ID。

## 算法输出

算法必须填充 `FourLidarBallLocalizationOutput`：

- `pose_valid`：本帧是否得到有效车辆位姿。
- `workshop_pose`：车辆 `base_link` 在车间坐标系下的位姿。
- `position_stddev_m`：位置不确定度。
- `yaw_stddev_rad`：航向不确定度。
- `tracking_loss_ratio`：跟踪丢失比例。
- `time_sync_offset_ms`：时间同步偏差。
- `quality_score`：质量分数，建议范围 `[0, 1]`。
- `observed_target_count`：本帧成功观测到的标定球数量。
- `diagnostic_messages`：调试和质量诊断信息。

## 运行方式

先复制模板配置并填写真实现场参数：

```bash
cp src/site_deployment/workshop_external_lidar_localization_real/config/four_lidar_ball_template.yaml \
  /data/agv_calib/site_a/four_lidar_ball.yaml
```

启动节点：

```bash
source install/setup.bash
python3 src/site_deployment/workshop_external_lidar_localization_real/workshop_external_lidar_localization_node.py \
  --config /data/agv_calib/site_a/four_lidar_ball.yaml \
  --session-dir /data/agv_calib/site_a/session_001 \
  --session-id session_001 \
  --site-id site_a \
  --vehicle-id AGV-001
```

启动后节点会额外发布诊断 JSON 字符串：

```text
/workshop/external_localization/diagnostics
```

诊断内容包括每台 LiDAR 是否有帧、帧年龄、点数、缺失/过期 LiDAR、当前位姿质量、算法诊断消息和落盘文件路径。

## 配置校验

节点启动时会强校验现场配置：

- `reference_source_name`、`workcell_zone_id` 不能为空或保留模板占位符。
- `output_topic` 建议使用 `/workshop/` 前缀。
- 默认要求 4 台 LiDAR 启用，至少 2 个 required 标定球。
- 每台 LiDAR 必须填写真实 `topic`、`frame_id`、`message_type` 和车间安装位姿。
- 每个标定球必须填写真实半径和 `base_link` 下安装坐标。
- 启用落盘时，`recording.session_dir` 和 `recording.dataset_index_path` 不能保留模板占位符。

## 数据落盘

配置中 `recording.enabled=true` 时，节点会自动写入：

- `external/external_observations.csv`：外部定位位姿、质量、标准差、有效 LiDAR 数等。
- `external/marker_observations.jsonl`：算法输出的标定球观测列表。
- `external/time_sync_diagnostics.jsonl`：每帧时间同步和 LiDAR 帧年龄诊断。
- `dataset_index.yaml`：把以上三类文件写入 `data_inputs.external.*`。

后续可继续执行：

```bash
python3 src/deployment/tools/finalize_site_session.py \
  --site-profile src/deployment/profiles/site_template.yaml \
  --session-dir /data/agv_calib/site_a/session_001
```

生成的 `site_data_input.yaml` 会把这些外部定位数据注入 `external_localization_service`。

## 离线回放自检

没有 ROS 或真实 LiDAR 时，可以先验证配置、算法入口、诊断输出和落盘：

```bash
python3 src/site_deployment/workshop_external_lidar_localization_real/replay_four_lidar_smoke.py \
  --config /data/agv_calib/site_a/four_lidar_ball.yaml \
  --session-dir /tmp/agv_calib_external_replay/session_001 \
  --session-id session_001 \
  --site-id site_a \
  --vehicle-id AGV-001
```

如果有回放 JSONL，可通过 `--frames-jsonl` 输入。每行支持：

```json
{"timestamp_us": 1711234567000000, "frames": [{"lidar_id": "corner_front_left", "frame_id": "lidar_corner_front_left", "points_xyz": [[1.0, 2.0, 0.5]]}]}
```

然后现场 launch 需要让 external service 订阅该节点输出：

```text
external_telemetry_topic:=/workshop/external_localization/vehicle/pose
expected_reference_source_name:=workshop_four_lidar_ball_truth
```

## 关键约束

- 只有一个标定球时，通常只能确定车辆位置，不能唯一确定车辆 yaw。
- 如果需要完整 `base_link` 位姿，建议至少两个球，三球及以上更稳健。
- 四台 LiDAR 的外参必须先统一到同一个车间坐标系。
- 算法输出的 `workshop_pose` 必须是车辆 `base_link` 位姿，不是某个标定球球心位姿。
- 质量字段必须保守填写；质量不足时应发布 `pose_valid=false` 或降低 `quality_score`。
