fix(nav): preserve unknown cells in saved maps

ros2_ws/src/mars_bot/mars_nav/CMakeLists.txt

@@ -151,6 +151,12 @@ install(
   DESTINATION share/ament_index/resource_index/packages
 )
 
+if(BUILD_TESTING)
+  find_package(ament_cmake_pytest REQUIRED)
+  ament_add_pytest_test(test_grid_scoring test/test_grid_scoring.py)
+  ament_add_pytest_test(test_map_metadata test/test_map_metadata.py)
+endif()
+
 # Export dependencies for downstream packages
 ament_export_include_directories(include)
 ament_export_libraries(dynamic_goal_checker)

ros2_ws/src/mars_bot/mars_nav/mars_nav/grid_localizer.py

@@ -42,6 +42,8 @@
 from std_msgs.msg import String
 from std_srvs.srv import Trigger
 
+from mars_nav.grid_scoring import endpoint_mismatch_scores, occupancy_masks
+
 
 class GridLocalizer(Node):
     """GPU-accelerated grid localization lifecycle node."""
@@ -59,6 +61,8 @@ class GridLocalizer(Node):
     map_received: bool = False
     map_free = None
     map_free_gpu = None
+    map_occupied = None
+    map_occupied_gpu = None
     resolution = None
     origin = None
     map_h = None
@@ -113,6 +117,8 @@ def on_configure(self, state: State) -> TransitionCallbackReturn:
         self.map_received = False
         self.map_free = None
         self.map_free_gpu = None
+        self.map_occupied = None
+        self.map_occupied_gpu = None
         self.resolution = None
         self.origin = None
 
@@ -245,7 +251,10 @@ def _cleanup_resources(self):
         if self.map_free_gpu is not None:
             del self.map_free_gpu
             self.map_free_gpu = None
-            cp.get_default_memory_pool().free_all_blocks()
+        if self.map_occupied_gpu is not None:
+            del self.map_occupied_gpu
+            self.map_occupied_gpu = None
+        cp.get_default_memory_pool().free_all_blocks()
 
     def on_cleanup(self, state: State) -> TransitionCallbackReturn:
         """Inactive → Unconfigured: Destroy all resources and free GPU memory."""
@@ -298,12 +307,19 @@ def _map_cb(self, msg: OccupancyGrid):
                 self.get_logger().info("Freeing old GPU map memory")
                 del self.map_free_gpu
                 self.map_free_gpu = None
+            if self.map_occupied_gpu is not None:
+                del self.map_occupied_gpu
+                self.map_occupied_gpu = None
             cp.get_default_memory_pool().free_all_blocks()
 
             if self.map_free is not None:
                 del self.map_free
                 self.map_free = None
 
+            if self.map_occupied is not None:
+                del self.map_occupied
+                self.map_occupied = None
+
             if self.free_pixels is not None:
                 del self.free_pixels
                 self.free_pixels = None
@@ -327,16 +343,14 @@ def _map_cb(self, msg: OccupancyGrid):
         # -1: unknown, 0: free, 100: occupied
         data = np.array(msg.data, dtype=np.int8).reshape((msg.info.height, msg.info.width))
 
-        # grid_localizer expects self.map_free where 1.0=free, 0.0=occupied
-        # and row 0 = top of image (due to coordinate conversion logic)
+        # Keep known-free and occupied distinct. Unknown cells are neither:
+        # they cannot host candidate poses and must not count as obstacle hits.
+        map_free_binary, map_occupied_binary = occupancy_masks(data)
 
-        # 1. Create binary free map (0 is free in OccupancyGrid)
-        # Treat unknown (-1) as occupied for safety
-        map_free_binary = (data == 0).astype(np.float32)
-
-        # 2. Flip vertically to match "image coordinates" expected by _generate_candidates_for_batch
+        # Flip vertically to match "image coordinates" expected by _generate_candidates_for_batch
         # (which uses map_h - pix_y)
         self.map_free = np.flipud(map_free_binary)
+        self.map_occupied = np.flipud(map_occupied_binary)
 
         self.map_h, self.map_w = self.map_free.shape
 
@@ -351,6 +365,7 @@ def _map_cb(self, msg: OccupancyGrid):
 
         # Move map to GPU
         self.map_free_gpu = cp.asarray(self.map_free)
+        self.map_occupied_gpu = cp.asarray(self.map_occupied)
 
         # Record time map was received to allow for a startup delay
         self.map_received_time = self.get_clock().now()
