From 4966ed10e1dad166f15d77497e17db7816fe7acb Mon Sep 17 00:00:00 2001 From: dag00dag33 Date: Tue, 25 Nov 2025 16:05:16 +0100 Subject: [PATCH] Fix column-major GridMap input for masked_replace and align patch orientation --- .../elevation_mapping.py | 26 ++- .../scripts/elevation_mapping_node.py | 57 +++++-- scripts/masked_replace_tool.py | 160 +++++++++++++++--- 3 files changed, 204 insertions(+), 39 deletions(-) diff --git a/elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping.py b/elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping.py index 15584e4e..ee3e7832 100644 --- a/elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping.py +++ b/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: diff --git a/elevation_mapping_cupy/scripts/elevation_mapping_node.py b/elevation_mapping_cupy/scripts/elevation_mapping_node.py index ef427c49..41576717 100755 --- a/elevation_mapping_cupy/scripts/elevation_mapping_node.py +++ b/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: diff --git a/scripts/masked_replace_tool.py b/scripts/masked_replace_tool.py index 88266444..699c8852 100755 --- a/scripts/masked_replace_tool.py +++ b/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()