Merge pull request 'feat(perception): lane/path edge detector (Issue #339)' (#341) from sl-perception/issue-339-path-edges into main

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,
+    )

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()

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',
         ],
     },
 )

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

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
 )
 

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