feat: add competition task 2 workflow

factory_alignment_demo/CMakeLists.txt

@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(factory_alignment_demo)
+find_package(catkin REQUIRED COMPONENTS
+  actionlib
+  actionlib_msgs
+  geometry_msgs
+  move_base_msgs
+  nav_msgs
+  rospy
+  std_msgs
+  std_srvs
+  tf2_ros
+)
+catkin_package(CATKIN_DEPENDS actionlib actionlib_msgs geometry_msgs move_base_msgs nav_msgs rospy std_msgs std_srvs tf2_ros)
+catkin_install_python(PROGRAMS
+  scripts/factory_alignment_controller.py
+  scripts/factory_candidate_navigator.py
+  scripts/factory_search_point_adapter.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+install(DIRECTORY config launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

factory_alignment_demo/README.md

@@ -0,0 +1,113 @@
+# 子任务二：厂房朝向 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` 包。

factory_alignment_demo/config/factory_alignment.yaml

@@ -0,0 +1,50 @@
+angular_sign: -1.0
+kp: 0.25
+min_angular_speed: 0.10
+max_angular_speed: 0.12
+# Accept the middle 36% of the image; precise centring is not required.
+center_tolerance: 0.18
+center_confirm_frames: 3
+max_angular_acceleration: 0.15
+detection_timeout_seconds: 0.35
+alignment_timeout_seconds: 30.0
+control_rate: 20.0
+# At each completed 60-degree step, inference is enabled for this window.
+scan_steps: 6
+scan_direction: 1.0
+scan_angular_speed: 0.20
+scan_yaw_tolerance: 0.03
+scan_detection_window_seconds: 1.0
+odom_timeout_seconds: 0.50
+
+# Opt-in wall approach: ray-cast the static map after visual alignment.
+wall_approach_enabled: false
+wall_standoff_distance: 0.60
+wall_ray_max_distance: 5.0
+wall_occupied_threshold: 65
+wall_fit_radius: 0.60
+wall_line_inlier_distance: 0.05
+wall_line_min_length: 0.25
+wall_line_min_support: 8
+# Select the wall face whose normal is within 60 degrees of the camera ray.
+wall_line_min_facing_alignment: 0.50
+wall_line_max_points: 180
+# Prefer latest map -> base_link TF; use AMCL only if TF is temporarily unavailable.
+tf_map_frame: map
+tf_base_frame: base_link
+tf_lookup_timeout_seconds: 0.20
+amcl_pose_fallback_timeout_seconds: 5.0
+wall_goal_server_timeout_seconds: 2.0
+
+# After the final wall alignment, confirm the factory type from the full camera image.
+ocr_confirmation_enabled: true
+ocr_timeout_seconds: 15.0
+ocr_enable_service: /sign_recognition/set_enabled
+
+# After OCR confirmation, enter the wall-adjacent white box with move_base/TEB.
+entry_goal_enabled: true
+entry_standoff_distance: 0.28
+entry_white_box_depth: 0.50
+entry_vehicle_length: 0.335
+enabled: false
+cmd_vel_topic: /cmd_vel

factory_alignment_demo/config/factory_candidate_goals.yaml

@@ -0,0 +1,25 @@
+# Task 2 begins here after task 1 has completed.
+frame_id: map
+start_pose:
+  x: -1.003
+  y: -1.240
+  yaw: -1.591
+
+# The navigator tries these exactly in order and stops at the first success.
+candidate_goals:
+  - x: -1.662
+    y: -2.971
+    yaw: -3.136
+  - x: -1.675
+    y: -2.264
+    yaw: -3.132
+  - x: -1.703
+    y: -1.617
+    yaw: -3.117
+
+initial_pose_confirmation_timeout_seconds: 2.0
+initial_pose_position_tolerance_m: 0.30
+initial_pose_yaw_tolerance_rad: 0.20
+
+move_base_server_timeout_seconds: 10.0
+goal_timeout_seconds: 120.0

factory_alignment_demo/config/factory_search_adapter.yaml

@@ -0,0 +1,23 @@
+frame_id: map
+base_frame: base_link
+nominal_goal_topic: /factory_search_adapter/nominal_goal
+static_map_topic: /map
+local_costmap_topic: /move_base/local_costmap/costmap
+alignment_status_topic: /factory_alignment_demo/status
+make_plan_service: /move_base/make_plan
+
+# P itself, then two 8-direction rings around P.
+ring_radii: [0.40, 0.60]
+ring_directions: 8
+
+# Physical footprint 0.335 x 0.256 m plus 0.03 m on every side.
+checked_footprint_length: 0.395
+checked_footprint_width: 0.316
+static_occupied_threshold: 65
+local_cost_threshold: 80
+local_costmap_timeout_seconds: 2.0
+candidate_goal_timeout_seconds: 45.0
+candidate_search_timeout_seconds: 80.0
+move_base_server_timeout_seconds: 2.0
+make_plan_response_timeout_seconds: 0.4
+timer_period_seconds: 0.2

factory_alignment_demo/launch/factory_alignment_demo.launch

@@ -0,0 +1,34 @@
+<launch>
+  <!-- Start this launch before manually sending a navigation goal. -->
+  <arg name="enabled" default="false"/>
+  <arg name="require_order" default="false"/>
+  <arg name="cmd_vel_topic" default="/cmd_vel"/>
+  <arg name="wall_approach_enabled" default="false"/>
+  <arg name="ocr_confirmation_enabled" default="true"/>
+  <arg name="entry_goal_enabled" default="true"/>
+  <arg name="start_ocr_node" default="true"/>
+  <arg name="start_search_adapter" default="true"/>
+
+  <!-- Keep OCR resident but idle; the controller enables it at the terminal pose. -->
+  <node if="$(arg start_ocr_node)" pkg="sign_recognition"
+        type="sign_recognition_node.py" name="sign_recognition_node" output="screen">
+    <param name="enabled" value="false"/>
+    <param name="debug_save" value="false"/>
+  </node>
+
+  <node if="$(arg start_search_adapter)" pkg="factory_alignment_demo"
+        type="factory_search_point_adapter.py" name="factory_search_point_adapter" output="screen">
+    <rosparam command="load" file="$(find factory_alignment_demo)/config/factory_search_adapter.yaml"/>
+  </node>
+
+  <node pkg="factory_alignment_demo" type="factory_alignment_controller.py"
+        name="factory_alignment_demo" output="screen">
+    <rosparam command="load" file="$(find factory_alignment_demo)/config/factory_alignment.yaml"/>
+    <param name="enabled" value="$(arg enabled)"/>
+    <param name="require_order" value="$(arg require_order)"/>
+    <param name="cmd_vel_topic" value="$(arg cmd_vel_topic)"/>
+    <param name="wall_approach_enabled" value="$(arg wall_approach_enabled)"/>
+    <param name="ocr_confirmation_enabled" value="$(arg ocr_confirmation_enabled)"/>
+    <param name="entry_goal_enabled" value="$(arg entry_goal_enabled)"/>
+  </node>
+</launch>

factory_alignment_demo/launch/factory_candidate_navigator.launch

@@ -0,0 +1,6 @@
+<launch>
+  <node pkg="factory_alignment_demo" type="factory_candidate_navigator.py"
+        name="factory_candidate_navigator" output="screen">
+    <rosparam command="load" file="$(find factory_alignment_demo)/config/factory_candidate_goals.yaml"/>
+  </node>
+</launch>

factory_alignment_demo/launch/factory_search_point_adapter.launch

@@ -0,0 +1,6 @@
+<launch>
+  <node pkg="factory_alignment_demo" type="factory_search_point_adapter.py"
+        name="factory_search_point_adapter" output="screen">
+    <rosparam command="load" file="$(find factory_alignment_demo)/config/factory_search_adapter.yaml"/>
+  </node>
+</launch>

factory_alignment_demo/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>factory_alignment_demo</name>
+  <version>0.1.0</version>
+  <description>Task-2 demo: align the vehicle heading to a detected factory sign.</description>
+  <maintainer email="ucar@example.com">ucar</maintainer>
+  <license>BSD-3-Clause</license>
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>actionlib</build_depend>
+  <build_depend>actionlib_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>move_base_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <exec_depend>actionlib</exec_depend>
+  <exec_depend>actionlib_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>move_base_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <build_depend>std_srvs</build_depend>
+  <build_depend>tf2_ros</build_depend>
+  <exec_depend>std_srvs</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>sign_recognition</exec_depend>
+</package>

factory_alignment_demo/scripts/factory_alignment_controller.py

@@ -0,0 +1,922 @@
+#!/usr/bin/env python3
+"""Align to a factory sign, then optionally navigate to a stand-off point on its wall."""
+from __future__ import annotations
+
+import json
+import math
+import time
+
+import actionlib
+import rospy
+import tf2_ros
+from actionlib_msgs.msg import GoalStatus, GoalStatusArray
+from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped, Quaternion, Twist
+from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
+from nav_msgs.msg import OccupancyGrid, Odometry
+from std_msgs.msg import Bool, Float32, String
+from std_srvs.srv import SetBool
+
+
+class FactoryAlignmentDemo:
+    _RUNNING_STATES = {
+        GoalStatus.PENDING, GoalStatus.ACTIVE, GoalStatus.PREEMPTING, GoalStatus.RECALLING,
+    }
+    _FAILED_STATES = {
+        GoalStatus.PREEMPTED, GoalStatus.ABORTED, GoalStatus.REJECTED,
+        GoalStatus.RECALLED, GoalStatus.LOST,
+    }
+
+    def __init__(self):
+        rospy.init_node("factory_alignment_demo")
+        self._enabled = bool(rospy.get_param("~enabled", False))
+        # In competition mode only an explicit task-2 order may start recognition.
+        self._require_order = bool(rospy.get_param("~require_order", False))
+        self._current_order = None
+        self._completed_order_id = None
+        self._pending_start_order_id = None
+        self._armed_order_id = None
+        self._angular_sign = float(rospy.get_param("~angular_sign", -1.0))
+        self._kp = float(rospy.get_param("~kp", 0.25))
+        self._min_speed = float(rospy.get_param("~min_angular_speed", 0.10))
+        self._max_speed = float(rospy.get_param("~max_angular_speed", 0.12))
+        self._tolerance = float(rospy.get_param("~center_tolerance", 0.18))
+        self._center_frames_required = int(rospy.get_param("~center_confirm_frames", 3))
+        self._max_angular_accel = float(rospy.get_param("~max_angular_acceleration", 0.15))
+        self._detection_timeout = float(rospy.get_param("~detection_timeout_seconds", 0.35))
+        self._alignment_timeout = float(rospy.get_param("~alignment_timeout_seconds", 30.0))
+        self._scan_steps = int(rospy.get_param("~scan_steps", 6))
+        self._scan_direction = 1.0 if float(rospy.get_param("~scan_direction", 1.0)) >= 0.0 else -1.0
+        self._scan_speed = abs(float(rospy.get_param("~scan_angular_speed", 0.20)))
+        self._scan_yaw_tolerance = float(rospy.get_param("~scan_yaw_tolerance", 0.03))
+        self._scan_detection_window = float(rospy.get_param("~scan_detection_window_seconds", 1.0))
+        self._odom_timeout = float(rospy.get_param("~odom_timeout_seconds", 0.50))
+        rate = float(rospy.get_param("~control_rate", 20.0))
+
+        # This is explicitly opt-in because it sends a real move_base goal after alignment.
+        self._wall_approach_enabled = bool(rospy.get_param("~wall_approach_enabled", False))
+        self._wall_standoff = float(rospy.get_param("~wall_standoff_distance", 0.30))
+        self._wall_ray_max_distance = float(rospy.get_param("~wall_ray_max_distance", 5.0))
+        self._wall_occupied_threshold = int(rospy.get_param("~wall_occupied_threshold", 65))
+        self._wall_fit_radius = float(rospy.get_param("~wall_fit_radius", 0.60))
+        self._wall_line_inlier_distance = float(rospy.get_param("~wall_line_inlier_distance", 0.05))
+        self._wall_line_min_length = float(rospy.get_param("~wall_line_min_length", 0.25))
+        self._wall_line_min_support = int(rospy.get_param("~wall_line_min_support", 8))
+        self._wall_line_min_facing_alignment = float(
+            rospy.get_param("~wall_line_min_facing_alignment", 0.50)
+        )
+        self._wall_line_max_points = int(rospy.get_param("~wall_line_max_points", 180))
+        self._tf_map_frame = rospy.get_param("~tf_map_frame", "map")
+        self._tf_base_frame = rospy.get_param("~tf_base_frame", "base_link")
+        self._tf_lookup_timeout = float(rospy.get_param("~tf_lookup_timeout_seconds", 0.20))
+        self._amcl_fallback_timeout = float(
+            rospy.get_param("~amcl_pose_fallback_timeout_seconds", 5.0)
+        )
+        self._wall_goal_server_timeout = float(rospy.get_param("~wall_goal_server_timeout_seconds", 2.0))
+        self._ocr_confirmation_enabled = bool(
+            rospy.get_param("~ocr_confirmation_enabled", True)
+        )
+        self._ocr_timeout = float(rospy.get_param("~ocr_timeout_seconds", 15.0))
+        self._ocr_service_name = rospy.get_param(
+            "~ocr_enable_service", "/sign_recognition/set_enabled"
+        )
+        # Terminal white-box entry after OCR confirmation.  The box touches the
+        # wall and extends 0.50 m outward; base_link is at the vehicle centre.
+        self._entry_goal_enabled = bool(rospy.get_param("~entry_goal_enabled", True))
+        self._entry_standoff = float(rospy.get_param("~entry_standoff_distance", 0.28))
+        self._entry_white_box_depth = float(rospy.get_param("~entry_white_box_depth", 0.50))
+        self._entry_vehicle_length = float(rospy.get_param("~entry_vehicle_length", 0.335))
+        if self._scan_steps <= 0:
+            raise ValueError("scan_steps must be positive")
+        if self._wall_standoff <= 0.0 or self._wall_ray_max_distance <= 0.0:
+            raise ValueError("wall approach distances must be positive")
+        half_vehicle_length = self._entry_vehicle_length / 2.0
+        if (self._entry_standoff <= half_vehicle_length
+                or self._entry_standoff + half_vehicle_length > self._entry_white_box_depth):
+            raise ValueError("entry goal does not keep the vehicle inside the white box")
+
+        self._error = 0.0
+        self._visible = False
+        self._latest_odom_yaw = None
+        self._latest_odom_monotonic = 0.0
+        self._latest_amcl_pose = None
+        self._latest_amcl_monotonic = 0.0
+        self._static_map = None
+        self._scan_state = "IDLE"
+        self._scan_completed_steps = 0
+        self._scan_target_yaw = None
+        self._scan_detection_deadline = None
+        self._last_detection_monotonic = 0.0
+        self._detection_sequence = 0
+        self._processed_sequence = 0
+        self._center_frame_count = 0
+        self._target_angular_z = 0.0
+        self._current_angular_z = 0.0
+        self._last_control_monotonic = time.monotonic()
+        self._navigation_seen_active = False
+        self._navigation_succeeded = False
+        self._alignment_started_monotonic = None
+        self._has_control = False
+        self._last_status = None
+        self._aligned = False
+        self._wall_goal_active = False
+        self._wall_goal_finished = False
+        self._entry_goal_active = False
+        self._entry_goal_finished = False
+        self._final_alignment_active = False
+        self._ocr_active = False
+        self._ocr_started_monotonic = None
+        self._factory_type = None
+        self._ocr_enabled = False
+        self._recognition_enabled = None
+        self._recognition_service_name = rospy.get_param(
+            "~recognition_enable_service", "/factory_sign_recognition/set_enabled"
+        )
+        self._recognition_enable = rospy.ServiceProxy(self._recognition_service_name, SetBool)
+        self._ocr_enable = rospy.ServiceProxy(self._ocr_service_name, SetBool)
+        self._wall_goal_client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
+        self._tf_buffer = tf2_ros.Buffer()
+        self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)
+
+        cmd_vel_topic = rospy.get_param("~cmd_vel_topic", "/factory_alignment_demo/cmd_vel")
+        self._cmd_pub = rospy.Publisher(cmd_vel_topic, Twist, queue_size=1)
+        self._status_pub = rospy.Publisher("/factory_alignment_demo/status", String, queue_size=1, latch=True)
+        self._wall_goal_pub = rospy.Publisher(
+            "/factory_alignment_demo/wall_goal", PoseStamped, queue_size=1, latch=True
+        )
+        self._entry_goal_pub = rospy.Publisher(
+            "/factory_alignment_demo/entry_goal", PoseStamped, queue_size=1, latch=True
+        )
+        self._factory_type_pub = rospy.Publisher(
+            "/factory_alignment_demo/factory_type", String, queue_size=1, latch=True
+        )
+        rospy.Subscriber("/competition_task2/current_order", String, self._order_callback, queue_size=1)
+        rospy.Subscriber("/competition_task2/command", String, self._task_command_callback, queue_size=10)
+        rospy.Subscriber("/factory_sign/target_center_error", Float32, self._error_callback, queue_size=1)
+        rospy.Subscriber("/factory_sign/target_visible", Bool, self._visible_callback, queue_size=1)
+        rospy.Subscriber("/sign_recognition", String, self._ocr_result_callback, queue_size=1)
+        rospy.Subscriber("/move_base/status", GoalStatusArray, self._navigation_callback, queue_size=5)
+        rospy.Subscriber("/odom", Odometry, self._odom_callback, queue_size=10)
+        rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, self._amcl_callback, queue_size=10)
+        rospy.Subscriber("/map", OccupancyGrid, self._map_callback, queue_size=1)
+        rospy.Timer(rospy.Duration(1.0 / rate), self._control_callback)
+        rospy.on_shutdown(self._shutdown)
+        self._set_recognition_enabled(False, required=False)
+        self._publish_status("DISABLED" if not self._enabled else ("WAITING_FOR_ORDER" if self._require_order else "WAITING_FOR_NAV_GOAL"))
+        rospy.loginfo(
+            "factory alignment demo ready: enabled=%s wall_approach=%s output=%s",
+            self._enabled, self._wall_approach_enabled, cmd_vel_topic,
+        )
+
+    def _publish_status(self, status):
+        if status != self._last_status:
+            self._status_pub.publish(String(data=status))
+            self._last_status = status
+
+    @staticmethod
+    def _normalise_factory_category(value):
+        return {"食品": "食品", "日用品": "日用品", "电子": "电子", "电子产品": "电子"}.get(str(value).strip())
+
+    def _order_callback(self, message):
+        try:
+            order = json.loads(message.data)
+            order_id = str(order["order_id"]).strip()
+            expected = self._normalise_factory_category(order.get("category", order.get("factory_category", "")))
+            product = str(order["product"]).strip()
+            warehouse = str(order["warehouse"]).strip()
+            if not order_id or expected is None or not product or not warehouse:
+                raise ValueError("missing required order field")
+        except (ValueError, TypeError, KeyError, json.JSONDecodeError) as error:
+            self._current_order = None
+            self._publish_status("ORDER_INVALID %s" % error)
+            return
+        # A fresh order is the only permitted way to clear the terminal parking lock.
+        self._current_order = {"order_id": order_id, "category": expected,
+                               "product": product, "warehouse": warehouse}
+        self._armed_order_id = order_id if self._pending_start_order_id == order_id else None
+        if self._armed_order_id is not None:
+            self._pending_start_order_id = None
+        self._completed_order_id = None
+        self._navigation_seen_active = False
+        self._navigation_succeeded = False
+        self._wall_goal_finished = False
+        self._entry_goal_finished = False
+        self._publish_status("ORDER_READY order_id=%s expected=%s" % (order_id, expected))
+
+    def _task_command_callback(self, message):
+        command = message.data.strip()
+        if command == "STOP":
+            self._pending_start_order_id = None
+            self._armed_order_id = None
+            self._target_angular_z = 0.0
+            self._current_angular_z = 0.0
+            self._ocr_active = False
+            self._ocr_started_monotonic = None
+            self._publish_stop()
+            self._has_control = True
+            if self._wall_goal_active or self._entry_goal_active:
+                self._wall_goal_client.cancel_goal()
+            self._set_recognition_enabled(False, required=False)
+            self._set_ocr_enabled(False, required=False)
+            self._publish_status("TASK2_STOPPED")
+            return
+        if not command.startswith("START_ORDER order_id="):
+            return
+        order_id = command.split("=", 1)[1].strip()
+        if not order_id:
+            return
+        self._pending_start_order_id = order_id
+        if self._current_order is not None and self._current_order.get("order_id") == order_id:
+            self._armed_order_id = order_id
+            self._pending_start_order_id = None
+            self._publish_status("ORDER_ARMED order_id=%s" % order_id)
+
+    def _error_callback(self, message):
+        self._error = max(-1.0, min(1.0, message.data))
+        self._last_detection_monotonic = time.monotonic()
+        self._detection_sequence += 1
+
+    def _visible_callback(self, message):
+        self._visible = message.data
+
+    def _odom_callback(self, message):
+        orientation = message.pose.pose.orientation
+        self._latest_odom_yaw = self._yaw_from_quaternion(orientation)
+        self._latest_odom_monotonic = time.monotonic()
+
+    def _amcl_callback(self, message):
+        orientation = message.pose.pose.orientation
+        self._latest_amcl_pose = (
+            message.pose.pose.position.x,
+            message.pose.pose.position.y,
+            self._yaw_from_quaternion(orientation),
+        )
+        self._latest_amcl_monotonic = time.monotonic()
+
+    def _map_callback(self, message):
+        if message.header.frame_id.lstrip("/") != "map":
+            rospy.logwarn_throttle(5.0, "factory alignment ignored map frame %s", message.header.frame_id)
+            return
+        self._static_map = message
+
+    @staticmethod
+    def _yaw_from_quaternion(orientation):
+        return math.atan2(
+            2.0 * (orientation.w * orientation.z + orientation.x * orientation.y),
+            1.0 - 2.0 * (orientation.y * orientation.y + orientation.z * orientation.z),
+        )
+
+    @staticmethod
+    def _wrap_to_pi(angle):
+        return math.atan2(math.sin(angle), math.cos(angle))
+
+    def _set_recognition_enabled(self, enabled, required):
+        if self._recognition_enabled is enabled:
+            return True
+        try:
+            rospy.wait_for_service(self._recognition_service_name, timeout=2.0)
+            response = self._recognition_enable(enabled)
+        except (rospy.ROSException, rospy.ServiceException) as error:
+            if required:
+                self._publish_status("RECOGNITION_SERVICE_UNAVAILABLE")
+            else:
+                rospy.logwarn("factory recognition service unavailable: %s", error)
+            return False
+        if not response.success:
+            rospy.logwarn("factory recognition switch failed: %s", response.message)
+            return False
+        self._recognition_enabled = enabled
+        return True
+
+    def _set_ocr_enabled(self, enabled, required):
+        if not self._ocr_confirmation_enabled:
+            return not enabled
+        if self._ocr_enabled is enabled:
+            return True
+        try:
+            rospy.wait_for_service(self._ocr_service_name, timeout=2.0)
+            response = self._ocr_enable(enabled)
+        except (rospy.ROSException, rospy.ServiceException) as error:
+            if required:
+                self._publish_status("OCR_SERVICE_UNAVAILABLE")
+            else:
+                rospy.logwarn("sign OCR service unavailable: %s", error)
+            return False
+        if not response.success:
+            rospy.logwarn("sign OCR switch failed: %s", response.message)
+            if required:
+                self._publish_status("OCR_SERVICE_UNAVAILABLE")
+            return False
+        self._ocr_enabled = enabled
+        return True
+
+    def _start_ocr_confirmation(self):
+        self._ocr_active = True
+        self._ocr_started_monotonic = time.monotonic()
+        self._factory_type = None
+        if not self._set_ocr_enabled(True, required=True):
+            self._ocr_active = False
+            self._ocr_started_monotonic = None
+            return False
+        self._publish_status("OCR_READING")
+        return True
+
+    def _ocr_result_callback(self, message):
+        if not self._ocr_active:
+            return
+        result = message.data.strip()
+        self._ocr_active = False
+        self._ocr_started_monotonic = None
+        self._set_ocr_enabled(False, required=False)
+        self._set_recognition_enabled(False, required=False)
+        factory_type = self._normalise_factory_category(result)
+        if factory_type is not None:
+            self._factory_type = factory_type
+            self._factory_type_pub.publish(String(data=factory_type))
+            if self._require_order:
+                expected = self._current_order["category"] if self._current_order else None
+                if factory_type != expected:
+                    self._wall_goal_finished = True
+                    self._publish_status("FACTORY_MISMATCH order_id=%s detected=%s expected=%s" % (
+                        self._current_order["order_id"] if self._current_order else "NONE",
+                        factory_type, expected or "NONE"))
+                    return
+                self._publish_status("FACTORY_MATCHED order_id=%s type=%s" % (
+                    self._current_order["order_id"], factory_type))
+            else:
+                self._publish_status("FACTORY_CONFIRMED type=%s" % factory_type)
+            if self._entry_goal_enabled:
+                self._wall_goal_finished = False
+                self._start_entry_approach()
+                return
+            self._wall_goal_finished = True
+        else:
+            self._factory_type = None
+            self._wall_goal_finished = True
+            self._publish_status("FACTORY_OCR_FAILED result=%s" % (result or "EMPTY"))
+
+    def _navigation_callback(self, message):
+        if self._require_order and self._current_order is None:
+            return
+        if (self._require_order
+                and self._armed_order_id != self._current_order.get("order_id")):
+            return
+        if self._completed_order_id is not None:
+            return
+        if not message.status_list:
+            return
+        latest = max(
+            message.status_list,
+            key=lambda status: (status.goal_id.stamp.to_nsec(), status.goal_id.id),
+        )
+        state = latest.status
+        # move_base statuses generated by our own wall/entry goals must not reset this state machine.
+        if self._wall_goal_active or self._entry_goal_active:
+            return
+        if (self._wall_goal_finished or self._entry_goal_finished) and state not in self._RUNNING_STATES:
+            return
+        if state in self._RUNNING_STATES:
+            self._wall_goal_finished = False
+            self._entry_goal_finished = False
+            self._set_recognition_enabled(False, required=False)
+            self._set_ocr_enabled(False, required=False)
+            self._ocr_active = False
+            self._ocr_started_monotonic = None
+            self._entry_goal_active = False
+            self._entry_goal_finished = False
+            self._factory_type = None
+            self._factory_type_pub.publish(String(data=""))
+            self._navigation_seen_active = True
+            self._navigation_succeeded = False
+            self._aligned = False
+            self._final_alignment_active = False
+            self._alignment_started_monotonic = None
+            if self._has_control:
+                self._publish_stop()
+            self._center_frame_count = 0
+            self._scan_state = "IDLE"
+            self._scan_completed_steps = 0
+            self._scan_target_yaw = None
+            self._scan_detection_deadline = None
+            self._target_angular_z = 0.0
+            self._current_angular_z = 0.0
+            self._last_control_monotonic = time.monotonic()
+            self._processed_sequence = self._detection_sequence
+            self._has_control = False
+            return
+        if self._navigation_seen_active and state == GoalStatus.SUCCEEDED:
+            self._navigation_succeeded = True
+            if self._alignment_started_monotonic is None:
+                self._alignment_started_monotonic = time.monotonic()
+                self._scan_state = "INITIAL_DETECT"
+                self._scan_completed_steps = 0
+                self._scan_target_yaw = None
+                self._scan_detection_deadline = self._alignment_started_monotonic + self._scan_detection_window
+                self._set_recognition_enabled(True, required=True)
+                self._publish_status("INITIAL_DETECTING")
+            return
+        if self._navigation_seen_active and state in self._FAILED_STATES:
+            self._set_recognition_enabled(False, required=False)
+            self._set_ocr_enabled(False, required=False)
+            self._ocr_active = False
+            self._navigation_succeeded = False
+            self._aligned = False
+            self._alignment_started_monotonic = None
+            self._publish_status("NAVIGATION_NOT_SUCCEEDED")
+
+    def _publish_stop(self):
+        self._cmd_pub.publish(Twist())
+
+    def _shutdown(self):
+        self._wall_goal_client.cancel_goal()
+        if self._has_control:
+            self._publish_stop()
+
+    def _publish_smooth_command(self, desired_angular_z, now):
+        elapsed = max(0.0, min(0.2, now - self._last_control_monotonic))
+        max_delta = self._max_angular_accel * elapsed
+        delta = desired_angular_z - self._current_angular_z
+        if abs(delta) <= max_delta:
+            self._current_angular_z = desired_angular_z
+        else:
+            self._current_angular_z += math.copysign(max_delta, delta)
+        self._last_control_monotonic = now
+        command = Twist()
+        command.angular.z = self._current_angular_z
+        self._has_control = True
+        self._cmd_pub.publish(command)
+        return self._current_angular_z
+
+    def _run_search_scan(self, now):
+        if self._scan_state == "COMPLETE":
+            self._target_angular_z = 0.0
+            self._current_angular_z = 0.0
+            self._publish_stop()
+            self._has_control = True
+            self._publish_status("FACTORY_NOT_FOUND_AFTER_360_DEG_SCAN")
+            return
+
+        odom_fresh = (
+            self._latest_odom_yaw is not None
+            and now - self._latest_odom_monotonic <= self._odom_timeout
+        )
+        if self._scan_state == "TURN":
+            if not odom_fresh:
+                self._target_angular_z = 0.0
+                self._publish_smooth_command(0.0, now)
+                self._publish_status("WAITING_FOR_ODOM")
+                return
+            if self._scan_target_yaw is None:
+                self._scan_target_yaw = self._wrap_to_pi(
+                    self._latest_odom_yaw + self._scan_direction * 2.0 * math.pi / self._scan_steps
+                )
+            yaw_error = self._wrap_to_pi(self._scan_target_yaw - self._latest_odom_yaw)
+            if abs(yaw_error) <= self._scan_yaw_tolerance:
+                self._target_angular_z = 0.0
+                self._scan_state = "SETTLE"
+                command_z = self._publish_smooth_command(0.0, now)
+                self._publish_status("SCAN_STEP_%d_SETTLING command_z=%+.3f" % (
+                    self._scan_completed_steps + 1, command_z
+                ))
+                return
+            self._target_angular_z = math.copysign(self._scan_speed, yaw_error)
+            command_z = self._publish_smooth_command(self._target_angular_z, now)
+            self._publish_status("SCANNING_STEP_%d/%d yaw_error=%+.3f command_z=%+.3f" % (
+                self._scan_completed_steps + 1, self._scan_steps, yaw_error, command_z
+            ))
+            return
+
+        if self._scan_state == "SETTLE":
+            command_z = self._publish_smooth_command(0.0, now)
+            if abs(command_z) <= 0.005:
+                self._scan_state = "DETECT"
+                self._scan_detection_deadline = now + self._scan_detection_window
+                self._set_recognition_enabled(True, required=True)
+                self._publish_status("SCAN_STEP_%d_DETECTING" % (self._scan_completed_steps + 1))
+            else:
+                self._publish_status("SCAN_STEP_%d_SETTLING command_z=%+.3f" % (
+                    self._scan_completed_steps + 1, command_z
+                ))
+            return
+
+        if self._scan_state in ("INITIAL_DETECT", "DETECT"):
+            self._target_angular_z = 0.0
+            self._current_angular_z = 0.0
+            self._publish_stop()
+            self._has_control = True
+            detected = self._visible and now - self._last_detection_monotonic <= self._detection_timeout
+            if detected:
+                self._scan_state = "ALIGN"
+                self._alignment_started_monotonic = now
+                self._center_frame_count = 0
+                self._processed_sequence = self._detection_sequence
+                self._publish_status("FACTORY_FOUND_STARTING_ALIGNMENT")
+                return
+            if now < self._scan_detection_deadline:
+                state = "INITIAL_DETECTING" if self._scan_state == "INITIAL_DETECT" else "SCAN_STEP_%d_DETECTING" % (self._scan_completed_steps + 1)
+                self._publish_status(state)
+                return
+            self._set_recognition_enabled(False, required=False)
+            if self._scan_state == "INITIAL_DETECT":
+                self._scan_state = "TURN"
+                self._scan_target_yaw = None
+            else:
+                self._scan_completed_steps += 1
+                if self._scan_completed_steps >= self._scan_steps:
+                    self._scan_state = "COMPLETE"
+                else:
+                    self._scan_state = "TURN"
+                    self._scan_target_yaw = None
+
+    @staticmethod
+    def _map_origin_yaw(grid):
+        return FactoryAlignmentDemo._yaw_from_quaternion(grid.info.origin.orientation)
+
+    @staticmethod
+    def _occupied(grid, row, column, threshold):
+        if row < 0 or column < 0 or row >= grid.info.height or column >= grid.info.width:
+            return False
+        return grid.data[row * grid.info.width + column] >= threshold
+
+    @classmethod
+    def _map_to_grid(cls, grid, x, y):
+        resolution = grid.info.resolution
+        if resolution <= 0.0:
+            return None
+        yaw = cls._map_origin_yaw(grid)
+        dx = x - grid.info.origin.position.x
+        dy = y - grid.info.origin.position.y
+        column = int(math.floor((math.cos(yaw) * dx + math.sin(yaw) * dy) / resolution))
+        row = int(math.floor((-math.sin(yaw) * dx + math.cos(yaw) * dy) / resolution))
+        if row < 0 or column < 0 or row >= grid.info.height or column >= grid.info.width:
+            return None
+        return row, column
+
+    @classmethod
+    def _grid_to_map(cls, grid, row, column):
+        resolution = grid.info.resolution
+        yaw = cls._map_origin_yaw(grid)
+        local_x = (column + 0.5) * resolution
+        local_y = (row + 0.5) * resolution
+        origin = grid.info.origin.position
+        return (
+            origin.x + math.cos(yaw) * local_x - math.sin(yaw) * local_y,
+            origin.y + math.sin(yaw) * local_x + math.cos(yaw) * local_y,
+        )
+
+    def _raycast_wall(self, grid, pose):
+        x, y, heading = pose
+        step = max(grid.info.resolution * 0.5, 0.01)
+        previous_cell = None
+        samples = int(math.ceil(self._wall_ray_max_distance / step))
+        for sample in range(1, samples + 1):
+            distance = sample * step
+            cell = self._map_to_grid(grid, x + distance * math.cos(heading), y + distance * math.sin(heading))
+            if cell is None:
+                break
+            if cell == previous_cell:
+                continue
+            previous_cell = cell
+            row, column = cell
+            if self._occupied(grid, row, column, self._wall_occupied_threshold):
+                hit_x, hit_y = self._grid_to_map(grid, row, column)
+                return hit_x, hit_y, row, column
+        return None
+
+    def _wall_normal_toward_robot(self, grid, hit_row, hit_column, robot_x, robot_y, heading):
+        radius_cells = max(1, int(math.ceil(self._wall_fit_radius / grid.info.resolution)))
+        queue = [(hit_row, hit_column)]
+        visited = set()
+        points = []
+        while queue:
+            row, column = queue.pop()
+            if (row, column) in visited:
+                continue
+            visited.add((row, column))
+            if not self._occupied(grid, row, column, self._wall_occupied_threshold):
+                continue
+            if math.hypot(row - hit_row, column - hit_column) > radius_cells:
+                continue
+            points.append(self._grid_to_map(grid, row, column))
+            for delta_row in (-1, 0, 1):
+                for delta_column in (-1, 0, 1):
+                    if delta_row or delta_column:
+                        queue.append((row + delta_row, column + delta_column))
+        if len(points) < self._wall_line_min_support:
+            return None
+
+        # A corner joins two wall segments. Fit several local lines, then retain
+        # the one whose outward normal faces the visually aligned vehicle.
+        if len(points) > self._wall_line_max_points:
+            stride = float(len(points)) / self._wall_line_max_points
+            points = [points[int(index * stride)] for index in range(self._wall_line_max_points)]
+        hit_x, hit_y = self._grid_to_map(grid, hit_row, hit_column)
+        desired_normal_x = -math.cos(heading)
+        desired_normal_y = -math.sin(heading)
+        best = None
+        for first_index, first in enumerate(points):
+            for second in points[first_index + 1:]:
+                dx = second[0] - first[0]
+                dy = second[1] - first[1]
+                length = math.hypot(dx, dy)
+                if length < self._wall_line_min_length:
+                    continue
+                # The selected line must describe the actually struck wall cell.
+                hit_distance = abs(dy * (hit_x - first[0]) - dx * (hit_y - first[1])) / length
+                if hit_distance > self._wall_line_inlier_distance:
+                    continue
+                normal = math.atan2(dy, dx) + math.pi / 2.0
+                if math.cos(normal) * (robot_x - hit_x) + math.sin(normal) * (robot_y - hit_y) < 0.0:
+                    normal += math.pi
+                facing = math.cos(normal) * desired_normal_x + math.sin(normal) * desired_normal_y
+                if facing < self._wall_line_min_facing_alignment:
+                    continue
+                support = 0
+                for point in points:
+                    distance = abs(dy * (point[0] - first[0]) - dx * (point[1] - first[1])) / length
+                    if distance <= self._wall_line_inlier_distance:
+                        support += 1
+                if support < self._wall_line_min_support:
+                    continue
+                score = support * (0.5 + 0.5 * facing)
+                if best is None or score > best[0]:
+                    best = (score, normal)
+        if best is None:
+            return None
+        return self._wrap_to_pi(best[1])
+
+    def _map_pose(self):
+        try:
+            transform = self._tf_buffer.lookup_transform(
+                self._tf_map_frame, self._tf_base_frame, rospy.Time(0),
+                rospy.Duration(self._tf_lookup_timeout),
+            )
+            translation = transform.transform.translation
+            return (
+                translation.x, translation.y,
+                self._yaw_from_quaternion(transform.transform.rotation),
+            ), None
+        except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
+                tf2_ros.ExtrapolationException, tf2_ros.TimeoutException) as error:
+            rospy.logwarn_throttle(5.0, "factory alignment TF pose unavailable: %s", error)
+        if (self._latest_amcl_pose is not None
+                and time.monotonic() - self._latest_amcl_monotonic <= self._amcl_fallback_timeout):
+            return self._latest_amcl_pose, None
+        return None, "MAP_POSE_UNAVAILABLE"
+
+    def _compute_wall_goal(self, standoff_distance=None):
+        standoff = self._wall_standoff if standoff_distance is None else standoff_distance
+        if standoff <= 0.0:
+            return None, "WALL_GOAL_INVALID_STANDOFF"
+        if self._static_map is None:
+            return None, "STATIC_MAP_UNAVAILABLE"
+        pose, failure = self._map_pose()
+        if failure is not None:
+            return None, failure
+        grid = self._static_map
+        ray_hit = self._raycast_wall(grid, pose)
+        if ray_hit is None:
+            return None, "WALL_RAY_NO_HIT"
+        hit_x, hit_y, hit_row, hit_column = ray_hit
+        robot_x, robot_y, heading = pose
+        normal = self._wall_normal_toward_robot(
+            grid, hit_row, hit_column, robot_x, robot_y, heading
+        )
+        if normal is None:
+            return None, "WALL_NORMAL_UNAVAILABLE"
+        goal_x = hit_x + standoff * math.cos(normal)
+        goal_y = hit_y + standoff * math.sin(normal)
+        goal_cell = self._map_to_grid(grid, goal_x, goal_y)
+        if goal_cell is None:
+            return None, "WALL_GOAL_OUTSIDE_MAP"
+        if self._occupied(grid, goal_cell[0], goal_cell[1], self._wall_occupied_threshold):
+            return None, "WALL_GOAL_OCCUPIED"
+        return (goal_x, goal_y, self._wrap_to_pi(normal + math.pi), hit_x, hit_y), None
+
+    def _start_wall_approach(self):
+        computed, failure = self._compute_wall_goal()
+        if failure is not None:
+            self._wall_goal_finished = True
+            self._set_recognition_enabled(False, required=False)
+            self._publish_stop()
+            self._has_control = True
+            self._publish_status(failure)
+            return False
+        if not self._wall_goal_client.wait_for_server(rospy.Duration(self._wall_goal_server_timeout)):
+            self._wall_goal_finished = True
+            self._set_recognition_enabled(False, required=False)
+            self._publish_stop()
+            self._has_control = True
+            self._publish_status("WALL_GOAL_MOVE_BASE_UNAVAILABLE")
+            return False
+        goal_x, goal_y, goal_yaw, hit_x, hit_y = computed
+        goal = MoveBaseGoal()
+        goal.target_pose.header.frame_id = "map"
+        goal.target_pose.header.stamp = rospy.Time.now()
+        goal.target_pose.pose.position.x = goal_x
+        goal.target_pose.pose.position.y = goal_y
+        goal.target_pose.pose.orientation = Quaternion(
+            z=math.sin(goal_yaw / 2.0), w=math.cos(goal_yaw / 2.0)
+        )
+        self._wall_goal_pub.publish(goal.target_pose)
+        self._wall_goal_active = True
+        self._has_control = False
+        self._wall_goal_client.send_goal(goal, done_cb=self._wall_goal_done)
+        self._publish_status(
+            "WALL_GOAL_SENT x=%.3f y=%.3f yaw=%.3f wall_x=%.3f wall_y=%.3f" % (
+                goal_x, goal_y, goal_yaw, hit_x, hit_y
+            )
+        )
+        return True
+
+    def _start_entry_approach(self):
+        # Re-read map -> base_link and cast a fresh ray after OCR confirmation.
+        computed, failure = self._compute_wall_goal(self._entry_standoff)
+        if failure is not None:
+            self._entry_goal_finished = True
+            self._publish_stop()
+            self._has_control = True
+            self._publish_status("ENTRY_GOAL_%s" % failure)
+            return False
+        if not self._wall_goal_client.wait_for_server(rospy.Duration(self._wall_goal_server_timeout)):
+            self._entry_goal_finished = True
+            self._publish_stop()
+            self._has_control = True
+            self._publish_status("ENTRY_GOAL_MOVE_BASE_UNAVAILABLE")
+            return False
+        goal_x, goal_y, goal_yaw, hit_x, hit_y = computed
+        goal = MoveBaseGoal()
+        goal.target_pose.header.frame_id = "map"
+        goal.target_pose.header.stamp = rospy.Time.now()
+        goal.target_pose.pose.position.x = goal_x
+        goal.target_pose.pose.position.y = goal_y
+        goal.target_pose.pose.orientation = Quaternion(
+            z=math.sin(goal_yaw / 2.0), w=math.cos(goal_yaw / 2.0)
+        )
+        self._entry_goal_pub.publish(goal.target_pose)
+        self._entry_goal_active = True
+        self._has_control = False
+        self._wall_goal_client.send_goal(goal, done_cb=self._entry_goal_done)
+        self._publish_status(
+            "ENTRY_GOAL_SENT x=%.3f y=%.3f yaw=%.3f wall_x=%.3f wall_y=%.3f" % (
+                goal_x, goal_y, goal_yaw, hit_x, hit_y
+            )
+        )
+        return True
+
+    def _entry_goal_done(self, state, _result):
+        self._entry_goal_active = False
+        self._entry_goal_finished = True
+        self._target_angular_z = 0.0
+        self._current_angular_z = 0.0
+        self._publish_stop()
+        self._has_control = True
+        if state == GoalStatus.SUCCEEDED:
+            self._publish_status("FACTORY_ENTRY_COMPLETE type=%s" % (self._factory_type or "UNKNOWN"))
+            if self._require_order:
+                self._completed_order_id = self._current_order["order_id"] if self._current_order else None
+        else:
+            reason = "move_base_state=%d" % state
+            self._publish_status("FACTORY_ENTRY_FAILED %s" % reason)
+
+    def _wall_goal_done(self, state, _result):
+        self._wall_goal_active = False
+        self._target_angular_z = 0.0
+        self._current_angular_z = 0.0
+        self._publish_stop()
+        self._has_control = True
+        if state != GoalStatus.SUCCEEDED:
+            self._wall_goal_finished = True
+            self._final_alignment_active = False
+            self._set_recognition_enabled(False, required=False)
+            self._aligned = False
+            self._publish_status("WALL_APPROACH_FAILED move_base_state=%d" % state)
+            return
+
+        # The map-derived goal brings the vehicle to the wall stand-off point.
+        # Re-enable vision there for one final heading correction only.
+        self._wall_goal_finished = False
+        self._final_alignment_active = True
+        self._aligned = False
+        self._scan_state = "ALIGN"
+        self._alignment_started_monotonic = time.monotonic()
+        self._center_frame_count = 0
+        self._processed_sequence = self._detection_sequence
+        self._set_recognition_enabled(True, required=False)
+        self._publish_status("WALL_APPROACH_REFINING_ALIGNMENT")
+
+    def _control_callback(self, _event):
+        if not self._enabled:
+            self._publish_status("DISABLED")
+            return
+        if self._require_order and self._current_order is None:
+            self._publish_status("WAITING_FOR_ORDER")
+            return
+        if (self._require_order
+                and self._armed_order_id != self._current_order.get("order_id")):
+            self._publish_status("WAITING_FOR_START_ORDER")
+            return
+        if self._completed_order_id is not None:
+            return
+        if not self._navigation_seen_active:
+            self._publish_status("WAITING_FOR_NAV_GOAL")
+            return
+        if not self._navigation_succeeded:
+            self._publish_status("NAVIGATING")
+            return
+
+        now = time.monotonic()
+        if self._ocr_active:
+            if now - self._ocr_started_monotonic > self._ocr_timeout:
+                self._ocr_active = False
+                self._ocr_started_monotonic = None
+                self._set_ocr_enabled(False, required=False)
+                self._set_recognition_enabled(False, required=False)
+                self._wall_goal_finished = True
+                self._publish_status("FACTORY_OCR_FAILED timeout")
+            else:
+                self._publish_status("OCR_READING")
+            return
+        if self._wall_goal_active or self._entry_goal_active:
+            return
+        if self._wall_goal_finished or self._entry_goal_finished:
+            return
+        if self._scan_state not in ("ALIGN", "IDLE"):
+            self._run_search_scan(now)
+            return
+        if self._aligned:
+            self._set_recognition_enabled(False, required=False)
+            self._target_angular_z = 0.0
+            self._current_angular_z = 0.0
+            self._publish_stop()
+            self._has_control = True
+            self._publish_status("ALIGNED")
+            return
+        if now - self._alignment_started_monotonic > self._alignment_timeout:
+            self._set_recognition_enabled(False, required=False)
+            self._target_angular_z = 0.0
+            self._current_angular_z = 0.0
+            self._publish_stop()
+            self._has_control = True
+            self._publish_status("ALIGNMENT_TIMEOUT")
+            return
+
+        target_fresh = self._visible and now - self._last_detection_monotonic <= self._detection_timeout
+        if not target_fresh:
+            self._center_frame_count = 0
+            self._target_angular_z = 0.0
+            command_z = self._publish_smooth_command(0.0, now)
+            self._publish_status("SEARCHING_FACTORY command_z=%+.3f" % command_z)
+            return
+
+        if self._processed_sequence != self._detection_sequence:
+            self._processed_sequence = self._detection_sequence
+            if abs(self._error) <= self._tolerance:
+                self._center_frame_count += 1
+                self._target_angular_z = 0.0
+                if self._center_frame_count >= self._center_frames_required:
+                    self._current_angular_z = 0.0
+                    self._publish_stop()
+                    self._has_control = True
+                    if self._wall_approach_enabled and not self._final_alignment_active:
+                        self._start_wall_approach()
+                        return
+                    self._aligned = True
+                    self._final_alignment_active = False
+                    if self._wall_approach_enabled and self._ocr_confirmation_enabled:
+                        self._publish_status("WALL_APPROACH_ALIGNED")
+                        # Full-frame OCR subscribes directly to the camera, so the
+                        # RKNN locator can be stopped before the OCR attempt.
+                        self._set_recognition_enabled(False, required=False)
+                        if self._start_ocr_confirmation():
+                            return
+                        self._wall_goal_finished = True
+                        self._set_recognition_enabled(False, required=False)
+                        return
+                    self._wall_goal_finished = self._wall_approach_enabled
+                    self._set_recognition_enabled(False, required=False)
+                    self._publish_status("WALL_APPROACH_ALIGNED" if self._wall_approach_enabled else "ALIGNED")
+                    return
+            else:
+                self._center_frame_count = 0
+                target = self._angular_sign * self._kp * self._error
+                target = max(-self._max_speed, min(self._max_speed, target))
+                if abs(target) < self._min_speed:
+                    target = math.copysign(self._min_speed, target)
+                self._target_angular_z = target
+
+        command_z = self._publish_smooth_command(self._target_angular_z, now)
+        if self._center_frame_count:
+            label = "FINAL_CENTER_FRAME" if self._final_alignment_active else "CENTER_FRAME"
+            self._publish_status("%s %d/%d command_z=%+.3f" % (
+                label, self._center_frame_count, self._center_frames_required, command_z
+            ))
+        else:
+            label = "FINAL_SMOOTH_ALIGN" if self._final_alignment_active else "SMOOTH_ALIGN"
+            self._publish_status("%s error=%+.3f target_z=%+.3f command_z=%+.3f" % (
+                label, self._error, self._target_angular_z, command_z
+            ))
+
+
+if __name__ == "__main__":
+    FactoryAlignmentDemo()
+    rospy.spin()

