# 子任务二：厂房朝向 demo

该 demo 用于在 RViz 手动导航到指定点后，用 RKNN 仅定位厂牌并完成对中与靠近。RKNN 的食品/日用品/电子分类结果不作为最终结论；墙前最终对中后，系统关闭RKNN推理并对整张相机画面做多帧 OCR，以 OCR 结果确认厂区类别。

新增识别话题：

- `/factory_sign/target_visible` (`std_msgs/Bool`)：当前帧是否有任意厂房框。
- `/factory_sign/target_center_error` (`std_msgs/Float32`)：目标框中心的水平误差 `[-1, 1]`；负为画面左侧，正为右侧。

控制器订阅 `/move_base/status`；必须在发出 RViz 导航点前启动。导航过程中始终关闭厂房推理，也不发布速度。导航成功后，小车先保持静止并开启 1 秒推理窗口；若发现厂房就立即进入对中。若未发现，小车才按 `/odom` 朝同一方向原地旋转总计 360°，分为 6 段、每段约 60°：每一段转完且角速度降为零后，才开启下一次 1 秒推理窗口。任一窗口发现厂房立即取消剩余扫描，进入平滑对中；对中完成、扫描完未发现、超时、失败或下一次导航开始时自动关闭推理。对中时每帧 RKNN 结果只更新目标角速度，控制器以受限角加速度连续输出，避免每 140 ms 急刹。连续 3 帧位于中部才锁定停车。

## 构建

```bash
source /opt/ros/noetic/setup.bash
source ~/ucar_ws/devel/setup.bash
cd ~/ucar_ws
catkin_make
```

## 推荐验证顺序

1. 启动导航、相机及厂房识别节点；识别节点必须以关闭推理的状态启动：

   ```bash
   roslaunch factory_sign_recognition factory_sign_recognition.launch enabled:=false
   ```
2. 在发送 RViz `2D Nav Goal` 前，启动隔离输出模式：

   ```bash
   source ~/ucar_ws/devel/setup.bash
   roslaunch factory_alignment_demo factory_alignment_demo.launch enabled:=true
   rostopic echo /factory_alignment_demo/status
   rostopic echo /factory_alignment_demo/cmd_vel
   ```

3. 目标到达后，应先看到 `INITIAL_DETECTING`；未发现时再依次看到 `SCANNING_STEP_1/6`、`SCAN_STEP_1_SETTLING`、`SCAN_STEP_1_DETECTING`。若该段发现厂房，状态变为 `FACTORY_FOUND_STARTING_ALIGNMENT`，随后是 `SMOOTH_ALIGN`、`CENTER_FRAME 1/3`、`ALIGNED`；后续扫描不会执行。六段都没有厂房则为 `FACTORY_NOT_FOUND_AFTER_360_DEG_SCAN`。中部容差为画面宽度的 36%。
4. 确认旋转方向正确后，重新启动为真实底盘输出：

   ```bash
   roslaunch factory_alignment_demo factory_alignment_demo.launch \
     enabled:=true cmd_vel_topic:=/cmd_vel
   ```

如果厂房在画面右侧时小车向错误方向转动，将 `config/factory_alignment.yaml` 中的 `angular_sign` 从 `-1.0` 改为 `1.0`，重建后重启。先低速、留足安全空间验证。

扫描方向由同一文件的 `scan_direction` 控制；默认 `1.0`，设为 `-1.0` 可改为反向扫描半圈。

## 三点容错导航

`factory_candidate_navigator` 启动时会向 `/initialpose` 连续发布 3 次任务二起点位姿（每次间隔 0.1 秒），并等待 `/amcl_pose` 在 2 秒内确认位置误差不超过 0.3 m、朝向误差不超过 0.2 rad；未确认就不发送导航点。确认后依次把三个 `/map` 位姿发送给 `move_base`。每个点的导航超时为 120 秒；`move_base` 返回失败、被拒绝、丢失或超时时才尝试下一点。第一个成功到达的点会立即结束候选导航，后两个点不会发送；随后已启动的厂房对中控制器会接管，执行“先推理、未发现再 360° 分段扫描”的流程。

候选点和起点在 `config/factory_candidate_goals.yaml`，启动顺序为：先启动导航、相机、识别节点（`enabled:=false`）和厂房对中控制器；再启动候选导航：

```bash
roslaunch factory_alignment_demo factory_candidate_navigator.launch
rostopic echo /factory_candidate_navigator/status
```

状态包含 `INITIALIZING`、`INITIAL_POSE_CONFIRMED`，定位未收敛时为 `INITIAL_POSE_TIMEOUT`，`NAVIGATING: candidate 1/3`、失败后的 `trying next`，以及成功后的 `SUCCEEDED: candidate N/3 reached; remaining candidates skipped`。

## 随机锥桶搜索区域导航

若任务给的是名义搜索点 P，而 P 周围可能被随机雪糕筒占住，使用随厂房对中启动的 `factory_search_point_adapter`，不要同时启动上面的三点容错导航。适配器不发布 `/cmd_vel`；它只向 `move_base` 发送候选导航目标，TEB 全程仍负责避开局部代价地图中的锥桶。

启动厂房流程时该适配器默认已启动，但保持空闲：

```bash
roslaunch factory_alignment_demo factory_alignment_demo.launch \
  enabled:=true cmd_vel_topic:=/cmd_vel wall_approach_enabled:=true
```

