diff --git a/launches/launch.perception.py b/launches/launch.perception.py
index 3545678f..841aee9c 100644
--- a/launches/launch.perception.py
+++ b/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,
     ])
diff --git a/launches/launch_node_definitions.py b/launches/launch_node_definitions.py
index 21a9bd8a..b2ad00d2 100644
--- a/launches/launch_node_definitions.py
+++ b/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',
diff --git a/src/perception/segmentation/segmentation/lane_costmap.py b/src/perception/segmentation/segmentation/lane_costmap.py
new file mode 100644
index 00000000..c12bd158
--- /dev/null
+++ b/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
diff --git a/src/perception/segmentation/segmentation/route_costmap_v2_node.py b/src/perception/segmentation/segmentation/route_costmap_v2_node.py
new file mode 100644
index 00000000..271f01f0
--- /dev/null
+++ b/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()
diff --git a/src/perception/segmentation/segmentation/yolopv2_drivable_grid_node.py b/src/perception/segmentation/segmentation/yolopv2_drivable_grid_node.py
new file mode 100644
index 00000000..50e9124b
--- /dev/null
+++ b/src/perception/segmentation/segmentation/yolopv2_drivable_grid_node.py
@@ -0,0 +1,242 @@
+#!/usr/bin/env python3
+"""
+yolopv2_drivable_grid_node.py — projects YOLOPv2's own drivable-area
+segmentation mask (/drivable_mask/front) directly into the BEV grid, via
+the same per-pixel camera->ground-plane ray-cast lookup table
+(bev_geometry.CamLUT) already used elsewhere in this package.
+
+Author: Siddarth Nandyala
+Email: siddarth.nandyala@utdallas.edu
+
+Supersedes an earlier version of this node that tried to reconstruct a
+drivable area purely from YOLOPv2's LANE-LINE head (thin painted markings)
+via projection + corridor-fill + curve-following forward extension +
+width-based edge inference (lane_bounded_drivable.py, since deleted). That
+approach fought a real, fundamental problem the whole way: reconstructing
+a filled 2D region from sparse 1-cell-wide line detections is inherently
+lossy and heuristic-heavy.
+
+YOLOPv2 is a multi-task model and already has a SEPARATE head trained
+directly for drivable-area segmentation -- a dense per-pixel free-space
+mask, not lines to reconstruct area from. yolopv2_lane_node.py already
+runs that head every frame (one shared forward pass, no extra inference
+cost) and already publishes it on /drivable_mask/front -- confirmed live
+this session to have a real publisher and be actively computed every
+frame, just with zero subscribers until now. This node's whole job is
+now just: project that mask into the grid. No corridor-fill, no forward
+extension, no line reconstruction -- the model already gives us a filled
+region directly.
+
+How the projection works (bev_geometry.CamLUT, unchanged, the same LUT
+the lane-line version of this node already used): for every pixel in the
+front camera's image, a ray is cast from the camera through that pixel
+(via the inverse intrinsic matrix), rotated into the vehicle's base_link
+frame using the camera's fixed mounting rotation, and intersected with
+the ground plane (z=0) to get a real-world (x, y) point -- this assumes
+flat local ground, true for CARLA's road surface. That (x, y) is then
+converted to a grid (row, col) via the grid's origin/resolution. This is
+precomputed ONCE per camera at startup as a flat per-pixel lookup table
+(lut.gr, lut.gc, lut.valid), not recomputed per frame -- projecting a new
+mask each frame is then just: for every mask pixel, look up its
+precomputed cell and count it there.
+
+Aggregation still needs the same care as the lane-line version's fix: at
+range, a grid cell packs in far fewer camera pixels than a cell close to
+the vehicle (perspective), so a cell's confirmation can't just be "did
+any drivable pixel land here." Each cell needs a MINIMUM SAMPLE SIZE
+(MIN_SAMPLE_SIZE) of total projected pixels before trusting its
+drivable/total ratio at all, then a RATIO_THRESHOLD on top.
+
+MIN_SAMPLE_SIZE=10 (copied over from the lane-line version's tuning) was
+WRONG for this mask, confirmed live: sampling the actual live pixel
+density per cell showed cells beyond ~14m forward get only 4-7 total
+projected camera pixels even for real, solid road surface -- that never
+clears a floor of 10, so everything past 14m was silently discarded
+regardless of what the mask said, no matter how confidently the model
+had classified it. That floor made sense for the lane-line case (telling
+a thin real line apart from noise); it does not apply here, since this
+mask is a dense filled region rather than 1-cell-wide lines needing an
+anti-noise floor. A live sweep across MIN_SAMPLE_SIZE in {1,2,3,5,10}
+found 1,2,3 all recover the same live frame's far-range coverage
+identically (1879 confirmed cells past 14m vs 0 at a floor of 10), so 3
+was picked: low enough not to blank out real far-range coverage, still
+high enough to require more than one stray pixel before trusting a
+cell. RATIO_THRESHOLD=0.5 (plain majority vote, appropriate for a dense
+region rather than thin lines) changed the same sweep's result only
+mildly (0.3/0.5/0.7 -> 1879/1808/1768 far cells on that frame).
+
+STEP 2: once the raw per-frame projection above was confirmed live to be
+placed correctly, two more problems showed up that a single frame's mask
+can't fix on its own: the raw mask sometimes includes disconnected noise
+islands (or even a separate lane, unconnected to ours), and for a few
+frames at a time the model can accidentally bleed into an adjacent lane
+as if it were ours. Both are handled in lane_costmap.py, kept out of this
+node and unit-tested standalone (same pattern as the deleted
+lane_bounded_drivable.py): seeded_connected_mask keeps only the connected
+component touching the vehicle's own cell, dropping every disconnected
+fragment outright; RollingMajorityFilter keeps a short fixed-length
+window of recent per-cell masks and requires a majority to agree, so a
+brief mis-segmentation burst can't tip a cell on its own, and (unlike an
+OR-forever history) nothing lingers once it ages out of the window.
+
+Output is no longer strict binary. distance_cost_grid runs a distance
+transform on that robust region and turns it into a 0-100 cost grid: 0 at
+the deepest point of the region (the lane center, found for free as
+wherever is farthest from every edge -- no separate centerline-tracking
+step at all), ramping up to 100 at the region's own boundary, and 100
+(fully non-drivable) outside it entirely. Still no LiDAR, no
+vehicle-footprint prior.
+
+This is a v3 evaluation node, not a replacement: publishes to
+/grid/drivable/segmented_v3, side by side with the existing
+/grid/drivable/segmented (perception_drivable_grid_node.py, PSPNet-based,
+unchanged) for live comparison before anything downstream is switched
+over.
+"""
+
+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 = '/grid/drivable/segmented_v3'
+
+# Same reasoning as the lane-line version: a cell's drivable/total ratio
+# isn't trustworthy until it has enough total projected samples --
+# perspective means far cells get far fewer camera pixels than near ones.
+# Confirmed live: 10 (copied from the lane-line tuning) was too strict
+# here and silently blanked out everything past ~14m forward, where real
+# cells only get 4-7 total projected pixels even for solid road. 3 is the
+# lowest floor that still recovered that same far-range coverage in a
+# live sweep -- see module docstring.
+MIN_SAMPLE_SIZE = 3
+
+# Unlike the sparse 1-cell-wide lane-line case (which needed 0.3 to
+# survive thin real features), this mask is a dense filled region, so a
+# plain majority vote is the right threshold here.
+RATIO_THRESHOLD = 0.5
+
+# RollingMajorityFilter tuning: 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 Yolopv2DrivableGridNode(Node):
+
+    def __init__(self):
+        super().__init__('yolopv2_drivable_grid_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'Yolopv2DrivableGridNode ready (seeded + majority-filtered distance costmap) — {_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)
+        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 = Yolopv2DrivableGridNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/src/perception/segmentation/setup.py b/src/perception/segmentation/setup.py
index e0b4fe51..b08ba7ff 100755
--- a/src/perception/segmentation/setup.py
+++ b/src/perception/segmentation/setup.py
@@ -28,6 +28,8 @@
             'hybrid_perception_grid_node = segmentation.hybrid_perception_grid_node:main',
             'perception_drivable_grid_node = segmentation.perception_drivable_grid_node:main',
             'hybrid_drivable_grid_node = segmentation.hybrid_drivable_grid_node:main',
+            'yolopv2_drivable_grid_node = segmentation.yolopv2_drivable_grid_node:main',
+            'route_costmap_v2_node = segmentation.route_costmap_v2_node:main',
         ],
     },
 )