factory_alignment_demo/scripts/factory_candidate_navigator.py

@@ -0,0 +1,199 @@
+#!/usr/bin/env python3
+"""Try factory candidate poses in order until one move_base goal succeeds."""
+import math
+import sys
+import time
+
+import actionlib
+import rospy
+from actionlib_msgs.msg import GoalStatus
+from geometry_msgs.msg import PoseWithCovarianceStamped, Quaternion
+from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
+from std_msgs.msg import String
+
+
+class FactoryCandidateNavigator:
+    _FAILURE_STATES = {
+        GoalStatus.PREEMPTED,
+        GoalStatus.ABORTED,
+        GoalStatus.REJECTED,
+        GoalStatus.RECALLED,
+        GoalStatus.LOST,
+    }
+
+    def __init__(self):
+        rospy.init_node("factory_candidate_navigator")
+        self._status_pub = rospy.Publisher(
+            "/factory_candidate_navigator/status", String, queue_size=10, latch=True
+        )
+        self._initial_pose_pub = rospy.Publisher(
+            "/initialpose", PoseWithCovarianceStamped, queue_size=1
+        )
+        self._frame_id = rospy.get_param("~frame_id", "map")
+        self._start_pose = rospy.get_param("~start_pose")
+        self._candidates = rospy.get_param("~candidate_goals")
+        self._server_timeout = float(rospy.get_param("~move_base_server_timeout_seconds", 10.0))
+        self._goal_timeout = float(rospy.get_param("~goal_timeout_seconds", 120.0))
+        self._initial_pose_confirmation_timeout = float(
+            rospy.get_param("~initial_pose_confirmation_timeout_seconds", 5.0)
+        )
+        self._initial_pose_position_tolerance = float(
+            rospy.get_param("~initial_pose_position_tolerance_m", 0.3)
+        )
+        self._initial_pose_yaw_tolerance = float(
+            rospy.get_param("~initial_pose_yaw_tolerance_rad", 0.2)
+        )
+        self._latest_amcl_pose = None
+        self._latest_amcl_monotonic = 0.0
+        self._validate_config()
+        rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, self._amcl_callback, queue_size=10)
+        self._client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
+        rospy.on_shutdown(self._cancel_goal)
+
+    def _publish(self, text):
+        rospy.loginfo("factory candidate navigator: %s", text)
+        self._status_pub.publish(String(data=text))
+
+    def _validate_pose(self, pose, label):
+        try:
+            values = (float(pose["x"]), float(pose["y"]), float(pose["yaw"]))
+        except (KeyError, TypeError, ValueError) as error:
+            raise ValueError("%s pose is invalid: %s" % (label, error))
+        if not all(math.isfinite(value) for value in values):
+            raise ValueError("%s pose has non-finite values" % label)
+        return values
+
+    def _validate_config(self):
+        if self._frame_id != "map":
+            raise ValueError("factory candidate goals must use the map frame")
+        self._validate_pose(self._start_pose, "start")
+        if not isinstance(self._candidates, list) or len(self._candidates) != 3:
+            raise ValueError("candidate_goals must contain exactly three poses")
+        for index, candidate in enumerate(self._candidates, start=1):
+            self._validate_pose(candidate, "candidate %d" % index)
+        if self._goal_timeout <= 0.0 or self._server_timeout <= 0.0:
+            raise ValueError("navigation timeouts must be positive")
+        if self._initial_pose_confirmation_timeout <= 0.0:
+            raise ValueError("initial pose confirmation timeout must be positive")
+        if self._initial_pose_position_tolerance <= 0.0 or self._initial_pose_yaw_tolerance <= 0.0:
+            raise ValueError("initial pose confirmation tolerances must be positive")
+
+    def _amcl_callback(self, message):
+        orientation = message.pose.pose.orientation
+        yaw = math.atan2(
+            2.0 * (orientation.w * orientation.z + orientation.x * orientation.y),
+            1.0 - 2.0 * (orientation.y * orientation.y + orientation.z * orientation.z),
+        )
+        self._latest_amcl_pose = (
+            message.pose.pose.position.x, message.pose.pose.position.y, yaw
+        )
+        self._latest_amcl_monotonic = time.monotonic()
+
+
+    @staticmethod
+    def _wrap_to_pi(angle):
+        return math.atan2(math.sin(angle), math.cos(angle))
+
+    def _goal_from_candidate(self, candidate):
+        x, y, yaw = self._validate_pose(candidate, "candidate")
+        goal = MoveBaseGoal()
+        goal.target_pose.header.frame_id = self._frame_id
+        goal.target_pose.header.stamp = rospy.Time.now()
+        goal.target_pose.pose.position.x = x
+        goal.target_pose.pose.position.y = y
+        goal.target_pose.pose.orientation = Quaternion(
+            z=math.sin(yaw / 2.0), w=math.cos(yaw / 2.0)
+        )
+        return goal, x, y, yaw
+
+    def _publish_initial_pose(self):
+        x, y, yaw = self._validate_pose(self._start_pose, "start")
+        message = PoseWithCovarianceStamped()
+        message.header.frame_id = self._frame_id
+        message.header.stamp = rospy.Time.now()
+        message.pose.pose.position.x = x
+        message.pose.pose.position.y = y
+        message.pose.pose.orientation.z = math.sin(yaw / 2.0)
+        message.pose.pose.orientation.w = math.cos(yaw / 2.0)
+        message.pose.covariance[0] = 0.25
+        message.pose.covariance[7] = 0.25
+        message.pose.covariance[35] = 0.0685
+        self._publish("INITIALIZING: setting AMCL pose x=%.3f y=%.3f yaw=%.3f" % (x, y, yaw))
+        for _ in range(3):
+            message.header.stamp = rospy.Time.now()
+            self._initial_pose_pub.publish(message)
+            rospy.sleep(0.1)
+
+    def _wait_for_initial_pose_confirmation(self):
+        target_x, target_y, target_yaw = self._validate_pose(self._start_pose, "start")
+        deadline = time.monotonic() + self._initial_pose_confirmation_timeout
+        while not rospy.is_shutdown() and time.monotonic() < deadline:
+            pose = self._latest_amcl_pose
+            fresh = time.monotonic() - self._latest_amcl_monotonic <= 1.0
+            if pose is not None and fresh:
+                x, y, yaw = pose
+                position_error = math.hypot(x - target_x, y - target_y)
+                yaw_error = abs(self._wrap_to_pi(yaw - target_yaw))
+                if (position_error <= self._initial_pose_position_tolerance
+                        and yaw_error <= self._initial_pose_yaw_tolerance):
+                    self._publish("INITIAL_POSE_CONFIRMED: position_error=%.3f yaw_error=%.3f" % (
+                        position_error, yaw_error
+                    ))
+                    return True
+            rospy.sleep(0.05)
+        self._publish("INITIAL_POSE_TIMEOUT: candidate navigation not started")
+        return False
+
+    def _cancel_goal(self):
+        if hasattr(self, "_client"):
+            self._client.cancel_goal()
+
+    def run(self):
+        sx, sy, syaw = self._validate_pose(self._start_pose, "start")
+        self._publish_initial_pose()
+        if not self._wait_for_initial_pose_confirmation():
+            return 1
+        self._publish("START: task2 start x=%.3f y=%.3f yaw=%.3f" % (sx, sy, syaw))
+        if not self._client.wait_for_server(rospy.Duration(self._server_timeout)):
+            self._publish("FAILED: move_base unavailable")
+            return 1
+
+        for index, candidate in enumerate(self._candidates, start=1):
+            goal, x, y, yaw = self._goal_from_candidate(candidate)
+            self._publish("NAVIGATING: candidate %d/3 x=%.3f y=%.3f yaw=%.3f" % (
+                index, x, y, yaw
+            ))
+            self._client.send_goal(goal)
+            if not self._client.wait_for_result(rospy.Duration(self._goal_timeout)):
+                self._client.cancel_goal()
+                self._publish("FAILED: candidate %d/3 timed out; trying next" % index)
+                continue
+            state = self._client.get_state()
+            if state == GoalStatus.SUCCEEDED:
+                self._publish("SUCCEEDED: candidate %d/3 reached; remaining candidates skipped" % index)
+                return 0
+            if state in self._FAILURE_STATES:
+                self._publish("FAILED: candidate %d/3 move_base state=%d; trying next" % (
+                    index, state
+                ))
+            else:
+                self._client.cancel_goal()
+                self._publish("FAILED: candidate %d/3 unexpected state=%d; trying next" % (
+                    index, state
+                ))
+
+        self._publish("ALL_CANDIDATES_FAILED")
+        return 1
+
+
+def main():
+    try:
+        navigator = FactoryCandidateNavigator()
+        return navigator.run()
+    except (ValueError, rospy.ROSException) as error:
+        rospy.logerr("factory candidate navigator configuration error: %s", error)
+        return 2
+
+
+if __name__ == "__main__":
+    sys.exit(main())

