From bd9cb6da35eb7f7762ca5163e23ae7042e7c8be3 Mon Sep 17 00:00:00 2001 From: sl-perception Date: Tue, 3 Mar 2026 11:33:22 -0500 Subject: [PATCH] feat(perception): lane/path edge detector (Issue #339) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Adds Canny+Hough+bird-eye perspective pipeline for detecting left/right path edges from the forward camera. Pure-Python helper (_path_edges.py) is fully tested; ROS2 node publishes PathEdges on /saltybot/path_edges. - saltybot_scene_msgs/msg/PathEdges.msg — new message - saltybot_scene_msgs/CMakeLists.txt — register PathEdges.msg - saltybot_bringup/_path_edges.py — PathEdgeConfig, PathEdgesResult, build/apply_homography, canny_edges, hough_lines, classify_lines, average_line, warp_segments, process_frame - saltybot_bringup/path_edges_node.py — ROS2 node (sensor_msgs/Image → PathEdges, parameters for all tunable Canny/Hough/birdseye params) - test/test_path_edges.py — 38 tests, 38 passing - setup.py — add path_edges console_script Co-Authored-By: Claude Sonnet 4.6 --- .../saltybot_bringup/_path_edges.py | 379 ++++++++++++++++++ .../saltybot_bringup/path_edges_node.py | 164 ++++++++ jetson/ros2_ws/src/saltybot_bringup/setup.py | 2 + .../saltybot_bringup/test/test_path_edges.py | 364 +++++++++++++++++ .../src/saltybot_scene_msgs/CMakeLists.txt | 2 + .../src/saltybot_scene_msgs/msg/PathEdges.msg | 48 +++ 6 files changed, 959 insertions(+) create mode 100644 jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/_path_edges.py create mode 100644 jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/path_edges_node.py create mode 100644 jetson/ros2_ws/src/saltybot_bringup/test/test_path_edges.py create mode 100644 jetson/ros2_ws/src/saltybot_scene_msgs/msg/PathEdges.msg diff --git a/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/_path_edges.py b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/_path_edges.py new file mode 100644 index 0000000..45f6ce2 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/_path_edges.py @@ -0,0 +1,379 @@ +""" +_path_edges.py — Lane/path edge detection via Canny + Hough + bird-eye +perspective transform (no ROS2 deps). + +Algorithm +--------- +1. Crop the bottom `roi_frac` of the BGR image to an ROI. +2. Convert ROI to grayscale → Gaussian blur → Canny edge map. +3. Run probabilistic Hough (HoughLinesP) on the edge map to find + line segments. +4. Filter segments by slope: near-horizontal lines (|slope| < min_slope) + are discarded as ground noise; the remaining lines are classified as + left edges (negative slope) or right edges (positive slope). +5. Average each class into a single dominant edge line and extrapolate it + to span the full ROI height. +6. Apply a bird-eye perspective homography (computed from a configurable + source trapezoid in the ROI) to warp all segment endpoints to a + top-down view. +7. Return a PathEdgesResult with all data. + +Coordinate conventions +----------------------- +All pixel coordinates in PathEdgesResult are in the ROI frame: + origin = top-left of the bottom-half ROI + y = 0 → roi_top of the full image + y increases downward; x increases rightward. + +Bird-eye homography +------------------- +The homography H is computed via cv2.getPerspectiveTransform from four +source points (fractional ROI coords) to four destination points in a +square output image of size `birdseye_size`. Default source trapezoid +assumes a forward-looking camera at ~35–45° tilt above a flat surface. + +Public API +---------- + PathEdgeConfig dataclass — all tunable parameters with sensible defaults + PathEdgesResult NamedTuple — all outputs of process_frame() + build_homography(src_frac, roi_w, roi_h, birdseye_size) + → 3×3 float64 homography + apply_homography(H, points_xy) → np.ndarray (N, 2) warped points + canny_edges(bgr_roi, low, high, ksize) → uint8 edge map + hough_lines(edge_map, threshold, min_len, max_gap) → List[(x1,y1,x2,y2)] + classify_lines(lines, min_slope) → (left, right) + average_line(lines, roi_height) → Optional[(x1,y1,x2,y2)] + warp_segments(lines, H) → List[(bx1,by1,bx2,by2)] + process_frame(bgr, cfg) → PathEdgesResult +""" + +from __future__ import annotations + +import math +from dataclasses import dataclass, field +from typing import List, NamedTuple, Optional, Tuple + +import cv2 +import numpy as np + + +# ── Config ──────────────────────────────────────────────────────────────────── + +@dataclass +class PathEdgeConfig: + # ROI + roi_frac: float = 0.50 # bottom fraction of image used as ROI + + # Preprocessing + blur_ksize: int = 5 # Gaussian blur kernel (odd integer) + canny_low: int = 50 # Canny low threshold + canny_high: int = 150 # Canny high threshold + + # Hough + hough_rho: float = 1.0 # distance resolution (px) + hough_theta: float = math.pi / 180.0 # angle resolution (rad) + hough_threshold: int = 30 # minimum votes + min_line_len: int = 40 # minimum segment length (px) + max_line_gap: int = 20 # maximum gap within a segment (px) + + # Line classification + min_slope: float = 0.3 # |slope| below this → discard (near-horizontal) + + # Bird-eye perspective — source trapezoid as fractions of (roi_w, roi_h) + # Default: wide trapezoid for forward-looking camera at ~40° tilt + birdseye_src: List[List[float]] = field(default_factory=lambda: [ + [0.40, 0.05], # top-left (near horizon) + [0.60, 0.05], # top-right (near horizon) + [0.95, 0.95], # bottom-right (near robot) + [0.05, 0.95], # bottom-left (near robot) + ]) + birdseye_size: int = 400 # square bird-eye output image side (px) + + +# ── Result ──────────────────────────────────────────────────────────────────── + +class PathEdgesResult(NamedTuple): + lines: List[Tuple[float, float, float, float]] # ROI coords + left_lines: List[Tuple[float, float, float, float]] + right_lines: List[Tuple[float, float, float, float]] + left_edge: Optional[Tuple[float, float, float, float]] # None if absent + right_edge: Optional[Tuple[float, float, float, float]] + birdseye_lines: List[Tuple[float, float, float, float]] + birdseye_left: Optional[Tuple[float, float, float, float]] + birdseye_right: Optional[Tuple[float, float, float, float]] + H: np.ndarray # 3×3 homography (ROI → bird-eye) + roi_top: int # y-offset of ROI in the full image + + +# ── Homography ──────────────────────────────────────────────────────────────── + +def build_homography( + src_frac: List[List[float]], + roi_w: int, + roi_h: int, + birdseye_size: int, +) -> np.ndarray: + """ + Compute a perspective homography from ROI pixel coords to a square + bird-eye image. + + Parameters + ---------- + src_frac : four [fx, fy] pairs as fractions of (roi_w, roi_h) + roi_w, roi_h : ROI dimensions in pixels + birdseye_size : side length of the square output image + + Returns + ------- + H : (3, 3) float64 homography matrix; maps ROI px → bird-eye px. + """ + src = np.array( + [[fx * roi_w, fy * roi_h] for fx, fy in src_frac], + dtype=np.float32, + ) + S = float(birdseye_size) + dst = np.array([ + [S * 0.25, 0.0], + [S * 0.75, 0.0], + [S * 0.75, S], + [S * 0.25, S], + ], dtype=np.float32) + + H = cv2.getPerspectiveTransform(src, dst) + return H.astype(np.float64) + + +def apply_homography(H: np.ndarray, points_xy: np.ndarray) -> np.ndarray: + """ + Apply a 3×3 homography to an (N, 2) float array of 2-D points. + + Returns + ------- + (N, 2) float32 array of transformed points. + """ + if len(points_xy) == 0: + return np.empty((0, 2), dtype=np.float32) + + pts = np.column_stack([points_xy, np.ones(len(points_xy))]) # (N, 3) + warped = (H @ pts.T).T # (N, 3) + warped /= warped[:, 2:3] # homogenise + return warped[:, :2].astype(np.float32) + + +# ── Edge detection ──────────────────────────────────────────────────────────── + +def canny_edges(bgr_roi: np.ndarray, low: int, high: int, ksize: int) -> np.ndarray: + """ + Convert a BGR ROI to a Canny edge map. + + Parameters + ---------- + bgr_roi : uint8 BGR image (the bottom-half ROI) + low : Canny lower threshold + high : Canny upper threshold + ksize : Gaussian blur kernel size (must be odd ≥ 1) + + Returns + ------- + uint8 binary edge map (0 or 255), same spatial size as bgr_roi. + """ + grey = cv2.cvtColor(bgr_roi, cv2.COLOR_BGR2GRAY) + if ksize >= 3 and ksize % 2 == 1: + grey = cv2.GaussianBlur(grey, (ksize, ksize), 0) + return cv2.Canny(grey, low, high) + + +# ── Hough lines ─────────────────────────────────────────────────────────────── + +def hough_lines( + edge_map: np.ndarray, + threshold: int = 30, + min_len: int = 40, + max_gap: int = 20, + rho: float = 1.0, + theta: float = math.pi / 180.0, +) -> List[Tuple[float, float, float, float]]: + """ + Run probabilistic Hough on an edge map. + + Returns + ------- + List of (x1, y1, x2, y2) tuples in the edge_map pixel frame. + Empty list when no lines are found. + """ + raw = cv2.HoughLinesP( + edge_map, + rho=rho, + theta=theta, + threshold=threshold, + minLineLength=min_len, + maxLineGap=max_gap, + ) + if raw is None: + return [] + return [ + (float(x1), float(y1), float(x2), float(y2)) + for x1, y1, x2, y2 in raw[:, 0] + ] + + +# ── Line classification ─────────────────────────────────────────────────────── + +def classify_lines( + lines: List[Tuple[float, float, float, float]], + min_slope: float = 0.3, +) -> Tuple[List, List]: + """ + Classify Hough segments into left-edge and right-edge candidates. + + In image coordinates (y increases downward): + - Left lane lines have NEGATIVE slope (upper-right → lower-left) + - Right lane lines have POSITIVE slope (upper-left → lower-right) + - Near-horizontal segments (|slope| < min_slope) are discarded + + Returns + ------- + (left_lines, right_lines) + """ + left: List = [] + right: List = [] + for x1, y1, x2, y2 in lines: + dx = x2 - x1 + if abs(dx) < 1e-6: + continue # vertical — skip to avoid division by zero + slope = (y2 - y1) / dx + if slope < -min_slope: + left.append((x1, y1, x2, y2)) + elif slope > min_slope: + right.append((x1, y1, x2, y2)) + return left, right + + +# ── Line averaging ──────────────────────────────────────────────────────────── + +def average_line( + lines: List[Tuple[float, float, float, float]], + roi_height: int, +) -> Optional[Tuple[float, float, float, float]]: + """ + Average a list of collinear Hough segments into one representative line, + extrapolated to span the full ROI height (y=0 → y=roi_height-1). + + Returns None if the list is empty or the averaged slope is near-zero. + """ + if not lines: + return None + + slopes: List[float] = [] + intercepts: List[float] = [] + for x1, y1, x2, y2 in lines: + dx = x2 - x1 + if abs(dx) < 1e-6: + continue + m = (y2 - y1) / dx + b = y1 - m * x1 + slopes.append(m) + intercepts.append(b) + + if not slopes: + return None + + m_avg = float(np.mean(slopes)) + b_avg = float(np.mean(intercepts)) + if abs(m_avg) < 1e-6: + return None + + y_bot = float(roi_height - 1) + y_top = 0.0 + x_bot = (y_bot - b_avg) / m_avg + x_top = (y_top - b_avg) / m_avg + return (x_bot, y_bot, x_top, y_top) + + +# ── Bird-eye segment warping ────────────────────────────────────────────────── + +def warp_segments( + lines: List[Tuple[float, float, float, float]], + H: np.ndarray, +) -> List[Tuple[float, float, float, float]]: + """ + Warp a list of line-segment endpoints through the homography H. + + Returns + ------- + List of (bx1, by1, bx2, by2) in bird-eye pixel coordinates. + """ + if not lines: + return [] + pts = np.array( + [[x1, y1] for x1, y1, _, _ in lines] + + [[x2, y2] for _, _, x2, y2 in lines], + dtype=np.float32, + ) + warped = apply_homography(H, pts) + n = len(lines) + return [ + (float(warped[i, 0]), float(warped[i, 1]), + float(warped[i + n, 0]), float(warped[i + n, 1])) + for i in range(n) + ] + + +# ── Main entry point ────────────────────────────────────────────────────────── + +def process_frame(bgr: np.ndarray, cfg: PathEdgeConfig) -> PathEdgesResult: + """ + Run the full lane/path edge detection pipeline on one BGR frame. + + Parameters + ---------- + bgr : uint8 BGR image (H × W × 3) + cfg : PathEdgeConfig + + Returns + ------- + PathEdgesResult with all detected edges in ROI + bird-eye coordinates. + """ + h, w = bgr.shape[:2] + roi_top = int(h * (1.0 - cfg.roi_frac)) + roi = bgr[roi_top:, :] + roi_h, roi_w = roi.shape[:2] + + # Build perspective homography (ROI px → bird-eye px) + H = build_homography(cfg.birdseye_src, roi_w, roi_h, cfg.birdseye_size) + + # Edge detection + edges = canny_edges(roi, cfg.canny_low, cfg.canny_high, cfg.blur_ksize) + + # Hough line segments (in ROI coords) + lines = hough_lines( + edges, + threshold = cfg.hough_threshold, + min_len = cfg.min_line_len, + max_gap = cfg.max_line_gap, + rho = cfg.hough_rho, + theta = cfg.hough_theta, + ) + + # Classify and average + left_lines, right_lines = classify_lines(lines, cfg.min_slope) + left_edge = average_line(left_lines, roi_h) + right_edge = average_line(right_lines, roi_h) + + # Warp all segments to bird-eye + birdseye_lines = warp_segments(lines, H) + birdseye_left = (warp_segments([left_edge], H)[0] if left_edge else None) + birdseye_right = (warp_segments([right_edge], H)[0] if right_edge else None) + + return PathEdgesResult( + lines = lines, + left_lines = left_lines, + right_lines = right_lines, + left_edge = left_edge, + right_edge = right_edge, + birdseye_lines = birdseye_lines, + birdseye_left = birdseye_left, + birdseye_right = birdseye_right, + H = H, + roi_top = roi_top, + ) diff --git a/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/path_edges_node.py b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/path_edges_node.py new file mode 100644 index 0000000..d6638ba --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/path_edges_node.py @@ -0,0 +1,164 @@ +""" +path_edges_node.py — ROS2 node for lane/path edge detection (Issue #339). + +Subscribes +---------- + /camera/color/image_raw (sensor_msgs/Image) + +Publishes +--------- + /saltybot/path_edges (saltybot_scene_msgs/PathEdges) + +Parameters +---------- + roi_frac (float, default 0.50) bottom fraction of image as ROI + blur_ksize (int, default 5) Gaussian blur kernel size (odd) + canny_low (int, default 50) Canny lower threshold + canny_high (int, default 150) Canny upper threshold + hough_threshold (int, default 30) minimum Hough votes + min_line_len (int, default 40) minimum Hough segment length (px) + max_line_gap (int, default 20) maximum Hough gap (px) + min_slope (float, default 0.3) |slope| below this → discard + birdseye_size (int, default 400) bird-eye square image side (px) +""" + +from __future__ import annotations + +import cv2 +import numpy as np + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +from std_msgs.msg import Header + +from saltybot_scene_msgs.msg import PathEdges + +from ._path_edges import PathEdgeConfig, process_frame + + +class PathEdgesNode(Node): + def __init__(self) -> None: + super().__init__('path_edges_node') + + # Declare parameters + self.declare_parameter('roi_frac', 0.50) + self.declare_parameter('blur_ksize', 5) + self.declare_parameter('canny_low', 50) + self.declare_parameter('canny_high', 150) + self.declare_parameter('hough_threshold', 30) + self.declare_parameter('min_line_len', 40) + self.declare_parameter('max_line_gap', 20) + self.declare_parameter('min_slope', 0.3) + self.declare_parameter('birdseye_size', 400) + + self._sub = self.create_subscription( + Image, + '/camera/color/image_raw', + self._image_cb, + 10, + ) + self._pub = self.create_publisher(PathEdges, '/saltybot/path_edges', 10) + + self.get_logger().info('PathEdgesNode ready — subscribing /camera/color/image_raw') + + # ------------------------------------------------------------------ + + def _build_config(self) -> PathEdgeConfig: + p = self.get_parameter + return PathEdgeConfig( + roi_frac = p('roi_frac').value, + blur_ksize = p('blur_ksize').value, + canny_low = p('canny_low').value, + canny_high = p('canny_high').value, + hough_threshold = p('hough_threshold').value, + min_line_len = p('min_line_len').value, + max_line_gap = p('max_line_gap').value, + min_slope = p('min_slope').value, + birdseye_size = p('birdseye_size').value, + ) + + def _image_cb(self, msg: Image) -> None: + # Convert ROS Image → BGR numpy array + bgr = self._ros_image_to_bgr(msg) + if bgr is None: + return + + cfg = self._build_config() + result = process_frame(bgr, cfg) + + out = PathEdges() + out.header = msg.header + out.line_count = len(result.lines) + out.roi_top = result.roi_top + + # Flat float32 arrays: [x1,y1,x2,y2, x1,y1,x2,y2, ...] + out.segments_px = [v for seg in result.lines for v in seg] + out.segments_birdseye_px = [v for seg in result.birdseye_lines for v in seg] + + # Left edge + if result.left_edge is not None: + x1, y1, x2, y2 = result.left_edge + out.left_x1, out.left_y1 = float(x1), float(y1) + out.left_x2, out.left_y2 = float(x2), float(y2) + out.left_detected = True + else: + out.left_detected = False + + if result.birdseye_left is not None: + bx1, by1, bx2, by2 = result.birdseye_left + out.left_birdseye_x1, out.left_birdseye_y1 = float(bx1), float(by1) + out.left_birdseye_x2, out.left_birdseye_y2 = float(bx2), float(by2) + + # Right edge + if result.right_edge is not None: + x1, y1, x2, y2 = result.right_edge + out.right_x1, out.right_y1 = float(x1), float(y1) + out.right_x2, out.right_y2 = float(x2), float(y2) + out.right_detected = True + else: + out.right_detected = False + + if result.birdseye_right is not None: + bx1, by1, bx2, by2 = result.birdseye_right + out.right_birdseye_x1, out.right_birdseye_y1 = float(bx1), float(by1) + out.right_birdseye_x2, out.right_birdseye_y2 = float(bx2), float(by2) + + self._pub.publish(out) + + @staticmethod + def _ros_image_to_bgr(msg: Image) -> np.ndarray | None: + """Convert a sensor_msgs/Image to a uint8 BGR numpy array.""" + enc = msg.encoding.lower() + data = np.frombuffer(msg.data, dtype=np.uint8) + + if enc in ('rgb8', 'bgr8', 'mono8'): + channels = 1 if enc == 'mono8' else 3 + try: + img = data.reshape((msg.height, msg.width, channels)) + except ValueError: + return None + if enc == 'rgb8': + img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR) + elif enc == 'mono8': + img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR) + return img + + # Unsupported encoding + return None + + +def main(args=None) -> None: + rclpy.init(args=args) + node = PathEdgesNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == '__main__': + main() diff --git a/jetson/ros2_ws/src/saltybot_bringup/setup.py b/jetson/ros2_ws/src/saltybot_bringup/setup.py index b4aface..4d33740 100644 --- a/jetson/ros2_ws/src/saltybot_bringup/setup.py +++ b/jetson/ros2_ws/src/saltybot_bringup/setup.py @@ -53,6 +53,8 @@ setup( 'person_reid = saltybot_bringup.person_reid_node:main', # Dynamic obstacle velocity estimator (Issue #326) 'obstacle_velocity = saltybot_bringup.obstacle_velocity_node:main', + # Lane/path edge detector (Issue #339) + 'path_edges = saltybot_bringup.path_edges_node:main', ], }, ) diff --git a/jetson/ros2_ws/src/saltybot_bringup/test/test_path_edges.py b/jetson/ros2_ws/src/saltybot_bringup/test/test_path_edges.py new file mode 100644 index 0000000..76073a2 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/test/test_path_edges.py @@ -0,0 +1,364 @@ +""" +test_path_edges.py — pytest tests for _path_edges.py (no ROS2 required). + +Tests cover: + - build_homography: output shape, identity-like mapping + - apply_homography: empty input, single point, batch + - canny_edges: output shape, dtype, uniform image produces no edges + - hough_lines: empty edge map returns [] + - classify_lines: slope filtering, left/right split + - average_line: empty → None, single line, multi-line average + - warp_segments: empty list, segment endpoint ordering + - process_frame: smoke test on synthetic image +""" + +from __future__ import annotations + +import math + +import cv2 +import numpy as np +import pytest + +from saltybot_bringup._path_edges import ( + PathEdgeConfig, + PathEdgesResult, + apply_homography, + average_line, + build_homography, + canny_edges, + classify_lines, + hough_lines, + process_frame, + warp_segments, +) + + +# ── Helpers ─────────────────────────────────────────────────────────────────── + +def _solid_bgr(h: int = 100, w: int = 200, color=(128, 128, 128)) -> np.ndarray: + img = np.zeros((h, w, 3), dtype=np.uint8) + img[:] = color + return img + + +def _default_H(roi_w: int = 200, roi_h: int = 100) -> np.ndarray: + cfg = PathEdgeConfig() + return build_homography(cfg.birdseye_src, roi_w, roi_h, cfg.birdseye_size) + + +# ── build_homography ────────────────────────────────────────────────────────── + +class TestBuildHomography: + def test_shape(self): + H = _default_H() + assert H.shape == (3, 3) + + def test_dtype(self): + H = _default_H() + assert H.dtype == np.float64 + + def test_bottom_left_maps_near_left(self): + """Bottom-left source trapezoid point should map near left of bird-eye.""" + cfg = PathEdgeConfig() + H = build_homography(cfg.birdseye_src, 200, 100, cfg.birdseye_size) + # src[3] = [0.05, 0.95] → near bottom-left of ROI → should map to x≈100 (centre-left), y≈400 + src_pt = np.array([[0.05 * 200, 0.95 * 100]], dtype=np.float32) + dst = apply_homography(H, src_pt) + assert dst[0, 0] == pytest.approx(cfg.birdseye_size * 0.25, abs=5) + assert dst[0, 1] == pytest.approx(cfg.birdseye_size, abs=5) + + def test_bottom_right_maps_near_right(self): + cfg = PathEdgeConfig() + H = build_homography(cfg.birdseye_src, 200, 100, cfg.birdseye_size) + # src[2] = [0.95, 0.95] → bottom-right → maps to x≈300, y≈400 + src_pt = np.array([[0.95 * 200, 0.95 * 100]], dtype=np.float32) + dst = apply_homography(H, src_pt) + assert dst[0, 0] == pytest.approx(cfg.birdseye_size * 0.75, abs=5) + assert dst[0, 1] == pytest.approx(cfg.birdseye_size, abs=5) + + +# ── apply_homography ────────────────────────────────────────────────────────── + +class TestApplyHomography: + def test_empty_input(self): + H = _default_H() + out = apply_homography(H, np.empty((0, 2), dtype=np.float32)) + assert out.shape == (0, 2) + + def test_single_point_roundtrip(self): + """Warping then inverse-warping should recover the original point.""" + H = _default_H(200, 100) + H_inv = np.linalg.inv(H) + pt = np.array([[50.0, 40.0]], dtype=np.float32) + warped = apply_homography(H, pt) + unwarped = apply_homography(H_inv, warped) + np.testing.assert_allclose(unwarped, pt, atol=1e-3) + + def test_batch_output_shape(self): + H = _default_H() + pts = np.random.rand(5, 2).astype(np.float32) * 100 + out = apply_homography(H, pts) + assert out.shape == (5, 2) + assert out.dtype == np.float32 + + +# ── canny_edges ─────────────────────────────────────────────────────────────── + +class TestCannyEdges: + def test_output_shape(self): + roi = _solid_bgr(80, 160) + edges = canny_edges(roi, low=50, high=150, ksize=5) + assert edges.shape == (80, 160) + + def test_output_dtype(self): + roi = _solid_bgr() + edges = canny_edges(roi, low=50, high=150, ksize=5) + assert edges.dtype == np.uint8 + + def test_uniform_image_no_edges(self): + """A solid-colour image should produce no edges.""" + roi = _solid_bgr(80, 160, color=(100, 100, 100)) + edges = canny_edges(roi, low=50, high=150, ksize=5) + assert edges.max() == 0 + + def test_strong_edge_detected(self): + """A sharp horizontal boundary should produce edges.""" + roi = np.zeros((100, 200, 3), dtype=np.uint8) + roi[50:, :] = 255 # sharp boundary at y=50 + edges = canny_edges(roi, low=20, high=60, ksize=3) + assert edges.max() == 255 + + def test_ksize_even_skips_blur(self): + """Even ksize should not crash (blur is skipped for even kernels).""" + roi = _solid_bgr() + edges = canny_edges(roi, low=50, high=150, ksize=4) + assert edges.shape == (100, 200) + + +# ── hough_lines ─────────────────────────────────────────────────────────────── + +class TestHoughLines: + def test_empty_edge_map(self): + edge_map = np.zeros((100, 200), dtype=np.uint8) + lines = hough_lines(edge_map, threshold=30, min_len=40, max_gap=20) + assert lines == [] + + def test_returns_list_of_tuples(self): + """Draw a diagonal line and verify hough returns tuples of 4 floats.""" + edge_map = np.zeros((100, 200), dtype=np.uint8) + cv2.line(edge_map, (0, 0), (199, 99), 255, 2) + lines = hough_lines(edge_map, threshold=10, min_len=20, max_gap=5) + assert isinstance(lines, list) + if lines: + assert len(lines[0]) == 4 + assert all(isinstance(v, float) for v in lines[0]) + + def test_line_detected_on_drawn_segment(self): + """A drawn line segment should be detected.""" + edge_map = np.zeros((200, 400), dtype=np.uint8) + cv2.line(edge_map, (50, 100), (350, 150), 255, 2) + lines = hough_lines(edge_map, threshold=10, min_len=30, max_gap=10) + assert len(lines) >= 1 + + +# ── classify_lines ──────────────────────────────────────────────────────────── + +class TestClassifyLines: + def test_empty_returns_two_empty_lists(self): + left, right = classify_lines([], min_slope=0.3) + assert left == [] + assert right == [] + + def test_negative_slope_goes_left(self): + # slope = (y2-y1)/(x2-x1) = (50-0)/(0-100) = -0.5 → left + lines = [(100.0, 0.0, 0.0, 50.0)] + left, right = classify_lines(lines, min_slope=0.3) + assert len(left) == 1 + assert right == [] + + def test_positive_slope_goes_right(self): + # slope = (50-0)/(100-0) = 0.5 → right + lines = [(0.0, 0.0, 100.0, 50.0)] + left, right = classify_lines(lines, min_slope=0.3) + assert left == [] + assert len(right) == 1 + + def test_near_horizontal_discarded(self): + # slope = 0.1 → |slope| < 0.3 → discard + lines = [(0.0, 0.0, 100.0, 10.0)] + left, right = classify_lines(lines, min_slope=0.3) + assert left == [] + assert right == [] + + def test_vertical_line_skipped(self): + # dx ≈ 0 → skip + lines = [(50.0, 0.0, 50.0, 100.0)] + left, right = classify_lines(lines, min_slope=0.3) + assert left == [] + assert right == [] + + def test_mixed_lines(self): + lines = [ + (100.0, 0.0, 0.0, 50.0), # slope -0.5 → left + (0.0, 0.0, 100.0, 50.0), # slope +0.5 → right + (0.0, 0.0, 100.0, 5.0), # slope +0.05 → discard + ] + left, right = classify_lines(lines, min_slope=0.3) + assert len(left) == 1 + assert len(right) == 1 + + +# ── average_line ────────────────────────────────────────────────────────────── + +class TestAverageLine: + def test_empty_returns_none(self): + assert average_line([], roi_height=100) is None + + def test_single_line_extrapolated(self): + # slope=0.5, intercept=0: x = y/0.5 = 2y + # At y=99: x=198; at y=0: x=0 + lines = [(0.0, 0.0, 200.0, 100.0)] # slope = 100/200 = 0.5 + result = average_line(lines, roi_height=100) + assert result is not None + x_bot, y_bot, x_top, y_top = result + assert y_bot == pytest.approx(99.0, abs=1) + assert y_top == pytest.approx(0.0, abs=1) + + def test_two_parallel_lines_averaged(self): + # Both have slope=1.0, intercepts -50 and +50 → avg intercept=0 + # x = (y - 0) / 1.0 = y + lines = [ + (50.0, 0.0, 100.0, 50.0), # slope=1, intercept=-50 + (0.0, 50.0, 50.0, 100.0), # slope=1, intercept=50 + ] + result = average_line(lines, roi_height=100) + assert result is not None + x_bot, y_bot, x_top, y_top = result + assert y_bot == pytest.approx(99.0, abs=1) + # avg intercept = 0, m_avg=1 → x_bot = 99 + assert x_bot == pytest.approx(99.0, abs=2) + + def test_vertical_only_returns_none(self): + # dx == 0 → skip → no slopes → None + lines = [(50.0, 0.0, 50.0, 100.0)] + assert average_line(lines, roi_height=100) is None + + def test_returns_four_tuple(self): + lines = [(0.0, 0.0, 100.0, 50.0)] + result = average_line(lines, roi_height=100) + assert result is not None + assert len(result) == 4 + + +# ── warp_segments ───────────────────────────────────────────────────────────── + +class TestWarpSegments: + def test_empty_returns_empty(self): + H = _default_H() + result = warp_segments([], H) + assert result == [] + + def test_single_segment_returns_one_tuple(self): + H = _default_H(200, 100) + lines = [(10.0, 10.0, 90.0, 80.0)] + result = warp_segments(lines, H) + assert len(result) == 1 + assert len(result[0]) == 4 + + def test_start_and_end_distinct(self): + """Warped segment endpoints should be different from each other.""" + H = _default_H(200, 100) + lines = [(10.0, 10.0, 190.0, 90.0)] + result = warp_segments(lines, H) + bx1, by1, bx2, by2 = result[0] + # The two endpoints should differ + assert abs(bx1 - bx2) + abs(by1 - by2) > 1.0 + + def test_batch_preserves_count(self): + H = _default_H(200, 100) + lines = [(0.0, 0.0, 10.0, 10.0), (100.0, 0.0, 90.0, 50.0)] + result = warp_segments(lines, H) + assert len(result) == 2 + + +# ── process_frame ───────────────────────────────────────────────────────────── + +class TestProcessFrame: + def _lane_image(self, h: int = 480, w: int = 640) -> np.ndarray: + """ + Create a synthetic BGR image with two diagonal lines representing + left and right lane edges in the bottom half. + """ + img = np.zeros((h, w, 3), dtype=np.uint8) + roi_top = h // 2 + # Left edge: from bottom-left area upward to the right (negative slope in image coords) + cv2.line(img, (80, h - 10), (240, roi_top + 10), (255, 255, 255), 4) + # Right edge: from bottom-right area upward to the left (positive slope in image coords) + cv2.line(img, (w - 80, h - 10), (w - 240, roi_top + 10), (255, 255, 255), 4) + return img + + def test_returns_named_tuple(self): + bgr = _solid_bgr(120, 240) + cfg = PathEdgeConfig() + result = process_frame(bgr, cfg) + assert isinstance(result, PathEdgesResult) + + def test_roi_top_correct(self): + bgr = np.zeros((200, 400, 3), dtype=np.uint8) + cfg = PathEdgeConfig(roi_frac=0.5) + result = process_frame(bgr, cfg) + assert result.roi_top == 100 + + def test_uniform_image_no_lines(self): + bgr = _solid_bgr(200, 400, color=(80, 80, 80)) + cfg = PathEdgeConfig() + result = process_frame(bgr, cfg) + assert result.lines == [] + assert result.left_edge is None + assert result.right_edge is None + assert result.left_lines == [] + assert result.right_lines == [] + + def test_homography_matrix_shape(self): + bgr = _solid_bgr(200, 400) + result = process_frame(bgr, PathEdgeConfig()) + assert result.H.shape == (3, 3) + + def test_birdseye_segments_same_count(self): + """birdseye_lines and lines must have the same number of segments.""" + bgr = self._lane_image() + result = process_frame(bgr, PathEdgeConfig(hough_threshold=10, min_line_len=20)) + assert len(result.birdseye_lines) == len(result.lines) + + def test_lane_image_detects_edges(self): + """Synthetic lane image should detect at least one of left/right edge.""" + bgr = self._lane_image() + cfg = PathEdgeConfig( + roi_frac=0.5, + canny_low=30, + canny_high=100, + hough_threshold=10, + min_line_len=20, + max_line_gap=15, + ) + result = process_frame(bgr, cfg) + assert (result.left_edge is not None) or (result.right_edge is not None) + + def test_segments_px_flat_array_length(self): + """segments_px-equivalent length must be 4 × line_count.""" + bgr = self._lane_image() + cfg = PathEdgeConfig(hough_threshold=10, min_line_len=20) + result = process_frame(bgr, cfg) + assert len(result.lines) * 4 == sum(4 for _ in result.lines) + + def test_left_right_lines_subset_of_all_lines(self): + """left_lines + right_lines must be a subset of all lines.""" + bgr = self._lane_image() + cfg = PathEdgeConfig(hough_threshold=10, min_line_len=20) + result = process_frame(bgr, cfg) + all_set = set(result.lines) + for seg in result.left_lines: + assert seg in all_set + for seg in result.right_lines: + assert seg in all_set diff --git a/jetson/ros2_ws/src/saltybot_scene_msgs/CMakeLists.txt b/jetson/ros2_ws/src/saltybot_scene_msgs/CMakeLists.txt index ea476f9..9db8a9a 100644 --- a/jetson/ros2_ws/src/saltybot_scene_msgs/CMakeLists.txt +++ b/jetson/ros2_ws/src/saltybot_scene_msgs/CMakeLists.txt @@ -25,6 +25,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} # Issue #326 — dynamic obstacle velocity estimator "msg/ObstacleVelocity.msg" "msg/ObstacleVelocityArray.msg" + # Issue #339 — lane/path edge detector + "msg/PathEdges.msg" DEPENDENCIES std_msgs geometry_msgs vision_msgs builtin_interfaces ) diff --git a/jetson/ros2_ws/src/saltybot_scene_msgs/msg/PathEdges.msg b/jetson/ros2_ws/src/saltybot_scene_msgs/msg/PathEdges.msg new file mode 100644 index 0000000..19d30b5 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_scene_msgs/msg/PathEdges.msg @@ -0,0 +1,48 @@ +# PathEdges.msg — Lane/path edge detection result (Issue #339) +# +# All pixel coordinates are in the ROI frame: +# origin = top-left of the bottom-half ROI crop +# y=0 → roi_top of the full image; y increases downward; x increases rightward. +# +# Bird-eye coordinates are in the warped top-down perspective image. + +std_msgs/Header header + +# --- Raw Hough segments (ROI frame) --- +# Flat array of (x1, y1, x2, y2) tuples; length = 4 * line_count +float32[] segments_px + +# Same segments warped to bird-eye view; length = 4 * line_count +float32[] segments_birdseye_px + +# Number of Hough line segments detected +uint32 line_count + +# --- Dominant left edge (ROI frame) --- +float32 left_x1 +float32 left_y1 +float32 left_x2 +float32 left_y2 +bool left_detected + +# --- Dominant left edge (bird-eye frame) --- +float32 left_birdseye_x1 +float32 left_birdseye_y1 +float32 left_birdseye_x2 +float32 left_birdseye_y2 + +# --- Dominant right edge (ROI frame) --- +float32 right_x1 +float32 right_y1 +float32 right_x2 +float32 right_y2 +bool right_detected + +# --- Dominant right edge (bird-eye frame) --- +float32 right_birdseye_x1 +float32 right_birdseye_y1 +float32 right_birdseye_x2 +float32 right_birdseye_y2 + +# y-offset of ROI in the full image (pixels from top) +uint32 roi_top