@@ -743,25 +758,14 @@ def _score_batch_gpu(self, pos_x, pos_y, pos_theta, ranges_gpu, cos_angles, sin_
 
         pix_x = (pos_x_gpu[:, None] + ranges_gpu[None, :] * cos_world - self.origin[0]) * inv_res
         del cos_world
-        cp.clip(pix_x, 0, self.map_w - 1, out=pix_x)
-
         pix_y = self.map_h - (pos_y_gpu[:, None] + ranges_gpu[None, :] * sin_world - self.origin[1]) * inv_res
         del sin_world, pos_x_gpu, pos_y_gpu
-        cp.clip(pix_y, 0, self.map_h - 1, out=pix_y)
 
-        # Convert to int for indexing
-        pix_x_int = pix_x.astype(cp.int32)
-        pix_y_int = pix_y.astype(cp.int32)
+        # Lower is better: only an in-bounds occupied endpoint is a match.
+        # Unknown and out-of-bounds endpoints are mismatches, never fake walls.
+        scores = endpoint_mismatch_scores(self.map_occupied_gpu, pix_x, pix_y, xp=cp)
         del pix_x, pix_y
 
-        # Score: lower = better (endpoints hitting obstacles = 0 in map_free)
-        hit_free = self.map_free_gpu[pix_y_int, pix_x_int]
-        del pix_x_int, pix_y_int
-
-        # Mean across beams, keep on GPU
-        scores = cp.mean(hit_free, axis=1)
-        del hit_free
-
         return scores  # Return CuPy array, caller handles transfer
 
 

ros2_ws/src/mars_bot/mars_nav/mars_nav/grid_scoring.py

@@ -0,0 +1,31 @@
+#!/usr/bin/env python3
+# SPDX-License-Identifier: Apache-2.0
+# Copyright (c) 2026 Innate Inc
+
+"""Backend-agnostic occupancy helpers for grid localization."""
+
+import numpy as np
+
+
+def occupancy_masks(data):
+    """Return distinct known-free and occupied masks for an occupancy grid.
+
+    Unknown cells (-1) belong to neither mask. Positive occupancy values retain
+    the grid localizer's existing obstacle semantics for trinary and scale maps.
+    """
+    return (data == 0).astype(np.float32), (data > 0).astype(np.float32)
+
+
+def endpoint_mismatch_scores(occupied_map, pix_x, pix_y, *, xp=np):
+    """Score scan endpoints; only in-bounds occupied cells are matches.
+
+    Coordinates outside the map are sampled safely after clipping, but remain
+    mismatches. This prevents map-edge cells from becoming artificial obstacle
+    matches for rays that leave the map.
+    """
+    map_h, map_w = occupied_map.shape
+    in_bounds = (pix_x >= 0) & (pix_x < map_w) & (pix_y >= 0) & (pix_y < map_h)
+    pix_x_int = xp.clip(pix_x, 0, map_w - 1).astype(xp.int32)
+    pix_y_int = xp.clip(pix_y, 0, map_h - 1).astype(xp.int32)
+    hit_occupied = xp.where(in_bounds, occupied_map[pix_y_int, pix_x_int], 0.0)
+    return 1.0 - xp.mean(hit_occupied, axis=1)

ros2_ws/src/mars_bot/mars_nav/mars_nav/map_metadata.py

@@ -0,0 +1,54 @@
+#!/usr/bin/env python3
+# SPDX-License-Identifier: Apache-2.0
+# Copyright (c) 2026 Innate Inc
+
+"""Compatibility fixes for saved Nav2 map metadata."""
+
+import os
+import re
+from pathlib import Path
+
+TRINARY_FREE_THRESHOLD = 0.196
+_LEGACY_TRINARY_FREE_THRESHOLD = 0.25
+_NUMBER = r"(?:\d+(?:\.\d*)?|\.\d+)"
+_MODE_RE = re.compile(r"^(\s*mode\s*:\s*)([^\s#]+)(.*)$", re.MULTILINE)
+_FREE_THRESH_RE = re.compile(rf"^(\s*free_thresh\s*:\s*)({_NUMBER})(.*)$", re.MULTILINE)
+
+
+def normalize_legacy_trinary_metadata(text: str) -> tuple[str, bool]:
+    """Repair the unsafe threshold emitted by Nav2 Humble's trinary map saver.
+
+    Humble writes unknown occupancy cells as gray 205 but pairs them with its
+    default ``free_thresh: 0.25``. On reload, 205 becomes free because its
+    occupancy is about 0.196. Restrict the migration to that exact legacy
+    signature so imported maps and non-trinary modes retain their semantics.
+    """
+    mode_match = _MODE_RE.search(text)
+    threshold_match = _FREE_THRESH_RE.search(text)
+    if mode_match is None or threshold_match is None:
+        return text, False
+    if mode_match.group(2).lower() != "trinary":
+        return text, False
+    if abs(float(threshold_match.group(2)) - _LEGACY_TRINARY_FREE_THRESHOLD) > 1e-9:
+        return text, False
+
+    replacement = f"{threshold_match.group(1)}{TRINARY_FREE_THRESHOLD:.3f}{threshold_match.group(3)}"
+    start, end = threshold_match.span()
+    return text[:start] + replacement + text[end:], True
+
+
+def repair_legacy_trinary_map(yaml_path: str | Path) -> bool:
+    """Atomically repair one legacy map YAML; return whether it changed."""
+    path = Path(yaml_path)
+    original = path.read_text()
+    repaired, changed = normalize_legacy_trinary_metadata(original)
+    if not changed:
+        return False
+
+    temporary = path.with_name(f".{path.name}.{os.getpid()}.tmp")
+    try:
+        temporary.write_text(repaired)
+        os.replace(temporary, path)
+    finally:
+        temporary.unlink(missing_ok=True)
+    return True

ros2_ws/src/mars_bot/mars_nav/mars_nav/mode_manager.py

@@ -32,6 +32,7 @@
 from std_msgs.msg import String
 from std_srvs.srv import Trigger
 
+from mars_nav.map_metadata import TRINARY_FREE_THRESHOLD, repair_legacy_trinary_map
 from mars_nav.service_utils import call_service, get_node_state, transition_node
 
 # TODO: move this into launch file?
@@ -339,6 +340,14 @@ def discover_maps(self):
             yaml_files = glob.glob(yaml_pattern)
 
             for yaml_file in yaml_files:
+                try:
+                    if repair_legacy_trinary_map(yaml_file):
+                        self.get_logger().warning(
+                            f"Repaired legacy trinary map threshold in '{os.path.basename(yaml_file)}' "
+                            f"(0.25 -> {TRINARY_FREE_THRESHOLD:.3f}); unknown gray cells will no longer load as free"
+                        )
+                except (OSError, ValueError) as e:
+                    self.get_logger().warning(f"Could not check map metadata '{yaml_file}': {e}")
                 # Extract just the filename
                 map_name = os.path.basename(yaml_file)
                 map_files.append(map_name)
@@ -1193,6 +1202,8 @@ def save_map_callback(self, request, response):
                 "--ros-args",
                 "-p",
                 "save_map_timeout:=5000.0",
+                "-p",
+                f"free_thresh_default:={TRINARY_FREE_THRESHOLD:.3f}",
             ]
 
             # Run the map saver command