factory_alignment_demo/scripts/factory_search_point_adapter.py

@@ -0,0 +1,597 @@
+#!/usr/bin/env python3
+"""Adapt a nominal factory-search pose into obstacle-aware move_base candidates."""
+from __future__ import annotations
+
+import math
+import queue
+import threading
+import time
+
+import actionlib
+import rospy
+import tf2_ros
+from actionlib_msgs.msg import GoalStatus
+from geometry_msgs.msg import Pose, PoseArray, PoseStamped, Quaternion
+from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
+from nav_msgs.msg import OccupancyGrid
+from nav_msgs.srv import GetPlan, GetPlanRequest
+from std_msgs.msg import String
+from std_srvs.srv import Trigger, TriggerResponse
+
+
+class _PlanTask:
+    """One make_plan request handled by the sole background RPC worker."""
+
+    def __init__(self, request):
+        self.request = request
+        self.response = None
+        self.error = None
+        self.finished = threading.Event()
+
+
+class FactorySearchPointAdapter:
+    """Send only safe observation candidates; never publish cmd_vel."""
+
+    _NAV_FAILURE_STATES = {
+        GoalStatus.PREEMPTED, GoalStatus.ABORTED, GoalStatus.REJECTED,
+        GoalStatus.RECALLED, GoalStatus.LOST,
+    }
+    _SEARCH_RETRY_PREFIXES = (
+        "FACTORY_NOT_FOUND_AFTER_360_DEG_SCAN",
+        "ALIGNMENT_TIMEOUT",
+        "WALL_NORMAL_UNAVAILABLE",
+        "WALL_RAY_NO_HIT",
+        "WALL_GOAL_INVALID_STANDOFF",
+        "WALL_GOAL_OUTSIDE_MAP",
+        "WALL_GOAL_OCCUPIED",
+        "WALL_GOAL_MOVE_BASE_UNAVAILABLE",
+        "WALL_APPROACH_FAILED",
+        "FACTORY_OCR_FAILED",
+        "FACTORY_ENTRY_FAILED",
+    )
+
+    def __init__(self):
+        rospy.init_node("factory_search_point_adapter")
+        self._frame_id = rospy.get_param("~frame_id", "map")
+        self._base_frame = rospy.get_param("~base_frame", "base_link")
+        self._nominal_topic = rospy.get_param(
+            "~nominal_goal_topic", "/factory_search_adapter/nominal_goal"
+        )
+        self._static_map_topic = rospy.get_param("~static_map_topic", "/map")
+        self._local_costmap_topic = rospy.get_param(
+            "~local_costmap_topic", "/move_base/local_costmap/costmap"
+        )
+        self._alignment_status_topic = rospy.get_param(
+            "~alignment_status_topic", "/factory_alignment_demo/status"
+        )
+        self._plan_service_name = rospy.get_param("~make_plan_service", "/move_base/make_plan")
+        self._ring_radii = [float(value) for value in rospy.get_param("~ring_radii", [0.4, 0.6])]
+        self._ring_directions = int(rospy.get_param("~ring_directions", 8))
+        self._footprint_length = float(rospy.get_param("~checked_footprint_length", 0.395))
+        self._footprint_width = float(rospy.get_param("~checked_footprint_width", 0.316))
+        self._static_occupied_threshold = int(rospy.get_param("~static_occupied_threshold", 65))
+        self._local_cost_threshold = int(rospy.get_param("~local_cost_threshold", 80))
+        self._local_costmap_timeout = float(rospy.get_param("~local_costmap_timeout_seconds", 2.0))
+        self._goal_timeout = float(rospy.get_param("~candidate_goal_timeout_seconds", 45.0))
+        self._search_timeout = float(rospy.get_param("~candidate_search_timeout_seconds", 80.0))
+        self._server_timeout = float(rospy.get_param("~move_base_server_timeout_seconds", 2.0))
+        self._plan_response_timeout = float(rospy.get_param("~make_plan_response_timeout_seconds", 0.4))
+        self._timer_period = float(rospy.get_param("~timer_period_seconds", 0.2))
+        self._validate_config()
+
+        self._static_map = None
+        self._local_costmap = None
+        self._local_costmap_monotonic = 0.0
+        self._nominal_pose = None
+        self._candidates = []
+        self._candidate_index = 0
+        self._state = "IDLE"
+        self._active_sequence = 0
+        self._active_deadline = None
+        self._search_deadline = None
+        self._plan_timeout_this_round = False
+        self._plan_fallback_announced = False
+        self._last_status = None
+        self._completion_locked = False
+
+        self._client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
+        self._plan_client = rospy.ServiceProxy(self._plan_service_name, GetPlan)
+        self._plan_request_queue = queue.Queue(maxsize=1)
+        self._plan_worker_thread = threading.Thread(
+            target=self._plan_worker, name="factory_make_plan", daemon=True
+        )
+        self._plan_worker_thread.start()
+        self._tf_buffer = tf2_ros.Buffer()
+        self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)
+        self._status_pub = rospy.Publisher(
+            "/factory_search_adapter/status", String, queue_size=10, latch=True
+        )
+        self._candidate_pub = rospy.Publisher(
+            "/factory_search_adapter/candidates", PoseArray, queue_size=1, latch=True
+        )
+        self._selected_goal_pub = rospy.Publisher(
+            "/factory_search_adapter/selected_goal", PoseStamped, queue_size=1, latch=True
+        )
+        self._reset_service = rospy.Service("/factory_search_adapter/reset", Trigger, self._reset_callback)
+        rospy.Subscriber(self._nominal_topic, PoseStamped, self._nominal_callback, queue_size=1)
+        rospy.Subscriber(self._static_map_topic, OccupancyGrid, self._static_map_callback, queue_size=1)
+        rospy.Subscriber(self._local_costmap_topic, OccupancyGrid, self._local_costmap_callback, queue_size=1)
+        rospy.Subscriber(self._alignment_status_topic, String, self._alignment_status_callback, queue_size=10)
+        rospy.Timer(rospy.Duration(self._timer_period), self._timer_callback)
+        rospy.on_shutdown(self._cancel_own_goal)
+        self._publish_status("WAITING_FOR_NOMINAL_GOAL")
+
+    def _validate_config(self):
+        if self._frame_id != "map":
+            raise ValueError("factory search candidates must use the map frame")
+        if self._ring_directions < 4:
+            raise ValueError("ring_directions must be at least 4")
+        if any(radius <= 0.0 for radius in self._ring_radii):
+            raise ValueError("ring radii must be positive")
+        if self._footprint_length <= 0.0 or self._footprint_width <= 0.0:
+            raise ValueError("checked footprint dimensions must be positive")
+        if not 0 <= self._static_occupied_threshold <= 100:
+            raise ValueError("static_occupied_threshold must be in [0, 100]")
+        if not 1 <= self._local_cost_threshold <= 100:
+            raise ValueError("local_cost_threshold must be in [1, 100]")
+        if min(self._local_costmap_timeout, self._goal_timeout, self._search_timeout,
+               self._server_timeout, self._plan_response_timeout,
+               self._timer_period) <= 0.0:
+            raise ValueError("search adapter timeouts must be positive")
+
+    @staticmethod
+    def _yaw_from_quaternion(orientation):
+        return math.atan2(
+            2.0 * (orientation.w * orientation.z + orientation.x * orientation.y),
+            1.0 - 2.0 * (orientation.y * orientation.y + orientation.z * orientation.z),
+        )
+
+    @staticmethod
+    def _quaternion_from_yaw(yaw):
+        return Quaternion(z=math.sin(yaw / 2.0), w=math.cos(yaw / 2.0))
+
+    def _publish_status(self, status):
+        if status == self._last_status:
+            return
+        self._last_status = status
+        rospy.loginfo("factory search adapter: %s", status)
+        self._status_pub.publish(String(data=status))
+
+    def _static_map_callback(self, message):
+        if message.header.frame_id.lstrip("/") != self._frame_id:
+            rospy.logwarn_throttle(5.0, "factory search ignored static map frame %s", message.header.frame_id)
+            return
+        self._static_map = message
+
+    def _local_costmap_callback(self, message):
+        if message.header.frame_id.lstrip("/") != self._frame_id:
+            rospy.logwarn_throttle(
+                5.0, "factory search ignored local costmap frame %s (expected %s)",
+                message.header.frame_id, self._frame_id,
+            )
+            return
+        self._local_costmap = message
+        self._local_costmap_monotonic = time.monotonic()
+
+    def _reset_callback(self, _request):
+        self._cancel_own_goal()
+        self._active_sequence += 1
+        self._nominal_pose = None
+        self._candidates = []
+        self._candidate_index = 0
+        self._active_deadline = None
+        self._search_deadline = None
+        self._plan_timeout_this_round = False
+        self._plan_fallback_announced = False
+        self._completion_locked = False
+        self._state = "IDLE"
+        self._publish_status("RESET_READY_FOR_NEXT_OBSERVATION")
+        return TriggerResponse(success=True, message="factory search adapter reset")
+
+    def _nominal_callback(self, message):
+        if self._completion_locked:
+            self._publish_status("NOMINAL_GOAL_IGNORED_COMPLETE_LOCKED")
+            return
+        if message.header.frame_id.lstrip("/") != self._frame_id:
+            self._publish_status("NOMINAL_GOAL_WRONG_FRAME")
+            return
+        yaw = self._yaw_from_quaternion(message.pose.orientation)
+        values = (message.pose.position.x, message.pose.position.y, yaw)
+        if not all(math.isfinite(value) for value in values):
+            self._publish_status("NOMINAL_GOAL_INVALID")
+            return
+        self._cancel_own_goal()
+        self._active_sequence += 1
+        self._nominal_pose = values
+        self._candidates = []
+        self._candidate_index = 0
+        self._active_deadline = None
+        self._search_deadline = None
+        self._plan_timeout_this_round = False
+        self._plan_fallback_announced = False
+        self._state = "PREPARING"
+        self._publish_status("PREPARING_CANDIDATES x=%.3f y=%.3f yaw=%.3f" % values)
+
+    def _cancel_own_goal(self):
+        if hasattr(self, "_client") and self._state == "NAVIGATING":
+            self._client.cancel_goal()
+
+    @staticmethod
+    def _grid_origin_yaw(grid):
+        return FactorySearchPointAdapter._yaw_from_quaternion(grid.info.origin.orientation)
+
+    def _map_to_grid(self, grid, x, y):
+        resolution = grid.info.resolution
+        if resolution <= 0.0:
+            return None
+        origin = grid.info.origin.position
+        yaw = self._grid_origin_yaw(grid)
+        dx, dy = x - origin.x, y - origin.y
+        local_x = math.cos(yaw) * dx + math.sin(yaw) * dy
+        local_y = -math.sin(yaw) * dx + math.cos(yaw) * dy
+        column, row = int(math.floor(local_x / resolution)), int(math.floor(local_y / resolution))
+        if row < 0 or column < 0 or row >= grid.info.height or column >= grid.info.width:
+            return None
+        return row, column
+
+    def _grid_to_map(self, grid, row, column):
+        resolution = grid.info.resolution
+        origin = grid.info.origin.position
+        yaw = self._grid_origin_yaw(grid)
+        local_x, local_y = (column + 0.5) * resolution, (row + 0.5) * resolution
+        return (
+            origin.x + math.cos(yaw) * local_x - math.sin(yaw) * local_y,
+            origin.y + math.sin(yaw) * local_x + math.cos(yaw) * local_y,
+        )
+
+    @staticmethod
+    def _cost_at(grid, row, column):
+        return grid.data[row * grid.info.width + column]
+
+    def _footprint_cost(self, grid, x, y, yaw, threshold):
+        """Return max cell cost, or None if footprint reaches unknown/outside/lethal cells."""
+        resolution = grid.info.resolution
+        if resolution <= 0.0:
+            return None
+        radius = math.hypot(self._footprint_length / 2.0, self._footprint_width / 2.0)
+        centre = self._map_to_grid(grid, x, y)
+        if centre is None:
+            return None
+        radius_cells = int(math.ceil(radius / resolution)) + 1
+        max_cost = 0
+        found = False
+        for row in range(centre[0] - radius_cells, centre[0] + radius_cells + 1):
+            for column in range(centre[1] - radius_cells, centre[1] + radius_cells + 1):
+                if row < 0 or column < 0 or row >= grid.info.height or column >= grid.info.width:
+                    return None
+                cell_x, cell_y = self._grid_to_map(grid, row, column)
+                dx, dy = cell_x - x, cell_y - y
+                longitudinal = math.cos(yaw) * dx + math.sin(yaw) * dy
+                lateral = -math.sin(yaw) * dx + math.cos(yaw) * dy
+                if (abs(longitudinal) > self._footprint_length / 2.0
+                        or abs(lateral) > self._footprint_width / 2.0):
+                    continue
+                found = True
+                cost = self._cost_at(grid, row, column)
+                if cost < 0 or cost >= threshold:
+                    return None
+                max_cost = max(max_cost, cost)
+        return max_cost if found else None
+
+    def _footprint_is_inside_grid(self, grid, x, y, yaw):
+        """Whether the complete checked rectangle is covered by this grid.
+
+        The local costmap is a rolling window.  A candidate farther than that
+        window is not evidence of an obstacle; it simply cannot yet be checked
+        against live cone observations.  Static-map and global-plan checks
+        still apply in that case, and TEB will receive the current local map
+        while driving there.
+        """
+        half_length = self._footprint_length / 2.0
+        half_width = self._footprint_width / 2.0
+        for longitudinal in (-half_length, half_length):
+            for lateral in (-half_width, half_width):
+                corner_x = x + math.cos(yaw) * longitudinal - math.sin(yaw) * lateral
+                corner_y = y + math.sin(yaw) * longitudinal + math.cos(yaw) * lateral
+                if self._map_to_grid(grid, corner_x, corner_y) is None:
+                    return False
+        return True
+
+    def _fresh_local_costmap(self):
+        return (self._local_costmap is not None
+                and time.monotonic() - self._local_costmap_monotonic <= self._local_costmap_timeout)
+
+    def _current_pose(self):
+        try:
+            transform = self._tf_buffer.lookup_transform(
+                self._frame_id, self._base_frame, rospy.Time(0), rospy.Duration(0.2)
+            )
+        except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
+                tf2_ros.ExtrapolationException, tf2_ros.TimeoutException):
+            return None
+        translation = transform.transform.translation
+        return translation.x, translation.y, self._yaw_from_quaternion(transform.transform.rotation)
+
+    def _pose_stamped(self, x, y, yaw):
+        pose = PoseStamped()
+        pose.header.frame_id = self._frame_id
+        pose.header.stamp = rospy.Time.now()
+        pose.pose.position.x = x
+        pose.pose.position.y = y
+        pose.pose.orientation = self._quaternion_from_yaw(yaw)
+        return pose
+
+    def _plan_worker(self):
+        """Serialize potentially stuck service calls in one daemon worker."""
+        while True:
+            task = self._plan_request_queue.get()
+            try:
+                task.response = self._plan_client(task.request)
+            except Exception as error:  # rospy may expose several service exceptions.
+                task.error = error
+            finally:
+                task.finished.set()
+
+    def _announce_plan_fallback(self):
+        if self._plan_fallback_announced:
+            return
+        self._plan_fallback_announced = True
+        self._publish_status("MAKE_PLAN_TIMEOUT_FALLBACK_TO_MOVE_BASE")
+
+    def _plan_length(self, x, y, yaw):
+        """Return reachable, length, and whether a live plan was obtained.
+
+        The timer thread never calls the service directly.  A stalled RPC can
+        leave one daemon worker blocked, but this search round immediately
+        falls back and no additional make_plan calls are queued for its other
+        candidates.
+        """
+        if self._plan_timeout_this_round:
+            return True, float("inf"), False
+        # Do not probe the service from the timer callback.  In the field a
+        # registered service can still stall during a transport handshake.
+        # The only RPC is therefore made by _plan_worker below.
+        start = self._current_pose()
+        if start is None:
+            self._plan_timeout_this_round = True
+            self._announce_plan_fallback()
+            return True, float("inf"), False
+        request = GetPlanRequest()
+        request.start = self._pose_stamped(*start)
+        request.goal = self._pose_stamped(x, y, yaw)
+        request.tolerance = 0.0
+        task = _PlanTask(request)
+        try:
+            self._plan_request_queue.put_nowait(task)
+        except queue.Full:
+            self._plan_timeout_this_round = True
+            self._announce_plan_fallback()
+            return True, float("inf"), False
+        if not task.finished.wait(self._plan_response_timeout):
+            self._plan_timeout_this_round = True
+            self._announce_plan_fallback()
+            return True, float("inf"), False
+        if task.error is not None or task.response is None:
+            rospy.logwarn_throttle(2.0, "factory search make_plan failed: %s", task.error)
+            self._plan_timeout_this_round = True
+            self._announce_plan_fallback()
+            return True, float("inf"), False
+        poses = task.response.plan.poses
+        if len(poses) < 2:
+            return False, None, True
+        length = 0.0
+        for first, second in zip(poses, poses[1:]):
+            dx = second.pose.position.x - first.pose.position.x
+            dy = second.pose.position.y - first.pose.position.y
+            length += math.hypot(dx, dy)
+        return True, length, True
+
+    def _candidate_valid(self, candidate, with_plan):
+        if self._static_map is None:
+            return False, "STATIC_MAP_UNAVAILABLE", None
+        static_cost = self._footprint_cost(
+            self._static_map, candidate["x"], candidate["y"], candidate["yaw"],
+            self._static_occupied_threshold,
+        )
+        if static_cost is None:
+            return False, "STATIC_FOOTPRINT_BLOCKED", None
+        local_costmap_checked = (
+            self._fresh_local_costmap()
+            and self._footprint_is_inside_grid(
+                self._local_costmap, candidate["x"], candidate["y"], candidate["yaw"])
+        )
+        if local_costmap_checked:
+            local_cost = self._footprint_cost(
+                self._local_costmap, candidate["x"], candidate["y"], candidate["yaw"],
+                self._local_cost_threshold,
+            )
+            if local_cost is None:
+                return False, "LOCAL_FOOTPRINT_BLOCKED", None
+        else:
+            # A rolling local costmap may be absent or not cover a distant
+            # candidate.  That is not a cone collision: retain the static-map
+            # and global-plan checks, then let TEB use live obstacles in motion.
+            local_cost = self._local_cost_threshold - 1
+        plan_length = candidate.get("plan_length", float("inf"))
+        make_plan_checked = candidate.get("make_plan_checked", False)
+        if with_plan:
+            reachable, plan_length, make_plan_checked = self._plan_length(
+                candidate["x"], candidate["y"], candidate["yaw"]
+            )
+            if reachable is None:
+                return False, "MAP_BASE_TF_UNAVAILABLE", None
+            if not reachable:
+                return False, "GLOBAL_PLAN_UNREACHABLE", None
+        candidate["local_cost"] = local_cost
+        candidate["local_costmap_checked"] = local_costmap_checked
+        candidate["make_plan_checked"] = make_plan_checked
+        candidate["plan_length"] = plan_length
+        return True, None, candidate
+
+    def _generate_raw_candidates(self):
+        x, y, yaw = self._nominal_pose
+        generated = [{"x": x, "y": y, "yaw": yaw, "radius": 0.0}]
+        seen = {(round(x, 4), round(y, 4))}
+        for radius in self._ring_radii:
+            for index in range(self._ring_directions):
+                angle = 2.0 * math.pi * index / self._ring_directions
+                cx, cy = x + radius * math.cos(angle), y + radius * math.sin(angle)
+                key = round(cx, 4), round(cy, 4)
+                if key not in seen:
+                    seen.add(key)
+                    generated.append({"x": cx, "y": cy, "yaw": yaw, "radius": radius})
+        return generated
+
+    def _publish_candidates(self):
+        message = PoseArray()
+        message.header.frame_id = self._frame_id
+        message.header.stamp = rospy.Time.now()
+        for candidate in self._candidates:
+            pose = Pose()
+            pose.position.x, pose.position.y = candidate["x"], candidate["y"]
+            pose.orientation = self._quaternion_from_yaw(candidate["yaw"])
+            message.poses.append(pose)
+        self._candidate_pub.publish(message)
+
+    def _prepare_candidates(self):
+        if self._static_map is None:
+            self._publish_status("WAITING_FOR_STATIC_MAP")
+            return
+        if not self._fresh_local_costmap():
+            self._publish_status("LOCAL_COSTMAP_UNAVAILABLE_STATIC_PLAN_FALLBACK")
+        accepted = []
+        for candidate in self._generate_raw_candidates():
+            valid, _reason, value = self._candidate_valid(candidate, with_plan=True)
+            if valid:
+                accepted.append(value)
+        if not accepted:
+            self._state = "FAILED"
+            self._publish_status("NO_SAFE_SEARCH_CANDIDATES")
+            return
+        accepted.sort(key=lambda item: (item["radius"], item["local_cost"], item["plan_length"]))
+        self._candidates = accepted
+        self._candidate_index = 0
+        self._publish_candidates()
+        self._publish_status("CANDIDATES_READY count=%d" % len(self._candidates))
+        self._send_next_candidate("INITIAL")
+
+    def _send_next_candidate(self, reason):
+        while self._candidate_index < len(self._candidates):
+            candidate = self._candidates[self._candidate_index]
+            self._candidate_index += 1
+            valid, invalid_reason, candidate = self._candidate_valid(candidate, with_plan=True)
+            if not valid:
+                self._publish_status("CANDIDATE_SKIPPED reason=%s" % invalid_reason)
+                continue
+            if not self._client.wait_for_server(rospy.Duration(self._server_timeout)):
+                self._state = "FAILED"
+                self._publish_status("MOVE_BASE_UNAVAILABLE")
+                return
+            goal = MoveBaseGoal()
+            goal.target_pose = self._pose_stamped(candidate["x"], candidate["y"], candidate["yaw"])
+            self._active_sequence += 1
+            sequence = self._active_sequence
+            self._state = "NAVIGATING"
+            self._active_deadline = time.monotonic() + self._goal_timeout
+            self._selected_goal_pub.publish(goal.target_pose)
+            self._client.send_goal(
+                goal,
+                done_cb=lambda state, result, seq=sequence, item=candidate: self._goal_done(seq, item, state, result),
+            )
+            self._publish_status(
+                "CANDIDATE_SENT %d/%d reason=%s local_costmap=%s make_plan=%s x=%.3f y=%.3f yaw=%.3f" % (
+                    self._candidate_index, len(self._candidates), reason,
+                    "CHECKED" if candidate["local_costmap_checked"] else "FALLBACK",
+                    "CHECKED" if candidate["make_plan_checked"] else "FALLBACK",
+                    candidate["x"], candidate["y"], candidate["yaw"],
+                )
+            )
+            return
+        self._state = "FAILED"
+        self._active_deadline = None
+        self._publish_status("ALL_SEARCH_CANDIDATES_EXHAUSTED")
+
+    def _goal_done(self, sequence, candidate, state, _result):
+        if sequence != self._active_sequence or self._state != "NAVIGATING":
+            return
+        self._active_deadline = None
+        if state == GoalStatus.SUCCEEDED:
+            self._state = "WAITING_FOR_SEARCH"
+            self._search_deadline = time.monotonic() + self._search_timeout
+            self._publish_status(
+                "CANDIDATE_REACHED %d/%d; WAITING_FOR_FACTORY_SEARCH" % (
+                    self._candidate_index, len(self._candidates)
+                )
+            )
+            return
+        if state in self._NAV_FAILURE_STATES:
+            self._publish_status("CANDIDATE_NAV_FAILED state=%d; TRYING_NEXT" % state)
+        else:
+            self._publish_status("CANDIDATE_NAV_UNEXPECTED state=%d; TRYING_NEXT" % state)
+        self._send_next_candidate("NAVIGATION_FAILURE")
+
+    def _alignment_status_callback(self, message):
+        if self._state != "WAITING_FOR_SEARCH":
+            return
+        status = message.data
+        if status.startswith("FACTORY_ENTRY_COMPLETE"):
+            self._state = "COMPLETE"
+            self._completion_locked = True
+            self._search_deadline = None
+            self._publish_status("FACTORY_ENTRY_COMPLETE; REMAINING_CANDIDATES_CANCELLED_LOCKED")
+            return
+        if status.startswith("FACTORY_MISMATCH"):
+            self._state = "FAILED"
+            self._search_deadline = None
+            self._publish_status("OBSERVATION_MISMATCH")
+            return
+        entry_goal_failed = (
+            status.startswith("ENTRY_GOAL_")
+            and not status.startswith("ENTRY_GOAL_SENT")
+        )
+        if status.startswith(self._SEARCH_RETRY_PREFIXES) or entry_goal_failed:
+            self._search_deadline = None
+            self._publish_status("FACTORY_SEARCH_FAILED status=%s; TRYING_NEXT" % status)
+            self._send_next_candidate("SEARCH_FAILURE")
+
+    def _timer_callback(self, _event):
+        try:
+            self._timer_tick()
+        except Exception as error:
+            rospy.logerr("factory search timer error: %s", error)
+            self._publish_status("ADAPTER_TIMER_ERROR")
+
+    def _timer_tick(self):
+        if self._state == "PREPARING":
+            self._prepare_candidates()
+            return
+        active_deadline = self._active_deadline
+        if (self._state == "NAVIGATING"
+                and active_deadline is not None
+                and time.monotonic() > active_deadline):
+            self._client.cancel_goal()
+            self._active_sequence += 1
+            self._active_deadline = None
+            self._publish_status("CANDIDATE_NAV_TIMEOUT; TRYING_NEXT")
+            self._send_next_candidate("NAVIGATION_TIMEOUT")
+            return
+        search_deadline = self._search_deadline
+        if (self._state == "WAITING_FOR_SEARCH"
+                and search_deadline is not None
+                and time.monotonic() > search_deadline):
+            self._search_deadline = None
+            self._publish_status("FACTORY_SEARCH_TIMEOUT; TRYING_NEXT")
+            self._send_next_candidate("SEARCH_TIMEOUT")
+
+
+def main():
+    try:
+        FactorySearchPointAdapter()
+        rospy.spin()
+    except (ValueError, rospy.ROSException) as error:
+        rospy.logfatal("factory search adapter did not start: %s", error)
+        raise
+
+
+if __name__ == "__main__":
+    main()

