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
2 changes: 1 addition & 1 deletion CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +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).
- The verified warehouse occupancy map, aligned kinematic map, and Nav2 nominal path for cross-backend pallet placement (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
37 changes: 28 additions & 9 deletions backend/src/roboprobe/backends/kinematic2d/backend.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@
import threading
from collections.abc import Callable
from pathlib import Path
from typing import ClassVar
from typing import ClassVar, cast

import numpy as np

Expand All @@ -23,17 +23,18 @@
from roboprobe.backends.kinematic2d.episode import PARAM_ALONG, PARAM_DROPOUT, run_k2d_episode
from roboprobe.backends.kinematic2d.model import Kinematic2DModel
from roboprobe.backends.kinematic2d.world import (
dump_world_json,
load_world,
nominal_path_polyline,
world_document,
)
from roboprobe.backends.trace_io import write_trace
from roboprobe.core.errors import ValidationError
from roboprobe.core.settings import RoboProbeSettings
from roboprobe.core.types import NavStatus, Trace
from roboprobe.core.types import JsonValue, NavStatus, Trace
from roboprobe.perturbation.perception import PerceptionModel
from roboprobe.scenario.materialize import place_obstacle, side_sign
from roboprobe.scenario.repo_root import resolve_repo_path
from roboprobe.scenario.repo_root import discover_repo_root, resolve_repo_path
from roboprobe.scenario.task import load_nominal_path

__all__ = ["K2D_SUPPORTED_PARAMS", "Kinematic2DBackend"]

Expand Down Expand Up @@ -85,9 +86,23 @@ def materialize(self, req: EpisodeRequest) -> MaterializedScenario:
"""Write world JSON and resolved obstacle pose (byte-identical for equal inputs)."""
req.workdir.mkdir(parents=True, exist_ok=True)
world_path = req.workdir / "world.json"
world_path.write_text(dump_world_json(self._model), encoding="utf-8")
path = nominal_path_polyline(self._model.world)
obstacle: dict[str, float] | None = None
root = discover_repo_root()
source = (root / req.task.world.get("kinematic2d", "")).resolve()
if not source.is_relative_to(root):
msg = "kinematic2d world must stay inside the repository"
raise ValidationError(msg)
if source.is_file():
world_text = source.read_text(encoding="utf-8")
payload = cast("dict[str, JsonValue]", json.loads(world_text))
path = load_nominal_path(root / "tasks" / f"{req.task.id}.yaml", req.task)
payload["nominal_path"] = path.tolist()
else:
# Older task documents used a descriptive world path before a clone existed.
payload = world_document(self._model)
world_text = json.dumps(payload, sort_keys=True, separators=(",", ":")) + "\n"
path = nominal_path_polyline(self._model.world)
world_path.write_text(world_text, encoding="utf-8")
obstacle: dict[str, JsonValue] | None = None
if req.task.injected_obstacles:
spec = req.task.injected_obstacles[0]
along = float(req.scenario.values.get(spec.placement.along, 0.5))
Expand All @@ -99,7 +114,6 @@ def materialize(self, req: EpisodeRequest) -> MaterializedScenario:
json.dumps(obstacle, sort_keys=True, separators=(",", ":")) + "\n",
encoding="utf-8",
)
payload = world_document(self._model)
if obstacle is not None:
payload["obstacle"] = obstacle
return MaterializedScenario(
Expand All @@ -119,7 +133,12 @@ def run_episode(
artifacts: dict[str, Path] = {}
try:
world = load_world(materialized.files["world"])
path = nominal_path_polyline(self._model.world)
nominal_path = materialized.payload.get("nominal_path")
path = (
np.asarray(nominal_path, dtype=np.float64)
if nominal_path is not None
else nominal_path_polyline(self._model.world)
)
trace = run_k2d_episode(
world,
req.task,
Expand Down
59 changes: 44 additions & 15 deletions backend/src/roboprobe/backends/kinematic2d/costmap.py
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@
from roboprobe.backends.kinematic2d.model import Kinematic2DModel
from roboprobe.geometry import OccupancyGrid

__all__ = ["RollingCostmap", "inflate", "overlay_sensor_hits"]
__all__ = ["RollingCostmap", "inflate", "overlay_sensor_hits", "sensor_hit_occupancy"]

FloatArray = npt.NDArray[np.float64]

Expand All @@ -23,8 +23,10 @@ def inflate(occupied: npt.NDArray[np.bool_], radius_cells: int) -> npt.NDArray[n
"""Binary dilation by ``radius_cells`` (4-connected square)."""
if radius_cells <= 0:
return occupied.copy()
structure = np.ones((2 * radius_cells + 1, 2 * radius_cells + 1), dtype=np.bool_)
return np.asarray(ndimage.binary_dilation(occupied, structure=structure), dtype=np.bool_)
window = 2 * radius_cells + 1
return np.asarray(
ndimage.maximum_filter(occupied, size=window, mode="constant", cval=0), dtype=np.bool_
)


def overlay_sensor_hits( # noqa: PLR0913,PLR0917
Expand All @@ -39,27 +41,51 @@ def overlay_sensor_hits( # noqa: PLR0913,PLR0917
depth_inflation_cells: int = 0,
) -> OccupancyGrid:
"""Return a copy of ``grid`` with finite sensor endpoints marked occupied (optional inflate)."""
occupied = grid.occupied.copy()
occupied = sensor_hit_occupancy(
grid,
grid.occupied,
pose,
lidar_ranges,
lidar_angles,
depth_ranges,
depth_angles,
lidar_inflation_cells=lidar_inflation_cells,
depth_inflation_cells=depth_inflation_cells,
)
return OccupancyGrid(occupied, grid.resolution, grid.origin)


def sensor_hit_occupancy( # noqa: PLR0913,PLR0917 - two sensors and their inflation radii
grid: OccupancyGrid,
static_occupied: npt.NDArray[np.bool_],
pose: tuple[float, float, float],
lidar_ranges: FloatArray,
lidar_angles: FloatArray,
depth_ranges: FloatArray,
depth_angles: FloatArray,
*,
lidar_inflation_cells: int,
depth_inflation_cells: int,
) -> npt.NDArray[np.bool_]:
"""Compose the planning mask without computing a distance transform."""
occupied = static_occupied.copy()
specs = (
(lidar_ranges, lidar_angles, lidar_inflation_cells),
(depth_ranges, depth_angles, depth_inflation_cells),
)
for ranges, angles, inflate_cells in specs:
layer = np.zeros_like(occupied)
_mark_world_cells(layer, grid, pose, ranges, angles)
if inflate_cells > 0:
layer = inflate(layer, inflate_cells)
occupied |= layer
return OccupancyGrid(occupied, grid.resolution, grid.origin)
for sensor in specs:
_mark_world_cells(occupied, grid, pose, sensor)
return occupied


def _mark_world_cells(
occupied: npt.NDArray[np.bool_],
grid: OccupancyGrid,
pose: tuple[float, float, float],
ranges: FloatArray,
angles: FloatArray,
sensor: tuple[FloatArray, FloatArray, int],
) -> None:
ranges, angles, inflate_cells = sensor
radius = max(0, inflate_cells)
finite = np.isfinite(ranges)
if not np.any(finite):
return
Expand All @@ -69,8 +95,11 @@ def _mark_world_cells(
cells = grid.world_to_cell(np.stack([x, y], axis=1))
rows, cols = grid.shape
for row, col in cells:
if 0 <= row < rows and 0 <= col < cols and not occupied[row, col]:
occupied[row, col] = True
if 0 <= row < rows and 0 <= col < cols:
occupied[
max(0, row - radius) : min(rows, row + radius + 1),
max(0, col - radius) : min(cols, col + radius + 1),
] = True


class RollingCostmap:
Expand Down
15 changes: 6 additions & 9 deletions backend/src/roboprobe/backends/kinematic2d/episode.py
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@
import numpy as np

from roboprobe.backends.kinematic2d.controller import path_arclength, pure_pursuit
from roboprobe.backends.kinematic2d.costmap import inflate, overlay_sensor_hits
from roboprobe.backends.kinematic2d.costmap import inflate, sensor_hit_occupancy
from roboprobe.backends.kinematic2d.localization import Localizer, scan_match_gain
from roboprobe.backends.kinematic2d.model import Kinematic2DModel
from roboprobe.backends.kinematic2d.planner import plan_path
Expand Down Expand Up @@ -77,11 +77,6 @@ def _at_goal(x: float, y: float, yaw: float, task: TaskSpec) -> bool:
return abs(err) <= tol.yaw


def _global_inflated(world: OccupancyGrid, radius_m: float) -> OccupancyGrid:
cells = math.ceil(radius_m / world.resolution)
return OccupancyGrid(inflate(world.occupied, cells), world.resolution, world.origin)


def _circle_clearance(
world: OccupancyGrid, radius: float, x: float, y: float, pallet: Rect | None
) -> float:
Expand Down Expand Up @@ -180,7 +175,7 @@ def _run( # noqa: PLR0913,PLR0917,PLR0915
pallet = _pallet(task, path, scenario_values)
radius = model.robot.footprint_radius_m
inflate_r = radius + model.costmap.inflation_m
inflated_static = _global_inflated(world, inflate_r)
inflated_static = inflate(world.occupied, math.ceil(inflate_r / world.resolution))
delay_s = _f(scenario_values, PARAM_DELAY_MS, 20.0) / _MS_PER_S
delay: CommandDelayBuffer[tuple[float, float]] = CommandDelayBuffer(delay_s)
start = task.start
Expand Down Expand Up @@ -232,7 +227,8 @@ def _run( # noqa: PLR0913,PLR0917,PLR0915
lidar_cells = (
cm.lidar_hit_inflation_cells if noise >= cm.lidar_overlay_min_noise_std_m else 0
)
plan_grid = overlay_sensor_hits(
plan_occupied = sensor_hit_occupancy(
world,
inflated_static,
pose,
lidar_ranges if lidar_cells else np.full_like(lidar_ranges, np.nan),
Expand All @@ -243,9 +239,10 @@ def _run( # noqa: PLR0913,PLR0917,PLR0915
depth_inflation_cells=cm.depth_hit_inflation_cells,
)
planned = plan_path(
plan_grid,
world,
(localizer.x, localizer.y),
(task.goal.x, task.goal.y),
occupied=plan_occupied,
)
planned_s = path_arclength(planned) if planned is not None else planned_s
last_plan_t = t
Expand Down
25 changes: 17 additions & 8 deletions backend/src/roboprobe/backends/kinematic2d/planner.py
Original file line number Diff line number Diff line change
Expand Up @@ -32,40 +32,49 @@ def plan_path(
grid: OccupancyGrid,
start: tuple[float, float],
goal: tuple[float, float],
*,
occupied: npt.NDArray[np.bool_] | None = None,
) -> npt.NDArray[np.float64] | None:
"""8-connected A* on free cells. ``None`` if start/goal occupied or no path."""
start_cell = _cell(grid, start)
goal_cell = _cell(grid, goal)
if start_cell is None or goal_cell is None:
return None
rows, cols = grid.shape
occupied = grid.occupied
if occupied is None:
occupied = grid.occupied
sr, sc = start_cell
gr, gc = goal_cell
if occupied[sr, sc] or occupied[gr, gc]:
return None
if start_cell == goal_cell:
return grid.cell_to_world(np.array([[sr, sc]], dtype=np.int64))

g_score = np.full((rows, cols), np.inf)
g_score = {(sr, sc): 0.0}
came: dict[tuple[int, int], tuple[int, int]] = {}
g_score[sr, sc] = 0.0
open_heap: list[tuple[float, int, int]] = [(float(_heuristic(sr, sc, gr, gc)), sr, sc)]
closed = np.zeros((rows, cols), dtype=np.bool_)
closed: set[tuple[int, int]] = set()

while open_heap:
_, r, c = heapq.heappop(open_heap)
if closed[r, c]:
if (r, c) in closed:
continue
closed[r, c] = True
closed.add((r, c))
if (r, c) == (gr, gc):
return _reconstruct(grid, came, (r, c), (sr, sc))
for dr, dc, cost in _NEIGHBOURS:
nr, nc = r + dr, c + dc
if nr < 0 or nr >= rows or nc < 0 or nc >= cols or occupied[nr, nc] or closed[nr, nc]:
if (
nr < 0
or nr >= rows
or nc < 0
or nc >= cols
or occupied[nr, nc]
or (nr, nc) in closed
):
continue
tentative = g_score[r, c] + cost * grid.resolution
if tentative >= g_score[nr, nc]:
if tentative >= g_score.get((nr, nc), math.inf):
continue
came[nr, nc] = (r, c)
g_score[nr, nc] = tentative
Expand Down
48 changes: 20 additions & 28 deletions backend/src/roboprobe/scenario/map_io.py
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,8 @@
]

_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
_MAX_PIXEL = 255
_ORIGIN_LEN = 3


@dataclass(frozen=True, slots=True)
Expand All @@ -40,30 +41,27 @@ class OccupancyIoU:

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"
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[start:], dtype=np.uint8, count=width * rows)
if pixels.size != width * rows:
msg = f"{path}: PGM pixel payload truncated"
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_]:
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
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:
Expand All @@ -82,20 +80,17 @@ def load_ros_map(yaml_path: Path) -> OccupancyGrid:
msg = f"{yaml_path}: missing image"
raise errors.ValidationError(msg, context={"path": str(yaml_path)})
image_path = (yaml_path.parent / image_name).resolve()
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)
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]"
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)})
occupied = _threshold_occupied(meta, values)
# 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])))


Expand All @@ -113,10 +108,7 @@ def write_ros_map( # noqa: PLR0913
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)
pixels = np.flipud(np.where(grid.occupied, 0, _MAX_PIXEL)).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 = {
Expand Down
Loading
Loading