ros2_ws/src/mars_bot/mars_nav/package.xml

@@ -61,6 +61,7 @@
   <!-- Lint and testing dependencies -->
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

ros2_ws/src/mars_bot/mars_nav/test/test_grid_scoring.py

@@ -0,0 +1,22 @@
+# SPDX-License-Identifier: Apache-2.0
+# Copyright (c) 2026 Innate Inc
+
+import numpy as np
+
+from mars_nav.grid_scoring import endpoint_mismatch_scores, occupancy_masks
+
+
+def test_only_occupied_in_bounds_endpoints_are_matches():
+    cells = np.array([[0, -1, 100]], dtype=np.int8)
+    known_free, occupied = occupancy_masks(cells)
+
+    np.testing.assert_array_equal(known_free, [[1.0, 0.0, 0.0]])
+    np.testing.assert_array_equal(occupied, [[0.0, 0.0, 1.0]])
+
+    pix_x = np.array([[2.0, 1.0, 0.0, 3.0, -1.0], [2.0, 2.0, 2.0, 2.0, 2.0]], dtype=np.float32)
+    pix_y = np.zeros_like(pix_x)
+    scores = endpoint_mismatch_scores(occupied, pix_x, pix_y)
+
+    # First candidate matches only the occupied cell. Unknown, free, and both
+    # out-of-bounds endpoints are mismatches; the second is a perfect match.
+    np.testing.assert_allclose(scores, [0.8, 0.0])

ros2_ws/src/mars_bot/mars_nav/test/test_map_metadata.py

@@ -0,0 +1,28 @@
+# SPDX-License-Identifier: Apache-2.0
+# Copyright (c) 2026 Innate Inc
+
+from mars_nav.map_metadata import normalize_legacy_trinary_metadata, repair_legacy_trinary_map
+
+
+def test_repairs_humble_trinary_threshold_without_reformatting(tmp_path):
+    original = """image: OfficeClean.pgm
+mode: trinary
+resolution: 0.05
+free_thresh: 0.25  # Nav2 Humble default
+occupied_thresh: 0.65
+"""
+    map_yaml = tmp_path / "OfficeClean.yaml"
+    map_yaml.write_text(original)
+
+    changed = repair_legacy_trinary_map(map_yaml)
+
+    assert changed is True
+    assert map_yaml.read_text() == original.replace("free_thresh: 0.25", "free_thresh: 0.196")
+
+
+def test_leaves_safe_or_non_trinary_maps_unchanged():
+    safe = "mode: trinary\nfree_thresh: 0.196\n"
+    scale = "mode: scale\nfree_thresh: 0.25\n"
+
+    assert normalize_legacy_trinary_metadata(safe) == (safe, False)
+    assert normalize_legacy_trinary_metadata(scale) == (scale, False)