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
4 changes: 4 additions & 0 deletions .env.example
Original file line number Diff line number Diff line change
Expand Up @@ -221,6 +221,10 @@
# ROBOPROBE_BACKENDS__GAZEBO_NAV2__PERCEPTION_MODEL_FILE=configs/perception/oakd_lowlight_v1.yaml

# --- scenario ---
# Minimum occupied-cell IoU between kinematic2d and Gazebo maps.
# ROBOPROBE_SCENARIO__MAP_ALIGNMENT__MIN_IOU=0.95
# Allowed relative difference between backend nominal path lengths.
# ROBOPROBE_SCENARIO__MAP_ALIGNMENT__PATH_LENGTH_TOLERANCE_RATIO=0.02
# Sampler draws before rejection sampling of constrained spaces gives up.
# ROBOPROBE_SCENARIO__CONSTRAINT_MAX_ATTEMPTS=1000
# Largest absolute integer exponent allowed in a constraint (TM-03).
Expand Down
6 changes: 6 additions & 0 deletions .github/workflows/ci.yml
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,12 @@ jobs:
echo "::error::Resolve TBD_T091 before merging (T-091)"
exit 1
fi
- name: No TBD_T092 placeholders remain (T-092)
run: |
if grep -R "TBD_T092" tasks configs; then
echo "::error::Resolve TBD_T092 in tasks/configs before merging (T-092)"
exit 1
fi
- name: mypy --strict, tsc
run: make typecheck
- name: Every test has a tier marker
Expand Down
1 change: 1 addition & 0 deletions CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@ versioning: [Semantic Versioning 2.0.0](https://semver.org/) (policy in `docs/33

### Added

- Warehouse map export, Gazebo reference map, nominal path artefacts, and map alignment tooling (T-092).
- Verified TB4 Nav2 topic map (`ros_ws/src/roboprobe_ros/config/topics.yaml`), JSON Schema, and evidence capture script (T-091).
- Playwright E2E harness and journeys (T-117, T-118, T-114): compose-backed global setup with
seeded warehouse campaign, axe helper and page objects, journey specs, `e2e.yml`, Lighthouse
Expand Down
19 changes: 18 additions & 1 deletion backend/src/roboprobe/core/settings/scenario.py
Original file line number Diff line number Diff line change
Expand Up @@ -7,12 +7,29 @@

from roboprobe.core.settings._base import SettingsSection

__all__ = ["ScenarioSettings"]
__all__ = ["MapAlignmentSettings", "ScenarioSettings"]


class MapAlignmentSettings(SettingsSection):
"""Warehouse map alignment gates for ``warehouse_v1`` (docs/06 §1, T-092)."""

min_iou: float = Field(
default=0.95,
ge=0.0,
le=1.0,
description="Minimum occupied-cell IoU between kinematic2d and Gazebo maps.",
)
path_length_tolerance_ratio: float = Field(
default=0.02,
ge=0.0,
description="Allowed relative difference between backend nominal path lengths.",
)


class ScenarioSettings(SettingsSection):
"""Scenario sampling settings shared by every strategy."""

map_alignment: MapAlignmentSettings = Field(default_factory=MapAlignmentSettings)
constraint_max_attempts: int = Field(
default=1000,
ge=1,
Expand Down
186 changes: 186 additions & 0 deletions backend/src/roboprobe/scenario/map_io.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,186 @@
"""ROS ``map_server`` YAML/PGM load and write plus occupancy grid alignment (docs/10 §7.1, T-092).

Implements: FR-SIM-003, FR-SCN-002, T-092.
"""

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

__all__ = [
"OccupancyIoU",
"align_occupancy",
"load_ros_map",
"occupancy_iou",
"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")
_MIN_ORIGIN_LEN = 2


@dataclass(frozen=True, slots=True)
class OccupancyIoU:
"""Intersection-over-union of occupied cells on a shared sampling grid."""

iou: float
agreement: float
samples: int


def _read_pgm(path: Path) -> npt.NDArray[np.uint8]:
raw = path.read_bytes()
match = _PGM_MAGIC.search(raw)
if match is None:
msg = f"{path}: not a 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[start:], dtype=np.uint8, count=width * rows)
if pixels.size != width * rows:
msg = f"{path}: PGM pixel payload truncated"
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_]:
mode = str(meta.get("mode", "trinary"))
_ = float(meta["free_thresh"])
occupied_thresh = float(meta["occupied_thresh"])
if mode == "trinary":
return values >= int(np.ceil(occupied_thresh * 255))
scaled = values.astype(np.float64) / 255.0
return scaled > occupied_thresh


def load_ros_map(yaml_path: Path) -> OccupancyGrid:
"""Load a ROS map YAML and its referenced PGM image."""
try:
text = yaml_path.read_text(encoding="utf-8")
except FileNotFoundError as exc:
msg = f"map YAML not found: {yaml_path}"
raise errors.NotFoundError(msg, context={"path": str(yaml_path)}) from exc
meta = yaml.safe_load(text)
if not isinstance(meta, dict):
msg = f"{yaml_path}: map metadata must be a mapping"
raise errors.ValidationError(msg, context={"path": str(yaml_path)})
image_name = meta.get("image")
if not isinstance(image_name, str) or not image_name:
msg = f"{yaml_path}: missing image"
raise errors.ValidationError(msg, context={"path": str(yaml_path)})
image_path = (yaml_path.parent / image_name).resolve()
values = _read_pgm(image_path)
rows, cols = values.shape
height = int(meta["height"]) if "height" in meta else rows
width = int(meta["width"]) if "width" in meta else cols
if rows != height or cols != width:
meta = dict(meta)
meta["height"] = rows
meta["width"] = cols
resolution = float(meta["resolution"])
origin = meta["origin"]
if not isinstance(origin, list) or len(origin) < _MIN_ORIGIN_LEN:
msg = f"{yaml_path}: origin must be [x, y, yaw]"
raise errors.ValidationError(msg, context={"path": str(yaml_path)})
occupied = _threshold_occupied(meta, values)
return OccupancyGrid(occupied, resolution, origin=(float(origin[0]), float(origin[1])))


def write_ros_map( # noqa: PLR0913
yaml_path: Path,
grid: OccupancyGrid,
*,
image_name: str | None = None,
mode: str = "trinary",
free_thresh: float = 0.1,
occupied_thresh: float = 0.65,
) -> None:
"""Write ROS map metadata and a binary PGM next to ``yaml_path``."""
yaml_path.parent.mkdir(parents=True, exist_ok=True)
pgm_name = image_name or f"{yaml_path.stem}.pgm"
pgm_path = yaml_path.parent / pgm_name
rows, cols = grid.shape
if mode == "trinary":
pixels = np.where(grid.occupied, 254, 0).astype(np.uint8)
else:
pixels = np.where(grid.occupied, 255, 0).astype(np.uint8)
header = f"P5\n# RoboProbe map export (T-092)\n{cols} {rows}\n255\n".encode()
pgm_path.write_bytes(header + pixels.tobytes())
document = {
"image": pgm_name,
"mode": mode,
"resolution": float(grid.resolution),
"origin": [float(grid.origin[0]), float(grid.origin[1]), 0.0],
"negate": 0,
"occupied_thresh": occupied_thresh,
"free_thresh": free_thresh,
"height": rows,
"width": cols,
}
yaml_path.write_text(yaml.safe_dump(document, sort_keys=False), encoding="utf-8")


def align_occupancy( # noqa: PLR0913
source: OccupancyGrid,
*,
resolution: float,
origin: tuple[float, float],
rows: int,
cols: int,
outside_occupied: bool = True,
) -> npt.NDArray[np.bool_]:
"""Rasterise ``source`` onto a target lattice."""
xs = origin[0] + (np.arange(cols) + 0.5) * resolution
ys = origin[1] + (np.arange(rows) + 0.5) * resolution
xx, yy = np.meshgrid(xs, ys)
points = np.column_stack([xx.ravel(), yy.ravel()])
cells = source.world_to_cell(points).reshape(rows, cols, 2)
inside = (
(cells[..., 0] >= 0)
& (cells[..., 0] < source.shape[0])
& (cells[..., 1] >= 0)
& (cells[..., 1] < source.shape[1])
)
row = np.clip(cells[..., 0], 0, source.shape[0] - 1)
col = np.clip(cells[..., 1], 0, source.shape[1] - 1)
occupied = source.occupied[row, col] & inside
if outside_occupied:
occupied = occupied | ~inside
return occupied


def occupancy_iou(left: OccupancyGrid, right: OccupancyGrid) -> OccupancyIoU:
"""IoU after aligning both grids on the finer resolution lattice."""
resolution = min(left.resolution, right.resolution)
x0 = min(left.origin[0], right.origin[0])
y0 = min(left.origin[1], right.origin[1])
left_x1 = left.origin[0] + left.shape[1] * left.resolution
right_x1 = right.origin[0] + right.shape[1] * right.resolution
left_y1 = left.origin[1] + left.shape[0] * left.resolution
right_y1 = right.origin[1] + right.shape[0] * right.resolution
x1 = max(left_x1, right_x1)
y1 = max(left_y1, right_y1)
cols = max(1, int(np.ceil((x1 - x0) / resolution)))
rows = max(1, int(np.ceil((y1 - y0) / resolution)))
origin = (x0, y0)
left_mask = align_occupancy(left, resolution=resolution, origin=origin, rows=rows, cols=cols)
right_mask = align_occupancy(right, resolution=resolution, origin=origin, rows=rows, cols=cols)
inter = int(np.logical_and(left_mask, right_mask).sum())
union = int(np.logical_or(left_mask, right_mask).sum())
samples = left_mask.size
iou = inter / union if union else 1.0
agreement = float(np.mean(left_mask == right_mask))
return OccupancyIoU(iou=iou, agreement=agreement, samples=samples)
2 changes: 1 addition & 1 deletion backend/tests/fixtures/campaigns/wizard-default.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@ repeats: 1
schema_version: 1
space:
id: warehouse_v1
version: 4326a28381df
version: 98c8f6e8ce35
start: true
strategy:
id: S-RANDOM
Expand Down
2 changes: 1 addition & 1 deletion backend/tests/unit/cli/test_m3_commands.py
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@

REPO = Path(__file__).resolve().parents[4]
WAREHOUSE = REPO / "configs" / "scenario_spaces" / "warehouse_v1.yaml"
_SPACE_VERSION = "4326a28381df"
_SPACE_VERSION = "98c8f6e8ce35"

runner = CliRunner()

Expand Down
12 changes: 6 additions & 6 deletions backend/tests/unit/scenario/test_hashing.py
Original file line number Diff line number Diff line change
Expand Up @@ -26,18 +26,18 @@
# (space_version, values, canonical bytes, scenario_hash)
GOLDEN = [
(
"4326a28381df",
"98c8f6e8ce35",
{"robot.max_speed": 0.26, "env.illuminance": 300.0, "obstacle.clearance": 0.8},
b'{"space_version":"4326a28381df","values":{"env.illuminance":300.0,'
b'{"space_version":"98c8f6e8ce35","values":{"env.illuminance":300.0,'
b'"obstacle.clearance":0.8,"robot.max_speed":0.26}}',
"4c576b5be442f310",
"edf34eb8ac140374",
),
(
"4326a28381df",
"98c8f6e8ce35",
{"robot.max_speed": 0.5, "env.people": 3, "env.layout": "narrow"},
b'{"space_version":"4326a28381df","values":{"env.layout":"narrow","env.people":3,'
b'{"space_version":"98c8f6e8ce35","values":{"env.layout":"narrow","env.people":3,'
b'"robot.max_speed":0.5}}',
"21ded8540940e4c0",
"af36f0aff5a155c4",
),
(
"fb5449ae0c1c",
Expand Down
72 changes: 72 additions & 0 deletions backend/tests/unit/scenario/test_map_alignment.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,72 @@
"""Warehouse map alignment for ``warehouse_v1`` (T-092)."""

from __future__ import annotations

import json
from pathlib import Path

import pytest

from roboprobe.backends.kinematic2d.model import Kinematic2DModel
from roboprobe.backends.kinematic2d.planner import plan_path
from roboprobe.backends.kinematic2d.world import load_world
from roboprobe.core.settings import load_settings
from roboprobe.scenario.map_io import load_ros_map, occupancy_iou, write_ros_map
from roboprobe.scenario.task import load_nominal_path, load_task, polyline_length_m

pytestmark = pytest.mark.unit

REPO = Path(__file__).resolve().parents[4]
K2D_JSON = REPO / "configs" / "backends" / "warehouse_k2d.json"
K2D_MAP = REPO / "tasks" / "maps" / "warehouse.yaml"
GAZEBO_MAP = REPO / "configs" / "maps" / "warehouse_v1.yaml"
NOMINAL_JSON = REPO / "configs" / "maps" / "warehouse_v1_nominal_path.json"
WAREHOUSE_TASK = REPO / "tasks" / "warehouse_delivery_v1.yaml"


def test_k2d_ros_map_round_trip_iou(tmp_path: Path) -> None:
grid = load_world(K2D_JSON)
yaml_path = tmp_path / "roundtrip.yaml"
write_ros_map(yaml_path, grid)
loaded = load_ros_map(yaml_path)
result = occupancy_iou(grid, loaded)
assert result.iou == pytest.approx(1.0)
assert result.agreement == pytest.approx(1.0)


def test_committed_k2d_map_matches_json(tmp_path: Path) -> None:
reference = load_world(K2D_JSON)
exported = load_ros_map(K2D_MAP)
assert occupancy_iou(reference, exported).iou == pytest.approx(1.0)


def test_nominal_path_json_matches_planner_and_csv() -> None:
model = Kinematic2DModel.from_yaml(REPO / "configs" / "backends" / "kinematic2d_v1.yaml")
grid = load_world(K2D_JSON)
start = (model.world.start_x_m, model.world.start_y_m)
goal = (model.world.goal_x_m, model.world.goal_y_m)
planned = plan_path(grid, start, goal)
assert planned is not None
stored = json.loads(NOMINAL_JSON.read_text(encoding="utf-8"))
assert len(stored) == len(planned)
task = load_task(WAREHOUSE_TASK, strict=True)
csv_path = load_nominal_path(WAREHOUSE_TASK, task)
assert csv_path.shape == planned.shape
tol = load_settings().scenario.map_alignment.path_length_tolerance_ratio
planned_len = polyline_length_m(planned)
assert abs(polyline_length_m(csv_path) - planned_len) <= tol * planned_len


def test_gazebo_map_iou_recorded_for_followup() -> None:
"""Live sim cross-check is blocked without the owner's GPU host ([#331]); record IoU here."""
k2d = load_world(K2D_JSON)
gazebo = load_ros_map(GAZEBO_MAP)
result = occupancy_iou(k2d, gazebo)
assert result.samples > 0
assert 0.0 < result.iou < 1.0


def test_warehouse_task_loads_strict_after_t092() -> None:
task = load_task(WAREHOUSE_TASK, strict=True)
assert task.goal.y == pytest.approx(-8.3)
assert not task.provisional
4 changes: 3 additions & 1 deletion backend/tests/unit/scenario/test_schemas.py
Original file line number Diff line number Diff line change
Expand Up @@ -137,8 +137,10 @@ def test_valid_fixtures_validate(path: Path) -> None:

def test_task_schema_accepts_tbd_placeholders_for_the_loader_guard() -> None:
task = example("task-06-7")
assert task["goal"]["x"] == "TBD_T092"
assert task["goal"]["x"] == 2.12
assert errors("task", task) == []
placeholder_task = dict(task, goal={"x": "TBD_T092", "y": "TBD_T092", "yaw": 1.57})
assert errors("task", placeholder_task) == []


# --------------------------------------------------------------------------- invalid documents
Expand Down
8 changes: 3 additions & 5 deletions backend/tests/unit/scenario/test_space_loader.py
Original file line number Diff line number Diff line change
Expand Up @@ -117,11 +117,9 @@ def test_task_reference_resolves_against_base_dir(monkeypatch: pytest.MonkeyPatc
assert ScenarioSpace.from_yaml(WAREHOUSE).version == load_warehouse().version


def test_strict_task_rejects_provisional_values() -> None:
with pytest.raises(errors.ValidationError, match="provisional") as exc:
ScenarioSpace.from_yaml(WAREHOUSE, base_dir=REPO_ROOT, strict_task=True)
assert exc.value.code is errors.ErrorCode.SPACE_INVALID
assert issue_paths(exc) == ["/task"]
def test_strict_task_loads_after_t092() -> None:
space = ScenarioSpace.from_yaml(WAREHOUSE, base_dir=REPO_ROOT, strict_task=True)
assert space.task.goal.y == pytest.approx(-8.3)


def test_unknown_parameter_lookup() -> None:
Expand Down
Loading
Loading