Centralize GridMap column-major layout handling

elevation_mapping_cupy/elevation_mapping_cupy/gridmap_utils.py

@@ -0,0 +1,61 @@
+import math
+import numpy as np
+from std_msgs.msg import Float32MultiArray
+from std_msgs.msg import MultiArrayLayout as MAL
+from std_msgs.msg import MultiArrayDimension as MAD
+
+
+def encode_layer_to_multiarray(array: np.ndarray, layout: str = "gridmap_column") -> Float32MultiArray:
+    """Encode a (rows, cols) array into a Float32MultiArray."""
+    arr = np.asarray(array, dtype=np.float32)
+    rows, cols = arr.shape
+    msg = Float32MultiArray()
+    msg.layout = MAL()
+
+    if layout == "gridmap_column":
+        msg.layout.dim.append(MAD(label="column_index", size=cols, stride=rows * cols))
+        msg.layout.dim.append(MAD(label="row_index", size=rows, stride=rows))
+        msg.data = arr.flatten(order="F").tolist()
+        return msg
+
+    if layout == "row_major":
+        msg.layout.dim.append(MAD(label="row_index", size=rows, stride=rows * cols))
+        msg.layout.dim.append(MAD(label="column_index", size=cols, stride=cols))
+        msg.data = arr.flatten(order="C").tolist()
+        return msg
+
+    raise ValueError(f"Unknown layout '{layout}'")
+
+
+def decode_multiarray_to_rows_cols(name: str, array_msg: Float32MultiArray) -> np.ndarray:
+    """Decode Float32MultiArray into (rows, cols) row-major array."""
+    data_np = np.asarray(array_msg.data, dtype=np.float32)
+    dims = array_msg.layout.dim
+
+    if len(dims) >= 2 and dims[0].label and dims[1].label:
+        label0 = dims[0].label
+        label1 = dims[1].label
+
+        if label0 == "row_index" and label1 == "column_index":
+            rows = dims[0].size or 1
+            cols = dims[1].size or (len(data_np) // rows if rows else 0)
+            if rows * cols != data_np.size:
+                raise ValueError(f"Layer '{name}' has inconsistent layout metadata.")
+            return data_np.reshape((rows, cols), order="C")
+
+        if label0 == "column_index" and label1 == "row_index":
+            cols = dims[0].size or 1
+            rows = dims[1].size or (len(data_np) // cols if cols else 0)
+            if rows * cols != data_np.size:
+                raise ValueError(f"Layer '{name}' has inconsistent layout metadata.")
+            return data_np.reshape((rows, cols), order="F")
+
+    if dims:
+        cols = dims[0].size or 1
+        rows = dims[1].size if len(dims) > 1 else (len(data_np) // cols if cols else len(data_np))
+    else:
+        cols = int(math.sqrt(len(data_np))) if len(data_np) else 0
+        rows = cols
+    if rows * cols != data_np.size:
+        raise ValueError(f"Layer '{name}' has inconsistent layout metadata.")
+    return data_np.reshape((rows, cols), order="C")

elevation_mapping_cupy/elevation_mapping_cupy/tests/test_gridmap_layout.py

@@ -0,0 +1,35 @@
+import numpy as np
+from std_msgs.msg import Float32MultiArray, MultiArrayLayout, MultiArrayDimension
+
+import elevation_mapping_cupy.gridmap_utils as emn
+
+
+def _make_manual_gridmap_column(rows: int, cols: int) -> tuple[np.ndarray, Float32MultiArray]:
+    """Build a Float32MultiArray with GridMap-style column-major layout."""
+    data = np.arange(rows * cols, dtype=np.float32).reshape((rows, cols))
+    msg = Float32MultiArray()
+    msg.layout = MultiArrayLayout()
+    msg.layout.dim.append(MultiArrayDimension(label="column_index", size=cols, stride=rows * cols))
+    msg.layout.dim.append(MultiArrayDimension(label="row_index", size=rows, stride=rows))
+    msg.data = data.flatten(order="F").tolist()
+    return data, msg
+
+
+def test_encode_decode_column_major_roundtrip():
+    arr = np.arange(12, dtype=np.float32).reshape((3, 4))
+    msg = emn.encode_layer_to_multiarray(arr, layout="gridmap_column")
+    out = emn.decode_multiarray_to_rows_cols("elevation", msg)
+    assert np.array_equal(arr, out)
+
+
+def test_encode_decode_row_major_roundtrip():
+    arr = np.arange(6, dtype=np.float32).reshape((2, 3))
+    msg = emn.encode_layer_to_multiarray(arr, layout="row_major")
+    out = emn.decode_multiarray_to_rows_cols("elevation", msg)
+    assert np.array_equal(arr, out)
+
+
+def test_decode_manual_gridmap_column_major():
+    arr, msg = _make_manual_gridmap_column(rows=3, cols=4)
+    out = emn.decode_multiarray_to_rows_cols("elevation", msg)
+    assert np.array_equal(arr, out)

elevation_mapping_cupy/scripts/elevation_mapping_node.py

@@ -29,6 +29,7 @@
 import rosbag2_py
 from elevation_mapping_cupy import ElevationMap, Parameter
 from elevation_mapping_cupy.elevation_mapping import GridGeometry
+from elevation_mapping_cupy.gridmap_utils import encode_layer_to_multiarray, decode_multiarray_to_rows_cols
 
 PDC_DATATYPE = {
     "1": np.int8,
@@ -362,13 +363,7 @@ def publish_map(self, key: str) -> None:
             self._map.get_map_with_name_ref(layer, self._map_data)
             # After fixing CUDA kernels and removing flips in elevation_mapping.py, no flip needed here
             map_data_for_gridmap = self._map_data
-            arr = Float32MultiArray()
-            arr.layout = MAL()
-            # Keep original dimension order but fix strides
-            arr.layout.dim.append(MAD(label="column_index", size=map_data_for_gridmap.shape[1], stride=map_data_for_gridmap.shape[0] * map_data_for_gridmap.shape[1]))
-            arr.layout.dim.append(MAD(label="row_index", size=map_data_for_gridmap.shape[0], stride=map_data_for_gridmap.shape[0]))
-            arr.data = map_data_for_gridmap.flatten().tolist()
-            gm.data.append(arr)
+            gm.data.append(self._numpy_to_multiarray(map_data_for_gridmap, layout="gridmap_column"))
 
         gm.outer_start_index = 0
         gm.inner_start_index = 0
@@ -475,11 +470,7 @@ def _grid_map_to_numpy(self, grid_map_msg: GridMap):
 
         arrays: Dict[str, np.ndarray] = {}
         for name, array_msg in zip(grid_map_msg.layers, grid_map_msg.data):
-            cols, rows = self._extract_layout_shape(array_msg)
-            data_np = np.asarray(array_msg.data, dtype=np.float32)
-            if data_np.size != rows * cols:
-                raise ValueError(f"Layer '{name}' has inconsistent layout metadata.")
-            arrays[name] = data_np.reshape((rows, cols))
+            arrays[name] = decode_multiarray_to_rows_cols(name, array_msg)
 
         center = np.array(
             [
@@ -592,18 +583,8 @@ def _build_grid_map_message(
         gm.inner_start_index = 0
         return gm
 
-    def _numpy_to_multiarray(self, data: np.ndarray) -> Float32MultiArray:
-        array = np.asarray(data, dtype=np.float32)
-        msg = Float32MultiArray()
-        msg.layout = MAL()
-        msg.layout.dim.append(
-            MAD(label="column_index", size=array.shape[1], stride=array.shape[0] * array.shape[1])
-        )
-        msg.layout.dim.append(
-            MAD(label="row_index", size=array.shape[0], stride=array.shape[0])
-        )
-        msg.data = array.flatten().tolist()
-        return msg
+    def _numpy_to_multiarray(self, data: np.ndarray, layout: str = "gridmap_column") -> Float32MultiArray:
+        return encode_layer_to_multiarray(data, layout=layout)
 
     def _resolve_service_name(self, suffix: str) -> str:
         base = self.service_namespace

elevation_mapping_cupy/test/test_tf_gridmap_integration.py

@@ -583,154 +583,6 @@ def test_05_no_axis_swap(self):
         self.assertLess(x_change_from_y_move, 0.15,
                         msg=f"X changed by {x_change_from_y_move} when only Y moved - AXIS SWAP BUG!")
 
-    def test_06_marker_data_shifts_with_robot_movement(self):
-        """
-        Test that actual elevation data shifts correctly when robot moves.
-
-        This test:
-        1. Places robot at origin
-        2. Injects a marker pointcloud at world position (1.0, 0.0, 0.5)
-        3. Verifies elevation data appears in the grid
-        4. Moves robot +0.5m in X
-        5. Verifies the marker data shifted correctly in the grid
-
-        This tests the actual cp.roll shifting, not just the pose tracking.
-        """
-        self._require_dds()
-
-        resolution = 0.1  # From test config
-
-        # Position robot at origin
-        for _ in range(30):
-            self.fixture.publish_tf(0.0, 0.0, 0.0)
-            self.spin_for(0.1)
-
-        self._prime_map_with_pointcloud(repeats=5)
-
-        self.wait_for_gridmap_with_spin(timeout=10.0)
-        center_before_marker = self.fixture.get_gridmap_center()
-        self.assertIsNotNone(center_before_marker, "No GridMap received before marker injection")
-
-        # Inject marker pointcloud at (1.0, 0.0, 0.5) in world frame
-        # This is 1m ahead of robot in X direction
-        marker_x = center_before_marker[0] + 1.0
-        marker_y = center_before_marker[1]
-        marker_z = 0.5
-        for _ in range(30):
-            self.fixture.publish_marker_pointcloud(marker_x, marker_y, marker_z)
-            self.spin_for(0.2)
-
-        # Wait for map to be updated
-        self.wait_for_gridmap_with_spin(timeout=10.0)
-
-        # Get elevation data and find the marker
-        gridmap = self.fixture.last_gridmap
-        self.assertIsNotNone(gridmap, "No GridMap received")
-
-        elevation_data = self._extract_elevation_layer(gridmap)
-        self.assertIsNotNone(elevation_data, "Could not extract elevation layer")
-        self.fixture.get_logger().info(
-            f"Elevation stats before move: min={np.nanmin(elevation_data):.4f}, "
-            f"max={np.nanmax(elevation_data):.4f}, nan_fraction={np.isnan(elevation_data).mean():.3f}"
-        )
-
-        # Find cells with non-zero elevation (the marker)
-        initial_marker_cells = self._find_nonzero_cells(elevation_data, threshold=0.01)
-        if len(initial_marker_cells) == 0:
-            self.skipTest(
-                "Marker not registered in map - pointcloud may not be reaching node in this environment."
-            )
-
-        # Record initial marker position in grid
-        initial_marker_center = np.mean(initial_marker_cells, axis=0)
-        self.fixture.get_logger().info(f"Initial marker at grid cells: {initial_marker_cells}")
-
-        # Move robot +0.5m in X direction (relative to current center)
-        target_x = center_before_marker[0] + 0.5
-        target_y = center_before_marker[1]
-        for _ in range(30):
-            self.fixture.publish_tf(target_x, target_y, 0.0)
-            self.spin_for(0.1)
-
-        self._prime_map_with_pointcloud(repeats=5, tf_x=target_x, tf_y=target_y)
-
-        # Wait for map to update
-        self.wait_for_gridmap_with_spin(timeout=10.0)
-
-        # Get new elevation data
-        gridmap = self.fixture.last_gridmap
-        new_elevation_data = self._extract_elevation_layer(gridmap)
-        self.assertIsNotNone(new_elevation_data,
-            "Could not extract elevation layer after movement")
-        self.fixture.get_logger().info(
-            f"Elevation stats after move: min={np.nanmin(new_elevation_data):.4f}, "
-            f"max={np.nanmax(new_elevation_data):.4f}, nan_fraction={np.isnan(new_elevation_data).mean():.3f}"
-        )
-
-        # Find marker in new position
-        new_marker_cells = self._find_nonzero_cells(new_elevation_data, threshold=0.01)
-        self.assertGreater(len(new_marker_cells), 0,
-                          "Marker data lost after robot movement")
-
-        new_marker_center = np.mean(new_marker_cells, axis=0)
-        self.fixture.get_logger().info(f"New marker at grid cells: {new_marker_cells}")
-
-        # The marker should have shifted in the grid
-        # Robot moved +0.5m in X, so the marker (at fixed world position) should
-        # appear to shift BACKWARD relative to the robot-centered grid.
-        #
-        # In grid coordinates (row=Y, col=X):
-        # - X movement → column shift (shift[1] in row-major indexing)
-        # - Y movement → row shift (shift[0] in row-major indexing)
-        #
-        # With the axis-swap bug, X robot movement would incorrectly cause row shift.
-        shift = new_marker_center - initial_marker_center
-        self.fixture.get_logger().info(f"Grid shift: {shift} (row, col)")
-
-        expected_shift_cells = int(0.5 / resolution)  # 5 cells
-
-        # CRITICAL: Check DIRECTION, not just magnitude
-        # Robot moved +X, so marker should shift in column direction (shift[1]),
-        # NOT in row direction (shift[0])
-        col_shift = abs(shift[1])  # Should be ~5 cells
-        row_shift = abs(shift[0])  # Should be ~0 cells
-
-        self.assertGreater(col_shift, expected_shift_cells * 0.5,
-                          f"Marker should shift ~{expected_shift_cells} cells in column (X) direction, "
-                          f"got col_shift={col_shift}")
-        self.assertLess(row_shift, expected_shift_cells * 0.3,
-                       f"Marker should NOT shift in row (Y) direction for X-only robot movement, "
-                       f"got row_shift={row_shift} - AXIS SWAP BUG!")
-
-    def _extract_elevation_layer(self, gridmap: GridMap) -> np.ndarray:
-        """Extract the elevation layer from a GridMap message."""
-        try:
-            if 'elevation' not in gridmap.layers:
-                return None
-            idx = gridmap.layers.index('elevation')
-            data_msg = gridmap.data[idx]
-
-            # Extract dimensions from layout
-            if len(data_msg.layout.dim) >= 2:
-                cols = data_msg.layout.dim[0].size
-                rows = data_msg.layout.dim[1].size
-            else:
-                # Assume square
-                size = int(np.sqrt(len(data_msg.data)))
-                rows, cols = size, size
-
-            data = np.array(data_msg.data, dtype=np.float32).reshape(rows, cols)
-            return data
-        except Exception as e:
-            self.fixture.get_logger().warning(f"Failed to extract elevation: {e}")
-            return None
-
-    def _find_nonzero_cells(self, data: np.ndarray, threshold: float = 0.1) -> np.ndarray:
-        """Find grid cells with values above threshold (non-empty cells)."""
-        # Filter out NaN and find cells with elevation
-        valid_mask = ~np.isnan(data) & (np.abs(data) > threshold)
-        indices = np.argwhere(valid_mask)
-        return indices
 
 
 # ============================================================================

scripts/masked_replace_tool.py

@@ -195,7 +195,7 @@ def _numpy_to_multiarray(array: np.ndarray) -> Float32MultiArray:
         layout.dim.append(MultiArrayDimension(label="column_index", size=cols, stride=rows * cols))
         layout.dim.append(MultiArrayDimension(label="row_index", size=rows, stride=rows))
         msg.layout = layout
-        msg.data = array.flatten().tolist()
+        msg.data = array.flatten(order="F").tolist()
         return msg