diff --git a/backend/src/roboprobe/geometry/ros_map.py b/backend/src/roboprobe/geometry/ros_map.py new file mode 100644 index 00000000..82ecd5a5 --- /dev/null +++ b/backend/src/roboprobe/geometry/ros_map.py @@ -0,0 +1,47 @@ +"""Decode Nav2 PGM occupancy pixels for the kinematic and ROS distance kernels.""" + +from __future__ import annotations + +import re + +import numpy as np + +from roboprobe.geometry.grid import OccupancyGrid + +__all__ = ["decode_ros_pgm"] + +_MAX_PIXEL = 255 +_HEADER = re.compile( + rb"P5\s*\n(?:#[^\n]*\n)*(?P\d+)\s+(?P\d+)\s+(?P\d+)(?:\r\n|\s)" +) + + +def decode_ros_pgm( + raw: bytes, + *, + resolution: float, + origin: tuple[float, float], + free_thresh: float, + negate: bool, +) -> OccupancyGrid: + """Build a metric grid from an 8-bit binary PGM in Nav2 trinary mode. + + PGM rows count from the top of the image. Grid rows count from the lower + world coordinate. Unknown cells, like occupied cells, constrain clearance. + """ + header = _HEADER.match(raw) + if header is None or int(header["maxval"]) != _MAX_PIXEL: + msg = "expected an 8-bit binary P5 PGM" + raise ValueError(msg) + cols, rows = int(header["cols"]), int(header["rows"]) + pixels = np.frombuffer(raw, dtype=np.uint8, offset=header.end()) + if cols < 1 or rows < 1 or pixels.size != cols * rows: + msg = "PGM pixel payload size does not match its dimensions" + raise ValueError(msg) + if not 0.0 <= free_thresh <= 1.0: + msg = "free_thresh must be between zero and one" + raise ValueError(msg) + + gray = pixels.reshape(rows, cols).astype(np.float64) / _MAX_PIXEL + probability = gray if negate else 1.0 - gray + return OccupancyGrid(np.flipud(probability >= free_thresh), resolution, origin=origin) diff --git a/backend/src/roboprobe/scenario/map_io.py b/backend/src/roboprobe/scenario/map_io.py index 8b5c0492..4a869665 100644 --- a/backend/src/roboprobe/scenario/map_io.py +++ b/backend/src/roboprobe/scenario/map_io.py @@ -5,10 +5,8 @@ from __future__ import annotations -import re from dataclasses import dataclass from pathlib import Path -from typing import Any import numpy as np import numpy.typing as npt @@ -16,6 +14,7 @@ from roboprobe.core import errors from roboprobe.geometry import OccupancyGrid +from roboprobe.geometry.ros_map import decode_ros_pgm __all__ = [ "OccupancyIoU", @@ -25,7 +24,6 @@ "write_ros_map", ] -_PGM_MAGIC = re.compile(rb"P5\s*\n(?:#[^\n]*\n)*(?P\d+)\s+(?P\d+)\s+(?P\d+)\s") _MAX_PIXEL = 255 _ORIGIN_LEN = 3 @@ -39,31 +37,6 @@ class OccupancyIoU: samples: int -def _read_pgm(path: Path) -> npt.NDArray[np.uint8]: - raw = path.read_bytes() - match = _PGM_MAGIC.match(raw) - if match is None or int(match.group("maxval")) != _MAX_PIXEL: - msg = f"{path}: not an 8-bit binary P5 PGM" - raise errors.ValidationError(msg, context={"path": str(path)}) - start = match.end() - width = int(match.group("dims")) - rows = int(match.group("rows")) - pixels = np.frombuffer(raw, dtype=np.uint8, offset=start) - if width < 1 or rows < 1 or pixels.size != width * rows: - msg = f"{path}: PGM pixel payload size does not match its dimensions" - raise errors.ValidationError(msg, context={"path": str(path), "expected": width * rows}) - return pixels.reshape(rows, width) - - -def _threshold_occupied( - meta: dict[str, Any], values: npt.NDArray[np.uint8] -) -> npt.NDArray[np.bool_]: - gray = values.astype(np.float64) / _MAX_PIXEL - probability = gray if int(meta.get("negate", 0)) else 1.0 - gray - # Only cells below the free threshold are traversable, including for a trinary map. - return probability >= float(meta["free_thresh"]) - - def load_ros_map(yaml_path: Path) -> OccupancyGrid: """Load a ROS map YAML and its referenced PGM image.""" try: @@ -83,15 +56,21 @@ def load_ros_map(yaml_path: Path) -> OccupancyGrid: if not image_path.is_relative_to(yaml_path.parent.resolve()): msg = f"{yaml_path}: image must stay inside the map directory" raise errors.ValidationError(msg) - values = _read_pgm(image_path) resolution = float(meta["resolution"]) origin = meta["origin"] if not isinstance(origin, list) or len(origin) != _ORIGIN_LEN or origin[2] != 0: msg = f"{yaml_path}: origin must be [x, y, 0]" raise errors.ValidationError(msg, context={"path": str(yaml_path)}) - # PGM is top-first. OccupancyGrid row zero is the lowest world y. - occupied = np.flipud(_threshold_occupied(meta, values)) - return OccupancyGrid(occupied, resolution, origin=(float(origin[0]), float(origin[1]))) + try: + return decode_ros_pgm( + image_path.read_bytes(), + resolution=resolution, + origin=(float(origin[0]), float(origin[1])), + free_thresh=float(meta["free_thresh"]), + negate=bool(int(meta.get("negate", 0))), + ) + except ValueError as exc: + raise errors.ValidationError(str(exc), context={"path": str(image_path)}) from exc def write_ros_map( # noqa: PLR0913 diff --git a/ros_ws/src/roboprobe_ros/roboprobe_ros/_vendor/geometry/ros_map.py b/ros_ws/src/roboprobe_ros/roboprobe_ros/_vendor/geometry/ros_map.py new file mode 100644 index 00000000..ca316d29 --- /dev/null +++ b/ros_ws/src/roboprobe_ros/roboprobe_ros/_vendor/geometry/ros_map.py @@ -0,0 +1,48 @@ +# Generated by scripts/vendor_geometry.py. Edit the backend source. +"""Decode Nav2 PGM occupancy pixels for the kinematic and ROS distance kernels.""" + +from __future__ import annotations + +import re + +import numpy as np + +from roboprobe_ros._vendor.geometry.grid import OccupancyGrid + +__all__ = ["decode_ros_pgm"] + +_MAX_PIXEL = 255 +_HEADER = re.compile( + rb"P5\s*\n(?:#[^\n]*\n)*(?P\d+)\s+(?P\d+)\s+(?P\d+)(?:\r\n|\s)" +) + + +def decode_ros_pgm( + raw: bytes, + *, + resolution: float, + origin: tuple[float, float], + free_thresh: float, + negate: bool, +) -> OccupancyGrid: + """Build a metric grid from an 8-bit binary PGM in Nav2 trinary mode. + + PGM rows count from the top of the image. Grid rows count from the lower + world coordinate. Unknown cells, like occupied cells, constrain clearance. + """ + header = _HEADER.match(raw) + if header is None or int(header["maxval"]) != _MAX_PIXEL: + msg = "expected an 8-bit binary P5 PGM" + raise ValueError(msg) + cols, rows = int(header["cols"]), int(header["rows"]) + pixels = np.frombuffer(raw, dtype=np.uint8, offset=header.end()) + if cols < 1 or rows < 1 or pixels.size != cols * rows: + msg = "PGM pixel payload size does not match its dimensions" + raise ValueError(msg) + if not 0.0 <= free_thresh <= 1.0: + msg = "free_thresh must be between zero and one" + raise ValueError(msg) + + gray = pixels.reshape(rows, cols).astype(np.float64) / _MAX_PIXEL + probability = gray if negate else 1.0 - gray + return OccupancyGrid(np.flipud(probability >= free_thresh), resolution, origin=origin) diff --git a/ros_ws/src/roboprobe_ros/roboprobe_ros/_vendor/manifest.json b/ros_ws/src/roboprobe_ros/roboprobe_ros/_vendor/manifest.json index 65f6e3a3..65bad29f 100644 --- a/ros_ws/src/roboprobe_ros/roboprobe_ros/_vendor/manifest.json +++ b/ros_ws/src/roboprobe_ros/roboprobe_ros/_vendor/manifest.json @@ -5,6 +5,7 @@ "geometry/footprint.py": "dcb0db40949b5e845d716597d1b22713e6d9059f06740bce04a2546c33aeb22d", "geometry/grid.py": "f959b70d481ee992cd967e9bbb87dd698de69719dca3686704b19da10a8fda62", "geometry/raycast.py": "350cdcf0d5f8c336274f8d2070edd347e6dd2cb47a087583fa40394cdf975e63", + "geometry/ros_map.py": "9bfeebc360fe8f4f47b45049846d0cc6d82f9ae401eda91225620e30e4399fc3", "perturbation/__init__.py": "4bdad240885a5daeee5996dd434bcbeae129239160ed4c923e3606f9ef1afb36", "perturbation/delay.py": "c8c218b7663320cd6843194f3a7c8001deb8ce065c1a3bbdedfde3d11b2f359b", "perturbation/friction.py": "4c0b24fac6e2b1419a9dd0ba08c0f0206f625867e38e5d1678f3e14403d5b777",