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