diff --git a/src/perception/segmentation/test/test_lane_costmap.py b/src/perception/segmentation/test/test_lane_costmap.py
new file mode 100644
index 00000000..a743e0f7
--- /dev/null
+++ b/src/perception/segmentation/test/test_lane_costmap.py
@@ -0,0 +1,195 @@
+#!/usr/bin/env python3
+"""
+test_lane_costmap.py — tests for lane_costmap.py.
+
+Author: Siddarth Nandyala
+Email: siddarth.nandyala@utdallas.edu
+"""
+
+import numpy as np
+
+from segmentation.lane_costmap import (
+    seeded_connected_mask, RollingMajorityFilter, distance_cost_grid,
+)
+
+H, W = 40, 40
+EGO_ROW, EGO_COL = 20, 20
+
+
+def _grid():
+    return np.zeros((H, W), dtype=bool)
+
+
+# ── seeded_connected_mask ────────────────────────────────────────────────
+
+def test_seeded_connected_mask_keeps_component_touching_seed():
+    mask = _grid()
+    mask[EGO_ROW - 2:EGO_ROW + 3, EGO_COL - 1:EGO_COL + 2] = True  # a blob around ego
+
+    out = seeded_connected_mask(mask, EGO_ROW, EGO_COL)
+
+    assert np.array_equal(out, mask)
+
+
+def test_seeded_connected_mask_drops_disconnected_island():
+    mask = _grid()
+    mask[EGO_ROW - 2:EGO_ROW + 3, EGO_COL - 1:EGO_COL + 2] = True  # ego's real lane
+    mask[5, 5] = True  # a totally disconnected noise speck elsewhere
+
+    out = seeded_connected_mask(mask, EGO_ROW, EGO_COL)
+
+    assert out[EGO_ROW, EGO_COL]
+    assert not out[5, 5]
+    assert out.sum() == mask.sum() - 1
+
+
+def test_seeded_connected_mask_drops_a_disconnected_other_lane():
+    """A separate lane's blob, not touching ours, must be dropped even
+    though it's a large, real, coherent region -- connectivity to the
+    vehicle is what matters, not size or shape."""
+    mask = _grid()
+    mask[EGO_ROW - 2:EGO_ROW + 3, EGO_COL - 1:EGO_COL + 2] = True   # our lane
+    mask[EGO_ROW - 2:EGO_ROW + 3, EGO_COL + 10:EGO_COL + 13] = True  # another lane, unconnected
+
+    out = seeded_connected_mask(mask, EGO_ROW, EGO_COL)
+
+    assert out[EGO_ROW, EGO_COL]
+    assert not out[EGO_ROW, EGO_COL + 11]
+
+
+def test_seeded_connected_mask_searches_nearby_when_seed_cell_empty():
+    mask = _grid()
+    mask[EGO_ROW + 3, EGO_COL] = True  # a real detection just ahead, not exactly at ego
+
+    out = seeded_connected_mask(mask, EGO_ROW, EGO_COL, seed_search_radius=5)
+
+    assert out[EGO_ROW + 3, EGO_COL]
+
+
+def test_seeded_connected_mask_returns_empty_when_nothing_within_radius():
+    mask = _grid()
+    mask[EGO_ROW + 15, EGO_COL] = True  # far outside the search radius
+
+    out = seeded_connected_mask(mask, EGO_ROW, EGO_COL, seed_search_radius=5)
+
+    assert not out.any()
+
+
+def test_seeded_connected_mask_empty_input_returns_empty():
+    mask = _grid()
+    out = seeded_connected_mask(mask, EGO_ROW, EGO_COL)
+    assert not out.any()
+
+
+# ── RollingMajorityFilter ────────────────────────────────────────────────
+
+def test_rolling_majority_filter_single_bad_frame_does_not_tip_result():
+    f = RollingMajorityFilter(window=5, min_votes=3)
+    steady = _grid()
+    steady[EGO_ROW, EGO_COL] = True
+    bleed = _grid()
+    bleed[EGO_ROW, EGO_COL] = True
+    bleed[EGO_ROW, EGO_COL + 10] = True  # one frame's worth of leaked adjacent lane
+
+    for _ in range(3):
+        out = f.update(steady)
+    out = f.update(bleed)
+
+    assert out[EGO_ROW, EGO_COL]
+    assert not out[EGO_ROW, EGO_COL + 10]  # single-frame leak must not appear
+
+
+def test_rolling_majority_filter_sustained_change_does_eventually_win():
+    f = RollingMajorityFilter(window=5, min_votes=3)
+    steady = _grid()
+    steady[EGO_ROW, EGO_COL] = True
+    changed = _grid()
+    changed[EGO_ROW, EGO_COL + 10] = True
+
+    for _ in range(3):
+        f.update(steady)
+    out = None
+    for _ in range(5):
+        out = f.update(changed)  # a real, sustained change over many frames
+
+    assert out[EGO_ROW, EGO_COL + 10]
+
+
+def test_rolling_majority_filter_stale_frame_ages_out_of_fixed_window():
+    """A cell that was True for a while but then stops must go False again
+    once enough fresh False frames have pushed it out of the window --
+    unlike an OR-forever history, nothing here lingers indefinitely."""
+    f = RollingMajorityFilter(window=5, min_votes=3)
+    on = _grid()
+    on[EGO_ROW, EGO_COL] = True
+    off = _grid()
+
+    for _ in range(5):
+        f.update(on)
+    out = None
+    for _ in range(5):
+        out = f.update(off)
+
+    assert not out[EGO_ROW, EGO_COL]
+
+
+def test_rolling_majority_filter_requires_min_votes_not_just_any_true():
+    f = RollingMajorityFilter(window=5, min_votes=3)
+    on = _grid()
+    on[EGO_ROW, EGO_COL] = True
+    off = _grid()
+
+    f.update(on)
+    f.update(off)
+    out = f.update(off)  # only 1 of 3 frames so far was True
+
+    assert not out[EGO_ROW, EGO_COL]
+
+
+# ── distance_cost_grid ───────────────────────────────────────────────────
+
+def test_distance_cost_grid_zero_at_deep_center():
+    mask = _grid()
+    mask[5:35, 15:25] = True  # a wide, deep region
+
+    cost = distance_cost_grid(mask, max_dist_cells=4)
+
+    assert cost[20, 20] == 0  # far from every edge
+
+
+def test_distance_cost_grid_high_near_boundary():
+    mask = _grid()
+    mask[5:35, 15:25] = True
+
+    cost = distance_cost_grid(mask, max_dist_cells=4)
+
+    assert cost[5, 20] > 50  # right at the region's own edge row
+
+
+def test_distance_cost_grid_max_outside_mask():
+    mask = _grid()
+    mask[5:35, 15:25] = True
+
+    cost = distance_cost_grid(mask, max_dist_cells=4)
+
+    assert cost[0, 0] == 100
+    assert cost[39, 39] == 100
+
+
+def test_distance_cost_grid_empty_mask_is_all_max_cost():
+    mask = _grid()
+    cost = distance_cost_grid(mask, max_dist_cells=4)
+    assert (cost == 100).all()
+
+
+def test_distance_cost_grid_narrower_region_never_reaches_zero():
+    """A region narrower than 2*max_dist_cells can't contain any point
+    that's max_dist_cells from every edge -- cost should bottom out above
+    0 everywhere, not falsely claim a safe center that isn't there."""
+    mask = _grid()
+    mask[10:30, 19:21] = True  # only 2 cells wide
+
+    cost = distance_cost_grid(mask, max_dist_cells=8)
+
+    assert cost[20, 19] > 0
+    assert cost[20, 20] > 0