随后把 RViz 的 `2D Nav Goal` 工具的 Topic 改为 `/factory_search_adapter/nominal_goal`，再在地图中点击任务给出的 P(x, y, yaw)。不要向默认的 `/move_base_simple/goal` 发送这个搜索点，否则会绕过适配器直接导航。

适配器生成 P 本身、半径 0.40 m 与 0.60 m 两环、每环 8 个方向的候选点（最多 17 个）。每个点固定使用 P 的预设朝向，并按以下条件处理：

- 静态 `/map` 中可通行，且 `0.395 × 0.316 m` 的检查 footprint 可完全放下；
- 只有在 `/move_base/local_costmap/costmap` 有 2 秒内的新数据且候选点在其覆盖范围内时，才用该图过滤未知、高代价和锥桶；没有新局部地图或候选点在范围外时，状态会提示 `LOCAL_COSTMAP_UNAVAILABLE_STATIC_PLAN_FALLBACK`，仍继续静态地图和 `/move_base/make_plan` 预检并发送目标；
- `/move_base/make_plan` 的 RPC 只在唯一后台线程中执行；快速返回时必须有全局路径，并用于路径长度排序。服务未注册、调用异常、无响应或 0.4 秒超时时，状态为 `MAKE_PLAN_TIMEOUT_FALLBACK_TO_MOVE_BASE`，本轮立即降级为由 `move_base` action 最终判定，候选准备不会被服务探测或 RPC 卡住；
- 同半径候选优先局部代价低、全局路径短的点；导航失败、超时或搜索链路失败会立刻尝试下一点。

可在 RViz 添加以下显示或终端观察状态：

```bash
rostopic echo /factory_search_adapter/status
rostopic echo /factory_alignment_demo/status
```

- `/factory_search_adapter/candidates`（`PoseArray`）：本次通过预检的候选观察点；
- `/factory_search_adapter/selected_goal`（`PoseStamped`）：当前发送给 `move_base` 的候选；
- 当任一候选到达后，原有“推理、360°分段扫描、视觉对中、墙前 0.60m、最终对中、整图 OCR、白框 0.28m 进入”链路自动开始；`FACTORY_CONFIRMED` 后适配器不会再发送其余候选点。

## 静态地图墙前站位（可选）

默认仅完成视觉对中。传入 `wall_approach_enabled:=true` 后，控制器会在视觉对中完成时读取 `/amcl_pose` 与静态 `/map`：沿当前车头方向做射线检测，命中第一个占用栅格后在附近候选墙线中选择与当前视线最一致的一面并拟合法线，生成墙前 `0.60 m` 的 `/map` 导航点，并把该点发布到 `/factory_alignment_demo/wall_goal`。`move_base/TEB` 负责前往该点并使用局部代价地图绕开雪糕筒。到达后，控制器会重新开启厂房推理并执行一次仅旋转的最终视觉对中。OCR确认厂区类别后，控制器重新读取 `map → base_link`、重新沿当前车头射线求墙，并通过 `move_base/TEB` 前往墙前 `0.28 m` 的白框进入目标 `/factory_alignment_demo/entry_goal`。

```bash
roslaunch factory_alignment_demo factory_alignment_demo.launch \
  enabled:=true cmd_vel_topic:=/cmd_vel wall_approach_enabled:=true
```

先在 RViz 添加两个 `Pose` 显示：`/factory_alignment_demo/wall_goal` 用于0.60m站位，`/factory_alignment_demo/entry_goal` 用于白框最终进入点；两者都应正对墙面。白框深度为0.50m，`base_link` 位于车体中心、车长0.335m，最终目标距墙0.28m，因此车头距墙0.1125m、车尾距白框外侧0.0525m。若无法获取静态地图、map 到 base_link 位姿、射线墙面或可靠墙法线，节点不会发送导航目标；位姿不可用时状态为 `MAP_POSE_UNAVAILABLE`。最终对中后状态先到 `WALL_APPROACH_ALIGNED`，随后自动进入 `OCR_READING`。OCR 最多尝试 10 个整图帧；任一帧的同一预处理结果中只要包含某类别的全部汉字，就立即确认：`食`+`品` 为食品、`日`+`用`+`品` 为日用品、`电`+`子` 或 `生`+`产` 为电子。字符顺序和中间夹杂字符不影响判断。成功状态为 `FACTORY_CONFIRMED type=食品`（或日用品、电子），最终类别发布到 `/factory_alignment_demo/factory_type`；10帧均未命中或超时为 `FACTORY_OCR_FAILED`。

OCR 输入与调试话题：

- `/usb_cam/image_raw`：OCR输入的整张相机画面；节点按当前相机安装方式水平翻转后处理。
- `/factory_sign/target_roi_image`：RKNN仍会发布的厂牌ROI调试图，但OCR不再使用它。
- `/sign_recognition/debug_image`：OCR实际处理的整图灰度/二值图。
- `/sign_recognition/status`：逐帧OCR票数和结果。
- `/factory_alignment_demo/factory_type`：最终确认的厂区类别。
- `/factory_alignment_demo/entry_goal`：OCR确认后发送给 `move_base` 的白框进入目标。

OCR直接调用系统 `tesseract`，当前已验证存在 `chi_sim` 中文语言数据，不依赖 Python 的 `pytesseract` 包。