Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
47 changes: 47 additions & 0 deletions backend/src/roboprobe/geometry/ros_map.py
Original file line number Diff line number Diff line change
@@ -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<cols>\d+)\s+(?P<rows>\d+)\s+(?P<maxval>\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)
43 changes: 11 additions & 32 deletions backend/src/roboprobe/scenario/map_io.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,17 +5,16 @@

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
import yaml

from roboprobe.core import errors
from roboprobe.geometry import OccupancyGrid
from roboprobe.geometry.ros_map import decode_ros_pgm

__all__ = [
"OccupancyIoU",
Expand All @@ -25,7 +24,6 @@
"write_ros_map",
]

_PGM_MAGIC = re.compile(rb"P5\s*\n(?:#[^\n]*\n)*(?P<dims>\d+)\s+(?P<rows>\d+)\s+(?P<maxval>\d+)\s")
_MAX_PIXEL = 255
_ORIGIN_LEN = 3

Expand All @@ -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:
Expand All @@ -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
Expand Down
Original file line number Diff line number Diff line change
@@ -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<cols>\d+)\s+(?P<rows>\d+)\s+(?P<maxval>\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)
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand Down
Loading