sign_recognition/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(sign_recognition)
+find_package(catkin REQUIRED COMPONENTS
+  cv_bridge
+  rospy
+  sensor_msgs
+  std_msgs
+  std_srvs
+)
+catkin_package(CATKIN_DEPENDS cv_bridge rospy sensor_msgs std_msgs std_srvs)
+catkin_install_python(PROGRAMS
+  scripts/sign_recognition_node.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

sign_recognition/launch/sign_recognition.launch

@@ -0,0 +1,11 @@
+<launch>
+  <arg name="start_camera" default="false"/>
+  <arg name="enabled" default="false"/>
+  <arg name="debug_save" default="false"/>
+  <include if="$(arg start_camera)" file="$(find usb_cam)/launch/usb_cam-test.launch"/>
+  <node pkg="sign_recognition" type="sign_recognition_node.py"
+        name="sign_recognition_node" output="screen">
+    <param name="enabled" value="$(arg enabled)"/>
+    <param name="debug_save" value="$(arg debug_save)"/>
+  </node>
+</launch>

sign_recognition/package.xml

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>sign_recognition</name>
+  <version>0.1.0</version>
+  <description>Service-gated multi-frame OCR confirmation for factory sign ROIs.</description>
+  <maintainer email="ucar@example.com">ucar</maintainer>
+  <license>BSD-3-Clause</license>
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>cv_bridge</depend>
+  <depend>rospy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+</package>

sign_recognition/scripts/sign_recognition_node.py

@@ -0,0 +1,195 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""Service-gated full-frame OCR confirmation for a factory sign."""
+
+import os
+import subprocess
+
+import cv2
+import rospy
+from cv_bridge import CvBridge, CvBridgeError
+from sensor_msgs.msg import Image
+from std_msgs.msg import String
+from std_srvs.srv import SetBool, SetBoolResponse
+
+
+class SignRecognitionNode:
+    def __init__(self):
+        rospy.init_node("sign_recognition_node")
+        self._bridge = CvBridge()
+        self._enabled = bool(rospy.get_param("~enabled", False))
+        self._image_topic = rospy.get_param("~image_topic", "/usb_cam/image_raw")
+        self._flip_horizontal = bool(rospy.get_param("~flip_horizontal", True))
+        self._keywords = tuple(rospy.get_param("~keywords", ["食品", "日用品", "电子"]))
+        self._interval = float(rospy.get_param("~interval_seconds", 0.50))
+        self._max_frames = int(rospy.get_param("~max_frames", 10))
+        self._scale = float(rospy.get_param("~scale", 2.0))
+        self._language = rospy.get_param("~language", "chi_sim")
+        self._psm = int(rospy.get_param("~psm", 6))
+        self._tesseract_timeout = float(rospy.get_param("~tesseract_timeout_seconds", 4.0))
+        self._debug_save = bool(rospy.get_param("~debug_save", False))
+        self._debug_dir = rospy.get_param("~debug_dir", "/tmp/sign_debug")
+        if self._max_frames <= 0:
+            raise ValueError("max_frames must be positive")
+        if self._tesseract_timeout <= 0.0:
+            raise ValueError("tesseract_timeout_seconds must be positive")
+        if self._debug_save:
+            os.makedirs(self._debug_dir, exist_ok=True)
+
+        self._last_process_time = 0.0
+        self._frames_processed = 0
+        self._result_pub = rospy.Publisher("/sign_recognition", String, queue_size=1)
+        self._status_pub = rospy.Publisher(
+            "/sign_recognition/status", String, queue_size=1, latch=True
+        )
+        self._debug_pub = rospy.Publisher(
+            "/sign_recognition/debug_image", Image, queue_size=1
+        )
+        self._image_sub = rospy.Subscriber(
+            self._image_topic, Image, self._image_callback, queue_size=1, buff_size=2 ** 24
+        )
+        self._enable_service = rospy.Service(
+            "/sign_recognition/set_enabled", SetBool, self._set_enabled_callback
+        )
+        self._publish_status("OCR_WAITING_FOR_IMAGE" if self._enabled else "OCR_IDLE")
+        rospy.loginfo(
+            "sign OCR ready: enabled=%s image_topic=%s max_frames=%d",
+            self._enabled, self._image_topic, self._max_frames,
+        )
+
+    def _publish_status(self, status):
+        self._status_pub.publish(String(data=status))
+
+    def _reset_attempt(self):
+        self._last_process_time = 0.0
+        self._frames_processed = 0
+
+    def _set_enabled_callback(self, request):
+        self._enabled = bool(request.data)
+        self._reset_attempt()
+        self._publish_status("OCR_WAITING_FOR_IMAGE" if self._enabled else "OCR_IDLE")
+        state = "enabled" if self._enabled else "disabled"
+        rospy.loginfo("sign OCR %s", state)
+        return SetBoolResponse(success=True, message="sign OCR %s" % state)
+
+    @staticmethod
+    def _clean_text(raw_text):
+        return raw_text.replace(" ", "").replace("\n", "").replace("\u3000", "")
+
+    def _preprocess_variants(self, frame):
+        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+        enlarged = cv2.resize(
+            gray, None, fx=self._scale, fy=self._scale, interpolation=cv2.INTER_CUBIC
+        )
+        equalized = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8)).apply(enlarged)
+        _threshold, binary = cv2.threshold(
+            equalized, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU
+        )
+        return (("gray", enlarged), ("binary", binary))
+
+    def _run_tesseract(self, image):
+        success, encoded = cv2.imencode(".png", image)
+        if not success:
+            raise RuntimeError("cannot encode OCR image")
+        try:
+            completed = subprocess.run(
+                ["tesseract", "stdin", "stdout", "-l", self._language,
+                 "--psm", str(self._psm)],
+                input=encoded.tobytes(), stdout=subprocess.PIPE, stderr=subprocess.PIPE,
+                timeout=self._tesseract_timeout, check=False,
+            )
+        except (OSError, subprocess.TimeoutExpired) as error:
+            raise RuntimeError("tesseract execution failed: %s" % error)
+        if completed.returncode != 0:
+            raise RuntimeError(completed.stderr.decode("utf-8", errors="replace").strip())
+        return completed.stdout.decode("utf-8", errors="replace")
+
+    def _match_category(self, cleaned):
+        # OCR often splits Chinese characters or inserts unrelated characters.
+        # A single full-frame OCR result is sufficient if it contains every character of one
+        # factory category, regardless of order or intervening characters.
+        for category in self._keywords:
+            if all(character in cleaned for character in category):
+                return category
+        # The electronics factory sign may show "生产" rather than "电子".
+        if "电子" in self._keywords and all(character in cleaned for character in "生产"):
+            return "电子"
+        return None
+
+    def _extract_keyword(self, frame):
+        last_variant = None
+        for variant_name, variant in self._preprocess_variants(frame):
+            last_variant = variant
+            raw_text = self._run_tesseract(variant)
+            cleaned = self._clean_text(raw_text)
+            rospy.loginfo("[OCR %s] raw=%r cleaned=%r", variant_name, raw_text, cleaned)
+            category = self._match_category(cleaned)
+            if category is not None:
+                return category, last_variant
+        return None, last_variant
+
+    def _publish_debug(self, image):
+        if image is None:
+            return
+        try:
+            self._debug_pub.publish(self._bridge.cv2_to_imgmsg(image, encoding="mono8"))
+        except CvBridgeError as error:
+            rospy.logerr_throttle(5.0, "OCR debug image conversion failed: %s", error)
+        if self._debug_save:
+            cv2.imwrite(
+                os.path.join(self._debug_dir, "ocr_%03d.jpg" % self._frames_processed), image
+            )
+
+    def _finish(self, result, status):
+        self._enabled = False
+        self._result_pub.publish(String(data=result))
+        self._publish_status(status)
+
+    def _image_callback(self, message):
+        if not self._enabled:
+            return
+        now = rospy.Time.now().to_sec()
+        if now - self._last_process_time < self._interval:
+            return
+        self._last_process_time = now
+        try:
+            frame = self._bridge.imgmsg_to_cv2(message, desired_encoding="bgr8")
+        except CvBridgeError as error:
+            rospy.logerr_throttle(5.0, "OCR image conversion failed: %s", error)
+            return
+        if self._flip_horizontal:
+            frame = cv2.flip(frame, 1)
+        if frame.size == 0 or frame.shape[0] < 8 or frame.shape[1] < 8:
+            rospy.logwarn_throttle(2.0, "OCR ignored an empty or too-small image")
+            return
+
+        self._frames_processed += 1
+        try:
+            keyword, debug_image = self._extract_keyword(frame)
+        except (RuntimeError, OSError) as error:
+            rospy.logerr_throttle(5.0, "OCR call failed: %s", error)
+            keyword, debug_image = None, None
+        self._publish_debug(debug_image)
+
+        if keyword is not None:
+            rospy.loginfo("factory OCR confirmed from one full image frame: %s", keyword)
+            self._finish(keyword, "OCR_CONFIRMED type=%s" % keyword)
+            return
+        else:
+            self._publish_status(
+                "OCR_READING frame=%d/%d no_keyword" % (
+                    self._frames_processed, self._max_frames
+                )
+            )
+
+        if self._frames_processed >= self._max_frames:
+            rospy.logwarn("factory OCR failed after %d ROI frames", self._frames_processed)
+            self._finish("UNKNOWN", "OCR_FAILED no_consensus")
+
+
+if __name__ == "__main__":
+    try:
+        SignRecognitionNode()
+        rospy.spin()
+    except (rospy.ROSInterruptException, ValueError):
+        pass

task2/config/task2_observation_points.yaml

@@ -0,0 +1,6 @@
+# Manually recorded map-frame observations.  Edit only after field recording.
+frame_id: map
+observation_points:
+  - {x: -1.669, y: -2.222, yaw: 1.553}
+  - {x: 0.350, y: -2.898, yaw: 1.646}
+  - {x: 1.553, y: -2.246, yaw: -1.564}

task2/scripts/competition_task2_executor.py

@@ -0,0 +1,207 @@
+#!/usr/bin/env python3
+"""Execute one task-2 order through the configured factory observation route."""
+from __future__ import annotations
+
+import json
+import math
+import threading
+from pathlib import Path
+
+import rospy
+import rospkg
+from geometry_msgs.msg import PoseStamped, Quaternion
+from std_msgs.msg import String
+from std_srvs.srv import Trigger
+
+
+class Task2Executor:
+    _NEXT_POINT_PREFIXES = ('ALL_SEARCH_CANDIDATES_EXHAUSTED', 'NO_SAFE_SEARCH_CANDIDATES',
+                            'MOVE_BASE_UNAVAILABLE', 'OBSERVATION_MISMATCH')
+
+    def __init__(self):
+        rospy.init_node('competition_task2_executor')
+        self._lock = threading.RLock()
+        self._points = self._load_points()
+        self._current_order = None
+        self._active = False
+        self._pending_start_order_id = None
+        self._index = 0
+        self._last_alignment_status = ''
+        self._status_pub = rospy.Publisher('/competition_task2/status', String, queue_size=10, latch=True)
+        self._result_pub = rospy.Publisher('/competition_task2/order_result', String, queue_size=10, latch=True)
+        self._nominal_pub = rospy.Publisher('/factory_search_adapter/nominal_goal', PoseStamped, queue_size=1)
+        self._reset = rospy.ServiceProxy('/factory_search_adapter/reset', Trigger)
+        rospy.Subscriber('/competition_task2/current_order', String, self._order_cb, queue_size=1)
+        rospy.Subscriber('/competition_task2/command', String, self._command_cb, queue_size=10)
+        rospy.Subscriber('/factory_search_adapter/status', String, self._adapter_cb, queue_size=10)
+        rospy.Subscriber('/factory_alignment_demo/status', String, self._alignment_cb, queue_size=10)
+        self._publish('IDLE')
+
+    @staticmethod
+    def _load_points():
+        path = Path(rospkg.RosPack().get_path('ucar_nav')) / 'config' / 'competition' / 'task2_observation_points.yaml'
+        try:
+            import yaml
+            with path.open(encoding='utf-8') as stream:
+                config = yaml.safe_load(stream)
+        except (OSError, ImportError) as error:
+            raise rospy.ROSException('任务二观察点配置不可读：%s' % error)
+        if not isinstance(config, dict) or config.get('frame_id') != 'map':
+            raise rospy.ROSException('任务二观察点必须使用map坐标系')
+        points = config.get('observation_points')
+        if not isinstance(points, list) or not points:
+            raise rospy.ROSException('任务二观察点不能为空')
+        out = []
+        for item in points:
+            values = tuple(float(item[key]) for key in ('x', 'y', 'yaw'))
+            if not all(math.isfinite(value) for value in values):
+                raise rospy.ROSException('任务二观察点包含非有限数值')
+            out.append(values)
+        return out
+
+    def _publish(self, detail):
+        rospy.loginfo('task2 executor: %s', detail)
+        self._status_pub.publish(String(data=detail))
+
+    def _order_cb(self, message):
+        try:
+            order = json.loads(message.data)
+            if not all(str(order[key]).strip() for key in ('order_id', 'product', 'category', 'warehouse')):
+                raise ValueError('订单字段不完整')
+        except (ValueError, TypeError, KeyError, json.JSONDecodeError) as error:
+            self._publish('ORDER_INVALID %s' % error)
+            return
+        with self._lock:
+            self._current_order = order
+            self._active = False
+            self._publish('ORDER_READY order_id=%s' % order['order_id'])
+            if self._pending_start_order_id == order['order_id']:
+                self._pending_start_order_id = None
+                self._start_order_locked(order['order_id'])
+
+    def _start_order_locked(self, order_id):
+        if self._active and self._current_order.get('order_id') == order_id:
+            self._publish('START_DUPLICATE_IGNORED order_id=%s' % order_id)
+            return
+        self._active = True
+        self._index = 0
+        self._last_alignment_status = ''
+        self._send_current_point('START_ORDER')
+
+
+    def _command_cb(self, message):
+        command = message.data.strip()
+        with self._lock:
+            if command == 'STOP':
+                was_active = self._active
+                self._pending_start_order_id = None
+                self._active = False
+                if was_active:
+                    self._reset_adapter(required=False)
+                self._publish('STOPPED')
+                return
+            if not command.startswith('START_ORDER order_id='):
+                return
+            order_id = command.split('=', 1)[1].strip()
+            if not order_id:
+                return
+            if self._current_order is None or self._current_order.get('order_id') != order_id:
+                self._pending_start_order_id = order_id
+                self._publish('START_WAITING_FOR_ORDER order_id=%s' % order_id)
+                return
+            self._pending_start_order_id = None
+            self._start_order_locked(order_id)
+
+    def _reset_adapter(self, required=True):
+        try:
+            rospy.wait_for_service('/factory_search_adapter/reset', timeout=1.5)
+            response = self._reset()
+            if not response.success:
+                raise rospy.ServiceException(response.message)
+            return True
+        except (rospy.ROSException, rospy.ServiceException) as error:
+            if required:
+                self._fail('ADAPTER_RESET_FAILED %s' % error)
+            else:
+                rospy.logwarn('task2 executor could not reset adapter while stopping: %s', error)
+            return False
+
+    def _send_current_point(self, reason):
+        if not self._active:
+            return
+        if self._index >= len(self._points):
+            self._fail('SEARCH_ROUTE_EXHAUSTED')
+            return
+        if not self._reset_adapter():
+            return
+        x, y, yaw = self._points[self._index]
+        pose = PoseStamped()
+        pose.header.frame_id = 'map'
+        pose.header.stamp = rospy.Time.now()
+        pose.pose.position.x, pose.pose.position.y = x, y
+        pose.pose.orientation = Quaternion(z=math.sin(yaw / 2.0), w=math.cos(yaw / 2.0))
+        self._nominal_pub.publish(pose)
+        self._publish('OBSERVATION_SENT %d/%d reason=%s x=%.3f y=%.3f yaw=%.3f' % (
+            self._index + 1, len(self._points), reason, x, y, yaw))
+
+    def _next_point(self, reason):
+        if not self._active:
+            return
+        self._index += 1
+        self._send_current_point(reason)
+
+    def _adapter_cb(self, message):
+        status = message.data.strip()
+        with self._lock:
+            if not self._active:
+                return
+            if status.startswith(self._NEXT_POINT_PREFIXES):
+                self._next_point(status.split()[0])
+
+    def _alignment_cb(self, message):
+        status = message.data.strip()
+        with self._lock:
+            if not self._active or status == self._last_alignment_status:
+                return
+            self._last_alignment_status = status
+            if status.startswith('FACTORY_ENTRY_COMPLETE'):
+                self._complete_order()
+                return
+            # These are recoverable search-position failures.  The adapter
+            # retries another candidate and only route exhaustion is terminal.
+            entry_goal_failed = (
+                status.startswith('ENTRY_GOAL_')
+                and not status.startswith('ENTRY_GOAL_SENT')
+            )
+            if status.startswith('FACTORY_ENTRY_FAILED') or entry_goal_failed:
+                self._publish('ENTRY_RETRY_PENDING %s' % status)
+
+
+    def _complete_order(self):
+        if not self._active or self._current_order is None:
+            return
+        self._active = False
+        result = dict(self._current_order)
+        result['status'] = 'ORDER_PARKED'
+        self._result_pub.publish(String(
+            data=json.dumps(result, ensure_ascii=False, sort_keys=True)))
+        self._publish('ORDER_PARKED order_id=%s' % result['order_id'])
+
+    def _fail(self, reason):
+        if not self._active:
+            return
+        self._active = False
+        result = dict(self._current_order or {})
+        result.update({'status': 'FAILED', 'reason': reason})
+        self._result_pub.publish(String(
+            data=json.dumps(result, ensure_ascii=False, sort_keys=True)))
+        self._publish('FAILED %s' % reason)
+
+
+def main():
+    Task2Executor()
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    main()