Task2/factory_alignment_demo/scripts/factory_alignment_controller.py

#!/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()