Add vision-based route costmap from YOLOPv2 drivable-area segmentation

launches/launch.perception.py

@@ -47,4 +47,6 @@ def generate_launch_description():
         hybrid_grid,
         perception_drivable_grid,
         hybrid_drivable_grid,
+        yolopv2_drivable_grid,
+        route_costmap_v2,
     ])

launches/launch_node_definitions.py

@@ -287,6 +287,20 @@
     output="screen",
 )
 
+yolopv2_drivable_grid = Node(
+    package="segmentation",
+    executable="yolopv2_drivable_grid_node",
+    name="yolopv2_drivable_grid_node",
+    output="screen",
+)
+
+route_costmap_v2 = Node(
+    package="segmentation",
+    executable="route_costmap_v2_node",
+    name="route_costmap_v2_node",
+    output="screen",
+)
+
 autonomous_cruise_intersection_controller = Node(
     package='autonomous_cruise',
     executable='autonomous_cruise_intersection_node',

src/perception/segmentation/segmentation/lane_costmap.py

@@ -0,0 +1,146 @@
+#!/usr/bin/env python3
+"""
+lane_costmap.py — turns a raw per-frame confirmed-lane BEV mask into a
+robust distance-based cost grid, low at the lane center and rising
+toward the edges, with no explicit centerline ever extracted or tracked.
+
+Author: Siddarth Nandyala
+Email: siddarth.nandyala@utdallas.edu
+
+Why no centerline tracking: an earlier lane-line grid in this project
+spent a long time fighting exactly the failure mode a tracked,
+1-cell-wide feature has -- small per-frame noise makes a discrete
+tracked point or line jitter hard. A distance transform sidesteps that:
+the "center" is just wherever a cell is farthest from any edge of the
+region, which falls out of the whole mask's shape as a smooth field,
+not a fragile single measurement. A stray pixel or two at the boundary
+barely moves it.
+
+Two robustness problems specific to this mask source (confirmed live,
+not hypothetical) still needed solving on top of that:
+
+1. DISCONNECTED FRAGMENTS. The raw per-frame confirmed mask isn't
+always one clean blob -- noise islands elsewhere in the grid, or even a
+separate lane picked up disconnected from ours. seeded_connected_mask
+keeps ONLY the connected component that contains (or is nearest to) the
+vehicle's own cell, and drops everything else outright -- since the
+vehicle is by definition sitting in its own lane, seeding there is a
+reliable, cheap way to throw out anything not actually part of it,
+without needing any shape/size heuristic.
+
+Seeding needs a real search radius, not just the vehicle's exact cell:
+confirmed live, the front camera has a genuine blind spot in roughly the
+first 4m / 20 grid cells directly ahead of the vehicle (that range gets
+zero projected pixels at all, camera geometry/mounting height, not a
+bug), so the vehicle's own cell is essentially never marked drivable.
+seed_search_radius must comfortably exceed that blind spot's depth or
+seeding finds nothing every single frame -- confirmed live as the actual
+cause of an early version of this module returning an empty mask on
+every frame. 25 cells (5m) clears that with margin.
+
+2. TRANSIENT MIS-SEGMENTATION. For a few frames at a time, the model
+can accidentally classify an adjacent lane as part of ours too. A
+single frame can't tell this apart from a real, correct detection -- it
+needs cross-frame evidence. RollingMajorityFilter keeps a short,
+FIXED-length window of recent per-cell masks and requires a majority of
+them to agree. This is deliberately NOT the same as two approaches this
+project already tried and confirmed broken for an earlier lane-line
+grid: not an OR-forever history (that let a stale blob persist
+indefinitely once marked, since nothing ever un-marks a cell); not a
+plain temporal blend/EMA either (a single bad frame still nudges the
+result, it just takes longer to fade). A fixed window means a frame's
+influence disappears completely once it ages out no matter what; a
+majority vote means one bad frame, or even several, can never tip a
+cell on their own -- they have to be the majority of the whole window
+to be trusted.
+
+Output convention matches the rest of this project's occupancy grids:
+int8, 0-100. distance_cost_grid gives 0 at the deepest point of the
+robust region (>= max_dist_cells from every edge -- effectively "lane
+center or better"), ramping linearly up to 100 at the region's own
+boundary, and 100 (fully non-drivable) everywhere outside the region.
+"""
+
+import cv2
+import numpy as np
+
+
+def seeded_connected_mask(mask, seed_row, seed_col, seed_search_radius=25):
+    """mask: (H, W) bool. Returns mask restricted to the single connected
+    component containing the vehicle's own cell (seed_row, seed_col). If
+    that exact cell isn't marked drivable this frame (the common case --
+    see module docstring on the camera's own blind spot at the vehicle),
+    searches seed_search_radius cells around it for the nearest drivable
+    cell to seed from instead. Returns an all-False mask if no drivable
+    cell exists anywhere within the search radius -- deliberately not
+    falling back to any other component; a region unconnected to ego is
+    not "our lane" by definition here.
+    """
+    h, w = mask.shape
+    out = np.zeros_like(mask)
+    if not mask.any():
+        return out
+
+    num_labels, labels = cv2.connectedComponents(mask.astype(np.uint8), connectivity=8)
+
+    seed_label = 0
+    if 0 <= seed_row < h and 0 <= seed_col < w and mask[seed_row, seed_col]:
+        seed_label = int(labels[seed_row, seed_col])
+    else:
+        best_d2 = None
+        r0, r1 = max(0, seed_row - seed_search_radius), min(h, seed_row + seed_search_radius + 1)
+        c0, c1 = max(0, seed_col - seed_search_radius), min(w, seed_col + seed_search_radius + 1)
+        for r in range(r0, r1):
+            for c in range(c0, c1):
+                if mask[r, c]:
+                    d2 = (r - seed_row) ** 2 + (c - seed_col) ** 2
+                    if best_d2 is None or d2 < best_d2:
+                        best_d2 = d2
+                        seed_label = int(labels[r, c])
+
+    if seed_label == 0:
+        return out
+    return labels == seed_label
+
+
+class RollingMajorityFilter:
+    """Keeps the last `window` per-cell boolean masks; update() returns,
+    per cell, whether at least `min_votes` of the last `window` frames
+    (including this one) marked it True. See module docstring for why
+    this specific shape (fixed window + majority, not OR-forever, not a
+    blend) was chosen.
+    """
+
+    def __init__(self, window=5, min_votes=3):
+        if min_votes > window:
+            raise ValueError('min_votes cannot exceed window')
+        self.window = window
+        self.min_votes = min_votes
+        self._history = []
+
+    def update(self, mask):
+        self._history.append(mask.astype(np.uint8))
+        if len(self._history) > self.window:
+            self._history.pop(0)
+        votes = np.sum(self._history, axis=0)
+        return votes >= self.min_votes
+
+    def reset(self):
+        self._history = []
+
+
+def distance_cost_grid(mask, max_dist_cells):
+    """mask: (H, W) bool, the robust (seeded + temporally filtered)
+    drivable region. Returns an (H, W) int8 cost grid -- see module
+    docstring for the 0-100 convention. max_dist_cells is the distance
+    (in grid cells) from an edge at which cost bottoms out at 0; roughly
+    half a real lane's width is a reasonable starting point, not a
+    finalized value.
+    """
+    if not mask.any():
+        return np.full(mask.shape, 100, dtype=np.int8)
+
+    dist = cv2.distanceTransform(mask.astype(np.uint8), cv2.DIST_L2, 5)
+    normalized = np.clip(dist / float(max_dist_cells), 0.0, 1.0)
+    cost = np.where(mask, np.round(100 * (1.0 - normalized)), 100).astype(np.int8)
+    return cost

src/perception/segmentation/segmentation/route_costmap_v2_node.py

@@ -0,0 +1,215 @@
+#!/usr/bin/env python3
+"""
+route_costmap_v2_node.py — publishes a second, vision-based route cost
+grid for the vehicle's own lane, built from YOLOPv2's drivable-area
+segmentation mask (/drivable_mask/front), for a planner to prefer the
+lane center rather than just avoid marked-occupied cells.
+
+Author: Siddarth Nandyala
+Email: siddarth.nandyala@utdallas.edu
+
+Naming note: this project already has a route_costmap_node in the
+`costs` package (Justin Ruths), which subscribes to the planned
+/planning/route path and publishes /grid/route_distance -- a
+distance-to-planned-path cost, unrelated to this node's approach and
+left completely untouched here. This node is a second, independent way
+to get a route cost grid, built purely from live camera segmentation
+rather than the planned path, kept side by side for comparison -- hence
+the "_v2" in both the node name and its /route_costmap/segmented_v2
+topic, to avoid colliding with that existing node's ROS graph name while
+still being findable as "the other route costmap."
+
+This is also a distinct output from this package's drivable-AREA grids
+(perception_drivable_grid_node.py's /grid/drivable/segmented, and this
+package's own yolopv2_drivable_grid_node.py at
+/grid/drivable/segmented_v3): those answer "is this cell drivable or
+not," a binary occupancy question. This node answers a different
+question -- "how good is this cell as a point on our route through the
+current lane" -- a graded cost, lowest at the lane center and rising
+toward its edges, meant for a planner's cost-based search rather than a
+simple binary obstacle check. All three kinds of grid are kept side by
+side on purpose; this one does not replace any of them.
+
+Projection (unchanged, shared with yolopv2_drivable_grid_node.py): a
+per-pixel camera->ground-plane ray-cast lookup table
+(bev_geometry.CamLUT), precomputed once at startup, maps every pixel of
+the front camera's drivable-area mask into a BEV grid cell. A cell is
+confirmed only once it has enough total projected samples to trust its
+drivable/total ratio at all (MIN_SAMPLE_SIZE) and that ratio clears
+RATIO_THRESHOLD -- see yolopv2_drivable_grid_node.py's module docstring
+for the live tuning history behind both constants (a cell's available
+camera pixel budget shrinks sharply with distance under perspective, so
+neither a low sample floor nor a high one alone is correct).
+
+From there this node's job diverges: rather than publishing that raw
+confirmed mask directly, it turns it into a robust, graded cost field.
+See lane_costmap.py for the full reasoning and unit tests behind each
+step; in short:
+
+  1. seeded_connected_mask keeps only the connected component touching
+     the vehicle's own cell, dropping disconnected noise islands or an
+     unrelated lane picked up elsewhere in the frame.
+  2. RollingMajorityFilter requires a short window of recent frames to
+     agree before trusting a cell, so a few frames of the model
+     accidentally bleeding into an adjacent lane can't reach the output.
+  3. distance_cost_grid runs a distance transform on that robust region:
+     0 at the deepest point (the lane center, found for free as
+     whichever cell is farthest from every edge -- no separate
+     centerline-tracking step at all), ramping up to 100 at the region's
+     boundary, and 100 (fully non-drivable) outside it.
+
+Output is an OccupancyGrid on /route_costmap/segmented_v2, same 0-100
+int8 convention as the rest of this project's grids so it drops directly
+into RViz's existing Map display and any downstream code already reading
+an OccupancyGrid.
+"""
+
+import threading
+
+import cv2
+import numpy as np
+import rclpy
+from cv_bridge import CvBridge
+from nav_msgs.msg import OccupancyGrid
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
+from sensor_msgs.msg import Image
+
+from segmentation.bev_geometry import (
+    CAMERAS, CamLUT, GRID_SIZE, RESOLUTION, ORIGIN_X, ORIGIN_Y, VEHICLE_ROW, VEHICLE_COL)
+from segmentation.lane_costmap import seeded_connected_mask, RollingMajorityFilter, distance_cost_grid
+
+_DRIVABLE_MASK_TOPIC = '/drivable_mask/front'
+_OUTPUT_TOPIC = '/route_costmap/segmented_v2'
+
+# See yolopv2_drivable_grid_node.py's module docstring for the live
+# tuning history behind these two: a plain absolute pixel-count floor is
+# too strict at range (perspective shrinks a cell's camera pixel budget
+# with distance), and a plain ratio with too low a floor is unstable on
+# tiny sample sizes. Both are needed together.
+MIN_SAMPLE_SIZE = 3
+RATIO_THRESHOLD = 0.5
+
+# How far around the vehicle's own cell to search for a seed point (see
+# lane_costmap.seeded_connected_mask). Confirmed live: the front camera
+# has a real blind spot roughly 4m / 20 grid cells directly ahead of the
+# vehicle (zero projected pixels land there at all), so the vehicle's
+# exact cell is essentially never itself marked drivable -- this radius
+# must clear that blind spot with margin or seeding finds nothing every
+# frame.
+SEED_SEARCH_RADIUS = 25
+
+# At 20Hz, a 5-frame window is a quarter second -- long enough that a
+# few-frame mis-segmentation burst can't reach a majority, short enough
+# that a real, sustained change (an actual lane change) still wins
+# within a fraction of a second.
+MAJORITY_WINDOW = 5
+MAJORITY_MIN_VOTES = 3
+
+# Cap distance (in grid cells) at which distance_cost_grid's cost
+# bottoms out at 0. ~9 cells = 1.8m at this grid's 0.2m resolution,
+# roughly half a real lane's width -- a starting point, not a tuned
+# final value.
+MAX_COST_DIST_CELLS = 9
+
+_FRONT_NAME, _FRONT_TOPIC, _FRONT_T, _FRONT_R = next(c for c in CAMERAS if c[0] == 'front')
+
+
+class RouteCostmapV2Node(Node):
+
+    def __init__(self):
+        super().__init__('route_costmap_v2_node')
+        self.bridge = CvBridge()
+        self._lock = threading.Lock()
+
+        self._front_lut = CamLUT(_FRONT_T, _FRONT_R)
+        self._latest_drivable_mask = None
+        self._majority_filter = RollingMajorityFilter(window=MAJORITY_WINDOW, min_votes=MAJORITY_MIN_VOTES)
+
+        qos_be = QoSProfile(
+            reliability=QoSReliabilityPolicy.BEST_EFFORT,
+            history=QoSHistoryPolicy.KEEP_LAST, depth=1)
+
+        self.create_subscription(Image, _DRIVABLE_MASK_TOPIC, self._cb_drivable_mask, qos_be)
+
+        self.pub = self.create_publisher(OccupancyGrid, _OUTPUT_TOPIC, 10)
+        self.create_timer(0.05, self._publish_loop)
+        self.get_logger().info(f'RouteCostmapV2Node ready — {_OUTPUT_TOPIC}')
+
+    # ── callbacks ─────────────────────────────────────────────────────────
+
+    def _cb_drivable_mask(self, msg):
+        mask = self.bridge.imgmsg_to_cv2(msg, 'mono8') > 127
+        with self._lock:
+            self._latest_drivable_mask = mask
+
+    # ── mask → BEV projection ────────────────────────────────────────────
+
+    def _confirmed_area_from_mask(self, drivable_mask):
+        """Project one frame's drivable-area pixels into the BEV grid. A
+        cell is confirmed only once it has enough total samples to trust
+        a ratio at all (MIN_SAMPLE_SIZE) AND that ratio exceeds
+        RATIO_THRESHOLD."""
+        lut = self._front_lut
+        mask = drivable_mask
+        if mask.shape[0] != lut.img_h or mask.shape[1] != lut.img_w:
+            mask = cv2.resize(mask.astype(np.uint8), (lut.img_w, lut.img_h),
+                               interpolation=cv2.INTER_NEAREST).astype(bool)
+
+        total_cnt = np.zeros((GRID_SIZE, GRID_SIZE), dtype=np.int32)
+        bright_cnt = np.zeros((GRID_SIZE, GRID_SIZE), dtype=np.int32)
+        valid = lut.valid
+        np.add.at(total_cnt, (lut.gr[valid], lut.gc[valid]), 1)
+        hit = valid & mask.ravel()
+        if hit.any():
+            np.add.at(bright_cnt, (lut.gr[hit], lut.gc[hit]), 1)
+
+        enough_samples = total_cnt >= MIN_SAMPLE_SIZE
+        with np.errstate(divide='ignore', invalid='ignore'):
+            ratio = np.where(enough_samples, bright_cnt / np.maximum(total_cnt, 1), 0.0)
+        return enough_samples & (ratio > RATIO_THRESHOLD)
+
+    # ── publish loop ─────────────────────────────────────────────────────
+
+    def _publish_loop(self):
+        with self._lock:
+            mask_snap = self._latest_drivable_mask
+
+        if mask_snap is None:
+            confirmed = np.zeros((GRID_SIZE, GRID_SIZE), dtype=bool)
+        else:
+            confirmed = self._confirmed_area_from_mask(mask_snap)
+
+        seeded = seeded_connected_mask(confirmed, VEHICLE_ROW, VEHICLE_COL,
+                                        seed_search_radius=SEED_SEARCH_RADIUS)
+        robust = self._majority_filter.update(seeded)
+        grid_out = distance_cost_grid(robust, MAX_COST_DIST_CELLS)
+
+        msg = OccupancyGrid()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = 'base_link'
+        msg.info.resolution = RESOLUTION
+        msg.info.width = GRID_SIZE
+        msg.info.height = GRID_SIZE
+        msg.info.origin.position.x = ORIGIN_X
+        msg.info.origin.position.y = ORIGIN_Y
+        msg.info.origin.position.z = 0.0
+        msg.info.origin.orientation.w = 1.0
+        msg.data = grid_out.ravel().tolist()
+        self.pub.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = RouteCostmapV2Node()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()