Task3/line_follower/scripts/line_follow_debug.py

#!/usr/bin/env python3
"""Visualise white-line candidates without ever commanding the vehicle."""

import math
import threading

import cv2
import numpy as np
import rospy
from cv_bridge import CvBridge, CvBridgeError
from geometry_msgs.msg import PointStamped
from sensor_msgs.msg import Image
from std_msgs.msg import Bool, Float32, String


class LineFollowDebug:
    """Subscriber-only line visualiser used to tune the real route safely."""

    def __init__(self):
        rospy.init_node("line_follow_debug")
        self.image_topic = rospy.get_param("~image_topic", "/usb_cam/image_raw")
        self.lower = self._read_hsv("~hsv_lower", [0, 0, 180])
        self.upper = self._read_hsv("~hsv_upper", [180, 60, 255])
        self.use_perspective_transform = bool(
            rospy.get_param("~use_perspective_transform", True)
        )
        self.perspective_reference_size = self._read_size(
            "~perspective_reference_size", [640, 480]
        )
        self.src_points = self._read_points(
            "~src_pts", [[120, 205], [520, 205], [639, 479], [0, 479]]
        )
        self.dst_points = self._read_points(
            "~dst_pts", [[100, 200], [540, 200], [540, 479], [100, 479]]
        )
        self.processing_scale = self._read_ratio("~processing_scale", 0.5)
        self.processing_scale = max(0.25, self.processing_scale)
        self.display_rate = max(1.0, float(rospy.get_param("~display_rate", 10.0)))
        self.use_camera_front_offset = self._read_bool("~use_camera_front_offset", True)
        self.camera_to_front_offset_m = max(
            0.0, float(rospy.get_param("~camera_to_front_offset_m", 0.125))
        )
        self.metric_scale_px_per_m = max(
            1.0, float(rospy.get_param("~metric_scale_px_per_m", 400.0))
        )
        self.metric_origin_px = self._read_metric_origin(
            "~metric_origin_px", [320.0, 480.0]
        )
        self.lookahead_distance_m = max(
            0.05, float(rospy.get_param("~lookahead_distance_m", 0.50))
        )
        self.normal_lookahead_distance_m = max(
            0.05,
            float(
                rospy.get_param(
                    "~normal_lookahead_distance_m", self.lookahead_distance_m
                )
            ),
        )
        self.after_second_lookahead_distance_m = max(
            0.05,
            float(
                rospy.get_param(
                    "~after_second_lookahead_distance_m",
                    self.normal_lookahead_distance_m,
                )
            ),
        )
        self.after_second_inner_offset_m = max(
            0.0, float(rospy.get_param("~after_second_inner_offset_m", 0.025))
        )
        self.inner_offset_activation_m = max(
            0.0, float(rospy.get_param("~inner_offset_activation_m", 0.015))
        )
        self.task_state_topic = str(
            rospy.get_param("~task_state_topic", "/traffic_line_task/state")
        )
        self.ipm_origin_ahead_of_control_m = max(
            0.0, float(rospy.get_param("~ipm_origin_ahead_of_control_m", 0.125))
        )
        self.max_near_target_extrapolation_m = max(
            0.0,
            float(rospy.get_param("~max_near_target_extrapolation_m", 0.25)),
        )
        self.lookahead_frame_id = str(
            rospy.get_param("~lookahead_frame_id", "base_link")
        )
        self.camera_front_offset_px = max(
            0, int(rospy.get_param("~camera_front_offset_px", 0))
        )
        self.roi_top_ratio = self._read_ratio("~roi_top_ratio", 0.52)
        self.scanline_ratios = self._read_ratios(
            "~scanline_ratios", [0.70, 0.66, 0.62, 0.58, 0.54, 0.50, 0.46]
        )
        self.scanline_half_height = max(1, int(rospy.get_param("~scanline_half_height", 5)))
        self.min_run_width = max(1, int(rospy.get_param("~min_run_width", 3)))
        self.min_component_area = max(1, int(rospy.get_param("~min_component_area", 30)))
        self.use_component_filter = self._read_bool("~use_component_filter", False)
        self.center_reflection_half_width_ratio = self._read_ratio(
            "~center_reflection_half_width_ratio", 0.12
        )
        self.min_lane_width_ratio = self._read_ratio("~min_lane_width_ratio", 0.25)
        self.max_lane_width_ratio = self._read_ratio("~max_lane_width_ratio", 0.98)
        self.use_single_boundary_fallback = self._read_bool(
            "~use_single_boundary_fallback", True
        )
        self.single_boundary_timeout = max(
            0.1, float(rospy.get_param("~single_boundary_timeout", 1.5))
        )
        self.single_boundary_side_memory_timeout = max(
            0.1,
            float(rospy.get_param("~single_boundary_side_memory_timeout", 0.75)),
        )
        self.lane_width_alpha = min(
            1.0, max(0.01, float(rospy.get_param("~lane_width_alpha", 0.2)))
        )
        self.max_centerline_heading_rad = max(
            0.05, float(rospy.get_param("~max_centerline_heading_rad", 0.70))
        )
        kernel_size = max(1, int(rospy.get_param("~morphology_kernel", 3)))
        if kernel_size % 2 == 0:
            kernel_size += 1
        self.kernel = np.ones((kernel_size, kernel_size), dtype=np.uint8)

        self.bridge = CvBridge()
        self.lock = threading.Lock()
        self.display_image = None
        self.filtered_lane_width_px = None
        self.last_two_boundary_time = None
        self.single_boundary_side = None
        self.last_single_boundary_time = None
        self.after_second_active = False
        self.lane_error_pub = rospy.Publisher("~lane_error", Float32, queue_size=1)
        self.lane_heading_pub = rospy.Publisher("~lane_heading_error", Float32, queue_size=1)
        self.lane_valid_pub = rospy.Publisher("~lane_valid", Bool, queue_size=1)
        self.lookahead_target_pub = rospy.Publisher(
            "~lookahead_target", PointStamped, queue_size=1
        )
        self.subscriber = rospy.Subscriber(
            self.image_topic, Image, self.image_callback, queue_size=1
        )
        self.task_state_subscriber = rospy.Subscriber(
            self.task_state_topic, String, self.task_state_callback, queue_size=1
        )
        rospy.loginfo(
            "Line-follow debug is visualisation only; IPM=%s, subscribed to %s and will not publish /cmd_vel.",
            self.use_perspective_transform,
            self.image_topic,
        )

    def task_state_callback(self, message):
        after_second = message.data == "FOLLOW_AFTER_SECOND"
        requested = (
            self.after_second_lookahead_distance_m
            if after_second
            else self.normal_lookahead_distance_m
        )
        with self.lock:
            previous = self.lookahead_distance_m
            self.lookahead_distance_m = requested
            self.after_second_active = after_second
        if abs(previous - requested) > 1e-6:
            rospy.loginfo(
                "Line-follow lookahead changed to %.2f m for task state %s.",
                requested,
                message.data,
            )

    def apply_after_second_inner_offset(
        self, target_left, target_x, metric_scale
    ):
        """Move a curved-route target toward its inside only after turn two."""
        if (
            not self.after_second_active
            or abs(target_left) < self.inner_offset_activation_m
            or self.after_second_inner_offset_m <= 0.0
        ):
            return target_left, target_x
        direction = 1.0 if target_left > 0.0 else -1.0
        offset = direction * self.after_second_inner_offset_m
        # base_link +left maps to decreasing IPM image x.
        return target_left + offset, target_x - offset * metric_scale

    @staticmethod
    def _read_ratio(name, default):
        return float(max(0.0, min(1.0, rospy.get_param(name, default))))

    @staticmethod
    def _read_ratios(name, default):
        values = rospy.get_param(name, default)
        if not isinstance(values, (list, tuple)) or not values:
            rospy.logwarn("%s must be a non-empty list; using %s", name, default)
            values = default
        return sorted(
            [float(max(0.0, min(1.0, value))) for value in values], reverse=True
        )

    @staticmethod
    def _read_hsv(name, default):
        values = rospy.get_param(name, default)
        if not isinstance(values, (list, tuple)) or len(values) != 3:
            rospy.logwarn("%s must have three values; using %s", name, default)
            values = default
        values = [int(max(0, min(255, value))) for value in values]
        values[0] = min(180, values[0])
        return np.array(values, dtype=np.uint8)

    @staticmethod
    def _read_size(name, default):
        values = rospy.get_param(name, default)
        if not isinstance(values, (list, tuple)) or len(values) != 2:
            rospy.logwarn("%s must contain [width, height]; using %s", name, default)
            values = default
        return max(1, int(values[0])), max(1, int(values[1]))

    @staticmethod
    def _read_metric_origin(name, default):
        values = rospy.get_param(name, default)
        if not isinstance(values, (list, tuple)) or len(values) != 2:
            rospy.logwarn("%s must contain [x, y]; using %s", name, default)
            values = default
        return float(values[0]), float(values[1])

    @staticmethod
    def _read_bool(name, default):
        value = rospy.get_param(name, default)
        if isinstance(value, str):
            return value.strip().lower() in ("1", "true", "yes", "on")
        return bool(value)

    @staticmethod
    def _read_points(name, default):
        values = rospy.get_param(name, default)
        valid = isinstance(values, (list, tuple)) and len(values) == 4
        if valid:
            valid = all(isinstance(point, (list, tuple)) and len(point) == 2 for point in values)
        if not valid:
            rospy.logwarn("%s must contain four [x, y] points; using defaults", name)
            values = default
        return np.array(values, dtype=np.float32)

    def image_callback(self, message):
        try:
            image = self.bridge.imgmsg_to_cv2(message, desired_encoding="bgr8")
        except CvBridgeError as error:
            rospy.logwarn_throttle(2.0, "Cannot convert camera image: %s", error)
            return
        # Perception and lane publication run here at camera speed.  OpenCV GUI
        # display is deliberately kept out of this callback.
        display_image = self.make_display(image)
        with self.lock:
            self.display_image = display_image

    @staticmethod
    def white_runs(scanline, min_width):
        """Return contiguous white x-ranges from a horizontal mask scanline."""
        active = scanline > 0
        padded = np.pad(active.astype(np.int8), (1, 1), mode="constant")
        changes = np.flatnonzero(np.diff(padded))
        runs = []
        for start, end in zip(changes[0::2], changes[1::2]):
            if end - start >= min_width:
                runs.append((int(start), int(end - 1)))
        return runs

    def scan_data_at(self, mask, roi_top, scan_y):
        """Return white runs split around the vehicle centre for one band."""
        height, width = mask.shape
        centre_x = width // 2
        exclusion_half_width = int(width * self.center_reflection_half_width_ratio)
        exclusion_left = centre_x - exclusion_half_width
        exclusion_right = centre_x + exclusion_half_width

        scan_y = min(height - 1, max(roi_top, int(scan_y)))
        band_top = max(roi_top, scan_y - self.scanline_half_height)
        band_bottom = min(height, scan_y + self.scanline_half_height + 1)
        scanline = np.max(mask[band_top:band_bottom, :], axis=0)
        runs = self.white_runs(scanline, self.min_run_width)

        left_runs = [run for run in runs if run[1] < exclusion_left]
        right_runs = [run for run in runs if run[0] > exclusion_right]
        return (
            scan_y, band_top, band_bottom, runs, left_runs, right_runs,
            exclusion_left, exclusion_right,
        )

    def lane_pair_at(self, mask, roi_top, scan_y):
        """Find one valid left/right boundary pair in a given horizontal band."""
        width = mask.shape[1]
        min_lane_width = int(width * self.min_lane_width_ratio)
        max_lane_width = int(width * self.max_lane_width_ratio)
        data = self.scan_data_at(mask, roi_top, scan_y)
        (
            scan_y, band_top, band_bottom, runs, left_runs, right_runs,
            exclusion_left, exclusion_right,
        ) = data

        # A central highlight may be white in HSV, but it cannot become a
        # lane boundary.  Keep only runs completely outside its band.
        if not left_runs or not right_runs:
            return None
        left = max(left_runs, key=lambda run: run[1])
        right = min(right_runs, key=lambda run: run[0])
        left_x = (left[0] + left[1]) // 2
        right_x = (right[0] + right[1]) // 2
        lane_width = right_x - left_x
        if min_lane_width <= lane_width <= max_lane_width:
            return scan_y, band_top, band_bottom, runs, left, right, exclusion_left, exclusion_right

        return None

    def single_boundary_at(self, mask, roi_top, scan_y):
        """Infer the lane midpoint from one boundary and recent measured width."""
        if self.filtered_lane_width_px is None or self.last_two_boundary_time is None:
            return None
        if rospy.get_time() - self.last_two_boundary_time > self.single_boundary_timeout:
            return None

        width = mask.shape[1]
        data = self.scan_data_at(mask, roi_top, scan_y)
        (
            scan_y, band_top, band_bottom, runs, left_runs, right_runs,
            _exclusion_left, _exclusion_right,
        ) = data

        lane_width = float(self.filtered_lane_width_px)
        now = rospy.get_time()
        remembered_side = None
        if (
            self.single_boundary_side in ("LEFT", "RIGHT")
            and self.last_single_boundary_time is not None
            and now - self.last_single_boundary_time
            <= self.single_boundary_side_memory_timeout
        ):
            remembered_side = self.single_boundary_side

        if remembered_side is not None:
            # During a bend the same physical outer boundary can cross the
            # image centre. Do not relabel it merely because x changed sides.
            if not runs:
                return None
            if left_runs and right_runs:
                # Two distinct sides that fail the lane-width check are still
                # ambiguous; side memory must not turn them into one boundary.
                return None
            if remembered_side == "LEFT":
                boundary = min(runs, key=lambda run: run[0] + run[1])
                visible_side = "LEFT"
            else:
                boundary = max(runs, key=lambda run: run[0] + run[1])
                visible_side = "RIGHT"
        else:
            # At fallback entry exactly one image side must be visible. This
            # establishes the physical side identity used by later frames.
            if bool(left_runs) == bool(right_runs):
                return None
            if left_runs:
                boundary = max(left_runs, key=lambda run: run[1])
                visible_side = "LEFT"
            else:
                boundary = min(right_runs, key=lambda run: run[0])
                visible_side = "RIGHT"

        boundary_x = (boundary[0] + boundary[1]) // 2
        if visible_side == "LEFT":
            target_x = int(round(boundary_x + 0.5 * lane_width))
        else:
            target_x = int(round(boundary_x - 0.5 * lane_width))

        if not (0 <= target_x < width):
            return None
        return scan_y, band_top, band_bottom, runs, boundary, boundary_x, target_x, visible_side

    def select_lane_pair(self, mask, roi_top):
        """Find the nearest valid left/right pair, rejecting LED reflections."""
        height, _ = mask.shape
        for ratio in self.scanline_ratios:
            selection = self.lane_pair_at(mask, roi_top, int(height * ratio))
            if selection is not None:
                return selection
        return None

    def select_single_boundary(self, mask, roi_top, front_offset_px):
        """Prefer the compensated near band, then retry the ordinary bands."""
        height, _ = mask.shape
        offsets = [front_offset_px]
        if front_offset_px != 0:
            offsets.append(0)
        for offset in offsets:
            for ratio in self.scanline_ratios:
                selection = self.single_boundary_at(
                    mask, roi_top, int(height * ratio) + offset
                )
                if selection is not None:
                    return selection
        return None

    def front_offset_for_width(self, width):
        """Convert the configured physical nose margin to processing pixels."""
        if not self.use_camera_front_offset:
            return 0
        reference_width = float(self.perspective_reference_size[0])
        if self.camera_front_offset_px > 0:
            reference_offset_px = self.camera_front_offset_px
        else:
            reference_offset_px = self.camera_to_front_offset_m * self.metric_scale_px_per_m
        return int(round(reference_offset_px * width / reference_width))

    def centerline_points(self, mask, roi_top, front_offset_px):
        """Collect lane-centre samples from several near/far ground bands."""
        height, _ = mask.shape
        points_by_y = {}
        offsets = [front_offset_px]
        if front_offset_px != 0:
            offsets.append(0)

        for offset in offsets:
            for ratio in self.scanline_ratios:
                requested_y = int(height * ratio) + offset
                pair = self.lane_pair_at(mask, roi_top, requested_y)
                if pair is not None:
                    scan_y, _top, _bottom, _runs, left, right, _el, _er = pair
                    left_x = (left[0] + left[1]) // 2
                    right_x = (right[0] + right[1]) // 2
                    points_by_y[scan_y] = (left_x + right_x) // 2
                    continue

                if self.use_single_boundary_fallback:
                    single = self.single_boundary_at(mask, roi_top, requested_y)
                    if single is not None:
                        scan_y = single[0]
                        points_by_y[scan_y] = single[6]

            # Compensated samples are preferred.  Use ordinary bands only if
            # the near set alone cannot describe a curve.
            if len(points_by_y) >= 3:
                break

        return sorted(
            [(int(x), int(y)) for y, x in points_by_y.items()],
            key=lambda point: point[1],
        )

    def fit_centerline(self, points):
        """Fit x(y), returning drawable curve, heading, and polynomial."""
        if len(points) < 2:
            return points, 0.0, None

        xs = np.array([point[0] for point in points], dtype=np.float64)
        ys = np.array([point[1] for point in points], dtype=np.float64)
        degree = 2 if len(points) >= 3 else 1
        coefficients = np.polyfit(ys, xs, degree)
        sample_ys = np.linspace(float(np.min(ys)), float(np.max(ys)), 30)
        sample_xs = np.polyval(coefficients, sample_ys)
        curve = [
            (int(round(x)), int(round(y)))
            for x, y in zip(sample_xs, sample_ys)
            if np.isfinite(x) and np.isfinite(y)
        ]

        heading_y = 0.5 * (float(np.min(ys)) + float(np.max(ys)))
        derivative = np.polyval(np.polyder(coefficients), heading_y)
        # Image x grows to physical right, while forward grows toward smaller
        # image y.  Positive heading therefore means a right-hand curve.
        heading = float(np.arctan(-derivative))
        heading = max(
            -self.max_centerline_heading_rad,
            min(self.max_centerline_heading_rad, heading),
        )
        return curve, heading, coefficients

    def lookahead_target_from_fit(self, coefficients, center_samples, frame_shape):
        """Convert a fitted IPM centreline into a metric base-frame target."""
        if coefficients is None or len(center_samples) < 2:
            return None

        height, width = frame_shape[:2]
        reference_width, reference_height = self.perspective_reference_size
        scale_x = width / float(reference_width)
        scale_y = height / float(reference_height)
        # The metric IPM calibration uses the same scale in both directions.
        metric_scale = self.metric_scale_px_per_m * scale_x
        if metric_scale <= 0.0:
            return None

        origin_x = self.metric_origin_px[0] * scale_x
        origin_y = self.metric_origin_px[1] * scale_y
        forward_from_ipm_origin = max(
            0.0, self.lookahead_distance_m - self.ipm_origin_ahead_of_control_m
        )
        requested_y = origin_y - forward_from_ipm_origin * metric_scale

        sample_ys = [float(point[1]) for point in center_samples]
        far_y = min(sample_ys)
        near_y = max(sample_ys)
        max_near_y = min(
            float(height - 1),
            near_y + self.max_near_target_extrapolation_m * metric_scale,
        )
        target_y = min(max(requested_y, far_y), max_near_y)
        if target_y > near_y:
            # Continue only the nearest fitted tangent toward the vehicle.
            # Direct quadratic extrapolation can grow rapidly and select a
            # false branch at a junction.
            near_x = float(np.polyval(coefficients, near_y))
            near_slope = float(np.polyval(np.polyder(coefficients), near_y))
            max_slope = math.tan(self.max_centerline_heading_rad)
            near_slope = max(-max_slope, min(max_slope, near_slope))
            target_x = near_x + near_slope * (target_y - near_y)
        else:
            target_x = float(np.polyval(coefficients, target_y))
        target_forward = (
            self.ipm_origin_ahead_of_control_m
            + (origin_y - target_y) / metric_scale
        )
        target_left = -(target_x - origin_x) / metric_scale
        target_left, target_x = self.apply_after_second_inner_offset(
            target_left, target_x, metric_scale
        )

        values = (target_forward, target_left, target_x, target_y)
        if not all(np.isfinite(value) for value in values) or target_forward <= 0.0:
            return None
        return values

    def remove_small_components(self, mask):
        """Keep only connected white regions that can plausibly be route lines."""
        count, labels, stats, _ = cv2.connectedComponentsWithStats(mask, connectivity=8)
        # Vectorised lookup: the old per-component loop compared every label
        # against the full image and reduced the real-time rate to ~2 Hz.
        keep = stats[:, cv2.CC_STAT_AREA] >= self.min_component_area
        keep[0] = False  # Label 0 is the black background.
        return (keep[labels].astype(np.uint8) * 255)

    def scaled_perspective_points(self, frame_shape):
        """Return source and destination IPM points for the current resolution."""
        height, width = frame_shape[:2]
        reference_width, reference_height = self.perspective_reference_size
        scale = np.array([width / float(reference_width), height / float(reference_height)], dtype=np.float32)
        src = self.src_points * scale
        dst = self.dst_points * scale
        return src, dst

    def perspective_warp(self, frame):
        """Warp the configured ground trapezoid to a bird's-eye rectangle."""
        height, width = frame.shape[:2]
        src, dst = self.scaled_perspective_points(frame.shape)
        matrix = cv2.getPerspectiveTransform(src, dst)
        bird = cv2.warpPerspective(frame, matrix, (width, height), flags=cv2.INTER_LINEAR)
        return bird, src.astype(np.int32)

    def make_line_views(self, frame):
        height, width = frame.shape[:2]
        roi_top = int(height * self.roi_top_ratio)
        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
        mask = cv2.inRange(hsv, self.lower, self.upper)
        mask[:roi_top, :] = 0
        mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, self.kernel)
        mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, self.kernel)
        if self.use_component_filter:
            mask = self.remove_small_components(mask)

        centre_x = width // 2
        exclusion_half_width = int(width * self.center_reflection_half_width_ratio)
        exclusion_left = centre_x - exclusion_half_width
        exclusion_right = centre_x + exclusion_half_width
        # LED glare is not a route marking.  It is removed from the binary
        # image, rather than merely being rejected after candidate selection.
        mask[roi_top:, exclusion_left:exclusion_right + 1] = 0
        selection = self.select_lane_pair(mask, roi_top)
        front_offset_px = self.front_offset_for_width(width)
        if selection is not None and self.use_camera_front_offset:
            initial_scan_y = selection[0]
            # Larger bird-view y is closer to the vehicle.  Move the control
            # band toward the nose by the requested physical distance.
            compensated = self.lane_pair_at(mask, roi_top, initial_scan_y + front_offset_px)
            if compensated is not None:
                selection = compensated

        single_selection = None
        if selection is None and self.use_single_boundary_fallback:
            single_selection = self.select_single_boundary(mask, roi_top, front_offset_px)

        annotated = frame.copy()
        cv2.rectangle(annotated, (0, roi_top), (width - 1, height - 1), (0, 255, 255), 2)
        cv2.line(annotated, (centre_x, roi_top), (centre_x, height - 1), (120, 120, 120), 1)
        overlay = annotated.copy()
        cv2.rectangle(overlay, (exclusion_left, roi_top), (exclusion_right, height - 1), (0, 0, 0), -1)
        annotated = cv2.addWeighted(overlay, 0.25, annotated, 0.75, 0)
        cv2.rectangle(annotated, (exclusion_left, roi_top), (exclusion_right, height - 1), (90, 90, 90), 1)
        state = "NO LANE BOUNDARIES"
        scan_y = None
        lane_result_valid = False
        if selection is not None:
            scan_y, band_top, band_bottom, runs, left, right, exclusion_left, exclusion_right = selection
            cv2.rectangle(annotated, (0, band_top), (width - 1, band_bottom - 1), (255, 255, 0), 1)
            for start, end in runs:
                cv2.line(annotated, (start, scan_y), (end, scan_y), (0, 165, 255), 4)
            cv2.circle(annotated, ((left[0] + left[1]) // 2, scan_y), 7, (255, 0, 0), -1)
            cv2.circle(annotated, ((right[0] + right[1]) // 2, scan_y), 7, (0, 0, 255), -1)
            left_x = (left[0] + left[1]) // 2
            right_x = (right[0] + right[1]) // 2
            measured_lane_width = float(right_x - left_x)
            if self.filtered_lane_width_px is None:
                self.filtered_lane_width_px = measured_lane_width
            else:
                self.filtered_lane_width_px = (
                    self.lane_width_alpha * measured_lane_width
                    + (1.0 - self.lane_width_alpha) * self.filtered_lane_width_px
                )
            self.last_two_boundary_time = rospy.get_time()
            self.single_boundary_side = None
            self.last_single_boundary_time = None
            target_x = (left_x + right_x) // 2
            # The controller keeps the 640-pixel error convention even while
            # this node processes a smaller real-time image.
            error = (target_x - centre_x) * (self.perspective_reference_size[0] / float(width))
            state = "LANE MIDPOINT x=%d error=%+.0f px scan=%d offset=%d" % (
                target_x, error, scan_y, front_offset_px
            )
            lane_result_valid = True
            self.lane_valid_pub.publish(Bool(data=True))
            self.lane_error_pub.publish(Float32(data=float(error)))
        elif single_selection is not None:
            (
                scan_y, band_top, band_bottom, runs, boundary, boundary_x,
                target_x, visible_side,
            ) = single_selection
            cv2.rectangle(annotated, (0, band_top), (width - 1, band_bottom - 1), (255, 0, 255), 1)
            cv2.line(annotated, (boundary[0], scan_y), (boundary[1], scan_y), (0, 165, 255), 4)
            cv2.circle(annotated, (boundary_x, scan_y), 7, (0, 0, 255), -1)
            cv2.circle(annotated, (target_x, scan_y), 7, (255, 0, 255), -1)
            error = (target_x - centre_x) * (
                self.perspective_reference_size[0] / float(width)
            )
            age = rospy.get_time() - self.last_two_boundary_time
            self.single_boundary_side = visible_side
            self.last_single_boundary_time = rospy.get_time()
            state = "ONE %s BOUNDARY midpoint x=%d error=%+.0f px age=%.1fs" % (
                visible_side, target_x, error, age
            )
            lane_result_valid = True
            self.lane_valid_pub.publish(Bool(data=True))
            self.lane_error_pub.publish(Float32(data=float(error)))
        else:
            if (
                self.last_single_boundary_time is not None
                and rospy.get_time() - self.last_single_boundary_time
                > self.single_boundary_side_memory_timeout
            ):
                self.single_boundary_side = None
                self.last_single_boundary_time = None
            self.lane_valid_pub.publish(Bool(data=False))

        if lane_result_valid:
            center_samples = self.centerline_points(mask, roi_top, front_offset_px)
            center_curve, heading_error, coefficients = self.fit_centerline(center_samples)
            if len(center_curve) >= 2:
                cv2.polylines(
                    annotated,
                    [np.array(center_curve, dtype=np.int32)],
                    False,
                    (0, 255, 0),
                    2,
                )
            for sample_x, sample_y in center_samples:
                cv2.circle(annotated, (sample_x, sample_y), 3, (0, 255, 0), -1)
            self.lane_heading_pub.publish(Float32(data=heading_error))
            state += " heading=%+.1fdeg" % np.degrees(heading_error)
            target = self.lookahead_target_from_fit(
                coefficients, center_samples, frame.shape
            )
            if target is not None:
                target_forward, target_left, target_x, target_y = target
                message = PointStamped()
                message.header.stamp = rospy.Time.now()
                message.header.frame_id = self.lookahead_frame_id
                message.point.x = target_forward
                message.point.y = target_left
                message.point.z = 0.0
                self.lookahead_target_pub.publish(message)
                cv2.circle(
                    annotated,
                    (int(round(target_x)), int(round(target_y))),
                    8,
                    (255, 0, 255),
                    -1,
                )
                cv2.line(
                    annotated,
                    (centre_x, height - 1),
                    (int(round(target_x)), int(round(target_y))),
                    (255, 0, 255),
                    2,
                )
                state += " target=(%.2fm,%+.2fm left)" % (
                    target_forward,
                    target_left,
                )

        cv2.putText(annotated, "DEBUG ONLY - NO /cmd_vel", (12, 28),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
        cv2.putText(annotated, state, (12, 56),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 0), 2)
        scan_label = "none" if scan_y is None else str(scan_y)
        cv2.putText(annotated, "ROI y=%d..%d  selected scan y=%s" % (roi_top, height - 1, scan_label),
                    (12, height - 14), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 255, 255), 2)

        mask_view = cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR)
        cv2.putText(mask_view, "WHITE-LINE MASK (ground ROI only)", (12, 28),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 0), 2)
        if scan_y is not None:
            cv2.line(mask_view, (0, scan_y), (width - 1, scan_y), (255, 255, 0), 1)
        cv2.rectangle(mask_view, (exclusion_left, roi_top), (exclusion_right, height - 1), (90, 90, 90), 1)
        return annotated, mask_view

    def make_display(self, raw_frame):
        display_height, display_width = raw_frame.shape[:2]
        processing_frame = raw_frame
        if self.processing_scale < 1.0:
            processing_size = (
                max(1, int(display_width * self.processing_scale)),
                max(1, int(display_height * self.processing_scale)),
            )
            processing_frame = cv2.resize(raw_frame, processing_size, interpolation=cv2.INTER_AREA)

        if not self.use_perspective_transform:
            annotated, mask_view = self.make_line_views(processing_frame)
            if processing_frame.shape != raw_frame.shape:
                annotated = cv2.resize(annotated, (display_width, display_height), interpolation=cv2.INTER_LINEAR)
                mask_view = cv2.resize(mask_view, (display_width, display_height), interpolation=cv2.INTER_NEAREST)
            return np.hstack((annotated, mask_view))

        bird_frame, _ = self.perspective_warp(processing_frame)
        source_points, _ = self.scaled_perspective_points(raw_frame.shape)
        source_points = source_points.astype(np.int32)
        source_view = raw_frame.copy()
        cv2.polylines(source_view, [source_points], True, (0, 255, 255), 2)
        for index, point in enumerate(source_points):
            point_xy = tuple(int(value) for value in point)
            cv2.circle(source_view, point_xy, 5, (0, 0, 255), -1)
            cv2.putText(source_view, str(index + 1), (point_xy[0] + 6, point_xy[1] - 6),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 255, 255), 2)
        cv2.putText(source_view, "SOURCE: IPM GROUND TRAPEZOID", (12, 28),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 255), 2)
        annotated, mask_view = self.make_line_views(bird_frame)
        if processing_frame.shape != raw_frame.shape:
            annotated = cv2.resize(annotated, (display_width, display_height), interpolation=cv2.INTER_LINEAR)
            mask_view = cv2.resize(mask_view, (display_width, display_height), interpolation=cv2.INTER_NEAREST)
        cv2.putText(annotated, "BIRD'S-EYE VIEW", (12, 82),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 255), 2)
        return np.hstack((source_view, annotated, mask_view))

    def run(self):
        window = "Line following debug (Q/Esc: quit)"
        cv2.namedWindow(window, cv2.WINDOW_NORMAL)
        cv2.resizeWindow(window, 1920, 480)
        rate = rospy.Rate(self.display_rate)
        while not rospy.is_shutdown():
            with self.lock:
                display_image = self.display_image
            if display_image is not None:
                cv2.imshow(window, display_image)
            key = cv2.waitKey(1) & 0xFF
            if key in (ord("q"), ord("Q"), 27):
                break
            rate.sleep()
        cv2.destroyAllWindows()


if __name__ == "__main__":
    try:
        LineFollowDebug().run()
    except rospy.ROSInterruptException:
        pass