Fix column-major GridMap input for masked_replace and align patch orientation

Author: DAG00DAG33

elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping.py

@@ -1092,12 +1092,13 @@ def apply_masked_replace(
                 if np.any(valid_mask):
                     vals = incoming_slice[valid_mask]
                     min_max = (float(np.nanmin(vals)), float(np.nanmax(vals)))
-                map_extent = self._map_extent_from_slices(map_rows, map_cols)
+                map_extent = self._map_extent_from_mask(map_rows, map_cols, valid_mask) or self._map_extent_from_slices(map_rows, map_cols)
                 print(
                     f"[ElevationMap] masked_replace layer '{name}': wrote {written} cells, "
                     f"X∈[{map_extent['x_min']:.2f},{map_extent['x_max']:.2f}], "
                     f"Y∈[{map_extent['y_min']:.2f},{map_extent['y_max']:.2f}], "
-                    f"values {min_max if min_max else 'n/a'}"
+                    f"values {min_max if min_max else 'n/a'}",
+                    flush=True
                 )
 
         self._invalidate_caches()
@@ -1245,6 +1246,27 @@ def _map_extent_from_slices(self, rows: slice, cols: slice) -> Dict[str, float]:
         y_max = map_min_y + (rows.stop - 0.5) * self.resolution
         return {"x_min": x_min, "x_max": x_max, "y_min": y_min, "y_max": y_max}
 
+    def _map_extent_from_mask(self, rows: slice, cols: slice, valid_mask: np.ndarray) -> Optional[Dict[str, float]]:
+        """Compute extent based on the actual mask footprint; returns None if mask is empty."""
+        if valid_mask is None or not np.any(valid_mask):
+            return None
+        row_idx, col_idx = np.nonzero(valid_mask)
+        row_min = rows.start + int(row_idx.min())
+        row_max = rows.start + int(row_idx.max())
+        col_min = cols.start + int(col_idx.min())
+        col_max = cols.start + int(col_idx.max())
+
+        map_length = (self.cell_n - 2) * self.resolution
+        center_cpu = np.asarray(cp.asnumpy(self.center))
+        map_min_x = center_cpu[0] - map_length / 2.0
+        map_min_y = center_cpu[1] - map_length / 2.0
+
+        x_min = map_min_x + (col_min + 0.5) * self.resolution
+        x_max = map_min_x + (col_max + 0.5) * self.resolution
+        y_min = map_min_y + (row_min + 0.5) * self.resolution
+        y_max = map_min_y + (row_max + 0.5) * self.resolution
+        return {"x_min": x_min, "x_max": x_max, "y_min": y_min, "y_max": y_max}
+
     def _invalidate_caches(self, reset_plugins: bool = True):
         self.traversability_buffer[...] = cp.nan
         if reset_plugins:

elevation_mapping_cupy/scripts/elevation_mapping_node.py

@@ -469,17 +469,51 @@ def handle_load_map(self, request, response):
             response.success = False
         return response
 
+    def _float32_multiarray_to_numpy(self, name: str, array_msg: Float32MultiArray) -> np.ndarray:
+        """Convert a Float32MultiArray to a numpy array according to the layout labels."""
+        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
+            # self.get_logger().info(f"Layer '{name}' has labels: {label0} and {label1}")
+            if label0 == "row_index" and label1 == "column_index":
+                # Data is in row-major order
+                rows = dims[0].size or 1
+                cols = dims[1].size or (len(data_np) // rows if rows else 0)
+                expected = rows * cols
+                if expected != 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":
+                # Data is in column-major order
+                # We need to flip both axes, then transpose to swap X/Y into our row-major (row=Y, col=X) expectation.
+                cols = dims[0].size or 1
+                rows = dims[1].size or (len(data_np) // cols if cols else 0)
+                expected = rows * cols
+                if expected != data_np.size:
+                    raise ValueError(f"Layer '{name}' has inconsistent layout metadata.")
+                array = data_np.reshape((rows, cols), order="F")
+                # Align to internal row-major convention:
+                # Flip both axes, then transpose to swap X/Y into our row-major (row=Y, col=X) expectation.
+                array = np.flip(array, axis=0)
+                array = np.flip(array, axis=1)
+                array = array.T
+                return array
+
+        cols, rows = self._extract_layout_shape(array_msg)
+        if data_np.size != rows * cols:
+            raise ValueError(f"Layer '{name}' has inconsistent layout metadata.")
+        return data_np.reshape((rows, cols))
+
     def _grid_map_to_numpy(self, grid_map_msg: GridMap):
         if len(grid_map_msg.layers) != len(grid_map_msg.data):
             raise ValueError("Mismatch between GridMap layers and data arrays.")
 
         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] = self._float32_multiarray_to_numpy(name, array_msg)
 
         center = np.array(
             [
@@ -593,16 +627,15 @@ def _build_grid_map_message(
         return gm
 
     def _numpy_to_multiarray(self, data: np.ndarray) -> Float32MultiArray:
+        """Convert a 2D numpy array to Float32MultiArray honoring row-major layout labels."""
         array = np.asarray(data, dtype=np.float32)
+        rows, cols = array.shape
         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()
+        # Internal representation is always row-major: first dim is row_index, second is column_index.
+        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 = array.flatten(order="C").tolist()
         return msg
 
     def _resolve_service_name(self, suffix: str) -> str:

scripts/masked_replace_tool.py

@@ -46,6 +46,30 @@ def build_parser() -> argparse.ArgumentParser:
     parser.add_argument("--center-z", type=float, default=0.0, help="Patch center Z coordinate (meters).")
     parser.add_argument("--size-x", type=positive_float, default=1.0, help="Patch length in X (meters).")
     parser.add_argument("--size-y", type=positive_float, default=1.0, help="Patch length in Y (meters).")
+    parser.add_argument(
+        "--full-length-x",
+        type=positive_float,
+        default=None,
+        help="Optional total GridMap length in X (meters). If set, a full-size map is sent and only the patch region is marked in the mask."
+    )
+    parser.add_argument(
+        "--full-length-y",
+        type=positive_float,
+        default=None,
+        help="Optional total GridMap length in Y (meters). If set, a full-size map is sent and only the patch region is marked in the mask."
+    )
+    parser.add_argument(
+        "--full-center-x",
+        type=float,
+        default=0.0,
+        help="GridMap center X (meters) to use when sending a full-size map. Defaults to 0."
+    )
+    parser.add_argument(
+        "--full-center-y",
+        type=float,
+        default=0.0,
+        help="GridMap center Y (meters) to use when sending a full-size map. Defaults to 0."
+    )
     parser.add_argument("--resolution", type=positive_float, default=0.1, help="Grid resolution (meters per cell).")
     parser.add_argument("--elevation", type=float, default=0.1, help="Elevation value to set (meters).")
     parser.add_argument("--variance", type=non_negative_float, default=0.05, help="Variance value to set.")
@@ -84,6 +108,10 @@ class PatchConfig:
     mask_value: float
     add_valid_layer: bool
     invalidate_first: bool
+    full_length_x: Optional[float] = None
+    full_length_y: Optional[float] = None
+    full_center_x: float = 0.0
+    full_center_y: float = 0.0
 
     @property
     def shape(self) -> Dict[str, int]:
@@ -139,10 +167,17 @@ def _base_grid_map(self) -> GridMap:
         gm.header.frame_id = cfg.frame_id
         gm.header.stamp = self.get_clock().now().to_msg()
         gm.info.resolution = cfg.resolution
-        gm.info.length_x = cfg.actual_length_x
-        gm.info.length_y = cfg.actual_length_y
-        gm.info.pose.position.x = cfg.center_x
-        gm.info.pose.position.y = cfg.center_y
+        # If full map was requested, use the full lengths and center the GridMap at the full-map center.
+        if cfg.full_length_x or cfg.full_length_y:
+            gm.info.length_x = cfg.full_length_x or cfg.actual_length_x
+            gm.info.length_y = cfg.full_length_y or cfg.actual_length_y
+            gm.info.pose.position.x = cfg.full_center_x
+            gm.info.pose.position.y = cfg.full_center_y
+        else:
+            gm.info.length_x = cfg.actual_length_x
+            gm.info.length_y = cfg.actual_length_y
+            gm.info.pose.position.x = cfg.center_x
+            gm.info.pose.position.y = cfg.center_y
         gm.info.pose.position.z = cfg.center_z
         gm.info.pose.orientation.w = 1.0
         gm.basic_layers = ["elevation"]
@@ -157,45 +192,116 @@ def _mask_array(self, force_value: Optional[float] = None) -> np.ndarray:
             mask_value = 1.0
         return np.full((rows, cols), mask_value, dtype=np.float32)
 
+    def _make_full_arrays(self) -> Dict[str, np.ndarray]:
+        """Create full-size arrays (possibly larger than the patch) and place the patch in them."""
+        cfg = self._config
+        length_x = cfg.full_length_x or cfg.length_x
+        length_y = cfg.full_length_y or cfg.length_y
+        cols_full = max(1, ceil(length_x / cfg.resolution))
+        rows_full = max(1, ceil(length_y / cfg.resolution))
+
+        # Base arrays
+        mask_full = np.full((rows_full, cols_full), np.nan, dtype=np.float32)
+        elev_full = np.full((rows_full, cols_full), np.nan, dtype=np.float32)
+        var_full = np.full((rows_full, cols_full), np.nan, dtype=np.float32)
+        valid_full = np.zeros((rows_full, cols_full), dtype=np.float32)
+
+        # Patch arrays
+        patch_rows = cfg.shape["rows"]
+        patch_cols = cfg.shape["cols"]
+        row_offset = int(round(cfg.center_y / cfg.resolution))
+        col_offset = int(round(cfg.center_x / cfg.resolution))
+        row_start = rows_full // 2 + row_offset - patch_rows // 2
+        col_start = cols_full // 2 + col_offset - patch_cols // 2
+        row_end = row_start + patch_rows
+        col_end = col_start + patch_cols
+
+        # Safety: clamp if window would exceed bounds
+        if row_start < 0 or col_start < 0 or row_end > rows_full or col_end > cols_full:
+            raise ValueError("Patch exceeds full map bounds; adjust center/size or full map length.")
+
+        mask_val = cfg.mask_value
+        if np.isnan(mask_val):
+            mask_val = 1.0
+        mask_full[row_start:row_end, col_start:col_end] = mask_val
+        elev_full[row_start:row_end, col_start:col_end] = cfg.elevation
+        var_full[row_start:row_end, col_start:col_end] = cfg.variance
+        if cfg.add_valid_layer:
+            valid_full[row_start:row_end, col_start:col_end] = 1.0
+
+        return {
+            "mask": mask_full,
+            "elevation": elev_full,
+            "variance": var_full,
+            "is_valid": valid_full,
+            "rows_full": rows_full,
+            "cols_full": cols_full,
+        }
+
     def _build_validity_message(self, value: float) -> GridMap:
         gm = self._base_grid_map()
-        mask = self._mask_array()
-        rows, cols = mask.shape
-        gm.layers = [self._config.mask_layer, "is_valid"]
-        arrays = {
-            self._config.mask_layer: mask,
-            "is_valid": np.full((rows, cols), value, dtype=np.float32),
-        }
+        if self._config.full_length_x or self._config.full_length_y:
+            arrays_full = self._make_full_arrays()
+            gm.info.length_x = self._config.full_length_x or self._config.length_x
+            gm.info.length_y = self._config.full_length_y or self._config.length_y
+            gm.layers = [self._config.mask_layer, "is_valid"]
+            arrays = {
+                self._config.mask_layer: arrays_full["mask"],
+                "is_valid": np.full((arrays_full["rows_full"], arrays_full["cols_full"]), value, dtype=np.float32),
+            }
+        else:
+            mask = self._mask_array()
+            rows, cols = mask.shape
+            gm.layers = [self._config.mask_layer, "is_valid"]
+            arrays = {
+                self._config.mask_layer: mask,
+                "is_valid": np.full((rows, cols), value, dtype=np.float32),
+            }
         for layer in gm.layers:
             gm.data.append(self._numpy_to_multiarray(arrays[layer]))
         return gm
 
     def _build_data_message(self, valid_value: Optional[float]) -> GridMap:
         gm = self._base_grid_map()
-        mask = self._mask_array()
-        rows, cols = mask.shape
-        gm.layers = [self._config.mask_layer, "elevation", "variance"]
-        arrays = {
-            self._config.mask_layer: mask,
-            "elevation": np.full((rows, cols), self._config.elevation, dtype=np.float32),
-            "variance": np.full((rows, cols), self._config.variance, dtype=np.float32),
-        }
-        if valid_value is not None:
-            gm.layers.append("is_valid")
-            arrays["is_valid"] = np.full((rows, cols), valid_value, dtype=np.float32)
+        if self._config.full_length_x or self._config.full_length_y:
+            arrays_full = self._make_full_arrays()
+            gm.info.length_x = self._config.full_length_x or self._config.length_x
+            gm.info.length_y = self._config.full_length_y or self._config.length_y
+            gm.layers = [self._config.mask_layer, "elevation", "variance"]
+            arrays = {
+                self._config.mask_layer: arrays_full["mask"],
+                "elevation": arrays_full["elevation"],
+                "variance": arrays_full["variance"],
+            }
+            if valid_value is not None:
+                gm.layers.append("is_valid")
+                arrays["is_valid"] = arrays_full["is_valid"]
+        else:
+            mask = self._mask_array()
+            rows, cols = mask.shape
+            gm.layers = [self._config.mask_layer, "elevation", "variance"]
+            arrays = {
+                self._config.mask_layer: mask,
+                "elevation": np.full((rows, cols), self._config.elevation, dtype=np.float32),
+                "variance": np.full((rows, cols), self._config.variance, dtype=np.float32),
+            }
+            if valid_value is not None:
+                gm.layers.append("is_valid")
+                arrays["is_valid"] = np.full((rows, cols), valid_value, dtype=np.float32)
         for layer in gm.layers:
             gm.data.append(self._numpy_to_multiarray(arrays[layer]))
         return gm
 
     @staticmethod
     def _numpy_to_multiarray(array: np.ndarray) -> Float32MultiArray:
+        """Build a Float32MultiArray with explicit row-major layout labels."""
         msg = Float32MultiArray()
         layout = MultiArrayLayout()
         rows, cols = array.shape
-        layout.dim.append(MultiArrayDimension(label="column_index", size=cols, stride=rows * cols))
-        layout.dim.append(MultiArrayDimension(label="row_index", size=rows, stride=rows))
+        layout.dim.append(MultiArrayDimension(label="row_index", size=rows, stride=rows * cols))
+        layout.dim.append(MultiArrayDimension(label="column_index", size=cols, stride=cols))
         msg.layout = layout
-        msg.data = array.flatten().tolist()
+        msg.data = array.flatten(order="C").tolist()
         return msg
 
 
@@ -216,6 +322,10 @@ def main() -> None:
         mask_value=args.mask_value,
         add_valid_layer=args.valid_layer,
         invalidate_first=args.invalidate_first,
+        full_length_x=args.full_length_x,
+        full_length_y=args.full_length_y,
+        full_center_x=args.full_center_x,
+        full_center_y=args.full_center_y,
     )
 
     rclpy.init()