diff --git a/apps/crazy_robotaxi/README.md b/apps/crazy_robotaxi/README.md index 16703a47d..8a0718b60 100644 --- a/apps/crazy_robotaxi/README.md +++ b/apps/crazy_robotaxi/README.md @@ -1,8 +1,7 @@ # Crazy Robotaxi Crazy Robotaxi is a standalone game built on `omnidreams-game-engine` and the -legacy OmniDreams inference session. It does not import or modify the -Interactive Drive demo. +OmniDreams inference session. Launch the native game: @@ -10,21 +9,19 @@ Launch the native game: flashdreams-run crazy-robotaxi ``` -Select the bundled performance manifest and load the scene immediately: +Select the bundled performance manifest: ```bash flashdreams-run crazy-robotaxi \ - --world-model-manifest example_world_model_perf.yaml \ - --auto-start True + --world-model-manifest example_world_model_perf.yaml ``` `flashdreams-run` reserves `--manifest` for its launch-manifest format. Use -`--world-model-manifest` for the legacy OmniDreams model manifest, or use the -dedicated `crazy-robotaxi` executable below. Runner booleans use explicit -`True` / `False` values because the shared FlashDreams CLI disables implicit -boolean flag conversion. +`--world-model-manifest` for the OmniDreams model manifest. Runner +booleans use explicit `True` / `False` values because the shared FlashDreams +CLI disables implicit boolean flag conversion. -The dedicated entry point exposes the complete legacy option surface: +The dedicated entry point exposes the application options: ```bash crazy-robotaxi --help @@ -32,3 +29,100 @@ crazy-robotaxi --help Use `--stream-mjpeg HOST:PORT` with either entry point to run the browser HUD instead of opening a local Vulkan window. + +## Configuration files + +The standalone game keeps its portable configuration in three independent, +strict YAML documents: + +- `*.robotaxi.yaml` describes one map, including topology, geometry, profiles, + compiler settings, spawns, and visual seed variants. +- `default_renderer.yaml` describes primary-camera and BEV rendering. +- `default_game.yaml` describes rules, scoring, controls, taxi dimensions, and + arcade physics. + +The packaged renderer and game files are used when no path is supplied. Select +edited copies independently: + +```bash +flashdreams-run crazy-robotaxi \ + --renderer-config /path/to/renderer.yaml \ + --game-config /path/to/game.yaml +``` + +Existing explicit CLI tuning flags override YAML values. The world-model +manifest remains separate because it configures inference rather than the map, +renderer, or game. All three YAML formats reject missing and unknown fields. + +## Node-graph game maps + +Crazy Robotaxi maps use schema version 1 of the engine's node-graph format. +Roads are topological edges; intersections, road joints, cul-de-sacs, parking +lots, and driveways are explicitly posed graph nodes. Road joints provide +minimal tangent-continuous degree-two bends and own lane-count or lane-width +tapers between otherwise uniform road edges. Intersections have at least three +road arms and can apply the same taper to each inferred through-road pair +independently. Node footprints follow the bearings and centerline tangents of +the roads connected to them. + +The complete, authoritative format is documented in +[`../omnidreams_game_engine/NODE_GRAPH_MAP_FORMAT.md`](../omnidreams_game_engine/NODE_GRAPH_MAP_FORMAT.md). +The bundled `minimal_loop.robotaxi.yaml` is a compact working example, while +`boulevard_district.robotaxi.yaml` recreates the original scene's surface-street +layout at its source scale, including the curved arterial split, neighborhood +grid, eastern commercial loops, cul-de-sacs, and parking lots. The elevated +highway and its on-ramps in the northwest are intentionally omitted. + +Select a map with either entry point: + +```bash +flashdreams-run crazy-robotaxi \ + --map /path/to/city.robotaxi.yaml +``` + +Validate a map or produce top-down and spawn-camera previews without loading a +model: + +```bash +crazy-robotaxi-map validate /path/to/city.robotaxi.yaml +crazy-robotaxi-map preview /path/to/city.robotaxi.yaml --output city.svg +crazy-robotaxi-map preview-spawn /path/to/city.robotaxi.yaml \ + --spawn taxi_start --output taxi_start.png +``` + +The engine validates the topology, compiles roads and inferred parking-access +geometry, creates curb colliders, and derives directed navigation lanes. That +lane graph powers fare reachability and future NPC routing; it does not limit +where the player's taxi may physically drive. Parking-lot fare points are +sampled anywhere inside the authored polygon, while their route distance stops +at the connected driveway or intersection. + +The engine's private ClipGT archive is cached under +`$FLASHDREAMS_CACHE_DIR/omnidreams-game-engine/game-maps/`. The cache key includes the YAML, +resolved geometry, compiler implementation, and referenced seed assets, so +map or compiler edits rebuild it at the next load. The archive is runtime +output, not an authoring format. Pass `--force-map-recompile` when launching +the game to rebuild each selected map once for that process even if its cache +entry is valid. The standalone equivalent is: + +```bash +crazy-robotaxi-map compile /path/to/city.robotaxi.yaml --force-map-recompile +``` + +Parking-lot surfaces are emitted as green `ROI_POLYGON_ROADNET_MASK` regions. +Parking-space dividers remain intentionally omitted because ClipGT would encode +them as ordinary lane lines, which can condition the model to produce bike or +turn lanes. Lots do not contain inferred navigation aisles or turnarounds. + +Each visual variant may omit `image`. In that case, map compilation and scene +selection use a deterministic synthetic view projected from the spawn through +the runtime front camera. It aligns roads, boundaries, curbs, and markings but +does not synthesize buildings, vegetation, traffic, or other scenery. Authors +can inspect that fallback with `preview-spawn` before choosing or generating a +checked-in image. + +The bundled maps reuse the existing OmniDreams seed image. Their authored +geometry is not expected to match that image exactly, so the first generated +frames may visibly adjust toward the selected semantic map. Static prompts +describe visual setting and atmosphere rather than duplicating map topology; +the BEV remains the source of truth as maps change. diff --git a/apps/crazy_robotaxi/crazy_robotaxi/alignment_diagnostics.py b/apps/crazy_robotaxi/crazy_robotaxi/alignment_diagnostics.py index c7e420f03..478bc06eb 100644 --- a/apps/crazy_robotaxi/crazy_robotaxi/alignment_diagnostics.py +++ b/apps/crazy_robotaxi/crazy_robotaxi/alignment_diagnostics.py @@ -267,6 +267,8 @@ def _frame_telemetry(frame: PresentedFrame, sequence: int) -> dict[str, object]: physx_position = ( np.full(3, np.nan, dtype=np.float32) if debug is None else debug.ego_position_m ) + command = frame.driver_command + motion = frame.model_motion_metrics or {} return { "sequence": sequence, "timestamp_us": int(frame.timestamp_us), @@ -295,6 +297,16 @@ def _frame_telemetry(frame: PresentedFrame, sequence: int) -> dict[str, object]: state.x_m - float(physx_position[0]), state.y_m - float(physx_position[1]), ), + "command_throttle": math.nan if command is None else command.throttle, + "command_brake": math.nan if command is None else command.brake, + "command_steer": math.nan if command is None else command.steer, + "command_reverse": False if command is None else command.reverse, + "impact_kind": frame.impact_kind or "", + "motion_axis": motion.get("axis", ""), + "motion_mismatched": motion.get("mismatched", False), + "condition_motion_px": motion.get("condition_component_px", math.nan), + "generated_motion_px": motion.get("generated_component_px", math.nan), + "motion_check_ms": motion.get("elapsed_ms", math.nan), } diff --git a/apps/crazy_robotaxi/crazy_robotaxi/app.py b/apps/crazy_robotaxi/crazy_robotaxi/app.py index 3a5e31c30..d9fd9b128 100644 --- a/apps/crazy_robotaxi/crazy_robotaxi/app.py +++ b/apps/crazy_robotaxi/crazy_robotaxi/app.py @@ -19,6 +19,7 @@ import argparse from dataclasses import replace +from functools import partial from pathlib import Path from typing import Any @@ -31,8 +32,8 @@ ) from omnidreams_game_engine.backends.base import RenderBackend from omnidreams_game_engine.config import AppConfig +from omnidreams_game_engine.game_map.vicinity import GameMapVicinityResolver from omnidreams_game_engine.simulation.ground_snap import GroundSnapper -from omnidreams_game_engine.simulation.map_bounds import MapBounds from omnidreams_game_engine.types import ( SceneBundle, TrajectoryChunk, @@ -49,6 +50,7 @@ TaxiGameConfig, TaxiGameController, ) +from crazy_robotaxi.game_settings import load_game_settings from crazy_robotaxi.high_scores import ( default_high_scores_path, ) @@ -130,27 +132,30 @@ def __init__( self._presenter_config = presenter_config self._reference_route_world: Any | None = None self._navigation_lanes: tuple[Any, ...] = () + self._fare_regions: tuple[Any, ...] = () + self._vicinity_resolver: GameMapVicinityResolver | None = None self._ground_snapper: GroundSnapper | None = None - self._map_bounds: MapBounds | None = None - self._enclosure_segments_world = np.empty((0, 2, 3), dtype=np.float32) + self._curb_segments_world = np.empty((0, 2, 3), dtype=np.float32) def configure_presenter(self, presenter: Any) -> None: """Configure application presentation before scene loading.""" configure = getattr(presenter, "configure_taxi_hud", None) if callable(configure): - configure(self._presenter_config) + configure(self._presenter_config, self._config.vehicle) - def load_scene(self, scene: SceneBundle, map_bounds: MapBounds | None) -> None: + def load_scene(self, scene: SceneBundle) -> None: """Accept scene data already loaded by Interactive Drive.""" scene_data = load_scene_data(scene) self._reference_route_world = scene_data.reference_route_world self._navigation_lanes = scene_data.navigation_lanes - self._enclosure_segments_world = scene_data.enclosure_segments_world - self._ground_snapper = _build_taxi_ground_snapper(scene) - self._map_bounds = map_bounds + self._fare_regions = scene_data.fare_regions + assert scene.game_map is not None + self._vicinity_resolver = GameMapVicinityResolver(scene.game_map) + self._curb_segments_world = scene_data.curb_segments_world + self._ground_snapper = _build_taxi_ground_snapper(scene, self._config) logger.info( - "[crazy-robotaxi] play-area enclosure: perimeter_segments={}", - len(scene_data.perimeter_segments_world), + "[crazy-robotaxi] compiled curb segments={}", + len(scene_data.curb_segments_world), ) def configure_scene_presenter(self, presenter: Any, scene: SceneBundle) -> None: @@ -158,9 +163,6 @@ def configure_scene_presenter(self, presenter: Any, scene: SceneBundle) -> None: configure = getattr(presenter, "configure_taxi_camera", None) if callable(configure): configure(scene.selected_camera) - configure_enclosure = getattr(presenter, "configure_taxi_enclosure", None) - if callable(configure_enclosure): - configure_enclosure(self._enclosure_segments_world) def rollout_spec( self, @@ -178,8 +180,7 @@ def rollout_spec( physics_world_factory=lambda active_scene, vehicle: TaxiPhysicsWorld( active_scene, vehicle, - traffic_density=self._config.traffic_density, - enclosure_segments_world=self._enclosure_segments_world, + curb_segments_world=self._curb_segments_world, ), physics_step_fn=step_taxi_physics_world, visual_flare_enabled=False, @@ -198,10 +199,11 @@ def create_runtime( scene_id=scene.scene_id, reference_route_world=self._reference_route_world, navigation_lanes=self._navigation_lanes, + fare_regions=self._fare_regions, initial_state=simulation.current_state, config=self._config, initial_camera=scene.selected_camera, - map_bounds=self._map_bounds, + vicinity_resolver=self._vicinity_resolver, ) return CrazyRobotaxiRuntime(controller, self._keyboard) @@ -256,9 +258,11 @@ def taxi_config_from_args(args: argparse.Namespace) -> TaxiGameConfig: if args.taxi_highscores is not None else default_high_scores_path() ) - return TaxiGameConfig( - enabled=True, - traffic_density=float(args.traffic_density), + config = getattr(args, "_game_settings", None) or load_game_settings( + args.game_config + ) + return replace( + config, seed=None if args.taxi_seed is None else int(args.taxi_seed), high_scores_path=high_scores_path, alignment_diagnostics_enabled=( @@ -267,20 +271,26 @@ def taxi_config_from_args(args: argparse.Namespace) -> TaxiGameConfig: ) -def _build_taxi_ground_snapper(scene: SceneBundle) -> GroundSnapper | None: +def _build_taxi_ground_snapper( + scene: SceneBundle, config: TaxiGameConfig +) -> GroundSnapper | None: if scene.ground_mesh_vertices is None or scene.ground_mesh_faces is None: return None return GroundSnapper( scene.ground_mesh_vertices, scene.ground_mesh_faces, - max_absolute_rotation_deg=10.0, - invalid_sample_handler=settle_invalid_ground_attitude, + max_absolute_rotation_deg=config.ground_snap_max_absolute_rotation_deg, + invalid_sample_handler=partial( + settle_invalid_ground_attitude, + settle_fraction=config.ground_snap_settle_fraction, + ), ) -def settle_invalid_ground_attitude(state: VehicleState) -> VehicleState: +def settle_invalid_ground_attitude( + state: VehicleState, *, settle_fraction: float = 0.25 +) -> VehicleState: """Ease stale ground attitude toward level after an invalid Taxi sample.""" - settle_fraction = 0.25 pitch = state.pitch_rad * (1.0 - settle_fraction) roll = state.roll_rad * (1.0 - settle_fraction) if abs(pitch) < 1.0e-4: diff --git a/apps/crazy_robotaxi/crazy_robotaxi/assets/README.md b/apps/crazy_robotaxi/crazy_robotaxi/assets/README.md index d939b3f10..5c0246e66 100644 --- a/apps/crazy_robotaxi/crazy_robotaxi/assets/README.md +++ b/apps/crazy_robotaxi/crazy_robotaxi/assets/README.md @@ -1,7 +1,6 @@ # Assets -This directory holds the unpacked-scene-bundle loader -(`scene_bundle.py`) plus the bundled HUD control sprites under +This directory holds the bundled HUD control sprites under `wheel_and_pedals/`. ## `wheel_and_pedals/` @@ -20,10 +19,7 @@ the brake PNGs are also accepted under AlpaSim's `break_*.png` spelling. When a sprite is missing, the HUD falls back to a CPU-rendered vector wheel / fill-bar pedals. -## Scenes +## Maps -Scene USDZs themselves are staged into the shared `omnidreams` scene -cache under `$FLASHDREAMS_CACHE_DIR/omnidreams-scenes/`, **not** here. -See `omnidreams.scenes` and `omnidreams-prepare` for how staging -works; both the desktop demo and the WebRTC server consume from the -same cache root. +Crazy Robotaxi's `.robotaxi.yaml` maps live under `crazy_robotaxi/maps/`. +Seed images referenced by a map may be map-relative files or packaged assets. diff --git a/apps/crazy_robotaxi/crazy_robotaxi/cli.py b/apps/crazy_robotaxi/crazy_robotaxi/cli.py index d25c68d6a..517988eb8 100644 --- a/apps/crazy_robotaxi/crazy_robotaxi/cli.py +++ b/apps/crazy_robotaxi/crazy_robotaxi/cli.py @@ -11,18 +11,22 @@ import struct import threading import time -import zipfile -from collections.abc import Iterable, Sequence +from collections.abc import Sequence from dataclasses import dataclass, field, replace from pathlib import Path from typing import Any import numpy as np from loguru import logger -from omnidreams import scenes as _scenes -from omnidreams.scenes import normalise_scene_uuid, scenes_cache_root from omnidreams_game_engine.app import InteractiveDriveApp -from omnidreams_game_engine.config import BevConfig, RasterConfig +from omnidreams_game_engine.config import BevConfig +from omnidreams_game_engine.game_map import ( + GAME_MAP_SUFFIX, + load_game_map, + load_game_map_header, + render_spawn_first_frame, + resolve_seed_asset, +) from omnidreams_game_engine.input.wheel_profiles import ( EV_ABS, EV_KEY, @@ -43,7 +47,6 @@ user_wheel_profiles_dir, ) from omnidreams_game_engine.log import configure_logging -from omnidreams_game_engine.synthetic_scene import build_synthetic_scene_to_temp from PIL import Image from crazy_robotaxi import runtime_cli as _cli @@ -63,6 +66,9 @@ # ``cli.py`` defaults) so the realistic controls render out of the box # regardless of the user's cwd; ``--control-assets-dir`` overrides it. _BUNDLED_CONTROL_ASSETS_DIR = _cli._PACKAGE_ROOT / "assets" / "wheel_and_pedals" +_PACKAGE_ROOT = Path(__file__).resolve().parent +_BUNDLED_MAPS_DIR = _PACKAGE_ROOT / "maps" +_DEFAULT_GAME_MAP = _BUNDLED_MAPS_DIR / "minimal_loop.robotaxi.yaml" SCENE_THUMB_SIZE = (140, 64) KEYBOARD_STEER_SCALE = 0.75 KEYBOARD_STEER_RATE_PER_S = 0.6 @@ -109,8 +115,7 @@ class SceneOption: # dropdown. Variants without a dedicated preview map to the default image # so every row still shows a preview. variant_thumbnails: dict[str, Image.Image] = field(default_factory=dict) - # Variant slug -> its USDZ archive. Distinct sibling files for the current - # per-weather dataset; the single ``path`` for legacy in-zip-variant scenes. + # Variant slug -> the authored map containing that variant. variant_paths: dict[str, Path] = field(default_factory=dict) @@ -492,7 +497,7 @@ def build_parser() -> argparse.ArgumentParser: Union of: the backend args from :func:`omnidreams_game_engine.cli.build_parser`; HUD args - (``--scene-dir``, ``--wheel-*``, ...) ignored under ``--no-hud`` / + (``--map-dir``, ``--wheel-*``, ...) ignored under ``--no-hud`` / ``--stream-mjpeg``; and the ``--no-hud`` toggle (bare Vulkan window). """ parser = _cli.build_parser() @@ -505,6 +510,7 @@ def build_parser() -> argparse.ArgumentParser: parser.set_defaults( backend="omnidreams", manifest=_cli._PACKAGE_ROOT / "configs/example_world_model.yaml", + scene=_DEFAULT_GAME_MAP, ) parser.description = ( "Standalone Crazy Robotaxi. The default mode opens the native game HUD;" @@ -519,14 +525,14 @@ def build_parser() -> argparse.ArgumentParser: ), ) parser.add_argument( - "--scene-dir", + "--map-dir", + dest="scene_dir", type=Path, - default=scenes_cache_root(), + default=_BUNDLED_MAPS_DIR, + metavar="DIRECTORY", help=( - "Directory of USDZ scenes shown in the HUD scene selector. " - "Defaults to ``$FLASHDREAMS_CACHE_DIR/omnidreams-scenes/``, " - "the shared cache root used by both this demo and the " - "``omnidreams.webrtc.server`` scene pipeline." + "Directory of .robotaxi.yaml maps available for scene switching. " + "Defaults to the maps bundled with Crazy Robotaxi." ), ) parser.add_argument( @@ -535,7 +541,7 @@ def build_parser() -> argparse.ArgumentParser: action=argparse.BooleanOptionalAction, default=False, help=( - "Start loading --scene immediately instead of opening the HUD on" + "Start loading --map immediately instead of opening the HUD on" " Load Scene. Distinct from --preload-scenes (which only warms the" " parse cache in the background)." ), @@ -554,10 +560,10 @@ def build_parser() -> argparse.ArgumentParser: action=argparse.BooleanOptionalAction, default=False, help=( - "Parse every scene in --scene-dir in the background at startup so" - " switching scenes skips the USDZ parse (the per-scene geometry" + "Parse every map in --map-dir in the background at startup so" + " switching scenes skips map compilation and archive parsing (geometry" " upload and first-chunk generation still happen on switch)." - " Off by default; uses more memory the more scenes are staged." + " Off by default; uses more memory as more maps are loaded." ), ) parser.add_argument( @@ -613,104 +619,20 @@ def build_parser() -> argparse.ArgumentParser: return parser -def _has_discoverable_scenes(scene_dir: Path, scene: Path) -> bool: - """Whether the scene picker would find any staged USDZ to offer. - - Mirrors :func:`_discover_scene_options`'s directory sweep -- the - ``--scene-dir`` cache plus the requested scene's own folder -- so the - default-scene autostage can be skipped when a curated set of scenes is - already present. - """ - for directory in (scene_dir, scene.parent): - resolved = _project_path(directory) - if resolved.is_dir() and any(resolved.glob("*.usdz")): - return True - return False - - -def _maybe_autostage_scene(scene: Path, *, scene_dir: Path, allow_skip: bool) -> Path: - """Auto-download the default scene UUID on first launch. - - Triggers only for a missing ``clipgt-.usdz`` under the shared scenes - cache root; external / non-clipgt paths are returned unchanged. With - ``allow_skip`` (any scene-picker mode), a missing default is skipped when - the picker already has staged scenes, so a curated set never blocks on the - default UUID. ``omnidreams-prepare`` remains the way to pre-stage arbitrary - UUIDs. - """ - if scene.exists(): - return scene - if allow_skip and _has_discoverable_scenes(scene_dir, scene): - logger.info( - f"[interactive-drive] default scene '{scene.name}' is not staged; " - f"using the scenes already present under {scene_dir} instead.", - ) - return scene - cache_dir = scenes_cache_root().resolve() - if scene.resolve().parent != cache_dir: - return scene - stem = scene.stem - if not stem.startswith("clipgt-"): - return scene - bare_uuid = normalise_scene_uuid(stem) - if not os.environ.get("HF_TOKEN"): - raise SystemExit( - f"Scene '{scene.name}' is not staged yet and HF_TOKEN is not set.\n" - "Either export HF_TOKEN to enable auto-staging on launch, or run:\n" - f" uv run --package flashdreams-omnidreams omnidreams-prepare --scene-uuid {bare_uuid}" - ) - logger.info( - f"[interactive-drive] Scene '{stem}' not found locally; " - "auto-staging from Hugging Face (one-time download)..." - ) - from omnidreams.prepare import stage_scene - - staged_default = stage_scene(bare_uuid, force=False) - # Also stage the scene's other weather variants so the HUD shows a - # Default/Rain/Snow selector; discovery globs the cache dir for them. - try: - sibling_variants = [ - variant - for uuid, variant in _scenes.list_available_scene_files() - if uuid == bare_uuid and variant != _scenes.SCENE_VARIANT_DEFAULT - ] - except Exception as exc: # noqa: BLE001 - best-effort; base scene already staged - logger.info( - f"[interactive-drive] could not enumerate scene variants ({exc}); " - "staged the base scene only.", - ) - sibling_variants = [] - for variant in sibling_variants: - try: - stage_scene(bare_uuid, variant=variant, force=False) - except Exception as exc: # noqa: BLE001 - skip a variant, keep the rest - logger.info( - f"[interactive-drive] failed to stage variant {variant!r} " - f"({exc}); skipping.", - ) - return staged_default - - def main(argv: Sequence[str] | None = None) -> None: """Launch a standalone Crazy Robotaxi session.""" configure_logging() args = build_parser().parse_args(argv) + args.renderer_config = _cli.resolve_app_config_path(args.renderer_config) + args.game_config = _cli.resolve_app_config_path(args.game_config) + if args.taxi_game: + from crazy_robotaxi.game_settings import load_game_settings + + args._game_settings = load_game_settings(args.game_config) _validate_presenter_mode(args) - if not args.synthetic_scene: - # Only the bare ``--no-hud`` backend has no scene picker; the HUD - # and MJPEG paths both let the user pick from ``--scene-dir``, so a - # missing default scene there is fine as long as the directory - # already has other scenes staged (see _maybe_autostage_scene). - uses_scene_picker = args.stream_mjpeg is not None or not args.no_hud - args.scene = _maybe_autostage_scene( - args.scene, scene_dir=args.scene_dir, allow_skip=uses_scene_picker - ) # ``--stream-mjpeg`` runs through ``_run_streaming`` so the long-lived - # MJPEG presenter (HTTP server, browser session) survives across - # scene-change requests posted by the in-page picker. ``--no-hud`` - # without MJPEG drops straight through to the bare CLI's Vulkan - # window, which has no scene picker UI of its own. The default path - # is the slangpy HUD with full chrome. + # MJPEG presenter survives scene changes. ``--no-hud`` without MJPEG + # drops straight through to the bare CLI's Vulkan window. if args.stream_mjpeg is not None: _run_streaming(args) return @@ -735,11 +657,11 @@ def _run_slangpy_hud(args: argparse.Namespace) -> None: """Run the engine with the slangpy + PIL HUD presenter in one process. Builds one ``SlangPyHudPresenter`` and one long-lived - :class:`InteractiveDriveApp` at startup (model warmup overlaps the - scene-selection wait), then loops over scene-change requests calling - ``app.load_scene`` / ``app.run_scene`` per scene. The warmed model and the - window stay alive across switches (``close_presenter_on_exit=False``); the - wheel binds once to the app's single ``KeyboardState``. + :class:`InteractiveDriveApp` at startup, then loops over scene-change + requests calling ``app.load_scene`` / ``app.run_scene`` per scene. The + warmed model and the window stay alive across switches + (``close_presenter_on_exit=False``); the wheel binds once to the app's + single ``KeyboardState``. """ from omnidreams_game_engine.input.keyboard import KeyboardState @@ -756,17 +678,17 @@ def _run_slangpy_hud(args: argparse.Namespace) -> None: _apply_cuda_visible_devices_inplace(args.cuda_visible_devices) _resolve_demo_paths(args) - _materialize_synthetic_scene_for_picker(args) + renderer_settings = _cli.renderer_settings_from_args(args) scene_options = _discover_scene_options(args.scene_dir, args.scene) if not args.scene.exists() and scene_options: args.scene = scene_options[0].path # Validate paths up front so a typo in ``--manifest`` / - # ``--scene-dir`` / ``--control-assets-dir`` fails immediately, + # ``--map-dir`` / ``--control-assets-dir`` fails immediately, # before we open the slangpy window and the user wastes 30s on # world-model warmup that's about to ENOENT. Scene path is # validated lazily because ``_discover_scene_options`` already # backfills ``args.scene`` from the directory, so a missing - # ``--scene`` is only fatal if the directory is empty too. + # ``--map`` is only fatal if the directory is empty too. if args.backend == "omnidreams": if args.manifest is None: raise SystemExit("--manifest is required for the omnidreams backend") @@ -777,23 +699,16 @@ def _run_slangpy_hud(args: argparse.Namespace) -> None: "example_world_model.yaml)" ) if not scene_options and not args.scene.exists(): - raise SystemExit( - f"--scene path does not exist and --scene-dir contains no scenes: {args.scene}" - ) + raise SystemExit(f"--map path does not exist: {args.scene}") control_assets = _load_control_assets(args.control_assets_dir) wheel_selection = None if args.no_wheel else _select_wheel(args) - # Construct the presenter UPFRONT, before any backend, so the demo - # can open the HUD window in "Load Scene" mode and wait for the - # user to pick a scene from the dropdown when ``--auto-start`` - # is off. The placeholder ``KeyboardState`` is rebound to each - # successive ``InteractiveDriveApp``'s real keyboard via - # ``presenter.bind_keyboard`` in the factory below; no engine is - # listening to the placeholder, so events are harmlessly dropped - # during the initial wait. + # Construct the presenter before the backend. The placeholder + # ``KeyboardState`` is rebound to the app's real keyboard via + # ``presenter.bind_keyboard`` in the factory below. placeholder_keyboard = KeyboardState() presenter = SlangPyHudPresenter( - raster=RasterConfig(), + raster=renderer_settings.raster, keyboard=placeholder_keyboard, args=args, scene_options=scene_options, @@ -801,13 +716,9 @@ def _run_slangpy_hud(args: argparse.Namespace) -> None: wheel=None, ) - # Build the backend + engine ONCE, up front. Constructing the app - # starts the (scene-independent) model warmup on the pipeline worker - # thread immediately, so the long weight-load + compile overlaps with - # the user's scene-selection wait below instead of starting only after - # the first pick. The app owns one long-lived KeyboardState and rebinds - # the presenter to it; scenes are switched in place via - # ``app.load_scene`` so the warmed model is never rebuilt. + # Build the backend + engine once. The app owns one long-lived + # KeyboardState and rebinds the presenter to it; scenes are switched in + # place via ``app.load_scene`` so the warmed model is never rebuilt. config, backend = _cli.prepare_config_and_backend(args) app = _build_application( args, @@ -824,11 +735,9 @@ def _run_slangpy_hud(args: argparse.Namespace) -> None: callback=app.set_postprocess_enabled, ) - # Attach the wheel up front, bound to the app's long-lived keyboard, so - # the HUD's steering / pedal chrome reacts to the physical device during - # the initial scene-selection wait -- not only once a scene is running. - # The evdev reader thread starts now and runs for the process lifetime; - # the single keyboard means it never needs rebinding across scenes. + # Attach the wheel up front, bound to the app's long-lived keyboard. The + # evdev reader thread starts now and runs for the process lifetime; the + # single keyboard means it never needs rebinding across scenes. wheel: Any = None if wheel_selection is not None: profile, device_paths = wheel_selection @@ -846,40 +755,19 @@ def _run_slangpy_hud(args: argparse.Namespace) -> None: for opt in scene_options for variant in (opt.variants or ("default",)) ) - # Lock scene selection until every scene is cached so the user only + # Lock scene changes until every scene is cached so the user only # ever hits the instant (cache-hit) switch path. presenter.set_scene_selection_locked(app.preload_in_progress) - # First scene: prefer the resolved ``config.scene_path`` so - # ``--synthetic-scene`` (materialised to a temp USDZ) and any autostaged - # default are honoured; a dropdown selection overrides it below. scene_path: Any = config.scene_path variant = _resolve_scene_variant(scene_options, scene_path, config.variant) - presenter.acknowledge_scene_change(scene_path, variant) try: - # ``need_selection`` drives the scene-selection wait: True on first - # launch (unless ``--auto-start``) and again every time the user - # exits a scene back to the selector. While waiting the engine is - # idle, so the video model stops generating -- the whole point of the - # exit-scene affordance for long-running demos -- without closing the - # window or dropping the warmed model. - need_selection = not args.auto_start - # --auto-start + --preload-scenes: wait for the preloader to finish - # before the auto-load below so it hits the cache instead of racing - # the background thread with a second parse of the same USDZ. - if args.auto_start and app.preload_in_progress(): + if app.preload_in_progress(): presenter.wait_while_preloading(app.preload_in_progress) + presenter.acknowledge_scene_change(scene_path, variant) while True: - if need_selection: - request = presenter.wait_for_scene_selection() - if request is None: - break # window closed before any scene was loaded - scene_path, variant = request - presenter.acknowledge_scene_change(scene_path, variant) - need_selection = False - presenter.set_engine_active(True) - # load_scene parses the USDZ on a background thread while keeping + # load_scene compiles the map on a background thread while keeping # the window responsive; it returns False if the window closed # (or a new scene was requested) before the parse finished, so # we skip run_scene and let the pending checks below decide @@ -888,11 +776,8 @@ def _run_slangpy_hud(args: argparse.Namespace) -> None: app.run_scene() presenter.set_engine_active(False) if presenter.pending_exit_scene: - # ``x`` / bound exit button: tear down the rollout and go - # back to the selector over the same presenter. presenter.acknowledge_exit_scene() - need_selection = True - continue + break requested = presenter.pending_scene_change if requested is None: # Window closed (X / ESC) during load or run; we're done. @@ -909,8 +794,7 @@ def _run_streaming(args: argparse.Namespace) -> None: Like :func:`_run_slangpy_hud` but with a long-lived :class:`MJPEGStreamingPresenter`: the HTTP server / browser sessions stay - alive across scene swaps while only the scene is rebuilt. Scene options are - serialised to JSON for the in-browser ``/scenes`` dropdown. + alive across scene swaps while only the scene is rebuilt. """ from omnidreams_game_engine.input.keyboard import KeyboardState @@ -927,7 +811,6 @@ def _run_streaming(args: argparse.Namespace) -> None: _apply_cuda_visible_devices_inplace(args.cuda_visible_devices) _resolve_demo_paths(args) - _materialize_synthetic_scene_for_picker(args) scene_options = _discover_scene_options(args.scene_dir, args.scene) if not args.scene.exists() and scene_options: args.scene = scene_options[0].path @@ -941,9 +824,7 @@ def _run_streaming(args: argparse.Namespace) -> None: "example_world_model.yaml)" ) if not scene_options and not args.scene.exists(): - raise SystemExit( - f"--scene path does not exist and --scene-dir contains no scenes: {args.scene}" - ) + raise SystemExit(f"--map path does not exist: {args.scene}") # JSON-serialisable form of the discovered scenes for the browser # ``/scenes`` endpoint. Thumbnails are JPEG-encoded once at startup @@ -977,8 +858,9 @@ def _run_streaming(args: argparse.Namespace) -> None: bind_host, bind_port = parse_bind(args.stream_mjpeg) placeholder_keyboard = KeyboardState() + renderer_settings = _cli.renderer_settings_from_args(args) presenter = MJPEGStreamingPresenter( - raster=RasterConfig(), + raster=renderer_settings.raster, keyboard=placeholder_keyboard, bind_host=bind_host, bind_port=bind_port, @@ -986,11 +868,9 @@ def _run_streaming(args: argparse.Namespace) -> None: thumbnails=thumbnails, ) - # Build the backend + engine once so the model warms up (on the - # pipeline worker thread) while the browser is still choosing the first - # scene. The app rebinds the presenter to its long-lived keyboard and - # switches scenes in place via ``app.load_scene``, keeping the warmed - # model resident across scene changes. + # Build the backend + engine once. The app rebinds the presenter to its + # long-lived keyboard and switches scenes in place via ``app.load_scene``, + # keeping the warmed model resident across scene changes. config, backend = _cli.prepare_config_and_backend(args) app = _build_application( args, @@ -1013,39 +893,14 @@ def _run_streaming(args: argparse.Namespace) -> None: presenter.set_scene_selection_locked(app.preload_in_progress) try: - if args.auto_start: - # Headless / scriptable start: skip the browser scene picker and - # load the resolved ``--scene`` (or the first discovered scene) - # immediately. This lets the demo run with no GUI/browser. - # --auto-start + --preload-scenes: let the preloader finish first - # so the auto-load hits the cache instead of racing a second parse. - if app.preload_in_progress(): - presenter.wait_while_preloading(app.preload_in_progress) - scene_path = config.scene_path - variant = _resolve_scene_variant(scene_options, scene_path, config.variant) - presenter.acknowledge_scene_change(scene_path, variant) - logger.info( - f"[demo] streaming auto-start scene -> {scene_path.name} " - f"variant={variant!r}", - ) - else: - # Don't auto-load: always wait for the browser to pick the first - # scene. There's no Vulkan window to show progress in, so the - # presenter publishes an idle overlay frame ("Loading world - # model..." while warmup runs in the background, then "Select a - # scene to begin") so connected browsers have something to render - # while the wait spins. - logger.info( - "[demo] streaming presenter waiting for first scene selection...", - ) - request = presenter.wait_for_scene_selection() - if request is None: - return # presenter closed before any selection (Ctrl-C) - scene_path, variant = request - presenter.acknowledge_scene_change(scene_path, variant) - logger.info( - f"[demo] streaming initial scene -> {scene_path.name} variant={variant!r}", - ) + if app.preload_in_progress(): + presenter.wait_while_preloading(app.preload_in_progress) + scene_path = config.scene_path + variant = _resolve_scene_variant(scene_options, scene_path, config.variant) + presenter.acknowledge_scene_change(scene_path, variant) + logger.info( + f"[demo] streaming initial scene -> {scene_path.name} variant={variant!r}", + ) while True: # load_scene parses the USDZ on a background thread while the @@ -1127,36 +982,12 @@ def _resolve_demo_paths(args: argparse.Namespace) -> None: setattr(args, attr, _project_path(value)) if args.manifest is not None: args.manifest = _cli.resolve_manifest_path(args.manifest) + args.renderer_config = _cli.resolve_app_config_path(args.renderer_config) + args.game_config = _cli.resolve_app_config_path(args.game_config) if args.control_assets_dir is not None: args.control_assets_dir = _project_path(args.control_assets_dir) -def _materialize_synthetic_scene_for_picker(args: argparse.Namespace) -> None: - """Build ``--synthetic-scene`` before scene-picker discovery. - - The single-scene ``--no-hud`` path lets ``cli.prepare_config_and_backend`` - materialize the synthetic USDZ. HUD and MJPEG modes discover scenes first - so the picker can show options before a scene is loaded; those modes need - the temporary USDZ to exist before discovery runs. - """ - if not args.synthetic_scene: - return - scene_path = build_synthetic_scene_to_temp( - initial_rgb_path=args.synthetic_initial_rgb, - prompt=args.synthetic_prompt, - ) - logger.info( - "[interactive-drive] synthetic scene materialised at {}", - scene_path, - ) - args.scene = scene_path - # The synthetic inputs have been consumed into the temp USDZ. Clear them so - # the later shared backend builder treats the scene as a normal archive. - args.synthetic_scene = False - args.synthetic_initial_rgb = None - args.synthetic_prompt = None - - def _project_path(path: Path) -> Path: path = Path(path).expanduser() if path.is_absolute(): @@ -1173,22 +1004,13 @@ def _discover_scene_options( if selected_scene.exists(): paths.add(selected_scene.resolve()) if scene_dir.is_dir(): - paths.update(path.resolve() for path in scene_dir.glob("*.usdz")) + paths.update(path.resolve() for path in scene_dir.glob(f"*{GAME_MAP_SUFFIX}")) if selected_scene.parent.is_dir(): - paths.update(path.resolve() for path in selected_scene.parent.glob("*.usdz")) - - # Group archives by scene UUID so the per-weather sibling files - # (``clipgt--.usdz``) collapse into one scene with a variant - # selector. Single-archive scenes stay a group of one. - grouped: dict[str, dict[str, Path]] = {} - for path in sorted(paths): - uuid, variant = _scenes.parse_scene_stem(path.stem) - grouped.setdefault(uuid, {})[variant] = path - - options = tuple( - _scene_option_for_group(variant_paths) - for _uuid, variant_paths in sorted(grouped.items()) - ) + paths.update( + path.resolve() for path in selected_scene.parent.glob(f"*{GAME_MAP_SUFFIX}") + ) + + options = tuple(_scene_option_for_game_map(path) for path in sorted(paths)) logger.info( "[demo] discovered scenes: " + ( @@ -1202,99 +1024,46 @@ def _discover_scene_options( return options -def _order_variants(variants: Iterable[str]) -> tuple[str, ...]: - """Order variant slugs with ``default`` first, then the rest sorted.""" - unique = set(variants) - ordered = ["default"] if "default" in unique else [] - ordered.extend(sorted(unique - {"default"})) - return tuple(ordered) - - -def _scene_option_for_group(variant_paths: dict[str, Path]) -> SceneOption: - """Build one :class:`SceneOption` from a scene's variant archive(s). - - Multiple siblings => the weather variants are the files. A single archive - => fall back to in-zip variant discovery (legacy / synthetic scenes). - """ - if len(variant_paths) > 1: - variants = _order_variants(variant_paths.keys()) - base_path = variant_paths.get("default") or variant_paths[variants[0]] - resolved_paths = dict(variant_paths) - variant_thumbnails = _load_variant_file_thumbnails(resolved_paths, variants) - else: - base_path = next(iter(variant_paths.values())) - variants = _discover_variants(base_path) - resolved_paths = {variant: base_path for variant in variants} - variant_thumbnails = _load_variant_thumbnails(base_path, variants) - # Use the first variant's preview for the scene row so the scene and - # variant dropdowns agree, falling back to the standalone loader. - thumbnail = ( - variant_thumbnails.get(variants[0]) - or variant_thumbnails.get("default") - or _load_scene_thumbnail(base_path) - ) +def _scene_option_for_game_map(path: Path) -> SceneOption: + """Build scene metadata from the authored game map.""" + header = load_game_map_header(path) + variants = tuple(variant.name for variant in header.variants) + thumbnails: dict[str, Image.Image] = {} + generated_thumbnail: Image.Image | None = None + for variant in header.variants: + if variant.image is None: + if generated_thumbnail is None: + game_map = load_game_map(path) + generated_thumbnail = _make_thumbnail( + Image.fromarray( + render_spawn_first_frame(game_map, game_map.default_spawn) + ), + SCENE_THUMB_SIZE, + ) + thumbnails[variant.name] = generated_thumbnail.copy() + continue + try: + with Image.open(resolve_seed_asset(path, variant.image)) as image: + thumbnails[variant.name] = _make_thumbnail( + image.convert("RGB"), SCENE_THUMB_SIZE + ) + except OSError: + continue + thumbnail = thumbnails.get("default") or next(iter(thumbnails.values()), None) return SceneOption( - label=_scene_label(base_path), - path=base_path, + label=header.name, + path=path, variants=variants, thumbnail=thumbnail, - variant_thumbnails=variant_thumbnails, - variant_paths=resolved_paths, + variant_thumbnails=thumbnails, + variant_paths={variant: path for variant in variants}, ) -def _scene_label(path: Path) -> str: - scene_names = { - "0d404ff7-2b66-498c-b047-1ed8cded60d4": "Quiet Suburban Boulevard", - "7bd1eb2f-c375-44ee-b4ca-55473e0773a9": "Late Night Arrival in the Neighborhood", - "e2993759-36e1-4d97-868f-e2a737f1eb68": "Afternoon Commute Past the Park", - } - # Key by bare UUID so the label is stable across weather variant archives. - uuid, _variant = _scenes.parse_scene_stem(path.stem) - return scene_names.get(uuid, path.stem) - - -def _discover_variants(scene_path: Path) -> tuple[str, ...]: - variants: set[str] = set() - try: - with zipfile.ZipFile(scene_path, "r") as zf: - for name in zf.namelist(): - if "/" in name: - continue - stem = Path(name).stem - if name.startswith("first_image") and name.endswith(".png"): - variant = _scenes.variant_from_stem(stem, "first_image") - elif name.startswith("prompt") and name.endswith(".txt"): - variant = _scenes.variant_from_stem(stem, "prompt") - else: - continue - if variant is not None: - variants.add(variant) - except (OSError, zipfile.BadZipFile): - return ("default",) - # A bare ``default`` (prompt.txt / first_image.png) duplicates the first - # numbered variant, so when numbered variants exist we expose just those -- - # "1" is then the default selection. Scenes with no numbered variants show - # a single "default". - numbered = [value for value in variants if value != "default"] - if numbered: - numbered.sort(key=lambda v: (not v.isdigit(), int(v) if v.isdigit() else v)) - return tuple(numbered) - return ("default",) - - def _resolve_scene_variant( scene_options: tuple[SceneOption, ...], scene_path: Any, variant: str ) -> str: - """Return a variant that actually exists for *scene_path*. - - Numbered scenes no longer carry a bare ``default`` entry, so a configured - ``--variant default`` (or anything the scene lacks) falls back to the - scene's first variant rather than a selection the dropdown can't show. - For weather sibling archives, the path itself is also a source of truth: - ``clipgt-...-snow.usdz`` with the default CLI variant should start as - ``snow``, not silently load the clear/base archive. - """ + """Return a variant that exists for *scene_path*.""" for option in scene_options: path_variant = _scene_option_variant_for_path(option, scene_path) if path_variant is None: @@ -1316,9 +1085,6 @@ def _scene_option_variant_for_path(option: SceneOption, scene_path: Any) -> str resolved = None raw = str(scene_path) - # ``variant_paths`` is the authoritative map for weather sibling archives. - # For legacy single-archive scenes it maps every in-zip variant to the same - # path, so the first variant intentionally matches the old fallback. for variant, path in option.variant_paths.items(): if _same_scene_path(path, raw, resolved): return variant @@ -1333,86 +1099,6 @@ def _same_scene_path(path: Path, raw: str, resolved: Path | None) -> bool: return (resolved is not None and path == resolved) or str(path) == raw -def _load_scene_thumbnail(scene_path: Path) -> Image.Image | None: - try: - with zipfile.ZipFile(scene_path, "r") as zf: - names = [ - name - for name in zf.namelist() - if "/" not in name - and name.startswith("first_image") - and name.endswith(".png") - ] - if not names: - return None - name = "first_image.png" if "first_image.png" in names else sorted(names)[0] - with Image.open(io.BytesIO(zf.read(name))) as image: - return _make_thumbnail(image.convert("RGB"), SCENE_THUMB_SIZE) - except (OSError, zipfile.BadZipFile): - return None - - -def _load_variant_thumbnails( - scene_path: Path, variants: tuple[str, ...] -) -> dict[str, Image.Image]: - """Per-variant preview thumbnails for the HUD variant dropdown. - - Mirrors :func:`scene_loader._discover_first_images`: a bundle may ship - ``first_image_.png`` per variant alongside ``first_image.png`` - (the ``"default"`` variant). Each referenced image is decoded once; - variants without a dedicated image fall back to the default so every - dropdown row still shows a preview. Returns an empty mapping when the - archive has no parseable first images. - """ - decoded: dict[str, Image.Image] = {} - try: - with zipfile.ZipFile(scene_path, "r") as zf: - names_by_variant: dict[str, str] = {} - for name in zf.namelist(): - if ( - "/" in name - or not name.startswith("first_image") - or not name.endswith(".png") - ): - continue - variant = _scenes.variant_from_stem(Path(name).stem, "first_image") - if variant is not None: - names_by_variant[variant] = name - for variant, name in names_by_variant.items(): - with Image.open(io.BytesIO(zf.read(name))) as image: - decoded[variant] = _make_thumbnail( - image.convert("RGB"), SCENE_THUMB_SIZE - ) - except (OSError, zipfile.BadZipFile): - return {} - if not decoded: - return {} - default = decoded.get("default") or next(iter(decoded.values())) - return {variant: decoded.get(variant, default) for variant in variants} - - -def _load_variant_file_thumbnails( - variant_paths: dict[str, Path], variants: tuple[str, ...] -) -> dict[str, Image.Image]: - """Per-variant thumbnails when each variant is its own archive. - - Each preview comes from that variant file's ``first_image.png``; variants - with no usable preview reuse the default. Empty mapping if nothing decoded. - """ - decoded: dict[str, Image.Image] = {} - for variant in variants: - path = variant_paths.get(variant) - if path is None: - continue - thumb = _load_scene_thumbnail(path) - if thumb is not None: - decoded[variant] = thumb - if not decoded: - return {} - fallback = decoded.get("default") or next(iter(decoded.values())) - return {variant: decoded.get(variant, fallback) for variant in variants} - - def _make_thumbnail(image: Image.Image, size: tuple[int, int]) -> Image.Image: thumb = Image.new("RGB", size, (20, 20, 30)) fitted = _fit_image(image, size) @@ -1422,15 +1108,10 @@ def _make_thumbnail(image: Image.Image, size: tuple[int, int]) -> Image.Image: def _variant_label(variant: str) -> str: labels = { - # Per-weather variant archives. "default": "Default (Clear)", "clear": "Clear", "snow": "Snowstorm", "rain": "Night Rain", - # Legacy in-archive numbered variants. - "1": "Bright Midday Sun", - "2": "Snowstorm", - "3": "Night with Heavy Rain", } return labels.get(variant, variant) diff --git a/apps/crazy_robotaxi/crazy_robotaxi/configs/default_game.yaml b/apps/crazy_robotaxi/crazy_robotaxi/configs/default_game.yaml new file mode 100644 index 000000000..9a80e364c --- /dev/null +++ b/apps/crazy_robotaxi/crazy_robotaxi/configs/default_game.yaml @@ -0,0 +1,73 @@ +schema_version: 1 +rules: + waypoint_spacing_m: 10.0 + pickup_grid_spacing_m: 60.0 + pickup_min_distance_m: 20.0 + initial_pickup_max_distance_m: 200.0 + pickup_radius_m: 5.0 + dropoff_radius_m: 6.0 + fare_min_route_distance_m: 200.0 + fare_max_route_distance_m: 250.0 + target_speed_mps: 10.0 + grace_s: 8.0 + min_time_s: 12.0 + max_time_s: 45.0 + trip_time_multiplier: 2.0 + base_fare_points: 500 + bonus_points_per_second: 100 + event_banner_s: 2.0 + global_time_s: 60.0 + dropoff_time_bonus_s: 30.0 + ground_snap_max_absolute_rotation_deg: 10.0 + ground_snap_settle_fraction: 0.25 +vehicle: + wheel_base_m: 2.8 + max_steer_rad: 0.5 + steer_rate_rad_per_s: 0.55 + steer_return_rate_rad_per_s: 0.9 + speed_limit_enabled: true + max_speed_mps: 31.2928 + max_reverse_speed_mps: 6.0 + max_accel_mps2: 10.0 + max_brake_mps2: 14.0 + max_lateral_accel_mps2: 8.5 + drag_mps2: 0.7 + mass_kg: 1550.0 + tire_grip: 1.35 + rolling_resistance: 0.015 + aero_drag_coefficient: 0.42 + collision_restitution: 0.22 + collision_friction: 0.65 + max_collision_yaw_rate_radps: 0.35 + suspension_stiffness: 42.0 + suspension_damping: 9.0 + suspension_travel_m: 0.22 + suspension_visual_gain: 0.15 + max_body_roll_rad: 0.16 + max_body_pitch_rad: 0.5 + actor_collision_enabled: true + static_collision_enabled: true + aabb_length_m: 4.8 + aabb_width_m: 2.0 + aabb_height_m: 1.6 + reverse_accel_mps2: 10.0 + handbrake_decel_mps2: 18.0 + handbrake_yaw_gain: 3.25 + max_handbrake_yaw_rate_radps: 1.5 + curb_collision_restitution: 0.45 + curb_forward_momentum_retention: 0.85 + input_dt_cap_s: 0.1 + input_activation_threshold: 0.01 + keyboard_steer_rate_per_s: 3.5 + keyboard_steer_return_rate_per_s: 5.0 + direction_change_accel_multiplier: 1.5 + speed_taper_knee_fraction: 0.62 + speed_taper_low_floor: 0.2 + speed_taper_high_floor: 0.05 + speed_taper_exponent: 3.0 + manual_coast_decel_mps2: 0.5 + ragdoll_grip_rate: 4.0 + ragdoll_yaw_response_rate: 8.0 + handbrake_yaw_response_rate: 4.0 + handbrake_lateral_damping_rate: 2.0 + handbrake_lateral_accel_scale: 0.35 diff --git a/apps/crazy_robotaxi/crazy_robotaxi/configs/default_renderer.yaml b/apps/crazy_robotaxi/crazy_robotaxi/configs/default_renderer.yaml new file mode 100644 index 000000000..82153edf6 --- /dev/null +++ b/apps/crazy_robotaxi/crazy_robotaxi/configs/default_renderer.yaml @@ -0,0 +1,24 @@ +schema_version: 1 +raster: + width: 1280 + height: 704 + near_plane_m: 0.1 + far_plane_m: 200.0 + fog_start_m: 40.0 + fog_end_m: 140.0 + fog_power: 1.5 + triangle_raytrace_distance_m: 25.0 + triangle_raytrace_edge_samples: 8 + lane_segment_interval_m: 0.05 + polyline_segment_interval_m: 0.8 + line_width_px: 12.0 + pole_width_px: 5.0 + dual_line_offset_m: 0.1 +bev: + enabled: true + width: 1024 + height: 1024 + height_m: 75.0 + fov_deg: 60.0 + tilt_deg: 0.0 +visual_flare_enabled: false diff --git a/apps/crazy_robotaxi/crazy_robotaxi/driving.py b/apps/crazy_robotaxi/crazy_robotaxi/driving.py index 46aafb241..ef42debd3 100644 --- a/apps/crazy_robotaxi/crazy_robotaxi/driving.py +++ b/apps/crazy_robotaxi/crazy_robotaxi/driving.py @@ -30,6 +30,57 @@ class TaxiVehicleConfig(VehicleConfig): max_handbrake_yaw_rate_radps: float = 1.5 max_lateral_accel_mps2: float = 8.5 max_body_roll_rad: float = 0.16 + curb_collision_restitution: float = 0.45 + """Rebound coefficient for map curbs and other static barriers.""" + + curb_forward_momentum_retention: float = 0.85 + """Minimum forward-speed fraction retained through a glancing curb impact.""" + + input_dt_cap_s: float = 0.1 + """Maximum elapsed input time applied by one keyboard update.""" + + input_activation_threshold: float = 0.01 + """Minimum pedal or steering magnitude treated as active input.""" + + keyboard_steer_rate_per_s: float = 3.5 + """Keyboard steering-input rise rate.""" + + keyboard_steer_return_rate_per_s: float = 5.0 + """Keyboard steering-input centering rate.""" + + direction_change_accel_multiplier: float = 1.5 + """Braking multiplier while changing travel direction.""" + + speed_taper_knee_fraction: float = 0.62 + """Fraction of maximum speed where acceleration tapering changes regime.""" + + speed_taper_low_floor: float = 0.2 + """Minimum acceleration fraction below the speed-taper knee.""" + + speed_taper_high_floor: float = 0.05 + """Minimum acceleration fraction above the speed-taper knee.""" + + speed_taper_exponent: float = 3.0 + """Acceleration falloff exponent above the speed-taper knee.""" + + manual_coast_decel_mps2: float = 0.5 + """Manual-control deceleration while neither pedal is active.""" + + ragdoll_grip_rate: float = 4.0 + """Lateral-velocity damping rate during collision recovery.""" + + ragdoll_yaw_response_rate: float = 8.0 + """Yaw response rate during collision recovery.""" + + handbrake_yaw_response_rate: float = 4.0 + """Yaw response rate while the handbrake is active.""" + + handbrake_lateral_damping_rate: float = 2.0 + """Lateral-velocity damping rate while the handbrake is active.""" + + handbrake_lateral_accel_scale: float = 0.35 + """Body-roll acceleration scale while the handbrake is active.""" + speed_limit_enabled: bool = True actor_collision_enabled: bool = True static_collision_enabled: bool = True @@ -50,8 +101,9 @@ class TaxiKeyboardState: class TaxiKeyboardDriveState: """Taxi-only snappy steering, handbrake, and brake-to-reverse controls.""" - def __init__(self, control: Any) -> None: + def __init__(self, control: Any, vehicle: TaxiVehicleConfig | None = None) -> None: self._control = control + self._vehicle = vehicle or TaxiVehicleConfig() self._pressed: set[str] = set() self._state = TaxiKeyboardState() self._last_update_s = time.monotonic() @@ -88,14 +140,18 @@ def set_key(self, keysym: str, down: bool) -> bool: def update(self) -> TaxiKeyboardState: """Advance the input smoother and publish one Taxi drive command.""" now = time.monotonic() - dt_s = max(0.0, min(0.1, now - self._last_update_s)) + dt_s = max(0.0, min(self._vehicle.input_dt_cap_s, now - self._last_update_s)) self._last_update_s = now target_steer = 0.0 if {"a", "left"} & self._pressed: target_steer += 1.0 if {"d", "right"} & self._pressed: target_steer -= 1.0 - steer_rate = 3.5 if abs(target_steer) > 0.0 else 5.0 + steer_rate = ( + self._vehicle.keyboard_steer_rate_per_s + if abs(target_steer) > 0.0 + else self._vehicle.keyboard_steer_return_rate_per_s + ) steer = _move_towards(self._state.steering, target_steer, steer_rate * dt_s) throttle = 1.0 if {"w", "up"} & self._pressed else 0.0 brake = 1.0 if {"s", "down"} & self._pressed else 0.0 @@ -139,17 +195,19 @@ def release_control(self) -> None: def _update_target_speed( self, *, throttle: float, brake: float, handbrake: bool, dt_s: float ) -> float: - vehicle = TaxiVehicleConfig() + vehicle = self._vehicle speed = self._state.target_speed_mps if handbrake: speed = _move_towards(speed, 0.0, vehicle.handbrake_decel_mps2 * dt_s) - elif throttle > 0.01: + elif throttle > vehicle.input_activation_threshold: accel = vehicle.max_accel_mps2 * throttle * dt_s if speed < 0.0: - speed = min(0.0, speed + accel * 1.5) + speed = min( + 0.0, speed + accel * vehicle.direction_change_accel_multiplier + ) else: speed += accel - elif brake > 0.01: + elif brake > vehicle.input_activation_threshold: if speed > 0.0: speed = max(0.0, speed - vehicle.max_brake_mps2 * brake * dt_s) else: @@ -213,7 +271,7 @@ def integrate_taxi_vehicle( speed = _move_towards(speed, 0.0, vehicle.handbrake_decel_mps2 * dt_s) elif command.manual_control: intended_direction = -1.0 if command.reverse else 1.0 - if command.brake > 0.01: + if command.brake > vehicle.input_activation_threshold: speed = _apply_brake_or_reverse( speed, command, @@ -222,32 +280,38 @@ def integrate_taxi_vehicle( reverse_accel_mps2=vehicle.reverse_accel_mps2, max_reverse_speed_mps=vehicle.max_reverse_speed_mps, ) - elif command.throttle > 0.01: + elif command.throttle > vehicle.input_activation_threshold: accel = vehicle.max_accel_mps2 * command.throttle * dt_s if intended_direction < 0.0: speed -= accel elif vehicle.speed_limit_enabled: max_speed = vehicle.max_speed_mps current = abs(speed) - high_speed_knee = max_speed * 0.62 + high_speed_knee = max_speed * vehicle.speed_taper_knee_fraction if current < high_speed_knee: - taper = max(0.2, 1.0 - (current / high_speed_knee) ** 2 * 0.5) + taper = max( + vehicle.speed_taper_low_floor, + 1.0 - (current / high_speed_knee) ** 2 * 0.5, + ) else: excess = (current - high_speed_knee) / max( 1e-6, max_speed - high_speed_knee ) - taper = max(0.05, 0.5 * (1.0 - excess) ** 3) + taper = max( + vehicle.speed_taper_high_floor, + 0.5 * (1.0 - excess) ** vehicle.speed_taper_exponent, + ) speed += accel * taper else: speed += accel else: - speed = _move_towards(speed, 0.0, 0.5 * dt_s) + speed = _move_towards(speed, 0.0, vehicle.manual_coast_decel_mps2 * dt_s) if vehicle.speed_limit_enabled: speed = float( np.clip(speed, -vehicle.max_reverse_speed_mps, vehicle.max_speed_mps) ) else: - if command.brake > 0.01: + if command.brake > vehicle.input_activation_threshold: speed = _apply_brake_or_reverse( speed, command, @@ -256,11 +320,15 @@ def integrate_taxi_vehicle( reverse_accel_mps2=vehicle.reverse_accel_mps2, max_reverse_speed_mps=vehicle.max_reverse_speed_mps, ) - elif command.throttle > 0.01: + elif command.throttle > vehicle.input_activation_threshold: intended_direction = -1.0 if command.reverse else 1.0 accel_delta = command.throttle * vehicle.max_accel_mps2 * dt_s if speed * intended_direction < 0.0: - speed = _move_towards(speed, 0.0, accel_delta * 1.5) + speed = _move_towards( + speed, + 0.0, + accel_delta * vehicle.direction_change_accel_multiplier, + ) else: speed += intended_direction * accel_delta else: @@ -307,23 +375,25 @@ def integrate_taxi_vehicle( ) if state.ragdoll_active: lateral_speed = float(np.dot(velocity, left)) - grip = float(np.clip(vehicle.tire_grip * dt_s * 4.0, 0.0, 1.0)) + grip = float( + np.clip(vehicle.tire_grip * dt_s * vehicle.ragdoll_grip_rate, 0.0, 1.0) + ) velocity -= left * lateral_speed * grip longitudinal_speed = float(np.dot(velocity, forward)) velocity += forward * (speed - longitudinal_speed) - response = 1.0 - math.exp(-8.0 * dt_s) + response = 1.0 - math.exp(-vehicle.ragdoll_yaw_response_rate * dt_s) yaw_rate = ( state.yaw_rate_radps + (commanded_yaw_rate - state.yaw_rate_radps) * response ) elif command.handbrake: - response = 1.0 - math.exp(-4.0 * dt_s) + response = 1.0 - math.exp(-vehicle.handbrake_yaw_response_rate * dt_s) yaw_rate = ( state.yaw_rate_radps + (commanded_yaw_rate - state.yaw_rate_radps) * response ) lateral_speed = float(np.dot(velocity, left)) - lateral_speed *= max(0.0, 1.0 - 2.0 * dt_s) + lateral_speed *= max(0.0, 1.0 - vehicle.handbrake_lateral_damping_rate * dt_s) else: # Normal steering is an arcade control target, while PhysX remains # responsible for contact impulses and tire forces. Running a second @@ -346,7 +416,11 @@ def integrate_taxi_vehicle( y_m = state.y_m + float(velocity[1]) * dt_s longitudinal_accel = (speed - state.speed_mps) / max(dt_s, 1e-6) - lateral_accel = speed * yaw_rate * (0.35 if command.handbrake else 1.0) + lateral_accel = ( + speed + * yaw_rate + * (vehicle.handbrake_lateral_accel_scale if command.handbrake else 1.0) + ) target_pitch = float( np.clip( -longitudinal_accel diff --git a/apps/crazy_robotaxi/crazy_robotaxi/game.py b/apps/crazy_robotaxi/crazy_robotaxi/game.py index cddbf0eac..778aff886 100644 --- a/apps/crazy_robotaxi/crazy_robotaxi/game.py +++ b/apps/crazy_robotaxi/crazy_robotaxi/game.py @@ -14,6 +14,10 @@ import numpy as np import numpy.typing as npt from omnidreams_game_engine.camera import FThetaCameraModel +from omnidreams_game_engine.game_map.vicinity import ( + GameMapVicinity, + GameMapVicinityResolver, +) from omnidreams_game_engine.math3d import ( extract_yaw_from_transform, invert_transform, @@ -21,7 +25,6 @@ rig_pose_from_state, rig_pose_from_vehicle_state, ) -from omnidreams_game_engine.simulation.map_bounds import MapBounds from omnidreams_game_engine.types import ( CameraCalibration, TrajectoryChunk, @@ -38,6 +41,7 @@ ) from crazy_robotaxi.navigation import ( LanePosition, + NavigationFareRegion, NavigationLane, NavigationWaypoint, RoutePlan, @@ -62,9 +66,6 @@ class TaxiGameConfig: vehicle: TaxiVehicleConfig = TaxiVehicleConfig() """Taxi-only control and vehicle-dynamics configuration.""" - traffic_density: float = 0.4 - """Fraction of recorded motor traffic retained in Taxi mode.""" - seed: int | None = None """Debug seed mixed with the scene ID; ``None`` uses fresh entropy.""" @@ -74,9 +75,6 @@ class TaxiGameConfig: pickup_grid_spacing_m: float = 60.0 """Grid spacing used to distribute simultaneous pickup points across the map.""" - waypoint_edge_margin_m: float = 100.0 - """Minimum map-boundary clearance for pickup and dropoff targets.""" - pickup_min_distance_m: float = 20.0 """Minimum straight-line distance from the ego to a newly selected pickup.""" @@ -131,14 +129,16 @@ class TaxiGameConfig: alignment_diagnostics_enabled: bool = False """Whether the rollout captures frame-synchronized alignment evidence.""" + ground_snap_max_absolute_rotation_deg: float = 10.0 + """Maximum ground rotation accepted by the taxi ground snapper.""" + + ground_snap_settle_fraction: float = 0.25 + """Fraction of stale ground attitude removed after an invalid sample.""" + def __post_init__(self) -> None: """Validate Taxi-only values at configuration time.""" - if not 0.0 < self.traffic_density <= 1.0: - raise ValueError("traffic_density must be greater than 0 and at most 1") if self.pickup_grid_spacing_m <= 0.0: raise ValueError("pickup_grid_spacing_m must be positive") - if self.waypoint_edge_margin_m < 0.0: - raise ValueError("waypoint_edge_margin_m must be non-negative") @dataclass(frozen=True) @@ -513,15 +513,18 @@ def __init__( reference_route_world: npt.NDArray[np.float32], navigation_routes_world: tuple[npt.NDArray[np.float32], ...] = (), navigation_lanes: tuple[NavigationLane, ...] = (), + fare_regions: tuple[NavigationFareRegion, ...] = (), initial_state: VehicleState, config: TaxiGameConfig, initial_camera: CameraCalibration | None = None, - map_bounds: MapBounds | None = None, high_score_store: HighScoreStore | None = None, + vicinity_resolver: GameMapVicinityResolver | None = None, ) -> None: self._config = config rng_seed = None if config.seed is None else _stable_seed(scene_id, config.seed) self._rng = np.random.default_rng(rng_seed) + self._vicinity_resolver = vicinity_resolver + self._vicinity: GameMapVicinity | None = None offset = float(self._rng.uniform(0.0, config.waypoint_spacing_m)) if navigation_lanes: self._navigation = TaxiNavigationMap(navigation_lanes) @@ -533,8 +536,10 @@ def __init__( ) self._waypoints = self._navigation.sample_waypoints( config.waypoint_spacing_m, offset + ) + self._navigation.sample_fare_regions( + fare_regions, config.waypoint_spacing_m, self._rng ) - self._eligible_waypoint_indices = self._safe_waypoint_indices(map_bounds) + self._eligible_waypoint_indices = tuple(range(len(self._waypoints))) self._pickup_point_indices = self._sample_pickup_point_indices() self._phase: TaxiPhase = "seeking_pickup" self._session_state: TaxiSessionState = "playing" @@ -664,6 +669,12 @@ def snapshot(self, vehicle_state: VehicleState) -> TaxiGameSnapshot: def _snapshot_for_pose( self, x_m: float, y_m: float, yaw_rad: float ) -> TaxiGameSnapshot: + if self._vicinity_resolver is not None: + self._vicinity = self._vicinity_resolver.resolve( + x_m, + y_m, + previous=self._vicinity, + ) target_index = ( min( self._available_pickup_indices, @@ -690,6 +701,16 @@ def _snapshot_for_pose( float(target[0]), float(target[1]), ) + passenger_indices = tuple( + index + for index in self._available_pickup_indices + if self._waypoints[index].element_id is None + or ( + self._vicinity is not None + and self._waypoints[index].element_id + in self._vicinity.pedestrian_element_ids + ) + ) return TaxiGameSnapshot( phase=self._phase, target_xyz_m=(float(target[0]), float(target[1]), float(target[2])), @@ -725,7 +746,7 @@ def _snapshot_for_pose( pickup_passengers_xyz_m=( tuple( _passenger_xyz_tuple(self._waypoints[index]) - for index in self._available_pickup_indices + for index in passenger_indices ) if self._phase == "seeking_pickup" else () @@ -787,27 +808,6 @@ def _sample_pickup_point_indices(self) -> tuple[int, ...]: return selected return self._eligible_waypoint_indices[:2] - def _safe_waypoint_indices(self, map_bounds: MapBounds | None) -> tuple[int, ...]: - """Return targets separated from the playable map boundary.""" - if map_bounds is None or self._config.waypoint_edge_margin_m == 0.0: - return tuple(range(len(self._waypoints))) - margin = self._config.waypoint_edge_margin_m - eligible = tuple( - index - for index, waypoint in enumerate(self._waypoints) - if map_bounds.x_min + margin - <= float(waypoint.xyz_m[0]) - <= map_bounds.x_max - margin - and map_bounds.y_min + margin - <= float(waypoint.xyz_m[1]) - <= map_bounds.y_max - margin - ) - if len(eligible) < 2: - raise ValueError( - "Taxi map-boundary margin leaves fewer than two eligible waypoints." - ) - return eligible - def _collected_pickup_index(self, x_m: float, y_m: float) -> int | None: """Return the closest available pickup inside its activation radius.""" candidates = ( @@ -979,15 +979,22 @@ def _select_dropoff( vehicle_state.yaw_rad, ) pickup = self._waypoints[pickup_index] - fallback_source = LanePosition( - lane_index=pickup.lane_index, - distance_along_lane_m=pickup.distance_along_lane_m, - lateral_distance_m=0.0, - heading_error_rad=0.0, + fallback_sources = pickup.departure_anchors or ( + LanePosition( + lane_index=pickup.lane_index, + distance_along_lane_m=pickup.distance_along_lane_m, + lateral_distance_m=0.0, + heading_error_rad=0.0, + ), + ) + source_candidates = ( + fallback_sources + if pickup.departure_anchors + else tuple( + source for source in sources if source.lateral_distance_m <= 12.0 + ) + or fallback_sources ) - source_candidates = tuple( - source for source in sources if source.lateral_distance_m <= 12.0 - ) or (fallback_source,) for source in source_candidates: route_distances = self._navigation.route_distances(source, self._waypoints) diff --git a/apps/crazy_robotaxi/crazy_robotaxi/game_settings.py b/apps/crazy_robotaxi/crazy_robotaxi/game_settings.py new file mode 100644 index 000000000..72519b8e7 --- /dev/null +++ b/apps/crazy_robotaxi/crazy_robotaxi/game_settings.py @@ -0,0 +1,107 @@ +# SPDX-License-Identifier: Apache-2.0 +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. + +"""Strict Crazy Robotaxi gameplay configuration loading.""" + +from __future__ import annotations + +from dataclasses import fields +from pathlib import Path + +from omnidreams_game_engine.yaml_config import ( + StrictConfigError, + load_yaml_mapping, + require_bool, + require_exact_keys, + require_float, + require_mapping, + require_version, +) + +from crazy_robotaxi.driving import TaxiVehicleConfig +from crazy_robotaxi.game import TaxiGameConfig + +_RUNTIME_GAME_FIELDS = { + "enabled", + "seed", + "high_scores_path", + "alignment_diagnostics_enabled", +} +_RULE_FIELDS = ( + {field.name for field in fields(TaxiGameConfig)} + - _RUNTIME_GAME_FIELDS + - {"vehicle"} +) +_VEHICLE_FIELDS = {field.name for field in fields(TaxiVehicleConfig)} +_VEHICLE_BOOL_FIELDS = { + "speed_limit_enabled", + "actor_collision_enabled", + "static_collision_enabled", +} + + +def load_game_settings(path: Path) -> TaxiGameConfig: + """Load a complete Crazy Robotaxi game YAML document. + + Args: + path: Game YAML path. + + Returns: + Validated game rules and taxi dynamics. + """ + doc = load_yaml_mapping(path) + require_exact_keys(doc, {"schema_version", "rules", "vehicle"}, "game") + require_version(doc, "game") + raw_rules = require_mapping(doc["rules"], "game.rules") + require_exact_keys(raw_rules, _RULE_FIELDS, "game.rules") + rules = { + name: require_float(raw_rules[name], f"game.rules.{name}", minimum=0.0) + for name in _RULE_FIELDS + } + for integer_name in ("base_fare_points", "bonus_points_per_second"): + value = raw_rules[integer_name] + if type(value) is not int or value < 0: + raise StrictConfigError( + f"game.rules.{integer_name} must be a nonnegative integer" + ) + rules[integer_name] = value + if rules["fare_min_route_distance_m"] > rules["fare_max_route_distance_m"]: + raise StrictConfigError( + "game.rules.fare_min_route_distance_m must not exceed fare_max_route_distance_m" + ) + if rules["min_time_s"] > rules["max_time_s"]: + raise StrictConfigError("game.rules.min_time_s must not exceed max_time_s") + + raw_vehicle = require_mapping(doc["vehicle"], "game.vehicle") + require_exact_keys(raw_vehicle, _VEHICLE_FIELDS, "game.vehicle") + vehicle_values = { + name: require_bool(raw_vehicle[name], f"game.vehicle.{name}") + if name in _VEHICLE_BOOL_FIELDS + else require_float(raw_vehicle[name], f"game.vehicle.{name}", minimum=0.0) + for name in _VEHICLE_FIELDS + } + for name in ( + "wheel_base_m", + "max_speed_mps", + "aabb_length_m", + "aabb_width_m", + "aabb_height_m", + "input_dt_cap_s", + "speed_taper_knee_fraction", + ): + if vehicle_values[name] <= 0.0: + raise StrictConfigError(f"game.vehicle.{name} must be positive") + for name in ( + "speed_taper_knee_fraction", + "speed_taper_low_floor", + "speed_taper_high_floor", + "curb_collision_restitution", + "curb_forward_momentum_retention", + ): + if vehicle_values[name] > 1.0: + raise StrictConfigError(f"game.vehicle.{name} must be at most 1") + return TaxiGameConfig( + enabled=True, + vehicle=TaxiVehicleConfig(**vehicle_values), + **rules, + ) diff --git a/apps/crazy_robotaxi/crazy_robotaxi/hud_presenter.py b/apps/crazy_robotaxi/crazy_robotaxi/hud_presenter.py index 95dff2fcf..7f2bb9a97 100644 --- a/apps/crazy_robotaxi/crazy_robotaxi/hud_presenter.py +++ b/apps/crazy_robotaxi/crazy_robotaxi/hud_presenter.py @@ -16,10 +16,10 @@ """Single-process native HUD presenter for Crazy Robotaxi. Plugs into the same engine seam as ``SlangPyPresenter`` (``--no-hud``), but -draws PIL chrome (panel, dropdowns, BEV minimap, speed/wheel/pedals) over the +draws PIL chrome (panel, controls, BEV minimap, speed/wheel/pedals) over the camera frame -- composited on CUDA when interop is available, else on the CPU. -Input goes straight to ``KeyboardState``; dropdown scene/variant changes are -handled by the demo's outer loop over this same long-lived window. +Input goes straight to ``KeyboardState``; scene and variant changes are handled +by the demo's outer loop over this same long-lived window. """ from __future__ import annotations @@ -62,7 +62,6 @@ from crazy_robotaxi.game import ( TaxiCameraMarkerProjection, TaxiGameSnapshot, - project_segment_pose_to_bev, project_target_pose_to_bev, project_taxi_markers_to_camera, ) @@ -434,7 +433,6 @@ def __init__( self._wheel = wheel self._taxi_camera_calibration: CameraCalibration | None = None self._taxi_camera_models: dict[tuple[int, int], FThetaCameraModel] = {} - self._taxi_enclosure_segments_world = np.empty((0, 2, 3), dtype=np.float32) self._taxi_name_buffer = "" self._last_taxi_session_state: str | None = None # Composition-root key extensions (e.g. live-edit abilities): keysym @@ -447,15 +445,11 @@ def __init__( # Late-imports of helpers we need at runtime; ``demo`` imports # this module via the presenter factory, so direct top-level # imports would be circular. - from crazy_robotaxi.cli import ( - KeyboardDriveState, - _scene_label, - ) + from crazy_robotaxi.cli import KeyboardDriveState self._keyboard_drive = KeyboardDriveState( KeyboardStateDriveSink(keyboard, source="keyboard") ) - self._scene_label_fn = _scene_label # Window + device + surface setup mirrors SlangPyPresenter's # but with a resizable HUD-sized window and a display texture @@ -516,7 +510,6 @@ def __init__( self._wheel_base_size: int | None = None self._wheel_rotation_cache: _LRUCache = _LRUCache(maxsize=480) self._pedal_cache: _LRUCache = _LRUCache(maxsize=16) - self._scene_thumb_cache: dict[Any, Image.Image | None] = {} self._variant_thumb_cache: dict[tuple[Any, str], Image.Image | None] = {} self._bev_panel_cache_key: _BevPanelKey | None = None self._bev_panel_cache: Image.Image | None = None @@ -571,14 +564,10 @@ def __init__( # slangpy uploads to per frame. See :func:`_allocate_canvas`. self._canvas_buffer, self._canvas = _allocate_canvas(*self._configured_size) - self._scene_dropdown_open = False self._variant_dropdown_open = False - self._scene_header_rect: tuple[int, int, int, int] | None = None self._variant_header_rect: tuple[int, int, int, int] | None = None self._postprocess_rect: tuple[int, int, int, int] | None = None - self._scene_item_rects: list[tuple[tuple[int, int, int, int], Any]] = [] self._variant_item_rects: list[tuple[tuple[int, int, int, int], str]] = [] - self._hovered_scene_label: str | None = None self._hovered_variant: str | None = None self._mouse_pos: tuple[int, int] = (0, 0) self._speed_mph: float = 0.0 @@ -588,33 +577,26 @@ def __init__( self._current_scene = args.scene self._selected_variant = args.variant self._has_camera_frame = False - # ``_engine_active`` is False during the initial scene-selection - # wait (when the user hasn't picked a scene yet AND - # ``--auto-start`` was off) and during the brief gap between - # scene changes. Drives the camera-area placeholder text together - # with the model-warmup state below. Toggled by the demo wrapper - # via :meth:`set_engine_active` around each scene's run. + # ``_engine_active`` is False during the brief gap between scene + # changes. It drives the camera-area placeholder text together with + # the model-warmup state below. self._engine_active = False # Model-warmup status, wired by the demo via :meth:`set_model_status`. - # ``_model_can_prewarm`` is True when the model loads at startup - # (so the selection wait shows "Loading world model..." instead of - # "Load Scene"); ``_model_ready_probe`` returns True once warmup - # has finished. Defaults are inert so a presenter used without the - # wiring (or before it) behaves like the old "Load Scene" prompt. + # ``_model_can_prewarm`` is True when the model loads at startup; + # ``_model_ready_probe`` returns True once warmup has finished. self._model_can_prewarm = False self._model_ready_probe: Callable[[], bool] = lambda: True - # Scene-selection lock, wired by the demo via + # Scene-change lock, wired by the demo via # :meth:`set_scene_selection_locked` when --preload-scenes is on. - # While the probe returns True the scene/variant dropdowns ignore - # clicks and the placeholder shows a "Preloading scenes..." hint, so - # the user can't pick a scene until every scene is cached. + # While the probe returns True, changes wait until every scene is + # cached. self._scene_selection_locked_probe: Callable[[], bool] = lambda: False self._postprocess_preset = "" self._postprocess_enabled = False self._postprocess_callback: Callable[[bool], None] = lambda enabled: None - # Scene-change request set by the dropdown click handlers. The - # outer demo loop checks this after each ``app.run_scene`` returns: + # The outer demo loop checks scene-change requests after each + # ``app.run_scene`` returns: # if non-None, it calls ``app.load_scene`` for the requested scene # and re-enters the engine over the SAME presenter so the slangpy # window (and the warmed model) stay alive. @@ -622,9 +604,8 @@ def __init__( # Exit-to-selection request set by the ``x`` key or a wheel's bound # exit button. The outer demo loop checks this (ahead of # ``pending_scene_change``) after each ``app.run_scene`` returns: when - # set it tears down the rollout and re-enters the scene selector over - # the SAME presenter, so a long-running demo can stop the video model - # generating without closing the window or reloading the model. + # set it tears down the rollout. A future main menu can handle the + # request without rebuilding the presenter or reloading the model. self._pending_exit_scene = False self._key_codes = self._build_key_codes() @@ -1479,8 +1460,6 @@ def _render_canvas( ) self._draw_taxi_hud(draw, camera_area) - if self._scene_dropdown_open: - self._draw_scene_dropdown(canvas, draw) if self._variant_dropdown_open: self._draw_variant_dropdown(canvas, draw) @@ -1976,15 +1955,9 @@ def _draw_panel( header_x = px + margin header_w = panel_size[0] - margin * 2 header_y = py + 8 - variant_y = header_y + bar_h + 4 + variant_y = header_y postprocess_available = bool(self._postprocess_preset) postprocess_y = variant_y + bar_h + 4 - self._scene_header_rect = ( - header_x, - header_y, - header_x + header_w, - header_y + bar_h, - ) self._variant_header_rect = ( header_x, variant_y, @@ -2054,21 +2027,13 @@ def _get_panel_chrome(self, panel_size: tuple[int, int]) -> Image.Image: has_multiple_variants = ( current_scene_option is not None and len(current_scene_option.variants) > 1 ) - # ``_engine_active`` is part of the cache key because the scene - # header label changes shape ("Select Scene" when the engine - # isn't running, "Running clipgt-...\u2026" when it is). The - # demo wrapper also explicitly invalidates the cache around - # ``set_engine_active``; the key entry here is belt-and-braces. key = ( panel_size, str(self._current_scene), self._selected_variant, - self._scene_dropdown_open, self._variant_dropdown_open, has_multiple_variants, self._engine_active, - # Scene header reads "Preloading scenes..." while locked, so the - # lock state has to invalidate the cached chrome too. self._scene_selection_locked(), self._postprocess_preset, self._postprocess_enabled, @@ -2088,45 +2053,9 @@ def _get_panel_chrome(self, panel_size: tuple[int, int]) -> Image.Image: header_w = panel_w - margin * 2 header_y = 8 - # Scene header bar. Reserve room on the left for the green - # status dot and on the right for the dropdown arrow; the - # remaining width is what the scene label gets to use, and we - # truncate-with-ellipsis to fit. - scene_rect = (margin, header_y, margin + header_w, header_y + bar_h) - d.rounded_rectangle(scene_rect, radius=6, fill=HEADER_BG + (255,)) - d.ellipse( - (margin + 8, header_y + 11, margin + 18, header_y + 21), - fill=NVIDIA_GREEN + (255,), - ) - if self._engine_active: - scene_label_full = ( - f"Running {self._scene_label_fn(self._current_scene)}\u2026" - ) - elif self._scene_selection_locked(): - scene_label_full = "Preloading scenes\u2026" - else: - scene_label_full = "Select Scene" - scene_label_max_w = header_w - 26 - 30 # 26 left for dot, 30 right for arrow - scene_label = _truncate_text_to_width( - self._font_small, scene_label_full, scene_label_max_w - ) - d.text( - (margin + 26, header_y + 6), - scene_label, - fill=TEXT_COLOR, - font=self._font_small, - ) - scene_arrow = "\u25b2" if self._scene_dropdown_open else "\u25bc" - d.text( - (margin + header_w - 24, header_y + 6), - scene_arrow, - fill=LABEL_COLOR, - font=self._font_small, - ) - # Variant header bar. Same truncation pattern in case the # variant string is unusually long. - variant_y = header_y + bar_h + 4 + variant_y = header_y variant_rect = (margin, variant_y, margin + header_w, variant_y + bar_h) d.rounded_rectangle(variant_rect, radius=6, fill=HEADER_BG + (255,)) variant_full = f"Variant: {self._selected_variant}" @@ -2161,9 +2090,7 @@ def _get_panel_chrome(self, panel_size: tuple[int, int]) -> Image.Image: margin + header_w, postprocess_y + bar_h, ) - postprocess_clickable = not ( - self._scene_dropdown_open or self._variant_dropdown_open - ) + postprocess_clickable = not self._variant_dropdown_open d.rounded_rectangle(postprocess_rect, radius=6, fill=HEADER_BG + (255,)) d.text( (margin + 10, postprocess_y + 6), @@ -2528,7 +2455,6 @@ def _draw_bev( inner[1] + offset_y + scaled_h, ) marker_size = max(10, min(inner_w, inner_h) // 14) - self._draw_bev_taxi_enclosure(draw, content_rect) self._draw_bev_taxi_target(draw, content_rect, marker_size) ego_dimensions = getattr(self, "_latest_ego_dimensions_lwh", None) @@ -2540,43 +2466,6 @@ def _draw_bev( ego_dimensions = _DEFAULT_EGO_DIMENSIONS_LWH self._draw_bev_ego_footprint(draw, content_rect, ego_dimensions, bev) - def _draw_bev_taxi_enclosure( - self, - draw: ImageDraw.ImageDraw, - content_rect: tuple[int, int, int, int], - ) -> None: - frame = getattr(self, "_latest_presented_frame", None) - bev = self._bev_config - if ( - frame is None - or frame.bev_rig_to_world is None - or bev is None - or not bev.enabled - ): - return - left, top, right, bottom = content_rect - content_w = right - left - content_h = bottom - top - if content_w <= 0 or content_h <= 0: - return - for segment in self._taxi_enclosure_segments_world: - projected = project_segment_pose_to_bev( - segment, frame.bev_rig_to_world, bev - ) - if projected is None: - continue - start, end = projected - draw.line( - ( - round(left + start[0] * content_w), - round(top + start[1] * content_h), - round(left + end[0] * content_w), - round(top + end[1] * content_h), - ), - fill=(235, 50, 50, 255), - width=4, - ) - def _draw_bev_taxi_target( self, draw: ImageDraw.ImageDraw, @@ -2684,56 +2573,7 @@ def _draw_bev_ego_footprint( # is unambiguous even when the footprint is only a few pixels wide. draw.line((footprint[0], footprint[1]), fill=(220, 255, 170, 255), width=2) - # -- Dropdowns --------------------------------------------------- - - def _draw_scene_dropdown( - self, canvas: Image.Image, draw: ImageDraw.ImageDraw - ) -> None: - if self._scene_header_rect is None: - return - sx, _sy, sr, sb = self._scene_header_rect - if not self._scene_options: - empty = (sx, sb + 2, sr, sb + 36) - draw.rounded_rectangle(empty, radius=6, fill=(70, 35, 35, 255)) - draw.text( - (sx + 12, sb + 9), - f"No scenes found in {self._args.scene_dir}", - fill=(255, 220, 220), - font=self._font_tiny, - ) - return - - item_h = 80 - items_top = sb + 2 - bg = (sx, items_top - 1, sr, items_top + len(self._scene_options) * item_h + 1) - draw.rounded_rectangle(bg, radius=6, fill=(35, 35, 50, 255)) - draw.rounded_rectangle(bg, radius=6, outline=(60, 60, 80, 255), width=1) - - self._scene_item_rects = [] - for idx, scene in enumerate(self._scene_options): - top = items_top + idx * item_h - rect = (sx, top, sr, top + item_h) - self._scene_item_rects.append((rect, scene)) - if self._scene_option_matches_current(scene): - draw.rectangle(rect, fill=ACTIVE_BG + (255,)) - elif scene.label == self._hovered_scene_label: - draw.rectangle(rect, fill=HOVER_BG + (255,)) - text_x = rect[0] + 12 - text_y = top + item_h // 2 - 8 - thumb = self._get_scene_thumbnail(scene) - if thumb is not None: - tw, th = thumb.size - tx = rect[0] + 6 - ty = top + max(0, (item_h - th) // 2) - canvas.paste(thumb, (tx, ty)) - draw.rectangle( - (tx, ty, tx + tw, ty + th), outline=(60, 60, 80, 255), width=1 - ) - text_x = tx + tw + 10 - label = _truncate_text_to_width( - self._font_tiny, scene.label, max(0, rect[2] - text_x - 8) - ) - draw.text((text_x, text_y), label, fill=TEXT_COLOR, font=self._font_tiny) + # -- Variant dropdown -------------------------------------------- def _draw_variant_dropdown( self, canvas: Image.Image, draw: ImageDraw.ImageDraw @@ -2745,7 +2585,7 @@ def _draw_variant_dropdown( return vx, vy, vr, vb = self._variant_header_rect # Taller rows when the scene ships per-variant previews, matching the - # scene dropdown; fall back to compact text-only rows otherwise. + # picker; fall back to compact text-only rows otherwise. has_thumbs = bool(scene_option.variant_thumbnails) item_h = 80 if has_thumbs else 34 items_top = vb + 2 @@ -2784,18 +2624,6 @@ def _draw_variant_dropdown( ) draw.text((text_x, text_y), label, fill=TEXT_COLOR, font=self._font_tiny) - def _get_scene_thumbnail(self, scene: Any) -> Image.Image | None: - if scene.path in self._scene_thumb_cache: - return self._scene_thumb_cache[scene.path] - if scene.thumbnail is None: - self._scene_thumb_cache[scene.path] = None - return None - thumb = scene.thumbnail - if thumb.mode != "RGBA": - thumb = thumb.convert("RGBA") - self._scene_thumb_cache[scene.path] = thumb - return thumb - def _get_variant_thumbnail(self, scene: Any, variant: str) -> Image.Image | None: key = (scene.path, variant) if key in self._variant_thumb_cache: @@ -3054,13 +2882,7 @@ def _on_mouse_event(self, event: Any) -> None: self._handle_click(self._mouse_pos) def _update_hover(self, pos: tuple[int, int]) -> None: - self._hovered_scene_label = None self._hovered_variant = None - if self._scene_dropdown_open: - for rect, scene in self._scene_item_rects: - if _rect_contains(rect, pos): - self._hovered_scene_label = scene.label - break if self._variant_dropdown_open: for rect, variant in self._variant_item_rects: if _rect_contains(rect, pos): @@ -3068,7 +2890,7 @@ def _update_hover(self, pos: tuple[int, int]) -> None: break def _handle_click(self, pos: tuple[int, int]) -> None: - dropdown_open = self._scene_dropdown_open or self._variant_dropdown_open + dropdown_open = self._variant_dropdown_open if ( not dropdown_open and self._postprocess_rect @@ -3085,13 +2907,10 @@ def _handle_click(self, pos: tuple[int, int]) -> None: self._postprocess_preset, ) return - # While scenes are still preloading, the scene/variant dropdowns are - # locked (the only mouse-clickable HUD elements), so ignore clicks - # until every scene is cached and selection is instant. + # Ignore variant changes until preloading completes. if self._scene_selection_locked(): return - # Variant dropdown sits on top of the scene dropdown items, so - # check it first. + # Handle variant rows before the header or underlying controls. if self._variant_dropdown_open: for rect, variant in self._variant_item_rects: if _rect_contains(rect, pos): @@ -3105,27 +2924,9 @@ def _handle_click(self, pos: tuple[int, int]) -> None: self._variant_dropdown_open = False return - if self._scene_dropdown_open: - for rect, scene in self._scene_item_rects: - if _rect_contains(rect, pos): - self._restart_backend(scene) - return - if self._scene_header_rect and _rect_contains(self._scene_header_rect, pos): - self._scene_dropdown_open = False - return - self._scene_dropdown_open = False - return - - if self._scene_header_rect and _rect_contains(self._scene_header_rect, pos): - self._scene_dropdown_open = True - self._variant_dropdown_open = False - self._panel_chrome_cache_key = None - return - # The variant dropdown is only meaningful once a scene is actually - # loaded/running. Before that (the initial selection wait and the gap - # between scene switches) the engine is inactive, so ignore clicks on - # the variant header. + # loaded/running. Between scene switches the engine is inactive, so + # ignore clicks on the variant header. current_scene_option = self._current_scene_option() if ( self._engine_active @@ -3135,16 +2936,10 @@ def _handle_click(self, pos: tuple[int, int]) -> None: and len(current_scene_option.variants) > 1 ): self._variant_dropdown_open = True - self._scene_dropdown_open = False self._panel_chrome_cache_key = None # -- Scene / variant restart ------------------------------------- - def _restart_backend(self, scene: Any) -> None: - logger.info(f"[demo] switching scene -> {scene.label}") - new_variant = scene.variants[0] if scene.variants else "default" - self._signal_scene_change(scene.path, new_variant) - def _restart_variant(self, variant: str) -> None: if variant == self._selected_variant: self._variant_dropdown_open = False @@ -3163,7 +2958,7 @@ def _signal_scene_change(self, scene_path: Any, variant: str) -> None: self._args.scene = scene_path self._args.variant = variant self._pending_scene_change = (scene_path, variant) - # An explicit scene pick supersedes any pending exit-to-selection. + # An explicit scene change supersedes any pending exit request. self._pending_exit_scene = False self._should_close_flag = True # Drop the wheel-set DriverCommand so a stale steer/throttle doesn't @@ -3172,15 +2967,14 @@ def _signal_scene_change(self, scene_path: Any, variant: str) -> None: @property def pending_scene_change(self) -> tuple[Any, str] | None: - """``(scene_path, variant)`` if a dropdown click is pending, else None.""" + """Return the requested ``(scene_path, variant)``, if any.""" return self._pending_scene_change def exit_scene(self) -> None: - """Request a return to the scene selector, keeping the window alive. + """Request that the current scene end while keeping the window alive. - Like :meth:`_signal_scene_change` but sets ``_pending_exit_scene`` so - the outer loop re-enters :meth:`wait_for_scene_selection`. No-op unless - a scene is running. + A future main menu can consume this separately from a direct scene + change. No-op unless a scene is running. """ if not self._engine_active: return @@ -3194,16 +2988,11 @@ def exit_scene(self) -> None: @property def pending_exit_scene(self) -> bool: - """True when the user asked to exit back to the scene selector.""" + """Return whether the user asked to end the current scene.""" return self._pending_exit_scene def acknowledge_exit_scene(self) -> None: - """Clear the exit request and reset per-rollout view state for the selector. - - Called before the outer loop re-enters :meth:`wait_for_scene_selection`; - resets the close flag, the selected variant, and the last rollout's - camera/BEV/speed so the selector doesn't ghost them. - """ + """Clear the exit request and reset per-rollout view state.""" self._pending_exit_scene = False self._should_close_flag = False self._reset_selected_variant_to_default() @@ -3300,9 +3089,9 @@ def wait_for_scene_selection(self) -> tuple[Any, str] | None: def wait_while_preloading(self, in_progress: Callable[[], bool]) -> None: """Pump the "Preloading scenes..." chrome until ``in_progress()`` clears. - Used by ``--auto-start`` + ``--preload-scenes`` so the auto-loaded - scene waits for the background preloader to finish (and is served from - its cache) instead of racing it with a second parse of the same USDZ. + Used by ``--preload-scenes`` so the selected map waits for the + background preloader to finish instead of racing it with a second + compile. Returns early if the window closes. Keeps the engine inactive so the camera area shows the locked "Preloading scenes..." placeholder. """ @@ -3337,7 +3126,6 @@ def _reset_scene_view_state(self) -> None: :meth:`acknowledge_exit_scene` so the next state starts clean instead of ghosting the just-ended rollout. """ - self._scene_dropdown_open = False self._variant_dropdown_open = False # The next backend renders into a fresh ``rgb_host_uint8`` buffer # so the camera resize cache (keyed on ``id(buffer)``) is now @@ -3389,7 +3177,7 @@ def bind_keyboard(self, keyboard: KeyboardState) -> None: KeyboardStateDriveSink(keyboard, source="keyboard") ) - def configure_taxi_hud(self, bev: BevConfig) -> None: + def configure_taxi_hud(self, bev: BevConfig, vehicle: Any = None) -> None: """Configure BEV projection used by taxi target overlays.""" from crazy_robotaxi.driving import ( TaxiKeyboardDriveState, @@ -3397,7 +3185,7 @@ def configure_taxi_hud(self, bev: BevConfig) -> None: self._bev_config = bev self._keyboard_drive = TaxiKeyboardDriveState( - KeyboardStateDriveSink(self._keyboard, source="keyboard") + KeyboardStateDriveSink(self._keyboard, source="keyboard"), vehicle ) def configure_taxi_camera(self, calibration: CameraCalibration) -> None: @@ -3405,13 +3193,6 @@ def configure_taxi_camera(self, calibration: CameraCalibration) -> None: self._taxi_camera_calibration = calibration self._taxi_camera_models.clear() - def configure_taxi_enclosure(self, segments_world: np.ndarray) -> None: - """Configure static Taxi-only closure lines drawn over the BEV.""" - segments = np.asarray(segments_world, dtype=np.float32) - if segments.ndim != 3 or segments.shape[1:] != (2, 3): - raise ValueError("Taxi enclosure segments must have shape (N, 2, 3).") - self._taxi_enclosure_segments_world = segments.copy() - # -- Module-level helpers --------------------------------------------- diff --git a/apps/crazy_robotaxi/crazy_robotaxi/map_cli.py b/apps/crazy_robotaxi/crazy_robotaxi/map_cli.py new file mode 100644 index 000000000..651e4e36c --- /dev/null +++ b/apps/crazy_robotaxi/crazy_robotaxi/map_cli.py @@ -0,0 +1,64 @@ +# SPDX-License-Identifier: Apache-2.0 +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. + +"""Command-line validation and previews for Crazy Robotaxi maps.""" + +from __future__ import annotations + +import argparse +from pathlib import Path +from typing import Sequence + +from omnidreams_game_engine.game_map import ( + compile_game_map, + load_game_map, + write_game_map_preview, + write_spawn_first_frame_preview, +) + + +def build_parser() -> argparse.ArgumentParser: + """Build the semantic-map utility parser.""" + parser = argparse.ArgumentParser(prog="crazy-robotaxi-map") + subparsers = parser.add_subparsers(dest="command", required=True) + validate = subparsers.add_parser("validate", help="validate a semantic YAML map") + validate.add_argument("map", type=Path) + preview = subparsers.add_parser("preview", help="write a top-down SVG preview") + preview.add_argument("map", type=Path) + preview.add_argument("--output", type=Path, required=True) + preview_spawn = subparsers.add_parser( + "preview-spawn", help="write a spawn-aligned synthetic first-frame PNG" + ) + preview_spawn.add_argument("map", type=Path) + preview_spawn.add_argument("--output", type=Path, required=True) + preview_spawn.add_argument("--spawn", help="spawn id (defaults to the first)") + compile_parser = subparsers.add_parser( + "compile", help="populate the private renderer cache" + ) + compile_parser.add_argument("map", type=Path) + compile_parser.add_argument( + "--force-map-recompile", + action="store_true", + help="Rebuild the compiled archive even when its cache entry exists.", + ) + return parser + + +def main(argv: Sequence[str] | None = None) -> None: + """Run a semantic-map utility command.""" + args = build_parser().parse_args(argv) + if args.command == "validate": + game_map = load_game_map(args.map) + print( + f"valid: {game_map.name} ({len(game_map.elements)} elements, " + f"{len(game_map.lanes)} directed lanes)" + ) + elif args.command == "preview": + print(write_game_map_preview(args.map, args.output)) + elif args.command == "preview-spawn": + print( + write_spawn_first_frame_preview(args.map, args.output, spawn_id=args.spawn) + ) + else: + compiled = compile_game_map(args.map, force=args.force_map_recompile) + print(compiled.archive_path) diff --git a/apps/crazy_robotaxi/crazy_robotaxi/maps/boulevard_district.robotaxi.yaml b/apps/crazy_robotaxi/crazy_robotaxi/maps/boulevard_district.robotaxi.yaml new file mode 100644 index 000000000..a9d265ac5 --- /dev/null +++ b/apps/crazy_robotaxi/crazy_robotaxi/maps/boulevard_district.robotaxi.yaml @@ -0,0 +1,408 @@ +schema_version: 1 +id: crazy-robotaxi-boulevard-district +name: Original Boulevard District + +compiler: + sample_spacing_m: 2.0 + ground_margin_m: 20.0 + intersection_connector_samples: 8 + +profiles: + arterial: + lane_width_m: 3.6 + curb_offset_m: 0.6 + lanes: [backward, backward, forward, forward] + speed_limit_mps: 15.6 + lane_marking: {style: DASHED_SINGLE, color: WHITE} + divider_markings: + - {style: DASHED_SINGLE, color: WHITE} + - {style: SOLID_GROUP, color: YELLOW} + - {style: DASHED_SINGLE, color: WHITE} + street: + lane_width_m: 3.6 + curb_offset_m: 0.6 + lanes: [backward, forward] + speed_limit_mps: 11.2 + lane_marking: {style: SOLID_GROUP, color: YELLOW} + divider_markings: + - {style: SOLID_GROUP, color: YELLOW} + local: + lane_width_m: 3.2 + curb_offset_m: 0.5 + lanes: [backward, forward] + speed_limit_mps: 8.9 + lane_marking: {style: SOLID_GROUP, color: YELLOW} + divider_markings: + - {style: SOLID_GROUP, color: YELLOW} + +nodes: + # Original spawn-area arterial and northwest surface-street grid. The + # elevated highway and its ramps are deliberately omitted. + - {id: west_arterial_end, type: cul_de_sac, pose: {x_m: -180, y_m: 0}, culdesac_radius_m: 12} + - {id: west_arterial_crossing, type: intersection, pose: {x_m: -128, y_m: 0}} + - {id: central_arterial_crossing, type: intersection, pose: {x_m: 75, y_m: 0}} + - {id: diagonal_arterial_crossing, type: intersection, pose: {x_m: 284, y_m: 0}, lane_transition_length_m: 20} + + - {id: west_north_crossing, type: intersection, pose: {x_m: -123, y_m: 100}} + - {id: west_upper_crossing, type: intersection, pose: {x_m: -123, y_m: 198}} + - {id: west_north_end, type: cul_de_sac, pose: {x_m: -123, y_m: 235}, culdesac_radius_m: 9} + - {id: central_north_crossing, type: intersection, pose: {x_m: 75, y_m: 93}} + - {id: central_upper_crossing, type: intersection, pose: {x_m: 75, y_m: 198}, lane_transition_length_m: 20} + - {id: central_north_end, type: cul_de_sac, pose: {x_m: 75, y_m: 260}, culdesac_radius_m: 9} + + - {id: west_south_crossing, type: intersection, pose: {x_m: -128, y_m: -84}} + - {id: west_lower_crossing, type: road_joint, pose: {x_m: -128, y_m: -184}, lane_transition_length_m: 20} + - {id: west_south_end, type: cul_de_sac, pose: {x_m: -128, y_m: -235}, culdesac_radius_m: 9} + - {id: southwest_crossing, type: intersection, pose: {x_m: -44, y_m: -84}} + - {id: southwest_north_end, type: cul_de_sac, pose: {x_m: -44, y_m: -30}, culdesac_radius_m: 8} + - {id: southwest_lot_driveway, type: driveway, pose: {x_m: 20, y_m: -85}} + - {id: central_south_crossing, type: intersection, pose: {x_m: 75, y_m: -85}, lane_transition_length_m: 20} + - {id: central_lower_crossing, type: intersection, pose: {x_m: 75, y_m: -120}} + - {id: central_south_end, type: cul_de_sac, pose: {x_m: 75, y_m: -200}, culdesac_radius_m: 9} + - {id: south_lot_driveway, type: driveway, pose: {x_m: 150, y_m: -120}} + - {id: south_local_end, type: cul_de_sac, pose: {x_m: 210, y_m: -120}, culdesac_radius_m: 9} + + # Diagonal north street and the arterial split visible east of the spawn. + - {id: diagonal_bend_lower, type: road_joint, pose: {x_m: 299, y_m: 95}} + - {id: diagonal_north_crossing, type: intersection, pose: {x_m: 307, y_m: 133}} + - {id: diagonal_bend_upper, type: road_joint, pose: {x_m: 314, y_m: 207}} + - {id: diagonal_north_end, type: cul_de_sac, pose: {x_m: 326, y_m: 285}, culdesac_radius_m: 9} + - {id: southwest_merge, type: intersection, pose: {x_m: 277, y_m: -122}} + - {id: southwest_boulevard_end, type: cul_de_sac, pose: {x_m: 185, y_m: -310}, culdesac_radius_m: 14} + - {id: arterial_merge_crossing, type: intersection, pose: {x_m: 410, y_m: -88}} + + # Northern cross street and its repeated north/south loops. + - {id: north_crossing_470, type: intersection, pose: {x_m: 470, y_m: 133}} + - {id: north_end_470, type: cul_de_sac, pose: {x_m: 470, y_m: 220}, culdesac_radius_m: 9} + - {id: north_crossing_550, type: intersection, pose: {x_m: 550, y_m: 130}} + - {id: north_crossing_630, type: intersection, pose: {x_m: 630, y_m: 126}} + - {id: north_crossing_687, type: road_joint, pose: {x_m: 687, y_m: 129}} + + # Long eastern arterial. + - {id: arterial_crossing_470, type: intersection, pose: {x_m: 470, y_m: -88}} + - {id: arterial_crossing_550, type: intersection, pose: {x_m: 550, y_m: -84}} + - {id: arterial_crossing_630, type: intersection, pose: {x_m: 630, y_m: -86}} + - {id: arterial_crossing_710, type: intersection, pose: {x_m: 710, y_m: -87}} + - {id: arterial_crossing_800, type: intersection, pose: {x_m: 800, y_m: -86}} + - {id: arterial_crossing_860, type: intersection, pose: {x_m: 860, y_m: -88}} + - {id: arterial_crossing_895, type: intersection, pose: {x_m: 895, y_m: -87}} + - {id: east_corner_lot_driveway, type: driveway, pose: {x_m: 955, y_m: -88}} + - {id: arterial_crossing_990, type: intersection, pose: {x_m: 990, y_m: -88}} + - {id: arterial_crossing_1082, type: intersection, pose: {x_m: 1082, y_m: -85}} + - {id: arterial_crossing_1170, type: intersection, pose: {x_m: 1170, y_m: -87}} + - {id: east_arterial_end, type: cul_de_sac, pose: {x_m: 1220, y_m: -88}, culdesac_radius_m: 12} + + # Southern commercial grid. + - {id: south_crossing_630, type: road_joint, pose: {x_m: 630, y_m: -303}, lane_transition_length_m: 20} + - {id: south_crossing_710, type: intersection, pose: {x_m: 710, y_m: -176}} + - {id: lower_crossing_710, type: intersection, pose: {x_m: 710, y_m: -262}, lane_transition_length_m: 20} + - {id: south_end_710, type: cul_de_sac, pose: {x_m: 710, y_m: -310}, culdesac_radius_m: 9} + - {id: south_crossing_860, type: intersection, pose: {x_m: 860, y_m: -176}, lane_transition_length_m: 20} + - {id: lower_crossing_860, type: intersection, pose: {x_m: 860, y_m: -262}} + - {id: south_upper_lot_driveway, type: driveway, pose: {x_m: 895, y_m: -176}} + - {id: south_crossing_930, type: intersection, pose: {x_m: 930, y_m: -176}} + - {id: lower_crossing_930, type: intersection, pose: {x_m: 930, y_m: -266}} + - {id: south_crossing_990, type: intersection, pose: {x_m: 990, y_m: -177}, lane_transition_length_m: 20} + - {id: lower_crossing_990, type: intersection, pose: {x_m: 990, y_m: -265}} + - {id: lower_lot_driveway, type: driveway, pose: {x_m: 785, y_m: -262}} + - {id: lower_crossing_1080, type: road_joint, pose: {x_m: 1080, y_m: -267}} + - {id: south_cul_de_sac_1080, type: cul_de_sac, pose: {x_m: 1082, y_m: -222}, culdesac_radius_m: 10} + - {id: bottom_lot_driveway, type: driveway, pose: {x_m: 550, y_m: -310}} + - {id: bottom_west_end, type: cul_de_sac, pose: {x_m: 470, y_m: -330}, culdesac_radius_m: 9} + + # Far-east surface loop. + - {id: far_east_north_crossing, type: intersection, pose: {x_m: 1082, y_m: -33}} + - {id: far_east_north_end, type: cul_de_sac, pose: {x_m: 1082, y_m: 71}, culdesac_radius_m: 9} + - {id: far_east_bend_southeast, type: driveway, pose: {x_m: 1170, y_m: -33}} + - {id: far_east_bend_northeast, type: road_joint, pose: {x_m: 1170, y_m: 75}} + - {id: far_east_bend_northwest, type: road_joint, pose: {x_m: 1120, y_m: 75}} + - {id: far_east_bend_southwest, type: road_joint, pose: {x_m: 1120, y_m: -33}} + + # Parking masks use original-scene scale and are connected by inferred + # access surfaces rather than authored road edges. + - id: southwest_parking_lot + type: parking_lot + connected_to: southwest_lot_driveway + opening_vertex: 2 + vertices: + - {x_m: -24, y_m: -136} + - {x_m: 14, y_m: -136} + - {x_m: 26, y_m: -136} + - {x_m: 65, y_m: -136} + - {x_m: 65, y_m: -189} + - {x_m: -24, y_m: -189} + - id: south_parking_lot + type: parking_lot + connected_to: south_lot_driveway + opening_vertex: 2 + vertices: + - {x_m: 135, y_m: -150} + - {x_m: 145, y_m: -150} + - {x_m: 157, y_m: -150} + - {x_m: 175, y_m: -150} + - {x_m: 175, y_m: -205} + - {x_m: 135, y_m: -205} + - id: central_bottom_parking_lot + type: parking_lot + connected_to: bottom_lot_driveway + opening_vertex: 4 + vertices: + - {x_m: 510, y_m: -266} + - {x_m: 570, y_m: -266} + - {x_m: 570, y_m: -296} + - {x_m: 556, y_m: -296} + - {x_m: 544, y_m: -296} + - {x_m: 510, y_m: -296} + - id: east_upper_parking_lot + type: parking_lot + connected_to: arterial_crossing_895 + opening_vertex: 2 + vertices: + - {x_m: 870, y_m: -115} + - {x_m: 889, y_m: -115} + - {x_m: 901, y_m: -115} + - {x_m: 920, y_m: -115} + - {x_m: 920, y_m: -165} + - {x_m: 870, y_m: -165} + - id: east_corner_parking_lot + type: parking_lot + connected_to: east_corner_lot_driveway + opening_vertex: 2 + vertices: + - {x_m: 935, y_m: -115} + - {x_m: 949, y_m: -115} + - {x_m: 961, y_m: -115} + - {x_m: 975, y_m: -115} + - {x_m: 975, y_m: -165} + - {x_m: 935, y_m: -165} + - id: east_lower_parking_lot + type: parking_lot + connected_to: south_upper_lot_driveway + opening_vertex: 2 + vertices: + - {x_m: 870, y_m: -195} + - {x_m: 889, y_m: -195} + - {x_m: 901, y_m: -195} + - {x_m: 920, y_m: -195} + - {x_m: 920, y_m: -245} + - {x_m: 870, y_m: -245} + - id: long_lower_parking_lot + type: parking_lot + connected_to: lower_lot_driveway + opening_vertex: 2 + vertices: + - {x_m: 721, y_m: -275} + - {x_m: 779, y_m: -275} + - {x_m: 791, y_m: -275} + - {x_m: 850, y_m: -275} + - {x_m: 850, y_m: -290} + - {x_m: 721, y_m: -290} + - id: far_east_parking_lot + type: parking_lot + connected_to: far_east_bend_southeast + opening_vertex: 5 + vertices: + - {x_m: 1192, y_m: -5} + - {x_m: 1222, y_m: -5} + - {x_m: 1222, y_m: -60} + - {x_m: 1192, y_m: -60} + - {x_m: 1192, y_m: -39} + - {x_m: 1192, y_m: -27} + +roads: + # Spawn-area arterial and neighborhood grid. + - {id: west_arterial_approach, from: west_arterial_end, to: west_arterial_crossing, profile: arterial} + - {id: spawn_arterial, from: west_arterial_crossing, to: central_arterial_crossing, profile: arterial} + - {id: central_arterial, from: central_arterial_crossing, to: diagonal_arterial_crossing, profile: arterial} + - {id: west_north_lower, from: west_arterial_crossing, to: west_north_crossing, profile: street} + - {id: west_north_upper, from: west_north_crossing, to: west_upper_crossing, profile: street} + - {id: west_north_tail, from: west_upper_crossing, to: west_north_end, profile: street} + - {id: central_north_lower, from: central_arterial_crossing, to: central_north_crossing, profile: street} + - {id: central_north_upper, from: central_north_crossing, to: central_upper_crossing, profile: street} + - {id: central_north_tail, from: central_upper_crossing, to: central_north_end, profile: local} + - {id: north_cross_street, from: west_north_crossing, to: central_north_crossing, profile: street} + - {id: upper_cross_street, from: west_upper_crossing, to: central_upper_crossing, profile: street} + - {id: west_south_upper, from: west_arterial_crossing, to: west_south_crossing, profile: street} + - {id: west_south_lower, from: west_south_crossing, to: west_lower_crossing, profile: street} + - {id: west_south_tail, from: west_lower_crossing, to: west_south_end, profile: local} + - {id: southwest_cross_west, from: west_south_crossing, to: southwest_crossing, profile: local} + - {id: southwest_cross_center, from: southwest_crossing, to: southwest_lot_driveway, profile: local} + - {id: southwest_cross_east, from: southwest_lot_driveway, to: central_south_crossing, profile: local} + - {id: southwest_north_stub, from: southwest_crossing, to: southwest_north_end, profile: local} + - {id: central_south_upper, from: central_arterial_crossing, to: central_south_crossing, profile: street} + - {id: central_south_middle, from: central_south_crossing, to: central_lower_crossing, profile: local} + - {id: central_south_tail, from: central_lower_crossing, to: central_south_end, profile: local} + - {id: south_local_west, from: central_lower_crossing, to: south_lot_driveway, profile: local} + - {id: south_local_east, from: south_lot_driveway, to: south_local_end, profile: local} + + # Diagonal street and the two broad arterial branches. + - {id: diagonal_north_lower, from: diagonal_arterial_crossing, to: diagonal_bend_lower, profile: street} + - {id: diagonal_north_middle, from: diagonal_bend_lower, to: diagonal_north_crossing, profile: street} + - {id: diagonal_north_upper, from: diagonal_north_crossing, to: diagonal_bend_upper, profile: street} + - {id: diagonal_north_tail, from: diagonal_bend_upper, to: diagonal_north_end, profile: street} + - {id: diagonal_south_link, from: diagonal_arterial_crossing, to: southwest_merge, profile: arterial} + - id: southwest_boulevard + from: southwest_boulevard_end + to: southwest_merge + profile: arterial + path: + - {x_m: 184, y_m: -255} + - {x_m: 205, y_m: -190} + - {x_m: 242, y_m: -145} + - id: arterial_sweep + from: diagonal_arterial_crossing + to: arterial_merge_crossing + profile: arterial + path: + - {x_m: 332, y_m: -8} + - {x_m: 370, y_m: -39} + - {x_m: 397, y_m: -72} + - id: merge_ramp + from: southwest_merge + to: arterial_merge_crossing + profile: arterial + path: + - {x_m: 322, y_m: -121} + - {x_m: 365, y_m: -108} + - {id: arterial_merge_connector, from: arterial_merge_crossing, to: arterial_crossing_470, profile: arterial} + + # Northern cross street. + - {id: north_cross_307_470, from: diagonal_north_crossing, to: north_crossing_470, profile: street} + - {id: north_cross_470_550, from: north_crossing_470, to: north_crossing_550, profile: street} + - {id: north_cross_550_630, from: north_crossing_550, to: north_crossing_630, profile: street} + - {id: north_cross_630_687, from: north_crossing_630, to: north_crossing_687, profile: street} + - {id: north_470_tail, from: north_crossing_470, to: north_end_470, profile: street} + - id: west_north_loop_leg + from: arterial_crossing_470 + to: north_crossing_470 + profile: street + path: + - {x_m: 470, y_m: -5} + - {x_m: 451, y_m: 35} + - {x_m: 451, y_m: 95} + - {id: north_loop_leg_550, from: arterial_crossing_550, to: north_crossing_550, profile: street} + - {id: north_loop_leg_630, from: arterial_crossing_630, to: north_crossing_630, profile: street} + - id: east_north_loop_leg + from: north_crossing_687 + to: arterial_crossing_710 + profile: street + path: + - {x_m: 696, y_m: 83} + - {x_m: 710, y_m: 45} + + # Eastern arterial and large northern loop. + - {id: arterial_470_550, from: arterial_crossing_470, to: arterial_crossing_550, profile: arterial} + - {id: arterial_550_630, from: arterial_crossing_550, to: arterial_crossing_630, profile: arterial} + - {id: arterial_630_710, from: arterial_crossing_630, to: arterial_crossing_710, profile: arterial} + - {id: arterial_710_800, from: arterial_crossing_710, to: arterial_crossing_800, profile: arterial} + - {id: arterial_800_860, from: arterial_crossing_800, to: arterial_crossing_860, profile: arterial} + - {id: arterial_860_895, from: arterial_crossing_860, to: arterial_crossing_895, profile: arterial} + - {id: arterial_895_955, from: arterial_crossing_895, to: east_corner_lot_driveway, profile: arterial} + - {id: arterial_955_990, from: east_corner_lot_driveway, to: arterial_crossing_990, profile: arterial} + - {id: arterial_990_1082, from: arterial_crossing_990, to: arterial_crossing_1082, profile: arterial} + - {id: arterial_1082_1170, from: arterial_crossing_1082, to: arterial_crossing_1170, profile: arterial} + - {id: arterial_1170_end, from: arterial_crossing_1170, to: east_arterial_end, profile: arterial} + - id: east_north_loop + from: arterial_crossing_800 + to: arterial_crossing_895 + profile: street + path: + - {x_m: 800, y_m: 30} + - {x_m: 800, y_m: 90} + - {x_m: 895, y_m: 90} + - {x_m: 895, y_m: 30} + + # Southern commercial grid and parking courts. + - {id: south_spine_630, from: arterial_crossing_630, to: south_crossing_630, profile: street} + - {id: south_710_upper, from: arterial_crossing_710, to: south_crossing_710, profile: street} + - {id: south_710_lower, from: south_crossing_710, to: lower_crossing_710, profile: street} + - {id: south_710_tail, from: lower_crossing_710, to: south_end_710, profile: local} + - {id: south_860_upper, from: arterial_crossing_860, to: south_crossing_860, profile: street} + - {id: south_860_lower, from: south_crossing_860, to: lower_crossing_860, profile: local} + - {id: south_990_upper, from: arterial_crossing_990, to: south_crossing_990, profile: street} + - {id: south_990_lower, from: south_crossing_990, to: lower_crossing_990, profile: local} + - {id: upper_commercial_west, from: south_crossing_710, to: south_crossing_860, profile: local} + - {id: upper_commercial_center_west, from: south_crossing_860, to: south_upper_lot_driveway, profile: local} + - {id: upper_commercial_center_east, from: south_upper_lot_driveway, to: south_crossing_930, profile: local} + - {id: upper_commercial_east, from: south_crossing_930, to: south_crossing_990, profile: local} + - {id: commercial_930_spine, from: south_crossing_930, to: lower_crossing_930, profile: local} + - {id: lower_commercial_west, from: lower_crossing_710, to: lower_lot_driveway, profile: local} + - {id: lower_commercial_midwest, from: lower_lot_driveway, to: lower_crossing_860, profile: local} + - {id: lower_commercial_center, from: lower_crossing_860, to: lower_crossing_930, profile: local} + - {id: lower_commercial_east, from: lower_crossing_930, to: lower_crossing_990, profile: local} + - {id: lower_commercial_tail, from: lower_crossing_990, to: lower_crossing_1080, profile: local} + - {id: east_south_cul_de_sac, from: lower_crossing_1080, to: south_cul_de_sac_1080, profile: local} + - id: bottom_west_road + from: bottom_west_end + to: bottom_lot_driveway + profile: local + path: + - {x_m: 500, y_m: -320} + - {id: bottom_east_road, from: bottom_lot_driveway, to: south_crossing_630, profile: local} + + # Far-east surface loop and local northern spur. + - {id: far_east_north_spur, from: arterial_crossing_1082, to: far_east_north_crossing, profile: local} + - {id: far_east_north_tail, from: far_east_north_crossing, to: far_east_north_end, profile: local} + - {id: far_east_loop_entry, from: arterial_crossing_1170, to: far_east_bend_southeast, profile: local} + - {id: far_east_loop_east, from: far_east_bend_southeast, to: far_east_bend_northeast, profile: local} + - {id: far_east_loop_north, from: far_east_bend_northeast, to: far_east_bend_northwest, profile: local} + - {id: far_east_loop_west, from: far_east_bend_northwest, to: far_east_bend_southwest, profile: local} + - {id: far_east_loop_exit, from: far_east_bend_southwest, to: far_east_north_crossing, profile: local} + +traffic_count: 100 + +traffic: + - id: arterial_eastbound + nodes: [west_arterial_crossing, arterial_crossing_1170] + end_behavior: reverse + - id: arterial_westbound + nodes: [arterial_crossing_1170, west_arterial_crossing] + end_behavior: reverse + + - id: northwest_clockwise + nodes: [west_arterial_crossing, west_north_crossing, west_upper_crossing, central_upper_crossing, central_north_crossing, central_arterial_crossing] + end_behavior: wrap + - id: northwest_counterclockwise + nodes: [central_upper_crossing, west_upper_crossing, west_north_crossing, west_arterial_crossing, central_arterial_crossing, central_north_crossing] + end_behavior: wrap + + - id: diagonal_clockwise + nodes: [diagonal_arterial_crossing, diagonal_north_crossing, north_crossing_470, arterial_crossing_470, arterial_merge_crossing] + end_behavior: wrap + - id: diagonal_counterclockwise + nodes: [north_crossing_470, diagonal_north_crossing, diagonal_arterial_crossing, arterial_merge_crossing, arterial_crossing_470] + end_behavior: wrap + + - id: southwest_clockwise + nodes: [west_arterial_crossing, west_south_crossing, southwest_crossing, southwest_lot_driveway, central_south_crossing, central_arterial_crossing] + end_behavior: wrap + - id: southwest_counterclockwise + nodes: [southwest_lot_driveway, southwest_crossing, west_south_crossing, west_arterial_crossing, central_arterial_crossing, central_south_crossing] + end_behavior: wrap + + - id: south_grid_clockwise + nodes: [arterial_crossing_710, south_crossing_710, south_crossing_860, lower_crossing_860, lower_crossing_930, lower_crossing_990, south_crossing_990, arterial_crossing_990, arterial_crossing_860, arterial_crossing_800] + end_behavior: wrap + - id: south_grid_counterclockwise + nodes: [lower_crossing_930, lower_crossing_860, south_crossing_860, south_crossing_710, arterial_crossing_710, arterial_crossing_800, arterial_crossing_860, arterial_crossing_990, south_crossing_990, lower_crossing_990] + end_behavior: wrap + + - id: far_east_clockwise + nodes: [arterial_crossing_1082, arterial_crossing_1170, far_east_bend_southeast, far_east_bend_northeast, far_east_bend_northwest, far_east_bend_southwest, far_east_north_crossing] + end_behavior: wrap + - id: far_east_counterclockwise + nodes: [far_east_bend_northwest, far_east_bend_northeast, far_east_bend_southeast, arterial_crossing_1170, arterial_crossing_1082, far_east_north_crossing, far_east_bend_southwest] + end_behavior: wrap + +spawns: + - id: original_area_start + road: spawn_arterial + lane: 2 + distance_m: 128 + variants: + default: + image: package://omnidreams_game_engine/screenshot.jpg + prompt: >- + A forward-facing view from a taxi moving through a quiet suburban + district in daylight, with low commercial buildings, houses, + landscaping, and parked cars. diff --git a/apps/crazy_robotaxi/crazy_robotaxi/maps/minimal_loop.robotaxi.yaml b/apps/crazy_robotaxi/crazy_robotaxi/maps/minimal_loop.robotaxi.yaml new file mode 100644 index 000000000..664411c9b --- /dev/null +++ b/apps/crazy_robotaxi/crazy_robotaxi/maps/minimal_loop.robotaxi.yaml @@ -0,0 +1,100 @@ +schema_version: 1 +id: crazy-robotaxi-minimal-loop +name: Minimal Loop and Parking Lot + +compiler: + sample_spacing_m: 2.0 + ground_margin_m: 20.0 + intersection_connector_samples: 8 + +profiles: + neighborhood: + lane_width_m: 3.6 + curb_offset_m: 0.6 + lanes: [backward, forward] + speed_limit_mps: 13.4 + lane_marking: {style: SOLID_GROUP, color: YELLOW} + divider_markings: + - {style: SOLID_GROUP, color: YELLOW} + +nodes: + - id: hub + type: intersection + pose: {x_m: 0, y_m: 0} + + - id: dead_end + type: cul_de_sac + pose: {x_m: 0, y_m: 30} + culdesac_radius_m: 10 + + - id: neighborhood_lot + type: parking_lot + connected_to: hub + opening_vertex: 2 + vertices: + - {x_m: -35, y_m: -7} + - {x_m: -3, y_m: -7} + - {x_m: 3, y_m: -7} + - {x_m: 35, y_m: -7} + - {x_m: 35, y_m: -27} + - {x_m: -35, y_m: -27} + + - id: neighborhood_loop_northeast_corner + type: road_joint + pose: {x_m: -50, y_m: 75} + + - id: neighborhood_loop_northwest_corner + type: road_joint + pose: {x_m: 50, y_m: 75} + + - id: neighborhood_loop_southeast_corner + type: road_joint + pose: {x_m: -50, y_m: 0} + + - id: neighborhood_loop_southwest_corner + type: road_joint + pose: {x_m: 50, y_m: 0} + +roads: + - id: neighborhood_loop_north + from: neighborhood_loop_northeast_corner + to: neighborhood_loop_northwest_corner + profile: neighborhood + + - id: neighborhood_loop_southeast + from: neighborhood_loop_southeast_corner + to: hub + profile: neighborhood + + - id: neighborhood_loop_southwest + from: neighborhood_loop_southwest_corner + to: hub + profile: neighborhood + + - id: neighborhood_loop_east + from: neighborhood_loop_northeast_corner + to: neighborhood_loop_southeast_corner + profile: neighborhood + + - id: neighborhood_loop_west + from: neighborhood_loop_northwest_corner + to: neighborhood_loop_southwest_corner + profile: neighborhood + + - id: dead_end_road + from: hub + to: dead_end + profile: neighborhood + +spawns: + - id: taxi_start + road: neighborhood_loop_southwest + lane: 1 + distance_m: 5 + variants: + default: + image: package://omnidreams_game_engine/screenshot.jpg + prompt: >- + A forward-facing view from a taxi moving through a quiet suburban + neighborhood in daylight, with houses, low commercial buildings, + landscaping, and parked cars. diff --git a/apps/crazy_robotaxi/crazy_robotaxi/navigation.py b/apps/crazy_robotaxi/crazy_robotaxi/navigation.py index 0b0f37059..b7a7363b6 100644 --- a/apps/crazy_robotaxi/crazy_robotaxi/navigation.py +++ b/apps/crazy_robotaxi/crazy_robotaxi/navigation.py @@ -36,6 +36,49 @@ """Half-width used when a legacy scene provides only a recorded route.""" +def _triangulate_clockwise_polygon(polygon: np.ndarray) -> tuple[np.ndarray, ...]: + """Triangulate a simple clockwise polygon while preserving its full area.""" + vertices = [np.asarray(point[:2], dtype=np.float64) for point in polygon] + triangles: list[np.ndarray] = [] + + def cross(first: np.ndarray, second: np.ndarray, third: np.ndarray) -> float: + first_edge = second - first + second_edge = third - first + return float(first_edge[0] * second_edge[1] - first_edge[1] * second_edge[0]) + + while len(vertices) > 3: + removed = False + for index, current in enumerate(vertices): + previous_index = (index - 1) % len(vertices) + following_index = (index + 1) % len(vertices) + previous = vertices[previous_index] + following = vertices[following_index] + signed_area = cross(previous, current, following) + if abs(signed_area) <= _MIN_SEGMENT_LENGTH_M: + vertices.pop(index) + removed = True + break + if signed_area > 0.0: + continue + contains_vertex = any( + other_index not in {previous_index, index, following_index} + and cross(previous, current, other) < -_MIN_SEGMENT_LENGTH_M + and cross(current, following, other) < -_MIN_SEGMENT_LENGTH_M + and cross(following, previous, other) < -_MIN_SEGMENT_LENGTH_M + for other_index, other in enumerate(vertices) + ) + if contains_vertex: + continue + triangles.append(np.asarray((previous, current, following))) + vertices.pop(index) + removed = True + break + if not removed: + raise ValueError("Could not triangulate fare-region polygon") + triangles.append(np.asarray(vertices)) + return tuple(triangles) + + @dataclass(frozen=True) class NavigationLane: """Directed lane centerline.""" @@ -49,10 +92,39 @@ class NavigationLane: allows_taxi_stops: bool = True """Whether pickup and dropoff candidates may be sampled from this lane.""" + lane_id: str | None = None + """Stable semantic lane identifier; ``None`` denotes legacy geometry.""" + + successor_ids: tuple[str, ...] | None = None + """Explicit legal successors; ``None`` enables legacy endpoint inference.""" + + element_id: str | None = None + """Owning semantic road or node identifier when sourced from a game map.""" + + +@dataclass(frozen=True) +class NavigationFareRegion: + """Surface geometry from which non-road fare targets are sampled.""" + + region_id: str + """Stable source element identifier.""" + + kind: str + """Sampling mode: ``area`` for polygons or ``boundary`` for polylines.""" + + geometry_world: tuple[npt.NDArray[np.float32], ...] + """World-space polygon or exposed-boundary polylines.""" + + arrival_lane_ids: tuple[str, ...] + """Lane endpoints where routed travel to the region stops.""" + + departure_lane_ids: tuple[str, ...] + """Lane endpoints where routed travel from the region begins.""" + @dataclass(frozen=True) class NavigationWaypoint: - """Sampled target position tied to a directed lane.""" + """Physical fare target with directed routing anchors.""" xyz_m: npt.NDArray[np.float32] """World-space waypoint position.""" @@ -66,6 +138,15 @@ class NavigationWaypoint: passenger_xyz_m: npt.NDArray[np.float32] | None = None """Waiting-passenger ground point, or ``None`` to use ``xyz_m``.""" + arrival_anchors: tuple[LanePosition, ...] = () + """Possible road-graph endpoints used when routing to this target.""" + + departure_anchors: tuple[LanePosition, ...] = () + """Possible road-graph origins used when routing away from this target.""" + + element_id: str | None = None + """Road or node whose vicinity controls passenger conditioning visibility.""" + @dataclass(frozen=True) class LanePosition: @@ -134,7 +215,14 @@ def __init__( else _normalize_polyline(lane.road_edge_world) ) normalized_lanes.append( - NavigationLane(points, road_edge, lane.allows_taxi_stops) + NavigationLane( + points, + road_edge, + lane.allows_taxi_stops, + lane.lane_id, + lane.successor_ids, + lane.element_id, + ) ) cumulative_distances.append(cumulative) road_edge_cumulative_distances.append( @@ -238,12 +326,126 @@ def sample_waypoints( lane_index, float(distance_m), passenger_point, + element_id=self._lanes[lane_index].element_id, ) ) if len(sampled) < 2: raise ValueError("Taxi mode requires at least two distinct road waypoints.") return tuple(sampled) + def sample_fare_regions( + self, + regions: tuple[NavigationFareRegion, ...], + spacing_m: float, + rng: np.random.Generator, + ) -> tuple[NavigationWaypoint, ...]: + """Sample physical targets from node boundaries and parking-lot areas. + + Args: + regions: Compiled surface regions and their routing endpoints. + spacing_m: Nominal spacing controlling target density. + rng: Random generator used for reproducible placement. + + Returns: + Physical targets carrying arrival and departure routing anchors. + """ + lane_indices = { + lane.lane_id: index + for index, lane in enumerate(self._lanes) + if lane.lane_id is not None + } + + def anchors( + lane_ids: tuple[str, ...], *, arrival: bool + ) -> tuple[LanePosition, ...]: + return tuple( + LanePosition( + lane_index=index, + distance_along_lane_m=( + float(self._lane_lengths[index]) if arrival else 0.0 + ), + lateral_distance_m=0.0, + heading_error_rad=0.0, + ) + for lane_id in lane_ids + for index in (lane_indices.get(lane_id),) + if index is not None + ) + + result: list[NavigationWaypoint] = [] + for region in regions: + arrivals = anchors(region.arrival_lane_ids, arrival=True) + departures = anchors(region.departure_lane_ids, arrival=False) + if not arrivals or not departures: + continue + points: list[np.ndarray] = [] + if region.kind == "area": + polygon = np.asarray(region.geometry_world[0], dtype=np.float64) + area = abs( + 0.5 + * float( + np.dot(polygon[:, 0], np.roll(polygon[:, 1], -1)) + - np.dot(polygon[:, 1], np.roll(polygon[:, 0], -1)) + ) + ) + count = max(1, int(math.ceil(area / (spacing_m * spacing_m)))) + triangles = _triangulate_clockwise_polygon(polygon[:, :2]) + triangle_areas = np.asarray( + [ + abs( + (triangle[1, 0] - triangle[0, 0]) + * (triangle[2, 1] - triangle[0, 1]) + - (triangle[1, 1] - triangle[0, 1]) + * (triangle[2, 0] - triangle[0, 0]) + ) + * 0.5 + for triangle in triangles + ] + ) + probabilities = triangle_areas / np.sum(triangle_areas) + for _index in range(count): + triangle = triangles[ + int(rng.choice(len(triangles), p=probabilities)) + ] + first_random, second_random = rng.random(2) + root = math.sqrt(float(first_random)) + candidate = ( + (1.0 - root) * triangle[0] + + root * (1.0 - second_random) * triangle[1] + + root * second_random * triangle[2] + ) + points.append(np.asarray([candidate[0], candidate[1], 0.0])) + elif region.kind == "boundary": + offset = float(rng.uniform(0.0, spacing_m)) + for polyline in region.geometry_world: + line = np.asarray(polyline, dtype=np.float32) + lengths = np.linalg.norm(np.diff(line[:, :2], axis=0), axis=1) + total = float(np.sum(lengths)) + distances = np.arange(offset, total + 1.0e-6, spacing_m) + if not len(distances): + distances = np.asarray([total * 0.5]) + cumulative = np.concatenate(([0.0], np.cumsum(lengths))) + points.extend( + _point_at_distance(line, cumulative, float(distance)) + for distance in distances + ) + else: + raise ValueError(f"Unsupported fare-region kind {region.kind!r}") + primary = arrivals[0] + result.extend( + NavigationWaypoint( + xyz_m=np.asarray(point, dtype=np.float32), + lane_index=primary.lane_index, + distance_along_lane_m=primary.distance_along_lane_m, + passenger_xyz_m=np.asarray(point, dtype=np.float32), + arrival_anchors=arrivals, + departure_anchors=departures, + element_id=region.region_id, + ) + for point in points + ) + return tuple(result) + def point_at( self, lane_index: int, distance_along_lane_m: float ) -> npt.NDArray[np.float32]: @@ -350,34 +552,37 @@ def route( ) -> RoutePlan | None: """Return the shortest directed route between two lane positions.""" distances_to_start, predecessors = self._shortest_tree(start) - direct_distance = math.inf - if ( - destination.lane_index == start.lane_index - and destination.distance_along_lane_m >= start.distance_along_lane_m - ): - direct_distance = ( - destination.distance_along_lane_m - start.distance_along_lane_m - ) - graph_distance = ( - float(distances_to_start[destination.lane_index]) - + destination.distance_along_lane_m + anchors = destination.arrival_anchors or ( + LanePosition( + destination.lane_index, + destination.distance_along_lane_m, + 0.0, + 0.0, + ), ) - if math.isfinite(direct_distance) and direct_distance <= graph_distance: - lane_path = (start.lane_index,) - distance_m = direct_distance - elif math.isfinite(graph_distance): - lane_path = self._reconstruct_path( - start.lane_index, destination.lane_index, predecessors + plans: list[RoutePlan] = [] + for anchor in anchors: + direct_distance = math.inf + if ( + anchor.lane_index == start.lane_index + and anchor.distance_along_lane_m >= start.distance_along_lane_m + ): + direct_distance = ( + anchor.distance_along_lane_m - start.distance_along_lane_m + ) + graph_distance = ( + float(distances_to_start[anchor.lane_index]) + + anchor.distance_along_lane_m ) - if not lane_path: - return None - distance_m = graph_distance - else: - return None - return RoutePlan( - lane_indices=lane_path, - distance_m=max(0.0, float(distance_m)), - ) + if math.isfinite(direct_distance) and direct_distance <= graph_distance: + plans.append(RoutePlan((start.lane_index,), direct_distance)) + elif math.isfinite(graph_distance): + lane_path = self._reconstruct_path( + start.lane_index, anchor.lane_index, predecessors + ) + if lane_path: + plans.append(RoutePlan(lane_path, graph_distance)) + return min(plans, key=lambda plan: plan.distance_m, default=None) def route_distances( self, @@ -388,24 +593,52 @@ def route_distances( distances_to_start, _predecessors = self._shortest_tree(start) result: list[float] = [] for destination in destinations: - direct_distance = math.inf - if ( - destination.lane_index == start.lane_index - and destination.distance_along_lane_m >= start.distance_along_lane_m - ): - direct_distance = ( - destination.distance_along_lane_m - start.distance_along_lane_m - ) - graph_distance = ( - float(distances_to_start[destination.lane_index]) - + destination.distance_along_lane_m + anchors = destination.arrival_anchors or ( + LanePosition( + destination.lane_index, + destination.distance_along_lane_m, + 0.0, + 0.0, + ), ) - result.append(min(direct_distance, graph_distance)) + distances: list[float] = [] + for anchor in anchors: + direct_distance = math.inf + if ( + anchor.lane_index == start.lane_index + and anchor.distance_along_lane_m >= start.distance_along_lane_m + ): + direct_distance = ( + anchor.distance_along_lane_m - start.distance_along_lane_m + ) + graph_distance = ( + float(distances_to_start[anchor.lane_index]) + + anchor.distance_along_lane_m + ) + distances.append(min(direct_distance, graph_distance)) + result.append(min(distances, default=math.inf)) return tuple(result) def _build_adjacency( self, endpoint_snap_tolerance_m: float ) -> tuple[tuple[tuple[int, float], ...], ...]: + if all( + lane.lane_id is not None and lane.successor_ids is not None + for lane in self._lanes + ): + indices = { + lane.lane_id: index + for index, lane in enumerate(self._lanes) + if lane.lane_id is not None + } + return tuple( + tuple( + (indices[successor], 0.0) + for successor in lane.successor_ids or () + if successor in indices + ) + for lane in self._lanes + ) cell_size = endpoint_snap_tolerance_m start_buckets: dict[tuple[int, int], list[int]] = {} for lane_index, lane in enumerate(self._lanes): diff --git a/apps/crazy_robotaxi/crazy_robotaxi/physics.py b/apps/crazy_robotaxi/crazy_robotaxi/physics.py index 067f434f4..44f6a6c50 100644 --- a/apps/crazy_robotaxi/crazy_robotaxi/physics.py +++ b/apps/crazy_robotaxi/crazy_robotaxi/physics.py @@ -5,59 +5,23 @@ from __future__ import annotations -import hashlib import math from dataclasses import replace import numpy as np from loguru import logger from ludus_renderer import RigidBodyModel -from omnidreams_game_engine.config import VehicleConfig -from omnidreams_game_engine.simulation.components import canonical_object_type from omnidreams_game_engine.simulation.game_physics import GamePhysicsWorld from omnidreams_game_engine.types import ( DriverCommand, PhysicsDebugFrame, SceneBundle, VehicleState, - WorldLineSegments, ) -_MOTOR_TRAFFIC_TYPES = frozenset({"car", "truck", "bus", "trailer"}) -_CHASSIS_INSET_M = 0.16 - +from crazy_robotaxi.driving import TaxiVehicleConfig -def select_traffic_tracks( - tracks: tuple[object, ...], density: float, scene_id: str -) -> tuple[object, ...]: - """Select a stable Taxi-only fraction of motor traffic.""" - if not 0.0 < density <= 1.0: - raise ValueError("traffic density must be greater than 0 and at most 1") - if density >= 1.0: - return tracks - motor_tracks = tuple( - track - for track in tracks - if canonical_object_type(str(track.object_type)) in _MOTOR_TRAFFIC_TYPES - ) - if not motor_tracks: - return tracks - retained_count = max(1, math.ceil(len(motor_tracks) * density)) - - def selection_key(track: object) -> bytes: - identity = f"{scene_id}:{track.track_id}".encode() - return hashlib.blake2b(identity, digest_size=8).digest() - - retained_ids = { - str(track.track_id) - for track in sorted(motor_tracks, key=selection_key)[:retained_count] - } - return tuple( - track - for track in tracks - if canonical_object_type(str(track.object_type)) not in _MOTOR_TRAFFIC_TYPES - or str(track.track_id) in retained_ids - ) +_CHASSIS_INSET_M = 0.16 def inset_vehicle_chassis(model: RigidBodyModel) -> RigidBodyModel: @@ -82,43 +46,34 @@ class TaxiPhysicsWorld(GamePhysicsWorld): def __init__( self, scene: SceneBundle, - vehicle: VehicleConfig, + vehicle: TaxiVehicleConfig, *, - traffic_density: float, - enclosure_segments_world: np.ndarray | None = None, + curb_segments_world: np.ndarray | None = None, ) -> None: - selected_tracks = select_traffic_tracks( - tuple(scene.vehicle_bbox_tracks), traffic_density, scene.scene_id - ) - line_layers = scene.line_layers - enclosure_segments = np.asarray( - enclosure_segments_world - if enclosure_segments_world is not None + curb_segments = np.asarray( + curb_segments_world + if curb_segments_world is not None else np.empty((0, 2, 3), dtype=np.float32), dtype=np.float32, ) - if enclosure_segments.ndim != 3 or enclosure_segments.shape[1:] != (2, 3): - raise ValueError("Taxi enclosure segments must have shape (N, 2, 3).") - if len(enclosure_segments): - line_layers = line_layers + ( - WorldLineSegments( - segments_world=enclosure_segments, - color_rgba=(1.0, 0.0, 0.0, 1.0), - width_px=3.0, - layer_name="crazy_robotaxi_enclosure_walls", - ), - ) - taxi_scene = replace( + if curb_segments.ndim != 3 or curb_segments.shape[1:] != (2, 3): + raise ValueError("Taxi curb segments must have shape (N, 2, 3).") + super().__init__( scene, - vehicle_bbox_tracks=selected_tracks, - line_layers=line_layers, + vehicle, + model_adapter=inset_vehicle_chassis, + static_barrier_segments_world=( + curb_segments + if getattr(scene, "game_map", None) is not None or len(curb_segments) + else None + ), + static_barrier_restitution=vehicle.curb_collision_restitution, ) - super().__init__(taxi_scene, vehicle, model_adapter=inset_vehicle_chassis) + self._taxi_vehicle = vehicle logger.info( "[crazy-robotaxi] Taxi physics active: app-authoritative heading, " - "arcade handbrake, inset chassis, traffic_density={:.2f}, enclosure_segments={}", - traffic_density, - len(enclosure_segments), + "arcade handbrake, inset chassis, curb_segments={}", + len(curb_segments), ) self._last_contact_resolved_state: VehicleState | None = None @@ -148,11 +103,25 @@ def step_with_command( ], dtype=np.float32, ) + forward_speed_mps = float(np.dot(velocity, forward)) + if ( + getattr(self, "last_step_static_barrier_collision", False) + and not command.handbrake + and command.brake <= 0.01 + and not command.stop + and state.speed_mps * forward_speed_mps > 0.0 + ): + retained_speed_mps = ( + abs(state.speed_mps) + * self._taxi_vehicle.curb_forward_momentum_retention + ) + if abs(forward_speed_mps) < retained_speed_mps: + forward_speed_mps = math.copysign(retained_speed_mps, state.speed_mps) resolved = replace( resolved, yaw_rad=state.yaw_rad, yaw_rate_radps=state.yaw_rate_radps, - speed_mps=float(np.dot(velocity, forward)), + speed_mps=forward_speed_mps, velocity_x_mps=float(velocity[0]), velocity_y_mps=float(velocity[1]), ) diff --git a/apps/crazy_robotaxi/crazy_robotaxi/runner.py b/apps/crazy_robotaxi/crazy_robotaxi/runner.py index d31f10403..853e34b22 100644 --- a/apps/crazy_robotaxi/crazy_robotaxi/runner.py +++ b/apps/crazy_robotaxi/crazy_robotaxi/runner.py @@ -31,12 +31,12 @@ SV_2STEPS_CHUNK2_LOC6_LIGHTVAE_LIGHTTAE, name="crazy-robotaxi", ) -"""Registry metadata for the legacy manifest-owned model session.""" +"""Registry metadata for the Crazy Robotaxi model session.""" @dataclass(kw_only=True) class CrazyRobotaxiRunnerConfig(RunnerConfig): - """Launch configuration for the standalone legacy game host.""" + """Launch configuration for the standalone game host.""" _target: type["CrazyRobotaxiRunner"] = field( default_factory=lambda: CrazyRobotaxiRunner @@ -46,17 +46,23 @@ class CrazyRobotaxiRunnerConfig(RunnerConfig): default_factory=lambda: _CRAZY_ROBOTAXI_PIPELINE ) - scene: Path | None = None - """Scene archive override; ``None`` uses the staged default scene.""" + map: Path | None = None + """Game-map override; ``None`` uses the packaged default map.""" world_model_manifest: Path | None = None - """Legacy world-model manifest; named to avoid the global ``--manifest``.""" + """World-model manifest; named to avoid the global ``--manifest``.""" + + renderer_config: Path | None = None + """Renderer YAML override; ``None`` uses the packaged default.""" + + game_config: Path | None = None + """Gameplay and taxi-physics YAML override; ``None`` uses the packaged default.""" camera: str | None = None """Camera name override for the selected scene.""" variant: str | None = None - """Weather or numbered scene variant override.""" + """Visual variant override.""" prompt: str | None = None """Text-conditioning prompt override.""" @@ -70,9 +76,6 @@ class CrazyRobotaxiRunnerConfig(RunnerConfig): auto_start: bool = False """Start loading the selected scene immediately after launch.""" - synthetic_scene: bool = False - """Use the procedural CPU-safe scene fixture.""" - synthetic_model: bool | None = None """Override synthetic model construction when set.""" @@ -86,11 +89,11 @@ class CrazyRobotaxiRunnerConfig(RunnerConfig): """Optional alignment artifact output directory.""" app_args: tuple[str, ...] = () - """Additional legacy application arguments parsed before typed overrides.""" + """Additional application arguments parsed before typed overrides.""" class CrazyRobotaxiRunner(Runner): - """Runner adapter that preserves the legacy application lifecycle.""" + """Runner adapter for the standalone application lifecycle.""" def __init__(self, config: CrazyRobotaxiRunnerConfig) -> None: self.config = config @@ -100,8 +103,10 @@ def run(self) -> None: from crazy_robotaxi.cli import main argv = list(self.config.app_args) - _append_value(argv, "--scene", self.config.scene) + _append_value(argv, "--map", self.config.map) _append_value(argv, "--manifest", self.config.world_model_manifest) + _append_value(argv, "--renderer-config", self.config.renderer_config) + _append_value(argv, "--game-config", self.config.game_config) _append_value(argv, "--camera", self.config.camera) _append_value(argv, "--variant", self.config.variant) _append_value(argv, "--prompt", self.config.prompt) @@ -114,8 +119,6 @@ def run(self) -> None: "--taxi-alignment-diagnostics", self.config.taxi_alignment_diagnostics, ) - if self.config.synthetic_scene: - argv.append("--synthetic-scene") if self.config.auto_start: argv.append("--auto-start") if self.config.synthetic_model is not None: @@ -134,7 +137,7 @@ def _append_value(argv: list[str], flag: str, value: object | None) -> None: CRAZY_ROBOTAXI_RUNNER = CrazyRobotaxiRunnerConfig( runner_name="crazy-robotaxi", - description="Standalone Crazy Robotaxi game using the legacy OmniDreams runtime.", + description="Standalone Crazy Robotaxi game using the OmniDreams runtime.", pipeline=_CRAZY_ROBOTAXI_PIPELINE, ) """Runner config discovered by the ``flashdreams.runner_configs`` entry point.""" diff --git a/apps/crazy_robotaxi/crazy_robotaxi/runtime_cli.py b/apps/crazy_robotaxi/crazy_robotaxi/runtime_cli.py index 60ccef914..878937285 100644 --- a/apps/crazy_robotaxi/crazy_robotaxi/runtime_cli.py +++ b/apps/crazy_robotaxi/crazy_robotaxi/runtime_cli.py @@ -10,7 +10,6 @@ from loguru import logger from omnidreams.hf_org import DEFAULT_HF_ORG, apply_cli_to_env from omnidreams.hf_org import ENV_VAR as _HF_ORG_ENV_VAR -from omnidreams.scenes import local_scene_archive_path from omnidreams_game_engine.app import InteractiveDriveApp from omnidreams_game_engine.backends.base import RenderBackend from omnidreams_game_engine.backends.raster import RasterRenderBackend @@ -18,12 +17,13 @@ from omnidreams_game_engine.cli_args import ExplicitArgTrackingArgumentParser from omnidreams_game_engine.config import ( AppConfig, - BevConfig, - RasterConfig, WorldModelProfileConfig, ) from omnidreams_game_engine.log import configure_logging -from omnidreams_game_engine.synthetic_scene import build_synthetic_scene_to_temp +from omnidreams_game_engine.renderer_settings import ( + RendererSettings, + load_renderer_settings, +) from omnidreams_game_engine.world_model.manifest import ( load_world_model_manifest, resolve_world_model_manifest_path, @@ -33,21 +33,11 @@ from flashdreams.plugins.registry import discover_postprocess_presets from flashdreams.serving.realtime.timing import TraceSink -# Package root (from this file's location) so packaged-asset defaults below -# resolve relative to the install, not the user's cwd. Bundled configs live at -# ``interactive_drive/configs/``; scene USDZs are staged into -# ``$FLASHDREAMS_CACHE_DIR/omnidreams-scenes/`` (shared with the webrtc server). +# Package root (from this file's location) so packaged config paths resolve +# relative to the install, not the user's cwd. _PACKAGE_ROOT = Path(__file__).resolve().parent _CONFIGS_ROOT = _PACKAGE_ROOT / "configs" -# Default scene UUID staged by ``omnidreams-prepare`` (clear-weather base -# archive in nvidia/omni-dreams-scenes). -DEFAULT_SCENE_UUID = "0d404ff7-2b66-498c-b047-1ed8cded60d4" - -# Default scene path under the shared ``$FLASHDREAMS_CACHE_DIR/omnidreams-scenes/`` -# cache, so a scene staged by the desktop demo or webrtc server is visible to both. -DEFAULT_SCENE = local_scene_archive_path(DEFAULT_SCENE_UUID) - def resolve_manifest_path(path: str | Path) -> Path: """Resolve a CLI manifest value. @@ -60,54 +50,33 @@ def resolve_manifest_path(path: str | Path) -> Path: return resolve_world_model_manifest_path(path) +def resolve_app_config_path(path: str | Path) -> Path: + """Resolve an application config from the working or packaged directory.""" + candidate = Path(path).expanduser() + if candidate.is_file(): + return candidate.resolve() + bundled = _CONFIGS_ROOT / candidate + if bundled.is_file(): + return bundled.resolve() + return candidate.resolve() + + def build_parser() -> argparse.ArgumentParser: parser = ExplicitArgTrackingArgumentParser( description="Standalone Crazy Robotaxi game" ) parser.add_argument( - "--scene", - type=Path, - default=DEFAULT_SCENE, - help=( - "Path to the input USDZ scene. Defaults to the scene staged by " - f"prepare.py at {DEFAULT_SCENE}; any UUID from " - "nvidia/omni-dreams-scenes/scenes/ works once staged." - ), - ) - parser.add_argument( - "--synthetic-scene", - action="store_true", - help=( - "Skip the USDZ download / staging and build a procedural," - " HD-map-data-free scene at startup instead. Useful for" - " demos in territories where the real-world scenes can't be" - " distributed. The generated scene is a wavy 2-lane road" - " with a single intersection; pair with --synthetic-initial-rgb" - " to supply a natural-looking starting camera frame." - ), - ) - parser.add_argument( - "--synthetic-initial-rgb", + "--map", + dest="scene", type=Path, default=None, - help=( - "Path to a JPG / PNG used as the initial camera frame when" - " --synthetic-scene is set. The world model is trained on" - " natural driving frames, so a real photo (any forward-facing" - " roadway) gives noticeably better generation than the" - " default debug gradient. Resized to the raster resolution" - " automatically." - ), + metavar="PATH", + help="Path to a .robotaxi.yaml game map.", ) parser.add_argument( - "--synthetic-prompt", - default=None, - help=( - "Optional text prompt embedded in the synthetic scene." - " Mutually overridable by --prompt at run time. When omitted," - " the synthetic-scene builder uses a generic forward-driving" - " caption." - ), + "--force-map-recompile", + action="store_true", + help="Rebuild each selected map's compiled cache once in this process.", ) # ``--backend`` exists primarily for the test suite, which exercises # the raster path (~30s warmup) instead of the full omnidreams pipeline @@ -128,10 +97,7 @@ def build_parser() -> argparse.ArgumentParser: parser.add_argument( "--variant", default="default", - help=( - "Scene variant to load: weather siblings (default, rain, snow) or " - "legacy in-archive numbered variants (1, 2, 3)." - ), + help="Visual variant defined by the game map.", ) parser.add_argument("--prompt", default=None, help="Optional prompt override") parser.add_argument( @@ -143,6 +109,18 @@ def build_parser() -> argparse.ArgumentParser: "config filename such as example_world_model_perf.yaml." ), ) + parser.add_argument( + "--renderer-config", + type=Path, + default=_CONFIGS_ROOT / "default_renderer.yaml", + help="Complete renderer YAML; defaults to the packaged game renderer.", + ) + parser.add_argument( + "--game-config", + type=Path, + default=_CONFIGS_ROOT / "default_game.yaml", + help="Complete game-rules and taxi-physics YAML.", + ) parser.add_argument( "--synthetic-model", action=argparse.BooleanOptionalAction, @@ -203,8 +181,8 @@ def build_parser() -> argparse.ArgumentParser: default=None, metavar="ORG", help=( - "Hugging Face org that hosts the omni-dreams repos (models /" - f" samples / scenes). Defaults to {DEFAULT_HF_ORG!r}." + "Hugging Face org that hosts the omni-dreams model and sample" + f" repos. Defaults to {DEFAULT_HF_ORG!r}." f" Equivalent to setting {_HF_ORG_ENV_VAR}; the flag wins when" " both are present. Stamped into the env var early in main()" " so every downstream HF lookup -- including URLs read from" @@ -247,16 +225,6 @@ def build_parser() -> argparse.ArgumentParser: "the vehicle speed limit." ), ) - parser.add_argument( - "--traffic-density", - type=float, - default=0.4, - metavar="FRACTION", - help=( - "Fraction of recorded motor vehicles to retain in taxi-game mode " - "(default: 0.4). Pedestrians, cyclists, and motorcycles are unaffected." - ), - ) parser.add_argument( "--disable-visual-flare", action="store_true", @@ -265,7 +233,7 @@ def build_parser() -> argparse.ArgumentParser: parser.add_argument( "--bev", action=argparse.BooleanOptionalAction, - default=True, + default=None, help=( "Render a synthetic top-down BEV map alongside the main camera and" " publish it on /bev_stream. The default is a straight-down," @@ -275,7 +243,7 @@ def build_parser() -> argparse.ArgumentParser: ) parser.add_argument( "--bev-resolution", - default="1024x1024", + default=None, help=( "BEV render resolution as WIDTHxHEIGHT (default: 1024x1024). The" " HUD panel is roughly 470x400, so 1024 gives ~2x SSAA per axis" @@ -287,19 +255,19 @@ def build_parser() -> argparse.ArgumentParser: parser.add_argument( "--bev-height-m", type=float, - default=BevConfig().height_m, + default=None, help="BEV camera altitude in metres above the rig.", ) parser.add_argument( "--bev-fov-deg", type=float, - default=BevConfig().fov_deg, + default=None, help="BEV camera vertical field-of-view in degrees.", ) parser.add_argument( "--bev-tilt-deg", type=float, - default=BevConfig().tilt_deg, + default=None, help=( "Advanced override for the BEV camera pitch in degrees. The" " default ``0`` keeps the mini-map straight down; positive values" @@ -339,74 +307,6 @@ def build_parser() -> argparse.ArgumentParser: "Captures the standalone game session." ), ) - parser.add_argument( - "--oob-warn-proximity", - type=float, - default=None, - metavar="FLOAT", - help=( - "Proximity at which the loop overlays " - "'Approaching map edge, turn back to avoid respawn' on the " - "frame. Mirrors alpasim's ``oob_proximity``: 0.0 is solidly " - "inside the navigable AABB+margin, 1.0 is at the AABB+margin " - "edge (the warning band ramps linearly across a 100 m zone " - "inside the edge), 2.0 is the off-map sentinel. Default 0.6, " - "matching alpasim's 'approaching' threshold." - ), - ) - parser.add_argument( - "--oob-respawn-proximity", - type=float, - default=None, - metavar="FLOAT", - help=( - "Proximity above which the loop fires the auto-respawn (after " - "``--oob-respawn-debounce-chunks`` consecutive chunks at this " - "level). Default 2.0, matching alpasim: a hard binary trigger " - "that only fires when the ego has actually crossed the " - "AABB+margin boundary. Set to 2.5 (or any value > 2.0) to " - "disable auto-respawn entirely while keeping the warning " - "overlay." - ), - ) - parser.add_argument( - "--oob-respawn-debounce-chunks", - type=int, - default=None, - metavar="N", - help=( - "Number of consecutive chunks the proximity must stay at or " - "above ``--oob-respawn-proximity`` before the auto-respawn " - "fires. Default 1, matching alpasim's immediate-on-step " - "behaviour. Raise this for an added buffer; useful mainly " - "if you've lowered the respawn threshold below 2.0." - ), - ) - parser.add_argument( - "--oob-margin-m", - type=float, - default=None, - metavar="METERS", - help=( - "Margin (in metres) added around the scene's spatial-content " - "AABB before any in-bounds check. The respawn fires only " - "once the ego is past AABB+margin, so larger values give " - "more room to leave the explicitly mapped area. Default 50, " - "matching alpasim. Bump to 200+ on scenes whose geometry " - "layers don't cover the full driveable area." - ), - ) - parser.add_argument( - "--oob-warning-zone-m", - type=float, - default=None, - metavar="METERS", - help=( - "Depth of the linear warning-ramp band inside the AABB+margin " - "edge. Default 100, matching alpasim. Set to 0 to disable the " - "ramp and only ever show the binary on/off respawn signal." - ), - ) return parser @@ -425,26 +325,40 @@ def _parse_resolution(value: str) -> tuple[int, int]: return width, height -def _oob_kwargs(args: argparse.Namespace) -> dict[str, float | int]: - """Forward only the OOB flags the user actually passed. - - Each ``--oob-*`` flag defaults to ``None`` so the - :class:`AppConfig` field defaults stay authoritative; we only add - a kwarg to the ``AppConfig(**kwargs)`` call when the user passed - an explicit value. - """ - overrides: dict[str, float | int] = {} - if args.oob_warn_proximity is not None: - overrides["oob_warn_proximity"] = float(args.oob_warn_proximity) - if args.oob_respawn_proximity is not None: - overrides["oob_respawn_proximity"] = float(args.oob_respawn_proximity) - if args.oob_respawn_debounce_chunks is not None: - overrides["oob_respawn_debounce_chunks"] = int(args.oob_respawn_debounce_chunks) - if args.oob_margin_m is not None: - overrides["oob_margin_m"] = float(args.oob_margin_m) - if args.oob_warning_zone_m is not None: - overrides["oob_warning_zone_m"] = float(args.oob_warning_zone_m) - return overrides +def renderer_settings_from_args(args: argparse.Namespace) -> RendererSettings: + """Load renderer YAML and apply explicit visual CLI overrides.""" + cached = getattr(args, "_renderer_settings", None) + if cached is not None: + return cached + path = resolve_app_config_path( + getattr(args, "renderer_config", _CONFIGS_ROOT / "default_renderer.yaml") + ) + settings = load_renderer_settings(path) + bev = settings.bev + if getattr(args, "bev", None) is not None: + bev = replace(bev, enabled=bool(args.bev)) + if getattr(args, "bev_resolution", None) is not None: + width, height = _parse_resolution(args.bev_resolution) + bev = replace(bev, width=width, height=height) + for arg_name, field_name in ( + ("bev_height_m", "height_m"), + ("bev_fov_deg", "fov_deg"), + ("bev_tilt_deg", "tilt_deg"), + ): + value = getattr(args, arg_name, None) + if value is not None: + bev = replace(bev, **{field_name: float(value)}) + settings = replace(settings, bev=bev) + setattr(args, "_renderer_settings", settings) + args.renderer_config = path + # The HUD presenters still consume these resolved values from the argparse + # namespace. Keep that compatibility surface concrete after YAML + CLI merge. + args.bev = settings.bev.enabled + args.bev_resolution = f"{settings.bev.width}x{settings.bev.height}" + args.bev_height_m = settings.bev.height_m + args.bev_fov_deg = settings.bev.fov_deg + args.bev_tilt_deg = settings.bev.tilt_deg + return settings def main() -> None: @@ -466,8 +380,7 @@ def prepare_config_and_backend( hand it to a long-lived :class:`InteractiveDriveApp` that switches scenes in place (keeping the warmed model resident). """ - # Stamp the resolved HF org into the env var before anything fetches - # (manifest, scene staging, model build read it lazily). + # Stamp the resolved HF org before manifest and model artifact resolution. resolved_org = apply_cli_to_env(args.hf_org) if resolved_org != DEFAULT_HF_ORG: logger.info( @@ -475,31 +388,11 @@ def prepare_config_and_backend( ) scene_path = args.scene - if args.synthetic_scene: - # Materialise a procedural USDZ to a temp dir for this process. - # The scene loader treats it like any other USDZ; downstream code - # paths (rasterizer, world model, presenter) need no changes. - scene_path = build_synthetic_scene_to_temp( - initial_rgb_path=args.synthetic_initial_rgb, - prompt=args.synthetic_prompt, - ) - logger.info( - f"[interactive-drive] synthetic scene materialised at {scene_path}", - ) - elif args.synthetic_initial_rgb is not None or args.synthetic_prompt is not None: - raise SystemExit( - "--synthetic-initial-rgb / --synthetic-prompt require --synthetic-scene" - ) + if scene_path is None: + raise SystemExit("--map is required") - bev_width, bev_height = _parse_resolution(args.bev_resolution) - bev_config = BevConfig( - enabled=bool(args.bev), - width=bev_width, - height=bev_height, - height_m=float(args.bev_height_m), - fov_deg=float(args.bev_fov_deg), - tilt_deg=float(args.bev_tilt_deg), - ) + renderer_settings = renderer_settings_from_args(args) + bev_config = renderer_settings.bev manifest_path = ( resolve_manifest_path(args.manifest) if args.manifest is not None else None ) @@ -510,8 +403,10 @@ def prepare_config_and_backend( camera_name=args.camera, variant=args.variant, prompt_override=args.prompt, + force_map_recompile=bool(args.force_map_recompile), manifest_path=manifest_path, - raster=RasterConfig( + raster=replace( + renderer_settings.raster, compute_device=args.compute_device, sync_gpu_timing=args.sync_gpu_timing, ), @@ -524,8 +419,11 @@ def prepare_config_and_backend( game_mode=bool(args.game_mode), stream_mjpeg_bind=args.stream_mjpeg, stop_after_consumed_chunks=args.stop_after_chunks, - visual_flare_enabled=False if args.disable_visual_flare else None, - **_oob_kwargs(args), + visual_flare_enabled=( + False + if args.disable_visual_flare + else renderer_settings.visual_flare_enabled + ), ) backend: RenderBackend @@ -564,6 +462,9 @@ def prepare_config_and_backend( offload_text_encoder=config.world_model_offload_text_encoder, postprocess=config.postprocess, synchronize_bev_with_rgb=bool(args.taxi_game), + motion_conformance_diagnostics_enabled=( + args.taxi_alignment_diagnostics is not None + ), ) return config, backend diff --git a/apps/crazy_robotaxi/crazy_robotaxi/scene.py b/apps/crazy_robotaxi/crazy_robotaxi/scene.py index 1f69b7526..88c444609 100644 --- a/apps/crazy_robotaxi/crazy_robotaxi/scene.py +++ b/apps/crazy_robotaxi/crazy_robotaxi/scene.py @@ -17,26 +17,13 @@ from __future__ import annotations -import json -import zipfile from dataclasses import dataclass -from typing import Any import numpy as np import numpy.typing as npt -import pyarrow.parquet as pq from omnidreams_game_engine.types import SceneBundle -from shapely.geometry import Point, Polygon -from shapely.geometry.base import BaseGeometry -from shapely.ops import unary_union -from crazy_robotaxi.navigation import NavigationLane - -_PHYSICAL_ROAD_EDGE_STYLES = frozenset({"TALL_CURB", "ROAD_BOUNDARY", "WALL", "FENCE"}) -"""ClipGT edge styles that unambiguously bound drivable pavement.""" - -_PAINTED_ROAD_EDGE_STYLES = frozenset({"SOLID_SINGLE", "SOLID_GROUP"}) -"""Solid white edge styles usable when a physical curb is unavailable.""" +from crazy_robotaxi.navigation import NavigationFareRegion, NavigationLane @dataclass(frozen=True) @@ -44,333 +31,139 @@ class CrazyRobotaxiSceneData: """Navigation geometry loaded only when Crazy Robotaxi is selected.""" reference_route_world: np.ndarray - """Recorded ego route used when mapped lanes are unavailable.""" + """Route used to initialize navigation.""" navigation_lanes: tuple[NavigationLane, ...] """Directed car-lane centerlines used for target routing.""" - perimeter_segments_world: npt.NDArray[np.float32] - """Taxi-only walls enclosing the player's lane-network component.""" + fare_regions: tuple[NavigationFareRegion, ...] + """Parking areas and exposed node edges used for fare placement.""" + + curb_segments_world: npt.NDArray[np.float32] + """Physical curb segments compiled from map-element boundaries.""" @property def navigation_routes_world(self) -> tuple[np.ndarray, ...]: """Return centerline arrays for compatibility with route consumers.""" return tuple(lane.centerline_world for lane in self.navigation_lanes) - @property - def enclosure_segments_world(self) -> npt.NDArray[np.float32]: - """Return every Taxi-only enclosure wall.""" - return self.perimeter_segments_world - - -_PERIMETER_MARGIN_M = 20.0 -"""Distance between boundary-only legacy geometry and its outer wall.""" - -_LANE_JOIN_TOLERANCE_M = 0.25 -"""Morphological closing distance used to join adjacent lane polygons.""" - -_LANE_PERIMETER_CLEARANCE_M = 3.0 -"""Distance between mapped lane rails and the Taxi-only enclosure.""" - -_LANE_PERIMETER_SIMPLIFY_M = 0.5 -"""Maximum geometric deviation when simplifying the enclosure ring.""" - - -def _empty_segments() -> npt.NDArray[np.float32]: - return np.empty((0, 2, 3), dtype=np.float32) - def load_scene_data(scene: SceneBundle) -> CrazyRobotaxiSceneData: - """Load recorded and mapped routes only for a Crazy Robotaxi session.""" - with zipfile.ZipFile(scene.scene_path, "r") as archive: - trajectory_doc = json.loads(archive.read("rig_trajectories.json")) - poses = np.asarray( - trajectory_doc["rig_trajectories"][0]["T_rig_worlds"], - dtype=np.float32, + """Load Crazy Robotaxi navigation geometry from the compiled game map.""" + game_map = scene.game_map + assert game_map is not None, "compiled game-map metadata is required" + lanes = tuple( + NavigationLane( + centerline_world=lane.centerline_world, + road_edge_world=( + lane.roadside_edge_world if lane.allows_taxi_stops else None + ), + allows_taxi_stops=lane.allows_taxi_stops, + lane_id=lane.lane_id, + successor_ids=lane.successor_ids, + element_id=lane.element_id, ) - reference_route_world = poses[:, :3, 3].astype(np.float32) - lane_member = "clipgt/lane.parquet" - if lane_member not in archive.namelist(): - lane_rows: list[dict[str, Any]] = [] - navigation_lanes = () - else: - with archive.open(lane_member) as handle: - lane_rows = pq.read_table(handle).to_pylist() - mapped_lanes = _build_navigation_lanes(lane_rows) - navigation_lanes = ( - mapped_lanes - if any(lane.allows_taxi_stops for lane in mapped_lanes) - else () - ) - boundary_member = "clipgt/road_boundary.parquet" - if boundary_member in archive.namelist(): - with archive.open(boundary_member) as handle: - boundary_rows = pq.read_table(handle).to_pylist() - else: - boundary_rows = [] - - perimeter = _build_lane_network_perimeter( - lane_rows, - reference_route_world[0, :2], - ) - if not len(perimeter): - perimeter = _build_fallback_perimeter(lane_rows, boundary_rows) - - return CrazyRobotaxiSceneData( - reference_route_world=reference_route_world, - navigation_lanes=navigation_lanes, - perimeter_segments_world=perimeter, + for lane in game_map.lanes ) - - -def _points_from_records(points: list[dict[str, float]]) -> np.ndarray: - return np.array( - [[point["x"], point["y"], point["z"]] for point in points], - dtype=np.float32, + spawn_lane = next( + lane + for lane in game_map.lanes + if lane.lane_id == game_map.default_spawn.lane_id ) - - -def _sample_polyline_fractions( - points_xyz: np.ndarray, fractions: np.ndarray -) -> np.ndarray: - segment_lengths = np.linalg.norm(np.diff(points_xyz[:, :2], axis=0), axis=1) - cumulative = np.concatenate(([0.0], np.cumsum(segment_lengths))) - total_length = float(cumulative[-1]) - if total_length <= 1.0e-4: - return np.repeat(points_xyz[:1], len(fractions), axis=0) - distances = fractions * total_length - return np.stack( - [np.interp(distances, cumulative, points_xyz[:, axis]) for axis in range(3)], - axis=1, - ).astype(np.float32) - - -def _aligned_lane_rails( - payload: dict[str, Any], -) -> tuple[npt.NDArray[np.float32], npt.NDArray[np.float32]] | None: - left_rail = _points_from_records(payload.get("left_rail", [])) - right_rail = _points_from_records(payload.get("right_rail", [])) - if len(left_rail) < 2 or len(right_rail) < 2: - return None - aligned_cost = float( - np.linalg.norm(left_rail[0, :2] - right_rail[0, :2]) - + np.linalg.norm(left_rail[-1, :2] - right_rail[-1, :2]) - ) - reversed_cost = float( - np.linalg.norm(left_rail[0, :2] - right_rail[-1, :2]) - + np.linalg.norm(left_rail[-1, :2] - right_rail[0, :2]) - ) - if reversed_cost < aligned_cost: - right_rail = right_rail[::-1] - return left_rail, right_rail - - -def _car_lane(payload: dict[str, Any]) -> bool: - vehicle_types = { - str(vehicle_type).upper() - for vehicle_type in payload.get("vehicle_types", []) - if vehicle_type + curb_segments = [ + np.stack((start, end)) + for element in game_map.elements + for curb in element.curbs + for start, end in zip( + curb.polyline_world[:-1], curb.polyline_world[1:], strict=True + ) + ] + runtime_lanes = {lane.lane_id: lane for lane in game_map.lanes} + roads_by_node: dict[str, list[str]] = { + node.node_id: [] for node in game_map.topology.nodes } - return not vehicle_types or "CAR" in vehicle_types - - -def _polygon_components(geometry: BaseGeometry) -> tuple[Polygon, ...]: - """Return every nonempty polygon contained in a Shapely geometry.""" - if isinstance(geometry, Polygon): - return (geometry,) if geometry.area > 1.0e-2 else () - if hasattr(geometry, "geoms"): - return tuple( - polygon - for child in geometry.geoms - for polygon in _polygon_components(child) + for road in game_map.topology.roads: + roads_by_node[road.from_node_id].append(road.road_id) + roads_by_node[road.to_node_id].append(road.road_id) + + def node_anchors(node_id: str) -> tuple[tuple[str, ...], tuple[str, ...]]: + node = next(item for item in game_map.topology.nodes if item.node_id == node_id) + center = np.asarray([node.x_m, node.y_m], dtype=np.float32) + arrivals: list[str] = [] + departures: list[str] = [] + for road_id in roads_by_node[node_id]: + for lane_id, lane in runtime_lanes.items(): + if lane.element_id != road_id: + continue + start_distance = float( + np.linalg.norm(lane.centerline_world[0, :2] - center) + ) + end_distance = float( + np.linalg.norm(lane.centerline_world[-1, :2] - center) + ) + if end_distance <= start_distance: + arrivals.append(lane_id) + if start_distance <= end_distance: + departures.append(lane_id) + return tuple(dict.fromkeys(arrivals)), tuple(dict.fromkeys(departures)) + + node_anchor_cache = { + node.node_id: node_anchors(node.node_id) + for node in game_map.topology.nodes + if node.node_type != "parking_lot" + } + accesses_by_lot: dict[str, list[str]] = {} + for access in game_map.topology.parking_accesses: + accesses_by_lot.setdefault(access.parking_lot_node_id, []).append( + access.source_node_id ) - return () - - -def _build_lane_network_perimeter( - lane_rows: list[dict[str, Any]], - spawn_xy_m: npt.NDArray[np.float32], -) -> npt.NDArray[np.float32]: - """Build closed walls around the spawn-connected drivable lane surface. - - Args: - lane_rows: ClipGT lane records. - spawn_xy_m: Initial player position in world XY coordinates. - - Returns: - World-space wall segments with shape ``[N, 2, 3]``. Segments are - consecutive within each closed boundary ring. - """ - lane_surfaces: list[Polygon] = [] - lane_heights: list[npt.NDArray[np.float32]] = [] - for row in lane_rows: - payload = row.get("lane", {}) - if not _car_lane(payload): - continue - rails = _aligned_lane_rails(payload) - if rails is None: + elements = {element.element_id: element for element in game_map.elements} + fare_regions: list[NavigationFareRegion] = [] + for node in game_map.topology.nodes: + element = elements[node.node_id] + if node.node_type == "parking_lot": + source_ids = accesses_by_lot[node.node_id] + arrivals = tuple( + lane_id + for source_id in source_ids + for lane_id in node_anchor_cache[source_id][0] + ) + departures = tuple( + lane_id + for source_id in source_ids + for lane_id in node_anchor_cache[source_id][1] + ) + fare_regions.append( + NavigationFareRegion( + node.node_id, + "area", + (element.surface_world,), + tuple(dict.fromkeys(arrivals)), + tuple(dict.fromkeys(departures)), + ) + ) continue - left_rail, right_rail = rails - surface = Polygon( - np.concatenate((left_rail[:, :2], right_rail[::-1, :2]), axis=0) + arrivals, departures = node_anchor_cache[node.node_id] + boundaries = tuple( + boundary.polyline_world for boundary in element.road_boundaries ) - if not surface.is_valid: - surface = surface.buffer(0) - lane_surfaces.extend(_polygon_components(surface)) - lane_heights.extend((left_rail[:, 2], right_rail[:, 2])) - if not lane_surfaces: - return _empty_segments() - - joined_surface = unary_union(lane_surfaces) - joined_surface = joined_surface.buffer( - _LANE_JOIN_TOLERANCE_M, - join_style="mitre", - ).buffer(-_LANE_JOIN_TOLERANCE_M, join_style="mitre") - components = _polygon_components(joined_surface) - if not components: - return _empty_segments() - - spawn_point = Point(float(spawn_xy_m[0]), float(spawn_xy_m[1])) - playable_surface = min( - components, key=lambda component: component.distance(spawn_point) - ) - enclosure_geometry = playable_surface.buffer( - _LANE_PERIMETER_CLEARANCE_M, - join_style="mitre", - ).simplify(_LANE_PERIMETER_SIMPLIFY_M, preserve_topology=True) - enclosure_components = _polygon_components(enclosure_geometry) - if not enclosure_components: - return _empty_segments() - enclosure = min( - enclosure_components, - key=lambda component: component.distance(spawn_point), - ) - - z_m = float(np.median(np.concatenate(lane_heights))) - ring_segments: list[npt.NDArray[np.float32]] = [] - for ring in (enclosure.exterior, *enclosure.interiors): - ring_xy = np.asarray(ring.coords, dtype=np.float32) - if len(ring_xy) < 4: - continue - ring_xyz = np.column_stack( - (ring_xy, np.full(len(ring_xy), z_m, dtype=np.float32)) - ).astype(np.float32) - ring_segments.append(np.stack((ring_xyz[:-1], ring_xyz[1:]), axis=1)) - if not ring_segments: - return _empty_segments() - return np.concatenate(ring_segments, axis=0).astype(np.float32) - - -def _boundary_polylines( - rows: list[dict[str, Any]], -) -> tuple[npt.NDArray[np.float32], ...]: - polylines: list[npt.NDArray[np.float32]] = [] - for row in rows: - points = _points_from_records(row.get("road_boundary", {}).get("location", [])) - if len(points) >= 2: - polylines.append(points) - return tuple(polylines) - - -def _build_fallback_perimeter( - lane_rows: list[dict[str, Any]], - boundary_rows: list[dict[str, Any]], -) -> npt.NDArray[np.float32]: - points: list[npt.NDArray[np.float32]] = [] - for row in lane_rows: - payload = row.get("lane", {}) - if not _car_lane(payload): - continue - rails = _aligned_lane_rails(payload) - if rails is not None: - points.extend(rails) - points.extend(_boundary_polylines(boundary_rows)) - if not points: - return _empty_segments() - all_points = np.concatenate(points, axis=0) - x_min, y_min = np.min(all_points[:, :2], axis=0) - _PERIMETER_MARGIN_M - x_max, y_max = np.max(all_points[:, :2], axis=0) + _PERIMETER_MARGIN_M - z_m = float(np.median(all_points[:, 2])) - corners = np.asarray( - [ - [x_min, y_min, z_m], - [x_max, y_min, z_m], - [x_max, y_max, z_m], - [x_min, y_max, z_m], - ], - dtype=np.float32, - ) - return np.stack( - [np.stack((corners[index - 1], corners[index])) for index in range(4)] - ).astype(np.float32) - - -def _build_lane_centerlines(rows: list[dict[str, Any]]) -> tuple[np.ndarray, ...]: - """Return directed car-lane centerlines from ClipGT records.""" - return tuple(lane.centerline_world for lane in _build_navigation_lanes(rows)) - - -def _build_navigation_lanes( - rows: list[dict[str, Any]], -) -> tuple[NavigationLane, ...]: - """Return directed car lanes and their mapped roadside stopping edges.""" - lanes: list[NavigationLane] = [] - for row in rows: - payload = row["lane"] - if not _car_lane(payload): - continue - rails = _aligned_lane_rails(payload) - if rails is None: - continue - left_rail, right_rail = rails - sample_count = max(2, len(left_rail), len(right_rail)) - fractions = np.linspace(0.0, 1.0, sample_count, dtype=np.float32) - left_rail = _sample_polyline_fractions(left_rail, fractions) - right_rail = _sample_polyline_fractions(right_rail, fractions) - centerline = 0.5 * (left_rail + right_rail) - if float(np.linalg.norm(centerline[-1, :2] - centerline[0, :2])) > 1.0e-4: - road_edge = _roadside_edge(payload, left_rail, right_rail) - lanes.append( - NavigationLane( - centerline.astype(np.float32), - road_edge, - allows_taxi_stops=road_edge is not None, + if boundaries and arrivals and departures: + fare_regions.append( + NavigationFareRegion( + node.node_id, + "boundary", + boundaries, + arrivals, + departures, ) ) - - return tuple(lanes) - - -def _roadside_edge( - payload: dict[str, Any], - left_rail: np.ndarray, - right_rail: np.ndarray, -) -> np.ndarray | None: - left_score = _road_edge_score( - payload.get("left_edge_styles", []), - payload.get("left_edge_colors", []), - ) - right_score = _road_edge_score( - payload.get("right_edge_styles", []), - payload.get("right_edge_colors", []), + return CrazyRobotaxiSceneData( + reference_route_world=spawn_lane.centerline_world, + navigation_lanes=lanes, + fare_regions=tuple(fare_regions), + curb_segments_world=( + np.asarray(curb_segments, dtype=np.float32) + if curb_segments + else np.empty((0, 2, 3), dtype=np.float32) + ), ) - if left_score == right_score == 0: - return None - return right_rail if right_score >= left_score else left_rail - - -def _road_edge_score(styles: list[str] | None, colors: list[str] | None) -> int: - point_scores = [] - for style, color in zip(styles or (), colors or (), strict=True): - normalized_style = str(style).upper() - normalized_color = str(color).upper() - if normalized_style in _PHYSICAL_ROAD_EDGE_STYLES: - point_scores.append(2) - elif ( - normalized_style in _PAINTED_ROAD_EDGE_STYLES - and normalized_color == "WHITE" - ): - point_scores.append(1) - else: - point_scores.append(0) - return min(point_scores, default=0) diff --git a/apps/crazy_robotaxi/crazy_robotaxi/streaming_presenter.py b/apps/crazy_robotaxi/crazy_robotaxi/streaming_presenter.py index 1e8ac412a..98b0fe5c2 100644 --- a/apps/crazy_robotaxi/crazy_robotaxi/streaming_presenter.py +++ b/apps/crazy_robotaxi/crazy_robotaxi/streaming_presenter.py @@ -34,6 +34,7 @@ from http import HTTPStatus from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer from pathlib import Path +from typing import Any from urllib.parse import parse_qs, urlparse import numpy as np @@ -56,7 +57,6 @@ from crazy_robotaxi.game import ( TaxiCameraMarkerProjection, - project_segment_pose_to_bev, project_target_to_bev, project_taxi_markers_to_camera, ) @@ -277,9 +277,7 @@ def _print_port_conflict_help(host: str, port: int, exc: OSError) -> None: overflow: hidden; background: #222; pointer-events: none; } .taxi-map img { display: block; width: 100%; height: auto; } - .taxi-boundaries, #taxi-pins { position: absolute; inset: 0; width: 100%; height: 100%; } - .taxi-boundaries { overflow: hidden; } - .taxi-boundaries line { stroke: rgb(235, 50, 50); stroke-width: 1.8; vector-effect: non-scaling-stroke; } + #taxi-pins { position: absolute; inset: 0; width: 100%; height: 100%; } .taxi-pin { position: absolute; width: 18px; height: 18px; border-radius: 50%; border: 3px solid white; background: #76b900; @@ -332,122 +330,6 @@ def _print_port_conflict_help(host: str, port: int, exc: OSError) -> None: border-color: white; color: #111; } - .scene-picker { - position: fixed; bottom: 16px; right: 16px; - background: rgba(0, 0, 0, 0.7); - border: 1px solid rgba(255, 255, 255, 0.18); - border-radius: 10px; - color: white; - font-size: 12px; - display: flex; flex-direction: column; - max-height: 60vh; - backdrop-filter: blur(6px); - overflow: hidden; - /* Animate the collapse so the toggle feels physical rather than a - hard show/hide. ``max-height`` is the lever rather than ``display`` - because ``display: none`` short-circuits transitions. */ - transition: max-height 0.18s ease-out; - } - .scene-picker.hidden { display: none; } - .scene-picker.collapsed { max-height: 38px; } - .scene-picker-toggle { - background: none; border: none; color: white; - padding: 9px 12px; - display: flex; align-items: center; gap: 8px; - cursor: pointer; - font-size: 11px; font-weight: 600; - text-transform: uppercase; - letter-spacing: 0.06em; - width: 100%; - text-align: left; - flex-shrink: 0; - pointer-events: auto; - user-select: none; - } - .scene-picker-toggle:hover { background: rgba(255, 255, 255, 0.06); } - .scene-picker-count { - opacity: 0.55; - font-weight: 400; - text-transform: none; - letter-spacing: 0; - font-size: 11px; - } - .scene-picker-chevron { - margin-left: auto; - font-size: 10px; - transition: transform 0.18s ease-out; - } - .scene-picker.collapsed .scene-picker-chevron { - transform: rotate(-90deg); - } - .scene-picker-list { - display: flex; flex-direction: column; gap: 6px; - padding: 0 10px 10px 10px; - overflow-y: auto; - } - /* Hide the list's scroll viewport entirely while the panel is - collapsed so no scrollbar artifacts leak through the parent's - ``overflow: hidden`` clipping. */ - .scene-picker.collapsed .scene-picker-list { overflow: hidden; } - /* Replace Chromium's default scrollbar (which carries the up/down - arrow buttons that were poking out the bottom-right of the - collapsed panel) with a slim button-less rail. Firefox's - standards-track ``scrollbar-width`` covers the same ground. */ - .scene-picker-list { scrollbar-width: thin; scrollbar-color: rgba(255,255,255,0.22) transparent; } - .scene-picker-list::-webkit-scrollbar { width: 6px; } - .scene-picker-list::-webkit-scrollbar-track { background: transparent; } - .scene-picker-list::-webkit-scrollbar-thumb { - background: rgba(255, 255, 255, 0.22); - border-radius: 3px; - } - .scene-picker-list::-webkit-scrollbar-button { display: none; } - .scene-picker-list::-webkit-scrollbar-corner { background: transparent; } - .scene-card { - width: 160px; - border-radius: 6px; - overflow: hidden; - cursor: pointer; - border: 2px solid transparent; - transition: border-color 0.1s, transform 0.05s; - background: rgba(255, 255, 255, 0.05); - pointer-events: auto; - user-select: none; - } - .scene-card:hover { border-color: rgba(120, 200, 255, 0.7); } - .scene-card.loading { - border-color: rgba(120, 200, 255, 1.0); - pointer-events: none; - opacity: 0.7; - } - .scene-card img { - width: 100%; height: 72px; - object-fit: cover; - display: block; - background: #222; - } - .scene-card .scene-label { - padding: 6px 8px; - font-size: 11px; line-height: 1.3; - } - /* Weather-variant pills, shown only for multi-variant scenes. */ - .scene-variants { - display: flex; flex-wrap: wrap; gap: 4px; - padding: 0 8px 8px 8px; - } - .variant-pill { - background: rgba(255, 255, 255, 0.1); - border: 1px solid rgba(255, 255, 255, 0.25); - border-radius: 999px; - color: white; - font-size: 10px; - padding: 2px 8px; - cursor: pointer; - pointer-events: auto; - user-select: none; - transition: background-color 0.1s, border-color 0.1s; - } - .variant-pill:hover { background: rgba(120, 200, 255, 0.3); border-color: rgba(120, 200, 255, 0.7); } - .variant-pill.loading { border-color: rgba(120, 200, 255, 1.0); opacity: 0.7; pointer-events: none; } @@ -463,7 +345,6 @@ def _print_port_conflict_help(host: str, port: int, exc: OSError) -> None:
WASD / Arrows = Drive · 1 = World-Model RGB · 2 = HDMap · 3 = PhysX · R = Reset Rollout
-
-- @@ -537,8 +410,7 @@ def _print_port_conflict_help(host: str, port: int, exc: OSError) -> None: .catch(() => {}); // ignore network hiccups, next event will resync } // Skip key handling when focus is on a form input (e.g. a future -// settings panel). The scene picker is now click-driven so the -// keyboard never lands on a button there. +// settings panel). function shouldIgnoreKey(e) { const t = e.target; if (!t) return false; @@ -564,7 +436,6 @@ def _print_port_conflict_help(host: str, port: int, exc: OSError) -> None: const taxiStatusEl = document.getElementById('taxi-status'); const taxiEventEl = document.getElementById('taxi-event'); const taxiMapEl = document.getElementById('taxi-map'); -const taxiBoundariesEl = document.getElementById('taxi-boundaries'); const taxiPinsEl = document.getElementById('taxi-pins'); const gameOverEl = document.getElementById('game-over'); const gameOverTitleEl = document.getElementById('game-over-title'); @@ -648,15 +519,6 @@ def _print_port_conflict_help(host: str, port: int, exc: OSError) -> None: const markers = taxi.bev_targets || []; const showMap = taxi.bev_enabled; taxiMapEl.classList.toggle('hidden', !showMap); - taxiBoundariesEl.replaceChildren(); - (taxi.bev_enclosure_segments || []).forEach(segment => { - const line = document.createElementNS('http://www.w3.org/2000/svg', 'line'); - line.setAttribute('x1', `${segment.u0 * 100}`); - line.setAttribute('y1', `${segment.v0 * 100}`); - line.setAttribute('x2', `${segment.u1 * 100}`); - line.setAttribute('y2', `${segment.v1 * 100}`); - taxiBoundariesEl.appendChild(line); - }); taxiPinsEl.replaceChildren(); markers.filter(marker => marker.visible).forEach(marker => { const pin = document.createElement('div'); @@ -709,110 +571,6 @@ def _print_port_conflict_help(host: str, port: int, exc: OSError) -> None: setInterval(pollState, 100); pollState(); -// Scene picker. Hidden until /scenes returns at least one entry, -// then renders as a panel in the bottom-right. Auto-expanded on first -// load because nothing happens until the user picks a scene -- the -// server is blocked on ``wait_for_scene_selection`` and the MJPEG -// stream shows the "Select a scene to begin driving" overlay frame. -// After the first pick it auto-collapses (and click-outside collapses -// thereafter), so the panel stays out of the way during driving. -const scenePicker = document.getElementById('scene-picker'); -const scenePickerList = document.getElementById('scene-picker-list'); -const scenePickerToggle = document.getElementById('scene-picker-toggle'); -const scenePickerCount = document.getElementById('scene-picker-count'); -let SCENES = []; -let firstSceneLoaded = false; -function setScenePickerCollapsed(collapsed) { - scenePicker.classList.toggle('collapsed', collapsed); -} -scenePickerToggle.addEventListener('click', () => { - setScenePickerCollapsed(!scenePicker.classList.contains('collapsed')); -}); -// Click outside the panel collapses it -- but only after the user has -// actually picked their first scene. Pre-selection clicks (e.g. the -// user clicking on the camera area to dismiss something) don't tuck -// the picker away, since the panel is the only way to start driving. -document.addEventListener('mousedown', e => { - if (!firstSceneLoaded) return; - if (scenePicker.classList.contains('hidden')) return; - if (scenePicker.contains(e.target)) return; - setScenePickerCollapsed(true); -}); -async function fetchScenes() { - try { - const r = await fetch('/scenes', { cache: 'no-store' }); - if (!r.ok) return; - const data = await r.json(); - SCENES = Array.isArray(data.scenes) ? data.scenes : []; - scenePickerCount.textContent = SCENES.length ? `(${SCENES.length})` : ''; - if (!SCENES.length) { - scenePicker.classList.add('hidden'); - return; - } - scenePickerList.innerHTML = ''; - SCENES.forEach((s, i) => { - const card = document.createElement('div'); - card.className = 'scene-card'; - card.dataset.idx = String(i); - if (s.has_thumbnail) { - const img = document.createElement('img'); - img.src = '/thumbnail?scene=' + encodeURIComponent(s.path); - img.alt = ''; - img.onerror = () => { img.style.display = 'none'; }; - card.appendChild(img); - } - const label = document.createElement('div'); - label.className = 'scene-label'; - label.textContent = s.label || ('Scene ' + (i + 1)); - card.appendChild(label); - // Clicking the card (outside a pill) loads the default variant. - card.addEventListener('click', () => loadScene(i, card)); - const variants = Array.isArray(s.variants) ? s.variants : []; - if (variants.length > 1) { - const row = document.createElement('div'); - row.className = 'scene-variants'; - variants.forEach(v => { - const pill = document.createElement('button'); - pill.className = 'variant-pill'; - pill.type = 'button'; - pill.textContent = variantLabel(v); - pill.addEventListener('click', e => { - e.stopPropagation(); // don't also trigger the card's default-variant load - loadScene(i, card, v, pill); - }); - row.appendChild(pill); - }); - card.appendChild(row); - } - scenePickerList.appendChild(card); - }); - scenePicker.classList.remove('hidden'); - } catch {} -} -function variantLabel(v) { - const labels = { - default: 'Default', clear: 'Clear', snow: 'Snow', rain: 'Rain', - }; - return labels[v] || (v.charAt(0).toUpperCase() + v.slice(1)); -} -async function loadScene(idx, card, variant, pill) { - const scene = SCENES[idx]; - if (!scene) return; - (pill || card).classList.add('loading'); - try { - let url = '/scene/select?scene=' + encodeURIComponent(scene.path); - // No variant -> server uses the scene's default; a pill selects one. - if (variant) url += '&variant=' + encodeURIComponent(variant); - await fetch(url, { method: 'GET', cache: 'no-store' }); - } catch {} - // Tuck the panel away so the user gets the camera view back; the - // scene transition itself is driven by the server-side loop. From - // this point on, click-outside dismissal is enabled too. - firstSceneLoaded = true; - setScenePickerCollapsed(true); - setTimeout(() => { (pill || card).classList.remove('loading'); }, 1500); -} -fetchScenes(); @@ -853,7 +611,6 @@ def __init__( self._bev_config: BevConfig | None = None self._taxi_camera_calibration: CameraCalibration | None = None self._taxi_camera_models: dict[tuple[int, int], FThetaCameraModel] = {} - self._taxi_enclosure_segments_world = np.empty((0, 2, 3), dtype=np.float32) self._jpeg_quality = int(jpeg_quality) self._stop_event = threading.Event() self._frame_bus = LatestFrameBus[bytes]() @@ -1021,7 +778,7 @@ def bind_keyboard(self, keyboard: KeyboardState) -> None: _KeyboardDriveSink(keyboard) ) - def configure_taxi_hud(self, bev: BevConfig) -> None: + def configure_taxi_hud(self, bev: BevConfig, vehicle: Any = None) -> None: """Configure BEV projection used by browser taxi overlays.""" from crazy_robotaxi.driving import ( TaxiKeyboardDriveState, @@ -1029,9 +786,11 @@ def configure_taxi_hud(self, bev: BevConfig) -> None: self._taxi_enabled = True self._bev_config = bev - self._keyboard_drive_factory = TaxiKeyboardDriveState + self._keyboard_drive_factory = lambda sink: TaxiKeyboardDriveState( + sink, vehicle + ) self._keyboard_drive = TaxiKeyboardDriveState( - _KeyboardDriveSink(self._keyboard) + _KeyboardDriveSink(self._keyboard), vehicle ) def configure_taxi_camera(self, calibration: CameraCalibration) -> None: @@ -1039,13 +798,6 @@ def configure_taxi_camera(self, calibration: CameraCalibration) -> None: self._taxi_camera_calibration = calibration self._taxi_camera_models.clear() - def configure_taxi_enclosure(self, segments_world: np.ndarray) -> None: - """Configure static Taxi-only closure lines drawn over the browser BEV.""" - segments = np.asarray(segments_world, dtype=np.float32) - if segments.ndim != 3 or segments.shape[1:] != (2, 3): - raise ValueError("Taxi enclosure segments must have shape (N, 2, 3).") - self._taxi_enclosure_segments_world = segments.copy() - def process_events(self) -> None: # Update the integrator at simulation cadence regardless of how often # the browser posts /control events. @@ -1068,7 +820,14 @@ def prepare_frame(self, frame: PresentedFrame, view_mode: str) -> None: ) ) elif view_mode == "model_rgb" and frame.model_rgb_host_uint8 is not None: - _prefetch_to_numpy(frame.model_rgb_host_uint8) + _prefetch_to_numpy( + select_presented_rgb( + frame, + view_mode, + width=self._raster.width, + height=self._raster.height, + ) + ) else: _prefetch_to_numpy(frame.rgb_host_uint8) if frame.bev_host_uint8 is not None: @@ -1100,7 +859,13 @@ def with_flare(rgb: object) -> np.ndarray: ) elif view_mode == "model_rgb" and frame.model_rgb_host_uint8 is not None: image = _with_status_overlay( - frame.model_rgb_host_uint8, frame.status_message + select_presented_rgb( + frame, + view_mode, + width=self._raster.width, + height=self._raster.height, + ), + frame.status_message, ) else: image = _with_status_overlay(frame.rgb_host_uint8, frame.status_message) @@ -1349,32 +1114,6 @@ def _state_snapshot(self) -> dict[str, object]: taxi_payload["bev_targets"] = bev_targets else: taxi_payload["bev_targets"] = [] - bev_enclosure_segments = [] - if ( - frame is not None - and frame.bev_rig_to_world is not None - and self._bev_config is not None - ): - for segment in getattr( - self, - "_taxi_enclosure_segments_world", - np.empty((0, 2, 3), dtype=np.float32), - ): - projected = project_segment_pose_to_bev( - segment, frame.bev_rig_to_world, self._bev_config - ) - if projected is None: - continue - start, end = projected - bev_enclosure_segments.append( - { - "u0": start[0], - "v0": start[1], - "u1": end[0], - "v1": end[1], - } - ) - taxi_payload["bev_enclosure_segments"] = bev_enclosure_segments result["taxi"] = taxi_payload return result @@ -1458,10 +1197,7 @@ def _serve_index(self) -> None: self.send_response(HTTPStatus.OK) self.send_header("Content-Type", "text/html; charset=utf-8") self.send_header("Content-Length", str(len(body))) - # Aggressive no-cache so a browser that still has a - # pre-scene-picker tab open doesn't keep rendering the old - # HTML after a server upgrade. The page is tiny (~10 KB) so - # bypassing the cache on every reload costs nothing. + # Always serve the current control page after a server restart. self.send_header( "Cache-Control", "no-store, no-cache, must-revalidate, max-age=0" ) diff --git a/apps/crazy_robotaxi/pyproject.toml b/apps/crazy_robotaxi/pyproject.toml index 0d5ad31a1..482837eee 100644 --- a/apps/crazy_robotaxi/pyproject.toml +++ b/apps/crazy_robotaxi/pyproject.toml @@ -17,6 +17,7 @@ dependencies = [ [project.scripts] crazy-robotaxi = "crazy_robotaxi.cli:main" +crazy-robotaxi-map = "crazy_robotaxi.map_cli:main" [project.entry-points."flashdreams.runner_configs"] crazy-robotaxi = "crazy_robotaxi.runner:CRAZY_ROBOTAXI_RUNNER" @@ -44,6 +45,7 @@ crazy_robotaxi = [ "configs/wheels/*.yaml", "assets/README.md", "assets/wheel_and_pedals/*.png", + "maps/*.robotaxi.yaml", ] [tool.uv] diff --git a/apps/crazy_robotaxi/tests/maps/intersection_geometry.robotaxi.yaml b/apps/crazy_robotaxi/tests/maps/intersection_geometry.robotaxi.yaml new file mode 100644 index 000000000..c241664ca --- /dev/null +++ b/apps/crazy_robotaxi/tests/maps/intersection_geometry.robotaxi.yaml @@ -0,0 +1,58 @@ +schema_version: 1 +id: intersection-geometry-test +name: Intersection Geometry Test + +compiler: + sample_spacing_m: 1 + ground_margin_m: 10 + intersection_connector_samples: 8 + +profiles: + arterial: + lane_width_m: 3.6 + curb_offset_m: 0.6 + lanes: [backward, backward, forward, forward] + speed_limit_mps: 14 + lane_marking: {style: DASHED_SINGLE, color: WHITE} + divider_markings: + - {style: DASHED_SINGLE, color: WHITE} + - {style: SOLID_GROUP, color: YELLOW} + - {style: DASHED_SINGLE, color: WHITE} + street: + lane_width_m: 3.6 + curb_offset_m: 0.6 + lanes: [backward, forward] + speed_limit_mps: 10 + lane_marking: {style: SOLID_GROUP, color: YELLOW} + divider_markings: + - {style: SOLID_GROUP, color: YELLOW} + +nodes: + - {id: center, type: intersection, pose: {x_m: 0, y_m: 0}} + - id: west_end + type: cul_de_sac + pose: {x_m: -120, y_m: 0} + culdesac_radius_m: 12 + - id: east_end + type: cul_de_sac + pose: {x_m: 120, y_m: 20} + culdesac_radius_m: 12 + - id: north_end + type: cul_de_sac + pose: {x_m: 10, y_m: 100} + culdesac_radius_m: 10 + +roads: + - {id: west_road, from: west_end, to: center, profile: arterial} + - {id: east_road, from: center, to: east_end, profile: arterial} + - {id: north_road, from: center, to: north_end, profile: street} + +spawns: + - id: start + road: west_road + lane: 2 + distance_m: 30 + variants: + default: + image: package://omnidreams_game_engine/screenshot.jpg + prompt: A skewed three-way city intersection. diff --git a/apps/crazy_robotaxi/tests/maps/parking_driveway.robotaxi.yaml b/apps/crazy_robotaxi/tests/maps/parking_driveway.robotaxi.yaml new file mode 100644 index 000000000..45d3c64d6 --- /dev/null +++ b/apps/crazy_robotaxi/tests/maps/parking_driveway.robotaxi.yaml @@ -0,0 +1,56 @@ +schema_version: 1 +id: parking-driveway-test +name: Parking Driveway Test + +compiler: + sample_spacing_m: 1 + ground_margin_m: 10 + intersection_connector_samples: 8 + +profiles: + street: + lane_width_m: 3.6 + curb_offset_m: 0.6 + lanes: [backward, forward] + speed_limit_mps: 10 + lane_marking: {style: SOLID_GROUP, color: YELLOW} + divider_markings: + - {style: SOLID_GROUP, color: YELLOW} + +nodes: + - id: west_end + type: cul_de_sac + pose: {x_m: -100, y_m: 0} + culdesac_radius_m: 10 + - id: lot_driveway + type: driveway + pose: {x_m: 0, y_m: 0} + - id: east_end + type: cul_de_sac + pose: {x_m: 100, y_m: 0} + culdesac_radius_m: 10 + - id: parking_lot + type: parking_lot + connected_to: lot_driveway + opening_vertex: 2 + vertices: + - {x_m: -30, y_m: -20} + - {x_m: -6, y_m: -20} + - {x_m: 6, y_m: -20} + - {x_m: 30, y_m: -20} + - {x_m: 30, y_m: -60} + - {x_m: -30, y_m: -60} + +roads: + - {id: west_road, from: west_end, to: lot_driveway, profile: street} + - {id: east_road, from: lot_driveway, to: east_end, profile: street} + +spawns: + - id: start + road: west_road + lane: 1 + distance_m: 30 + variants: + default: + image: package://omnidreams_game_engine/screenshot.jpg + prompt: A street beside a small parking lot. diff --git a/apps/crazy_robotaxi/tests/maps/traffic_intersection.robotaxi.yaml b/apps/crazy_robotaxi/tests/maps/traffic_intersection.robotaxi.yaml new file mode 100644 index 000000000..b895682f5 --- /dev/null +++ b/apps/crazy_robotaxi/tests/maps/traffic_intersection.robotaxi.yaml @@ -0,0 +1,81 @@ +schema_version: 1 +id: traffic-intersection-test +name: Traffic Intersection Test + +compiler: + sample_spacing_m: 1 + ground_margin_m: 10 + intersection_connector_samples: 8 + +profiles: + wide: + lane_width_m: 3.6 + curb_offset_m: 0.6 + lanes: [backward, backward, forward, forward] + speed_limit_mps: 12 + lane_marking: {style: DASHED_SINGLE, color: WHITE} + divider_markings: + - {style: DASHED_SINGLE, color: WHITE} + - {style: SOLID_GROUP, color: YELLOW} + - {style: DASHED_SINGLE, color: WHITE} + narrow: + lane_width_m: 3.2 + curb_offset_m: 0.5 + curb: false + lanes: [backward, forward] + speed_limit_mps: 12 + lane_marking: {style: DASHED_SINGLE, color: WHITE} + divider_markings: + - {style: SOLID_GROUP, color: YELLOW} + +nodes: + - id: center + type: intersection + pose: {x_m: 0, y_m: 0} + lane_transition_length_m: 20 + - {id: north_joint, type: road_joint, pose: {x_m: 0, y_m: 100}} + - {id: south_joint, type: road_joint, pose: {x_m: 0, y_m: -100}} + - {id: east_joint, type: road_joint, pose: {x_m: 100, y_m: 0}} + - {id: west_joint, type: road_joint, pose: {x_m: -100, y_m: 0}} + - id: north_end + type: cul_de_sac + pose: {x_m: 0, y_m: 200} + culdesac_radius_m: 12 + - id: south_end + type: cul_de_sac + pose: {x_m: 0, y_m: -200} + culdesac_radius_m: 12 + - id: east_end + type: cul_de_sac + pose: {x_m: 200, y_m: 0} + culdesac_radius_m: 12 + - id: west_end + type: cul_de_sac + pose: {x_m: -200, y_m: 0} + culdesac_radius_m: 12 + +roads: + - {id: north_road, from: center, to: north_joint, profile: narrow} + - {id: south_road, from: center, to: south_joint, profile: wide} + - {id: east_road, from: center, to: east_joint, profile: narrow} + - {id: west_road, from: center, to: west_joint, profile: narrow} + - {id: north_outer, from: north_joint, to: north_end, profile: narrow} + - {id: south_outer, from: south_joint, to: south_end, profile: wide} + - {id: east_outer, from: east_joint, to: east_end, profile: narrow} + - {id: west_outer, from: west_joint, to: west_end, profile: narrow} + +spawns: + - id: start + road: north_outer + lane: 0 + distance_m: 20 + variants: + default: + image: package://omnidreams_game_engine/screenshot.jpg + prompt: A car approaching a four-way intersection. + +traffic: + - id: turning_car + nodes: [north_end, center, east_end] + end_behavior: reverse + start_distance_m: 30 diff --git a/apps/crazy_robotaxi/tests/maps/traffic_loop.robotaxi.yaml b/apps/crazy_robotaxi/tests/maps/traffic_loop.robotaxi.yaml new file mode 100644 index 000000000..458b802e2 --- /dev/null +++ b/apps/crazy_robotaxi/tests/maps/traffic_loop.robotaxi.yaml @@ -0,0 +1,48 @@ +schema_version: 1 +id: traffic-loop-test +name: Traffic Loop Test + +compiler: + sample_spacing_m: 1 + ground_margin_m: 10 + intersection_connector_samples: 8 + +profiles: + street: + lane_width_m: 3.6 + curb_offset_m: 0.6 + lanes: [backward, forward] + speed_limit_mps: 12 + lane_marking: {style: SOLID_GROUP, color: YELLOW} + divider_markings: + - {style: SOLID_GROUP, color: YELLOW} + +nodes: + - {id: southwest, type: road_joint, pose: {x_m: -150, y_m: -100}} + - {id: south, type: road_joint, pose: {x_m: 0, y_m: -100}} + - {id: southeast, type: road_joint, pose: {x_m: 150, y_m: -100}} + - {id: east, type: road_joint, pose: {x_m: 150, y_m: 0}} + - {id: northeast, type: road_joint, pose: {x_m: 150, y_m: 100}} + - {id: north, type: road_joint, pose: {x_m: 0, y_m: 100}} + - {id: northwest, type: road_joint, pose: {x_m: -150, y_m: 100}} + - {id: west, type: road_joint, pose: {x_m: -150, y_m: 0}} + +roads: + - {id: south_west, from: southwest, to: south, profile: street} + - {id: south_east, from: south, to: southeast, profile: street} + - {id: east_south, from: southeast, to: east, profile: street} + - {id: east_north, from: east, to: northeast, profile: street} + - {id: north_east, from: northeast, to: north, profile: street} + - {id: north_west, from: north, to: northwest, profile: street} + - {id: west_north, from: northwest, to: west, profile: street} + - {id: west_south, from: west, to: southwest, profile: street} + +spawns: + - id: start + road: south_west + lane: 1 + distance_m: 30 + variants: + default: + image: package://omnidreams_game_engine/screenshot.jpg + prompt: A car on a rectangular city loop. diff --git a/apps/crazy_robotaxi/tests/test_alignment_diagnostics.py b/apps/crazy_robotaxi/tests/test_alignment_diagnostics.py index 235179d9d..c6aa649f0 100644 --- a/apps/crazy_robotaxi/tests/test_alignment_diagnostics.py +++ b/apps/crazy_robotaxi/tests/test_alignment_diagnostics.py @@ -17,6 +17,7 @@ from omnidreams_game_engine.math3d import rig_pose_from_vehicle_state from omnidreams_game_engine.types import ( CameraCalibration, + DriverCommand, PhysicsDebugFrame, PresentedFrame, VehicleState, @@ -97,6 +98,13 @@ def _frame() -> PresentedFrame: physx_debug=debug, rig_to_world=rig_pose_from_vehicle_state(state), vehicle_state=state, + driver_command=DriverCommand(throttle=1.0, steer=-0.5), + impact_kind="static", + model_motion_metrics={ + "axis": "impact", + "mismatched": True, + "elapsed_ms": 1.25, + }, ) @@ -129,6 +137,9 @@ def test_diagnostic_presenter_writes_synchronized_artifact(tmp_path: Path) -> No assert float(rows[0]["state_rig_yaw_error_rad"]) == pytest.approx(0.0, abs=1e-6) assert float(rows[0]["state_physx_yaw_error_rad"]) == pytest.approx(0.0, abs=1e-6) assert float(rows[0]["state_physx_xy_error_m"]) == pytest.approx(0.0) + assert float(rows[0]["command_throttle"]) == pytest.approx(1.0) + assert rows[0]["impact_kind"] == "static" + assert rows[0]["motion_axis"] == "impact" assert diagnostic_frame.exists() with Image.open(diagnostic_frame) as image: assert image.width > image.height diff --git a/apps/crazy_robotaxi/tests/test_app_smoke.py b/apps/crazy_robotaxi/tests/test_app_smoke.py index d9df524c0..53e485e0b 100644 --- a/apps/crazy_robotaxi/tests/test_app_smoke.py +++ b/apps/crazy_robotaxi/tests/test_app_smoke.py @@ -14,8 +14,6 @@ import pytest from omnidreams_game_engine import _sample_assets -from omnidreams_game_engine._sample_assets import SAMPLE_SCENE -from omnidreams_game_engine.scene_fixture import build_synthetic_scene_usdz from pyvirtualdisplay.display import Display _WARMUP_SENTINEL = "[chunk-pipeline] warmup done" @@ -24,6 +22,9 @@ _WORLD_MODEL_TIMEOUT_S = 600.0 _LIVE_DURATION_S = 3.0 _SHUTDOWN_TIMEOUT_S = 15.0 +_GAME_MAP = ( + Path(__file__).parents[1] / "crazy_robotaxi" / "maps" / "minimal_loop.robotaxi.yaml" +) def _pump_stream( @@ -78,8 +79,8 @@ def _wait_for_sentinel( ) -def _run_raster_ui_smoke(scene_path: Path) -> None: - """Drive the full interactive_drive app subprocess under Xvfb against ``scene_path`` +def _run_raster_ui_smoke(map_path: Path) -> None: + """Drive the full interactive_drive app subprocess under Xvfb against ``map_path`` and assert it warms up, stays alive, and shuts down cleanly on SIGTERM. Does NOT validate raster output correctness - see @@ -105,14 +106,14 @@ def _run_raster_ui_smoke(scene_path: Path) -> None: # HUD; the raster backend then prints the warmup # sentinel directly to this process's stdout. "--no-hud", - "--scene", - str(scene_path), + "--map", + str(map_path), "--backend", "raster", "--camera", "camera_front_wide_120fov", "--variant", - "1", + "default", ], stdout=subprocess.PIPE, stderr=subprocess.STDOUT, @@ -169,7 +170,7 @@ def _run_raster_ui_smoke(scene_path: Path) -> None: display.stop() -def _run_synthetic_world_model_latency_smoke(scene_path: Path) -> None: +def _run_synthetic_world_model_latency_smoke(map_path: Path) -> None: """Run the synthetic world model through the interactive-drive latency path.""" display = Display(backend="xvfb", size=(1280, 720), visible=False) display.start() @@ -187,8 +188,8 @@ def _run_synthetic_world_model_latency_smoke(scene_path: Path) -> None: "-m", "omnidreams_game_engine", "--no-hud", - "--scene", - str(scene_path), + "--map", + str(map_path), "--backend", "omnidreams", "--manifest", @@ -200,7 +201,7 @@ def _run_synthetic_world_model_latency_smoke(scene_path: Path) -> None: "--camera", "camera_front_wide_120fov", "--variant", - "1", + "default", ], stdout=subprocess.PIPE, stderr=subprocess.STDOUT, @@ -250,24 +251,8 @@ def _run_synthetic_world_model_latency_smoke(scene_path: Path) -> None: @pytest.mark.gpu @pytest.mark.xvfb -# Opportunistic: runs opportunistically when the production USDZ has been -# fetched via ``prepare.py``. The synthetic-scene variant below exercises the -# same rendering path on every run, so silently skipping this one on -# workstations without the asset is safe. -@pytest.mark.skipif( - not SAMPLE_SCENE.exists(), - reason="sample scene is not available on this workstation", -) -def test_interactive_drive_raster_ui_smoke_real_scene() -> None: - _run_raster_ui_smoke(SAMPLE_SCENE) - - -@pytest.mark.gpu -@pytest.mark.xvfb -def test_interactive_drive_raster_ui_smoke_synthetic_scene(tmp_path: Path) -> None: - """Smoke test against a USDZ built in-process by ``build_synthetic_scene_usdz``.""" - scene_path = build_synthetic_scene_usdz(tmp_path / "synthetic_scene.usdz") - _run_raster_ui_smoke(scene_path) +def test_interactive_drive_raster_ui_smoke() -> None: + _run_raster_ui_smoke(_GAME_MAP) # gpu + xvfb -> routed to ``manual`` by conftest (see pytest_collection_modifyitems): @@ -276,6 +261,5 @@ def test_interactive_drive_raster_ui_smoke_synthetic_scene(tmp_path: Path) -> No # under ``xvfb-run`` in the benchmark job. @pytest.mark.gpu @pytest.mark.xvfb -def test_interactive_drive_synthetic_world_model_latency_smoke(tmp_path: Path) -> None: - scene_path = build_synthetic_scene_usdz(tmp_path / "synthetic_scene.usdz") - _run_synthetic_world_model_latency_smoke(scene_path) +def test_interactive_drive_synthetic_world_model_latency_smoke() -> None: + _run_synthetic_world_model_latency_smoke(_GAME_MAP) diff --git a/apps/crazy_robotaxi/tests/test_cli.py b/apps/crazy_robotaxi/tests/test_cli.py index f31f6ba2d..c19a0cacd 100644 --- a/apps/crazy_robotaxi/tests/test_cli.py +++ b/apps/crazy_robotaxi/tests/test_cli.py @@ -1,15 +1,64 @@ # SPDX-License-Identifier: Apache-2.0 # SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +import argparse +from collections.abc import Callable from pathlib import Path import pytest from crazy_robotaxi import runtime_cli as cli from crazy_robotaxi.runtime_cli import build_parser +from omnidreams_game_engine.cli import build_parser as build_engine_parser from omnidreams_game_engine.cli_args import arg_was_explicit pytestmark = pytest.mark.ci_cpu +_MAP_ARGS = ["--map", "city.robotaxi.yaml"] + + +@pytest.mark.parametrize("parser_factory", [build_parser, build_engine_parser]) +def test_map_flag_sets_internal_scene_path( + parser_factory: Callable[[], argparse.ArgumentParser], +) -> None: + args = parser_factory().parse_args(_MAP_ARGS) + + assert args.scene == Path("city.robotaxi.yaml") + + +@pytest.mark.parametrize("parser_factory", [build_parser, build_engine_parser]) +def test_force_map_recompile_flag( + parser_factory: Callable[[], argparse.ArgumentParser], +) -> None: + assert parser_factory().parse_args([]).force_map_recompile is False + assert ( + parser_factory().parse_args(["--force-map-recompile"]).force_map_recompile + is True + ) + + +def test_force_map_recompile_flag_is_forwarded_to_app_config( + monkeypatch: pytest.MonkeyPatch, +) -> None: + monkeypatch.setattr(cli, "RasterRenderBackend", lambda **_kwargs: object()) + + config, _backend = cli.prepare_config_and_backend( + build_parser().parse_args([*_MAP_ARGS, "--force-map-recompile"]) + ) + + assert config.force_map_recompile is True + + +@pytest.mark.parametrize( + "removed_flag", + ["--scene", "--synthetic-scene", "--synthetic-initial-rgb", "--synthetic-prompt"], +) +@pytest.mark.parametrize("parser_factory", [build_parser, build_engine_parser]) +def test_removed_map_input_flags_are_not_accepted( + removed_flag: str, parser_factory: Callable[[], argparse.ArgumentParser] +) -> None: + with pytest.raises(SystemExit): + parser_factory().parse_args([removed_flag]) + def test_offload_text_encoder_flag_defaults_disabled() -> None: args = build_parser().parse_args([]) @@ -40,7 +89,7 @@ def test_visual_flare_override_defaults_disabled() -> None: ("argv", "game_mode_enabled", "visual_flare_enabled"), [ ([], False, False), - (["--game-mode"], True, True), + (["--game-mode"], True, False), (["--game-mode", "--disable-visual-flare"], True, False), ], ) @@ -52,7 +101,9 @@ def test_game_mode_controls_speed_limit_collisions_and_visual_flare( ) -> None: monkeypatch.setattr(cli, "RasterRenderBackend", lambda **_k: object()) - config, _backend = cli.prepare_config_and_backend(build_parser().parse_args(argv)) + config, _backend = cli.prepare_config_and_backend( + build_parser().parse_args([*_MAP_ARGS, *argv]) + ) assert config.vehicle.speed_limit_enabled is game_mode_enabled assert config.vehicle.actor_collision_enabled is game_mode_enabled @@ -77,7 +128,7 @@ def build_backend(**kwargs: object) -> object: monkeypatch.setattr(cli, "RasterRenderBackend", build_backend) - cli.prepare_config_and_backend(build_parser().parse_args(argv)) + cli.prepare_config_and_backend(build_parser().parse_args([*_MAP_ARGS, *argv])) assert backend_kwargs["synchronize_bev_with_rgb"] is expected_synchronization @@ -90,6 +141,38 @@ def test_taxi_alignment_diagnostics_accepts_output_directory() -> None: assert args.taxi_alignment_diagnostics == Path("diagnostics") +@pytest.mark.parametrize( + ("diagnostic_args", "expected_enabled"), + [([], False), (["--taxi-alignment-diagnostics", "diagnostics"], True)], +) +def test_taxi_alignment_diagnostics_gate_motion_conformance( + monkeypatch: pytest.MonkeyPatch, + diagnostic_args: list[str], + expected_enabled: bool, +) -> None: + backend_kwargs: dict[str, object] = {} + + def build_backend(**kwargs: object) -> object: + backend_kwargs.update(kwargs) + return object() + + monkeypatch.setattr(cli, "WorldModelRenderBackend", build_backend) + args = build_parser().parse_args( + [ + *_MAP_ARGS, + "--backend", + "omnidreams", + "--manifest", + "example_world_model_synthetic.yaml", + *diagnostic_args, + ] + ) + + cli.prepare_config_and_backend(args) + + assert backend_kwargs["motion_conformance_diagnostics_enabled"] is expected_enabled + + def test_postprocess_preset_defaults_disabled() -> None: args = build_parser().parse_args([]) diff --git a/apps/crazy_robotaxi/tests/test_core_control.py b/apps/crazy_robotaxi/tests/test_core_control.py index 95f557fae..f5fe27a04 100644 --- a/apps/crazy_robotaxi/tests/test_core_control.py +++ b/apps/crazy_robotaxi/tests/test_core_control.py @@ -91,7 +91,7 @@ def test_sample_chunk_trajectory_advances_pose_and_time() -> None: chunk = sample_chunk_trajectory( start_state=state, start_timestamp_us=1000, - command=command, + commands=(command,) * 4, chunk_size=4, chunk_config=ChunkConfig(fps=10, initial_chunk_frames=2, chunk_frames=2), vehicle_config=VehicleConfig(), diff --git a/apps/crazy_robotaxi/tests/test_demo_scene_selection.py b/apps/crazy_robotaxi/tests/test_demo_scene_selection.py index abe741df6..b3ee206b2 100644 --- a/apps/crazy_robotaxi/tests/test_demo_scene_selection.py +++ b/apps/crazy_robotaxi/tests/test_demo_scene_selection.py @@ -5,17 +5,20 @@ import argparse import types +from collections.abc import Callable from pathlib import Path import pytest from crazy_robotaxi import cli as demo_mod from crazy_robotaxi.cli import ( SceneOption, - _materialize_synthetic_scene_for_picker, _resolve_scene_variant, _validate_presenter_mode, build_parser, ) +from omnidreams_game_engine.demo import build_parser as build_engine_demo_parser + +pytestmark = pytest.mark.ci_cpu def test_auto_start_flag_and_deprecated_alias() -> None: @@ -28,6 +31,17 @@ def test_auto_start_flag_and_deprecated_alias() -> None: assert parser.parse_args(["--no-autoload-scene"]).auto_start is False +@pytest.mark.parametrize("parser_factory", [build_parser, build_engine_demo_parser]) +def test_map_directory_flag_replaces_scene_directory( + parser_factory: Callable[[], argparse.ArgumentParser], +) -> None: + parser = parser_factory() + + assert parser.parse_args(["--map-dir", "maps"]).scene_dir == Path("maps") + with pytest.raises(SystemExit): + parser.parse_args(["--scene-dir", "scenes"]) + + def test_bare_native_taxi_mode_is_rejected() -> None: args = build_parser().parse_args(["--taxi-game", "--no-hud"]) @@ -43,49 +57,50 @@ def test_browser_taxi_mode_may_imply_no_hud() -> None: _validate_presenter_mode(args) -def test_resolve_scene_variant_prefers_weather_archive_path_for_default( - tmp_path: Path, -) -> None: - scene_uuid = "0d404ff7-2b66-498c-b047-1ed8cded60d4" - base = (tmp_path / f"clipgt-{scene_uuid}.usdz").resolve() - snow = (tmp_path / f"clipgt-{scene_uuid}-snow.usdz").resolve() +def test_resolve_scene_variant_uses_default_map_variant(tmp_path: Path) -> None: + game_map = (tmp_path / "city.robotaxi.yaml").resolve() option = SceneOption( - label="Quiet Suburban Boulevard", - path=base, + label="City", + path=game_map, variants=("default", "rain", "snow"), - variant_paths={"default": base, "snow": snow}, + variant_paths={ + "default": game_map, + "rain": game_map, + "snow": game_map, + }, ) - assert _resolve_scene_variant((option,), snow, "default") == "snow" + assert _resolve_scene_variant((option,), game_map, "default") == "default" -def test_resolve_scene_variant_keeps_explicit_weather_choice(tmp_path: Path) -> None: - scene_uuid = "0d404ff7-2b66-498c-b047-1ed8cded60d4" - base = (tmp_path / f"clipgt-{scene_uuid}.usdz").resolve() - snow = (tmp_path / f"clipgt-{scene_uuid}-snow.usdz").resolve() - rain = (tmp_path / f"clipgt-{scene_uuid}-rain.usdz").resolve() +def test_resolve_scene_variant_keeps_explicit_choice(tmp_path: Path) -> None: + game_map = (tmp_path / "city.robotaxi.yaml").resolve() option = SceneOption( - label="Quiet Suburban Boulevard", - path=base, + label="City", + path=game_map, variants=("default", "rain", "snow"), - variant_paths={"default": base, "rain": rain, "snow": snow}, + variant_paths={variant: game_map for variant in ("default", "rain", "snow")}, ) - assert _resolve_scene_variant((option,), snow, "rain") == "rain" + assert _resolve_scene_variant((option,), game_map, "rain") == "rain" -def test_resolve_scene_variant_legacy_option_without_variant_paths( +def test_resolve_scene_variant_falls_back_to_first_authored_variant( tmp_path: Path, ) -> None: - scene = (tmp_path / "legacy.usdz").resolve() - option = SceneOption(label="legacy", path=scene, variants=("1", "2")) + game_map = (tmp_path / "city.robotaxi.yaml").resolve() + option = SceneOption( + label="City", + path=game_map, + variants=("default", "rain"), + variant_paths={"default": game_map, "rain": game_map}, + ) - assert _resolve_scene_variant((option,), scene, "default") == "1" - assert _resolve_scene_variant((option,), scene, "2") == "2" + assert _resolve_scene_variant((option,), game_map, "missing") == "default" class _FakePresenter: - """Records the scene-selection calls ``_run_streaming`` makes.""" + """Records the scene lifecycle calls ``_run_streaming`` makes.""" def __init__(self, **_kwargs: object) -> None: self.wait_for_scene_selection_calls = 0 @@ -106,8 +121,6 @@ def wait_while_preloading(self, probe: object) -> None: self.calls.append("wait_while_preloading") def wait_for_scene_selection(self) -> tuple[Path, str] | None: - # The whole point of --auto-start is that this never runs; record any - # call so the test can assert the picker was skipped. self.wait_for_scene_selection_calls += 1 return None @@ -131,8 +144,7 @@ def __init__( ) -> None: self.loaded: list[tuple[Path, str]] = [] self.ran = 0 - # Successive return values for preload_in_progress(); the auto-start - # path checks it once before deciding to wait on the preloader. + # Successive return values for preload_in_progress(). self._preload_states = list(preload_states) def model_ready(self) -> bool: @@ -154,16 +166,19 @@ def shutdown(self) -> None: ... @pytest.mark.parametrize("preloading", [False, True]) -def test_run_streaming_auto_start_skips_scene_picker( - tmp_path: Path, monkeypatch: pytest.MonkeyPatch, preloading: bool +@pytest.mark.parametrize("auto_start", [False, True]) +def test_run_streaming_starts_command_line_map( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, + preloading: bool, + auto_start: bool, ) -> None: - scene = tmp_path / "scene.usdz" + scene = tmp_path / "scene.robotaxi.yaml" scene.write_bytes(b"") option = SceneOption(label="scene", path=scene, variants=("default",)) presenter = _FakePresenter() - # When preloading, preload_in_progress() reports True on the first check so - # the auto-start path must wait for the preloader, then False afterwards. + # When preloading, preload_in_progress() reports True on the first check. app = _FakeApp(preload_states=(True, False) if preloading else (False,)) monkeypatch.setattr( @@ -197,10 +212,7 @@ def test_run_streaming_auto_start_skips_scene_picker( stream_mjpeg="8080", preload_scenes=False, prompt=None, - auto_start=True, - synthetic_scene=False, - synthetic_initial_rgb=None, - synthetic_prompt=None, + auto_start=auto_start, ) demo_mod._run_streaming(args) @@ -220,39 +232,3 @@ def test_run_streaming_auto_start_skips_scene_picker( ) else: assert presenter.wait_while_preloading_probes == [] - - -def test_materialize_synthetic_scene_for_picker_consumes_synthetic_args( - monkeypatch, tmp_path: Path -) -> None: - parser = build_parser() - args = parser.parse_args( - [ - "--synthetic-scene", - "--synthetic-initial-rgb", - "seed.png", - "--synthetic-prompt", - "drive forward", - ] - ) - built_scene = tmp_path / "synthetic.usdz" - calls: list[tuple[Path | None, str | None]] = [] - - def fake_build_synthetic_scene_to_temp( - *, initial_rgb_path: Path | None = None, prompt: str | None = None - ) -> Path: - calls.append((initial_rgb_path, prompt)) - return built_scene - - monkeypatch.setattr( - "crazy_robotaxi.cli.build_synthetic_scene_to_temp", - fake_build_synthetic_scene_to_temp, - ) - - _materialize_synthetic_scene_for_picker(args) - - assert calls == [(Path("seed.png"), "drive forward")] - assert args.scene == built_scene - assert args.synthetic_scene is False - assert args.synthetic_initial_rgb is None - assert args.synthetic_prompt is None diff --git a/apps/crazy_robotaxi/tests/test_file_settings.py b/apps/crazy_robotaxi/tests/test_file_settings.py new file mode 100644 index 000000000..a5866e1ed --- /dev/null +++ b/apps/crazy_robotaxi/tests/test_file_settings.py @@ -0,0 +1,95 @@ +# SPDX-License-Identifier: Apache-2.0 +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. + +"""CPU-safe tests for standalone renderer and game YAML settings.""" + +from __future__ import annotations + +from pathlib import Path + +import pytest +import yaml +from crazy_robotaxi import runtime_cli +from crazy_robotaxi.game_settings import load_game_settings +from omnidreams_game_engine.renderer_settings import load_renderer_settings +from omnidreams_game_engine.yaml_config import StrictConfigError + +pytestmark = pytest.mark.ci_cpu + +_CONFIG_ROOT = Path(__file__).parents[1] / "crazy_robotaxi" / "configs" + + +def test_bundled_renderer_and_game_settings_are_complete() -> None: + """Load both packaged configuration documents without implicit fields.""" + renderer = load_renderer_settings(_CONFIG_ROOT / "default_renderer.yaml") + game = load_game_settings(_CONFIG_ROOT / "default_game.yaml") + + assert renderer.raster.line_width_px == pytest.approx(12.0) + assert renderer.bev.width == 1024 + assert not renderer.visual_flare_enabled + assert game.vehicle.aabb_width_m == pytest.approx(2.0) + assert game.vehicle.curb_forward_momentum_retention == pytest.approx(0.85) + + +def test_renderer_settings_reject_missing_and_unknown_keys(tmp_path: Path) -> None: + """Reject renderer documents that are incomplete or misspelled.""" + source = yaml.safe_load( + (_CONFIG_ROOT / "default_renderer.yaml").read_text(encoding="utf-8") + ) + del source["raster"]["line_width_px"] + source["raster"]["line_wdith_px"] = 12.0 + path = tmp_path / "renderer.yaml" + path.write_text(yaml.safe_dump(source), encoding="utf-8") + + with pytest.raises(StrictConfigError, match="missing required keys: line_width_px"): + load_renderer_settings(path) + + +def test_game_settings_reject_missing_fields(tmp_path: Path) -> None: + """Reject gameplay documents that omit a required vehicle property.""" + source = yaml.safe_load( + (_CONFIG_ROOT / "default_game.yaml").read_text(encoding="utf-8") + ) + del source["vehicle"]["aabb_width_m"] + path = tmp_path / "game.yaml" + path.write_text(yaml.safe_dump(source), encoding="utf-8") + + with pytest.raises(ValueError, match="missing required keys: aabb_width_m"): + load_game_settings(path) + + +def test_visual_cli_values_override_renderer_yaml() -> None: + """Apply explicit BEV flags after loading the renderer document.""" + args = runtime_cli.build_parser().parse_args( + [ + "--renderer-config", + str(_CONFIG_ROOT / "default_renderer.yaml"), + "--bev-resolution", + "640x480", + "--bev-height-m", + "90", + "--no-bev", + ] + ) + + settings = runtime_cli.renderer_settings_from_args(args) + + assert not settings.bev.enabled + assert (settings.bev.width, settings.bev.height) == (640, 480) + assert settings.bev.height_m == pytest.approx(90.0) + assert args.bev is False + assert args.bev_resolution == "640x480" + assert args.bev_height_m == pytest.approx(90.0) + + +def test_default_renderer_populates_legacy_presenter_arguments() -> None: + """Publish file defaults through the namespace consumed by HUD presenters.""" + args = runtime_cli.build_parser().parse_args([]) + + runtime_cli.renderer_settings_from_args(args) + + assert args.bev is True + assert args.bev_resolution == "1024x1024" + assert args.bev_height_m == pytest.approx(75.0) + assert args.bev_fov_deg == pytest.approx(60.0) + assert args.bev_tilt_deg == pytest.approx(0.0) diff --git a/apps/crazy_robotaxi/tests/test_game_map.py b/apps/crazy_robotaxi/tests/test_game_map.py new file mode 100644 index 000000000..4ebe6a005 --- /dev/null +++ b/apps/crazy_robotaxi/tests/test_game_map.py @@ -0,0 +1,1933 @@ +# SPDX-License-Identifier: Apache-2.0 +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. + +"""CPU coverage for Crazy Robotaxi node-graph maps.""" + +from __future__ import annotations + +import io +import math +import zipfile +from pathlib import Path +from typing import Any, cast + +import numpy as np +import pyarrow.parquet as pq +import pytest +import yaml +from crazy_robotaxi import cli +from crazy_robotaxi.game import TaxiGameConfig, TaxiGameController +from crazy_robotaxi.navigation import NavigationLane, TaxiNavigationMap +from crazy_robotaxi.scene import load_scene_data +from omnidreams_game_engine.config import RasterConfig, VehicleConfig +from omnidreams_game_engine.game_map import ( + GameMapError, + GameMapVicinityResolver, + compile_game_map, + load_game_map, + render_spawn_first_frame, + write_game_map_preview, + write_spawn_first_frame_preview, +) +from omnidreams_game_engine.game_map import compiler as game_map_compiler +from omnidreams_game_engine.game_map import vicinity as game_map_vicinity +from omnidreams_game_engine.game_map.types import ( + ResolvedGameMap, + game_map_from_dict, + game_map_to_dict, +) +from omnidreams_game_engine.scene_loader import load_scene_bundle +from omnidreams_game_engine.simulation.map_traffic import MapTrafficController +from omnidreams_game_engine.types import VehicleState +from PIL import Image +from shapely.geometry import LineString, Point, Polygon + +pytestmark = pytest.mark.ci_cpu + +_MAPS = Path(__file__).parents[1] / "crazy_robotaxi" / "maps" +_TEST_MAPS = Path(__file__).parent / "maps" +_STARTER_MAP = _MAPS / "minimal_loop.robotaxi.yaml" +_BOULEVARD_MAP = _MAPS / "boulevard_district.robotaxi.yaml" +_BUNDLED_MAPS = (_STARTER_MAP, _BOULEVARD_MAP) +_INTERSECTION_GEOMETRY_MAP = _TEST_MAPS / "intersection_geometry.robotaxi.yaml" +_PARKING_DRIVEWAY_MAP = _TEST_MAPS / "parking_driveway.robotaxi.yaml" +_TRAFFIC_INTERSECTION_MAP = _TEST_MAPS / "traffic_intersection.robotaxi.yaml" +_TRAFFIC_LOOP_MAP = _TEST_MAPS / "traffic_loop.robotaxi.yaml" +_COMPILER_TEST_MAPS = ( + _INTERSECTION_GEOMETRY_MAP, + _PARKING_DRIVEWAY_MAP, + _TRAFFIC_INTERSECTION_MAP, + _TRAFFIC_LOOP_MAP, +) + + +def _write_map(tmp_path: Path, source: dict[str, object], name: str = "map") -> Path: + path = tmp_path / f"{name}.robotaxi.yaml" + path.write_text(yaml.safe_dump(source, sort_keys=False), encoding="utf-8") + return path + + +def _road_joint_map(*, curved_approach: bool = False) -> dict[str, object]: + """Build a compact two-road map around one road joint.""" + approach: dict[str, object] = { + "id": "approach", + "from": "west_end", + "to": "bend", + "profile": "street", + } + if curved_approach: + approach["path"] = [{"x_m": -22, "y_m": -7}] + return { + "schema_version": 1, + "id": "road-joint-test", + "name": "Road Joint Test", + "compiler": { + "sample_spacing_m": 0.5, + "ground_margin_m": 5, + "intersection_connector_samples": 8, + }, + "profiles": { + "street": { + "lane_width_m": 3.6, + "curb_offset_m": 0.6, + "lanes": ["backward", "forward"], + "speed_limit_mps": 13.4, + "lane_marking": {"style": "SOLID_GROUP", "color": "YELLOW"}, + "divider_markings": [{"style": "SOLID_GROUP", "color": "YELLOW"}], + } + }, + "nodes": [ + { + "id": "west_end", + "type": "cul_de_sac", + "pose": {"x_m": -45, "y_m": 0}, + "culdesac_radius_m": 8, + }, + { + "id": "bend", + "type": "road_joint", + "pose": {"x_m": 0, "y_m": 0}, + }, + { + "id": "east_end", + "type": "cul_de_sac", + "pose": {"x_m": 35, "y_m": 35}, + "culdesac_radius_m": 8, + }, + ], + "roads": [ + approach, + { + "id": "exit", + "from": "bend", + "to": "east_end", + "profile": "street", + "speed_limit_mps": 8, + }, + ], + "spawns": [ + { + "id": "taxi_start", + "road": "approach", + "lane": 1, + "distance_m": 5, + "variants": { + "default": { + "image": "package://omnidreams_game_engine/screenshot.jpg", + "prompt": "A road bending through a quiet neighborhood.", + } + }, + } + ], + } + + +def _intersection_transition_map( + transition_length_m: float = 20, +) -> dict[str, object]: + """Load a four-way intersection with one narrower through arm.""" + source = yaml.safe_load(_TRAFFIC_INTERSECTION_MAP.read_text(encoding="utf-8")) + nodes = cast(list[dict[str, Any]], source["nodes"]) + center = next(node for node in nodes if node["id"] == "center") + center["lane_transition_length_m"] = transition_length_m + return cast(dict[str, object], source) + + +def _self_loop_map() -> dict[str, object]: + """Restore a compact self-loop fixture independent of the bundled demo.""" + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + source["nodes"] = [node for node in source["nodes"] if node["type"] != "road_joint"] + source["roads"] = [ + road for road in source["roads"] if road["id"] == "dead_end_road" + ] + source["roads"].insert( + 0, + { + "id": "neighborhood_loop", + "from": "hub", + "to": "hub", + "profile": "neighborhood", + "path": [ + {"x_m": 45, "y_m": 15}, + {"x_m": 45, "y_m": 50}, + {"x_m": -45, "y_m": 50}, + {"x_m": -45, "y_m": 15}, + ], + }, + ) + source["spawns"][0]["road"] = "neighborhood_loop" + return cast(dict[str, object], source) + + +def _reachable(game_map: ResolvedGameMap, start_lane_id: str) -> set[str]: + lanes = {lane.lane_id: lane for lane in game_map.lanes} + pending = [start_lane_id] + reached = {start_lane_id} + while pending: + for successor_id in lanes[pending.pop()].successor_ids: + if successor_id not in reached: + reached.add(successor_id) + pending.append(successor_id) + return reached + + +def _surface(game_map: ResolvedGameMap, element_id: str) -> Polygon: + element = next(item for item in game_map.elements if item.element_id == element_id) + return Polygon(element.surface_world[:, :2]) + + +def _curb_lines(game_map: ResolvedGameMap) -> list[LineString]: + return [ + LineString(curb.polyline_world[:, :2]) + for element in game_map.elements + for curb in element.curbs + ] + + +def _point_on_polyline(points: np.ndarray, distance_m: float) -> np.ndarray: + lengths = np.linalg.norm(np.diff(points[:, :2], axis=0), axis=1) + cumulative = np.concatenate(([0.0], np.cumsum(lengths))) + index = min( + max(int(np.searchsorted(cumulative, distance_m, side="right") - 1), 0), + len(lengths) - 1, + ) + alpha = (distance_m - cumulative[index]) / max(float(lengths[index]), 1e-9) + return points[index] + alpha * (points[index + 1] - points[index]) + + +@pytest.mark.parametrize("source_path", _BUNDLED_MAPS) +def test_bundled_map_compiles(source_path: Path, tmp_path: Path) -> None: + source = yaml.safe_load(source_path.read_text(encoding="utf-8")) + compiled = compile_game_map(source_path, cache_root=tmp_path / "cache") + game_map = compiled.game_map + + assert game_map.schema_version == source["schema_version"] == 1 + assert game_map.map_id == source["id"] + assert game_map.spawns + assert compiled.archive_path.is_file() + if "traffic_count" in source: + assert len(game_map.traffic) == source["traffic_count"] + authored_traffic_ids = {traffic["id"] for traffic in source.get("traffic", ())} + assert authored_traffic_ids <= {traffic.vehicle_id for traffic in game_map.traffic} + + +def test_unreleased_map_compiler_stays_at_version_1() -> None: + assert game_map_compiler._COMPILER_VERSION == "1" + + +def test_road_vicinity_expands_from_both_endpoints() -> None: + game_map = load_game_map(_TRAFFIC_LOOP_MAP) + resolver = GameMapVicinityResolver(game_map) + spawn = game_map.default_spawn + + vicinity = resolver.resolve( + float(spawn.position_world[0]), + float(spawn.position_world[1]), + ) + + assert vicinity is not None + assert vicinity.location_element_id == "south_west" + assert vicinity.traffic_element_ids == { + "east_south", + "south", + "south_east", + "south_west", + "southeast", + "southwest", + "west", + "west_north", + "west_south", + } + assert resolver.resolve(1.0e6, 1.0e6, previous=vicinity) is vicinity + + +def test_node_vicinity_adds_neighbor_nodes_and_their_roads() -> None: + game_map = load_game_map(_TRAFFIC_LOOP_MAP) + resolver = GameMapVicinityResolver(game_map) + node = next(node for node in game_map.topology.nodes if node.node_id == "south") + + vicinity = resolver.resolve(node.x_m, node.y_m) + + assert vicinity is not None + assert vicinity.location_element_id == node.node_id + assert vicinity.traffic_element_ids == { + "east_south", + "south", + "south_east", + "south_west", + "southeast", + "southwest", + "west_south", + } + + +def test_vicinity_rejects_distant_polygons_before_exact_tests( + monkeypatch: pytest.MonkeyPatch, +) -> None: + game_map = load_game_map(_TRAFFIC_LOOP_MAP) + resolver = GameMapVicinityResolver(game_map) + spawn = game_map.default_spawn + exact_test_count = 0 + polygon_contains = game_map_vicinity._polygon_contains + + def counted_polygon_contains(point: np.ndarray, polygon: np.ndarray) -> bool: + nonlocal exact_test_count + exact_test_count += 1 + return polygon_contains(point, polygon) + + monkeypatch.setattr( + game_map_vicinity, "_polygon_contains", counted_polygon_contains + ) + + vicinity = resolver.resolve( + float(spawn.position_world[0]), + float(spawn.position_world[1]), + ) + + assert vicinity is not None + assert vicinity.location_element_id == "south_west" + assert exact_test_count == 1 + + +def test_vicinity_exposes_parking_lot_pedestrian_at_an_expanded_node() -> None: + game_map = load_game_map(_PARKING_DRIVEWAY_MAP) + resolver = GameMapVicinityResolver(game_map) + access = game_map.topology.parking_accesses[0] + connecting_road = next( + road + for road in game_map.topology.roads + if access.source_node_id in {road.from_node_id, road.to_node_id} + ) + neighboring_node_id = ( + connecting_road.to_node_id + if connecting_road.from_node_id == access.source_node_id + else connecting_road.from_node_id + ) + neighboring_node = next( + node for node in game_map.topology.nodes if node.node_id == neighboring_node_id + ) + + vicinity = resolver.resolve(neighboring_node.x_m, neighboring_node.y_m) + + assert vicinity is not None + assert vicinity.location_element_id == neighboring_node_id + assert access.source_node_id in vicinity.traffic_element_ids + assert access.parking_lot_node_id not in vicinity.traffic_element_ids + assert access.parking_lot_node_id in vicinity.pedestrian_element_ids + + +@pytest.mark.parametrize( + "field", + [ + "intersection_arm_length_m", + "intersection_width_m", + "intersection_depth_m", + ], +) +def test_authored_intersection_geometry_is_rejected(tmp_path: Path, field: str) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + hub = next(node for node in source["nodes"] if node["id"] == "hub") + hub[field] = 24 + + with pytest.raises(GameMapError, match="unknown attributes"): + load_game_map(_write_map(tmp_path, source)) + + +def test_lane_transition_length_is_rejected_on_cul_de_sac(tmp_path: Path) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + dead_end = next(node for node in source["nodes"] if node["id"] == "dead_end") + dead_end["lane_transition_length_m"] = 10 + + with pytest.raises(GameMapError, match="unknown attributes"): + load_game_map(_write_map(tmp_path, source)) + + +def test_lane_transition_length_must_be_nonnegative(tmp_path: Path) -> None: + with pytest.raises(GameMapError, match="must be nonnegative"): + load_game_map(_write_map(tmp_path, _intersection_transition_map(-1))) + + +@pytest.mark.parametrize("road_count", [1, 2]) +def test_intersection_requires_at_least_three_road_arms( + tmp_path: Path, + road_count: int, +) -> None: + source = _road_joint_map() + nodes = cast(list[dict[str, Any]], source["nodes"]) + bend = next(node for node in nodes if node["id"] == "bend") + bend["type"] = "intersection" + roads = cast(list[dict[str, Any]], source["roads"]) + del roads[road_count:] + + with pytest.raises( + GameMapError, + match=rf"Intersection 'bend' must connect at least three road arms " + rf"\(found {road_count}\)", + ): + load_game_map(_write_map(tmp_path, source)) + + +def test_unknown_root_fields_are_rejected(tmp_path: Path) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + source["unexpected"] = [] + + with pytest.raises(GameMapError, match="Map has unknown fields"): + load_game_map(_write_map(tmp_path, source)) + + +def test_parking_accesses_are_not_a_top_level_authoring_field( + tmp_path: Path, +) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + source["parking_accesses"] = [] + + with pytest.raises(GameMapError, match="unknown fields.*parking_accesses"): + load_game_map(_write_map(tmp_path, source)) + + +def test_legacy_link_topology_is_rejected(tmp_path: Path) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + source["links"] = [] + + with pytest.raises(GameMapError, match="unknown fields.*links"): + load_game_map(_write_map(tmp_path, source)) + + +def test_parking_lot_vertices_must_be_clockwise(tmp_path: Path) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + lot = next(node for node in source["nodes"] if node["type"] == "parking_lot") + lot["vertices"].reverse() + + with pytest.raises(GameMapError, match="must be clockwise"): + load_game_map(_write_map(tmp_path, source)) + + +def test_parking_opening_vertex_must_select_a_polygon_edge(tmp_path: Path) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + lot = next(node for node in source["nodes"] if node["type"] == "parking_lot") + lot["opening_vertex"] = len(lot["vertices"]) + 1 + + with pytest.raises(GameMapError, match="opening_vertex must be between"): + load_game_map(_write_map(tmp_path, source)) + + +def test_boolean_is_not_accepted_as_a_numeric_setting(tmp_path: Path) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + source["compiler"]["sample_spacing_m"] = True + + with pytest.raises(GameMapError, match="sample_spacing_m must be a number"): + load_game_map(_write_map(tmp_path, source)) + + +def test_element_ids_are_unique_across_kinds(tmp_path: Path) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + source["roads"][1]["id"] = "hub" + + with pytest.raises(GameMapError, match="shared by a node and road"): + load_game_map(_write_map(tmp_path, source)) + + +def test_profiles_without_curbs_do_not_emit_collision_segments(tmp_path: Path) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + for profile in source["profiles"].values(): + profile["curb"] = False + for node in source["nodes"]: + if node["type"] in {"intersection", "cul_de_sac"}: + node["curb"] = False + + source_path = _write_map(tmp_path, source) + game_map = load_game_map(source_path) + + assert all( + not element.curbs + for element in game_map.elements + if element.element_type not in {"parking_lot", "parking_access"} + ) + assert all( + element.curbs + for element in game_map.elements + if element.element_type in {"parking_lot", "parking_access"} + ) + assert any(element.road_boundaries for element in game_map.elements) + rows = game_map_compiler._boundary_rows(game_map) + assert len(rows) == sum( + len(element.road_boundaries) for element in game_map.elements + ) + assert all(row["road_boundary"]["category"] == "road_boundary" for row in rows) + compiled = compile_game_map(source_path, cache_root=tmp_path / "cache") + with zipfile.ZipFile(compiled.archive_path) as archive: + archived_rows = pq.read_table( + io.BytesIO(archive.read("clipgt/road_boundary.parquet")) + ).to_pylist() + assert len(archived_rows) == len(rows) + assert all( + row["road_boundary"]["category"] == "road_boundary" for row in archived_rows + ) + + +def test_profile_is_optional_when_attributes_are_direct(tmp_path: Path) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + profile = source["profiles"]["neighborhood"] + road = next(item for item in source["roads"] if item["id"] == "dead_end_road") + del road["profile"] + road.update(profile) + + game_map = load_game_map(_write_map(tmp_path, source)) + resolved = next( + item for item in game_map.topology.roads if item.road_id == road["id"] + ) + + assert resolved.profile_id is None + assert resolved.attributes.lane_width_m == pytest.approx(3.6) + + +def test_direct_attributes_override_partial_profile_defaults(tmp_path: Path) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + profile = dict(source["profiles"]["neighborhood"]) + del profile["lane_width_m"] + source["profiles"]["partial"] = profile + road = next(item for item in source["roads"] if item["id"] == "dead_end_road") + road["profile"] = "partial" + road["lane_width_m"] = 4.1 + + game_map = load_game_map(_write_map(tmp_path, source)) + resolved = next( + item for item in game_map.topology.roads if item.road_id == road["id"] + ) + + assert resolved.profile_id == "partial" + assert resolved.attributes.lane_width_m == pytest.approx(4.1) + assert resolved.attributes.speed_limit_mps == pytest.approx(13.4) + + +def test_direct_attributes_override_values_present_in_profile(tmp_path: Path) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + road = next(item for item in source["roads"] if item["id"] == "dead_end_road") + road["lane_width_m"] = 4.1 + hub = next(item for item in source["nodes"] if item["id"] == "hub") + source["profiles"]["intersection_defaults"] = { + "curb": False, + } + hub["profile"] = "intersection_defaults" + hub["curb"] = True + + game_map = load_game_map(_write_map(tmp_path, source)) + resolved_road = next( + item for item in game_map.topology.roads if item.road_id == road["id"] + ) + resolved_hub = next( + item for item in game_map.topology.nodes if item.node_id == hub["id"] + ) + + assert resolved_road.attributes.lane_width_m == pytest.approx(4.1) + assert resolved_hub.geometry == {"lane_transition_length_m": 0} + assert resolved_hub.attributes.curb is True + + +def test_profiles_root_is_optional_when_all_attributes_are_direct( + tmp_path: Path, +) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + profiles = source.pop("profiles") + for road in source["roads"]: + road.update(profiles[road.pop("profile")]) + for node in source["nodes"]: + if "profile" in node: + node.update(profiles[node.pop("profile")]) + + game_map = load_game_map(_write_map(tmp_path, source)) + + assert all(road.profile_id is None for road in game_map.topology.roads) + assert all(node.profile_id is None for node in game_map.topology.nodes) + + +def test_profile_may_contain_attributes_irrelevant_to_consumer(tmp_path: Path) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + source["profiles"]["neighborhood"]["culdesac_radius_m"] = 50 + + game_map = load_game_map(_write_map(tmp_path, source)) + + road = next( + item for item in game_map.topology.roads if item.profile_id == "neighborhood" + ) + assert road.attributes.lane_width_m == pytest.approx(3.6) + + +def test_missing_effective_attribute_is_rejected(tmp_path: Path) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + del source["profiles"]["neighborhood"]["speed_limit_mps"] + + with pytest.raises(GameMapError, match="missing attributes.*speed_limit_mps"): + load_game_map(_write_map(tmp_path, source)) + + +def test_topology_round_trip_is_lossless() -> None: + original = load_game_map(_STARTER_MAP) + restored = game_map_from_dict(game_map_to_dict(original)) + + assert restored.topology == original.topology + assert restored.topology.adjacency == original.topology.adjacency + assert len(restored.lane_dividers) == len(original.lane_dividers) + for restored_divider, original_divider in zip( + restored.lane_dividers, original.lane_dividers, strict=True + ): + assert restored_divider.divider_id == original_divider.divider_id + assert restored_divider.lane_edges == original_divider.lane_edges + np.testing.assert_array_equal( + restored_divider.polyline_world, original_divider.polyline_world + ) + assert restored.default_spawn.lane_id == original.default_spawn.lane_id + assert [element.attributes for element in restored.elements] == [ + element.attributes for element in original.elements + ] + for restored_element, original_element in zip( + restored.elements, original.elements, strict=True + ): + np.testing.assert_array_equal( + restored_element.surface_world, original_element.surface_world + ) + assert [ + boundary.boundary_id for boundary in restored_element.road_boundaries + ] == [boundary.boundary_id for boundary in original_element.road_boundaries] + for restored_boundary, original_boundary in zip( + restored_element.road_boundaries, + original_element.road_boundaries, + strict=True, + ): + np.testing.assert_array_equal( + restored_boundary.polyline_world, original_boundary.polyline_world + ) + assert [curb.curb_id for curb in restored_element.curbs] == [ + curb.curb_id for curb in original_element.curbs + ] + for restored_curb, original_curb in zip( + restored_element.curbs, original_element.curbs, strict=True + ): + np.testing.assert_array_equal( + restored_curb.polyline_world, original_curb.polyline_world + ) + + +def test_traffic_compiles_and_round_trips(tmp_path: Path) -> None: + source = _road_joint_map() + source["traffic_count"] = 1 + source["traffic"] = [ + { + "id": "local_car", + "nodes": ["west_end", "bend", "east_end"], + "end_behavior": "reverse", + "speed_mps": 7.5, + "start_distance_m": 4, + } + ] + + source_path = _write_map(tmp_path, source) + original = load_game_map(source_path) + restored = game_map_from_dict(game_map_to_dict(original)) + preview = write_game_map_preview(source_path, tmp_path / "traffic-preview.svg") + + assert len(original.traffic) == 1 + traffic = original.traffic[0] + assert traffic.vehicle_id == "local_car" + assert traffic.vehicle_type == "car" + assert traffic.end_behavior == "reverse" + assert traffic.dimensions_lwh_m == pytest.approx((4.5, 1.8, 1.5)) + assert traffic.speed_mps == pytest.approx(7.5) + assert traffic.centerline_world.shape[1] == 3 + assert np.all(traffic.speed_limits_mps <= 7.5) + assert len(traffic.route_element_ids) == len(traffic.centerline_world) - 1 + assert set(traffic.route_element_ids) <= { + element.element_id for element in original.elements + } + np.testing.assert_array_equal( + restored.traffic[0].centerline_world, traffic.centerline_world + ) + assert restored.traffic[0].route_element_ids == traffic.route_element_ids + preview_text = preview.read_text(encoding="utf-8") + assert "#ef476f" in preview_text + assert "local_car" in preview_text + + +def test_traffic_turns_are_continuous_and_physically_limited() -> None: + game_map = load_game_map(_TRAFFIC_INTERSECTION_MAP) + lanes = {lane.lane_id: lane for lane in game_map.lanes} + node_types = {node.node_id: node.node_type for node in game_map.topology.nodes} + + for connector in ( + lane + for lane in game_map.lanes + if ":connector:" in lane.lane_id and node_types[lane.element_id] != "cul_de_sac" + ): + sources = [ + lane for lane in game_map.lanes if connector.lane_id in lane.successor_ids + ] + assert len(sources) == 1 + target = lanes[connector.successor_ids[0]] + tangent_pairs = ( + ( + sources[0].centerline_world[-1, :2] + - sources[0].centerline_world[-2, :2], + connector.centerline_world[1, :2] - connector.centerline_world[0, :2], + ), + ( + connector.centerline_world[-1, :2] - connector.centerline_world[-2, :2], + target.centerline_world[1, :2] - target.centerline_world[0, :2], + ), + ) + for first, second in tangent_pairs: + cosine = float( + np.dot(first, second) / (np.linalg.norm(first) * np.linalg.norm(second)) + ) + assert cosine >= 0.97, connector.lane_id + + cul_de_sacs = { + node.node_id + for node in game_map.topology.nodes + if node.node_type == "cul_de_sac" + } + for vehicle in game_map.traffic: + segments = np.diff(vehicle.centerline_world[:, :2], axis=0) + lengths = np.linalg.norm(segments, axis=1) + assert np.all(lengths >= 0.25 - 1.0e-5), vehicle.vehicle_id + + headings = np.arctan2(segments[:, 1], segments[:, 0]) + heading_changes = np.abs( + (headings - np.roll(headings, 1) + np.pi) % (2.0 * np.pi) - np.pi + ) + previous_lengths = np.roll(lengths, 1) + previous_speeds = np.roll(vehicle.speed_limits_mps[:-1], 1) + segment_speeds = np.maximum( + np.minimum(previous_speeds, vehicle.speed_limits_mps[:-1]), 0.1 + ) + yaw_rates = heading_changes * segment_speeds / previous_lengths + assert np.max(yaw_rates) <= 1.201, vehicle.vehicle_id + + for index, heading_change in enumerate(heading_changes): + previous = (index - 1) % len(vehicle.route_element_ids) + current = index % len(vehicle.route_element_ids) + if { + vehicle.route_element_ids[previous], + vehicle.route_element_ids[current], + }.isdisjoint(cul_de_sacs): + assert heading_change <= math.radians(30.0), ( + vehicle.vehicle_id, + index, + ) + + +def test_traffic_rejects_parking_lot_waypoint(tmp_path: Path) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + source["traffic"] = [ + { + "id": "parking_car", + "nodes": ["hub", "neighborhood_lot"], + "end_behavior": "wrap", + } + ] + + with pytest.raises(GameMapError, match="cannot visit parking-lot"): + load_game_map(_write_map(tmp_path, source)) + + +def test_traffic_count_rejects_more_authored_vehicles(tmp_path: Path) -> None: + source = _road_joint_map() + source["traffic_count"] = 1 + source["traffic"] = [ + { + "id": vehicle_id, + "nodes": ["west_end", "bend", "east_end"], + "end_behavior": "reverse", + } + for vehicle_id in ("first", "second") + ] + + with pytest.raises( + GameMapError, match=r"traffic_count is 1.*traffic defines 2 vehicles" + ): + load_game_map(_write_map(tmp_path, source)) + + +def test_zero_traffic_count_produces_empty_fleet(tmp_path: Path) -> None: + source = _road_joint_map() + source["traffic_count"] = 0 + + assert load_game_map(_write_map(tmp_path, source)).traffic == () + + +def test_traffic_count_generates_deterministic_graph_wide_cars( + tmp_path: Path, +) -> None: + source = _road_joint_map() + source["traffic_count"] = 3 + source_path = _write_map(tmp_path, source) + + first = load_game_map(source_path) + second = load_game_map(source_path) + restored = game_map_from_dict(game_map_to_dict(first)) + compiled_first = compile_game_map(source_path, cache_root=tmp_path / "cache") + compiled_second = compile_game_map(source_path, cache_root=tmp_path / "cache") + preview_text = write_game_map_preview( + source_path, tmp_path / "generated-traffic-preview.svg" + ).read_text(encoding="utf-8") + + assert len(first.traffic) == 3 + assert [vehicle.vehicle_id for vehicle in first.traffic] == [ + "generated-traffic-0001", + "generated-traffic-0002", + "generated-traffic-0003", + ] + assert all(vehicle.vehicle_type == "car" for vehicle in first.traffic) + assert all(vehicle.end_behavior == "reverse" for vehicle in first.traffic) + assert all("neighborhood_lot" not in vehicle.node_ids for vehicle in first.traffic) + assert len(restored.traffic) == 3 + assert compiled_first.cache_hit is False + assert compiled_second.cache_hit is True + assert [vehicle.vehicle_id for vehicle in compiled_first.game_map.traffic] == [ + vehicle.vehicle_id for vehicle in first.traffic + ] + assert all(vehicle.vehicle_id in preview_text for vehicle in first.traffic) + for generated, restored in zip(first.traffic, second.traffic, strict=True): + assert generated.node_ids == restored.node_ids + assert generated.start_distance_m == restored.start_distance_m + np.testing.assert_array_equal( + generated.centerline_world, restored.centerline_world + ) + assert all( + np.linalg.norm( + _point_on_polyline( + generated.centerline_world, generated.start_distance_m + )[:2] + - spawn.position_world[:2] + ) + >= 8 + for spawn in first.spawns + ) + + +def test_traffic_count_generates_loop_routes_without_authored_cars( + tmp_path: Path, +) -> None: + source = yaml.safe_load(_TRAFFIC_LOOP_MAP.read_text(encoding="utf-8")) + source["traffic_count"] = 2 + + game_map = load_game_map(_write_map(tmp_path, source)) + + assert len(game_map.traffic) == 2 + assert all(vehicle.end_behavior == "wrap" for vehicle in game_map.traffic) + + +def test_large_logical_fleet_activates_only_graph_nearby_cars( + tmp_path: Path, +) -> None: + source = yaml.safe_load(_TRAFFIC_LOOP_MAP.read_text(encoding="utf-8")) + source["traffic_count"] = 20 + game_map = load_game_map(_write_map(tmp_path, source, "dense-traffic-loop")) + spawn = game_map.default_spawn + vicinity = GameMapVicinityResolver(game_map).resolve( + float(spawn.position_world[0]), + float(spawn.position_world[1]), + ) + controller = MapTrafficController(game_map.traffic, VehicleConfig()) + + controller.set_vicinity(vicinity) + + assert len(game_map.traffic) == 20 + assert 0 < len(controller.active_objects) < len(game_map.traffic) + + +@pytest.mark.parametrize("count", [-1, 1.5, True, "2"]) +def test_traffic_count_requires_nonnegative_integer( + tmp_path: Path, count: object +) -> None: + source = _road_joint_map() + source["traffic_count"] = count + + with pytest.raises(GameMapError, match="nonnegative integer"): + load_game_map(_write_map(tmp_path, source)) + + +def test_traffic_count_rejects_unsafe_capacity(tmp_path: Path) -> None: + source = _road_joint_map() + source["traffic_count"] = 1_000 + + with pytest.raises(GameMapError, match=r"requests 1000 vehicles.*safe capacity"): + load_game_map(_write_map(tmp_path, source)) + + +def test_traffic_count_rejects_map_without_return_route(tmp_path: Path) -> None: + source = _road_joint_map() + source["profiles"]["street"]["lanes"] = ["forward"] + source["profiles"]["street"]["divider_markings"] = [] + source["spawns"][0]["lane"] = 0 + source["traffic_count"] = 1 + + with pytest.raises(GameMapError, match=r"requests 1 vehicles.*safe capacity for 0"): + load_game_map(_write_map(tmp_path, source)) + + +def test_legacy_serialized_curbs_supply_missing_road_boundaries() -> None: + serialized = game_map_to_dict(load_game_map(_STARTER_MAP)) + for element in serialized["elements"]: + element.pop("road_boundaries") + + restored = game_map_from_dict(serialized) + + for element in restored.elements: + assert len(element.road_boundaries) == len(element.curbs) + for boundary, curb in zip(element.road_boundaries, element.curbs, strict=True): + np.testing.assert_array_equal(boundary.polyline_world, curb.polyline_world) + + +def test_serialized_road_boundaries_do_not_require_physical_curbs() -> None: + serialized = game_map_to_dict(load_game_map(_STARTER_MAP)) + for element in serialized["elements"]: + element.pop("curbs") + + restored = game_map_from_dict(serialized) + + assert any(element.road_boundaries for element in restored.elements) + assert not any(element.curbs for element in restored.elements) + + +def test_node_rotation_is_not_an_authored_pose_field(tmp_path: Path) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + hub = next(node for node in source["nodes"] if node["id"] == "hub") + hub["pose"]["rotation_deg"] = 45 + + with pytest.raises(GameMapError, match="pose requires x_m and y_m"): + load_game_map(_write_map(tmp_path, source)) + + +def test_curb_defaults_to_true_for_roads_and_boundary_nodes() -> None: + game_map = load_game_map(_STARTER_MAP) + + assert all(road.attributes.curb for road in game_map.topology.roads) + assert all( + node.attributes.curb + for node in game_map.topology.nodes + if node.node_type in {"intersection", "cul_de_sac"} + ) + serialized = game_map_to_dict(game_map) + assert all("rotation_deg" not in node for node in serialized["topology"]["nodes"]) + + +def test_askew_intersection_road_angles_are_geometry_driven() -> None: + game_map = load_game_map(_INTERSECTION_GEOMETRY_MAP) + nodes = {node.node_id: node for node in game_map.topology.nodes} + road = next(item for item in game_map.topology.roads if item.road_id == "east_road") + start, end = nodes[road.from_node_id], nodes[road.to_node_id] + bearing = np.degrees(np.arctan2(end.y_m - start.y_m, end.x_m - start.x_m)) % 360 + + assert start.node_id == "center" + assert bearing == pytest.approx(9.46, abs=0.01) + + +def test_road_joint_trims_roads_and_emits_visible_curve(tmp_path: Path) -> None: + source_path = _write_map(tmp_path, _road_joint_map()) + game_map = load_game_map(source_path) + node = next(node for node in game_map.topology.nodes if node.node_id == "bend") + joint_lanes = [lane for lane in game_map.lanes if lane.element_id == "bend"] + lanes = {lane.lane_id: lane for lane in game_map.lanes} + + assert node.node_type == "road_joint" + assert node.geometry == {"lane_transition_length_m": 0} + assert node.attributes.speed_limit_mps == pytest.approx(8) + assert len(joint_lanes) == 2 + assert all(lane.conditioning_visible for lane in joint_lanes) + assert all(not lane.allows_taxi_stops for lane in joint_lanes) + assert lanes["approach:lane:1"].successor_ids == ("bend:lane:1",) + assert lanes["bend:lane:1"].successor_ids == ("exit:lane:1",) + assert lanes["bend:lane:1"].speed_limit_mps == pytest.approx(13.4) + assert lanes["bend:lane:0"].speed_limit_mps == pytest.approx(8) + assert lanes["approach:lane:1"].centerline_world[-1, 0] < -1 + + joint = _surface(game_map, "bend") + approach = _surface(game_map, "approach") + exit_road = _surface(game_map, "exit") + assert joint.is_valid + assert joint.intersection(approach).area < 1.0e-4 + assert joint.intersection(exit_road).area < 1.0e-4 + assert joint.boundary.intersection(approach.boundary).length > 7 + assert joint.boundary.intersection(exit_road.boundary).length > 7 + element = next(item for item in game_map.elements if item.element_id == "bend") + assert element.road_boundaries + assert element.curbs + assert max(len(curb.polyline_world) for curb in element.curbs) > 3 + curved_rail = max(element.curbs, key=lambda curb: len(curb.polyline_world)) + rail_xy = curved_rail.polyline_world[:, :2] + chord = LineString((rail_xy[0], rail_xy[-1])) + assert max(chord.distance(Point(point)) for point in rail_xy[1:-1]) > 0.1 + assert any( + divider.divider_id.startswith("bend:") for divider in game_map.lane_dividers + ) + + restored = game_map_from_dict(game_map_to_dict(game_map)) + restored_node = next( + node for node in restored.topology.nodes if node.node_id == "bend" + ) + assert restored_node.attributes == node.attributes + assert restored_node.geometry == node.geometry + + preview_path = write_game_map_preview( + source_path, tmp_path / "road-joint-preview.svg" + ) + assert "bend [node:road_joint]" in preview_path.read_text(encoding="utf-8") + compiled = compile_game_map(source_path, cache_root=tmp_path / "cache") + with zipfile.ZipFile(compiled.archive_path) as archive: + lane_rows = pq.read_table(io.BytesIO(archive.read("clipgt/lane.parquet"))) + divider_rows = pq.read_table( + io.BytesIO(archive.read("clipgt/lane_line.parquet")) + ) + assert lane_rows.num_rows == sum( + lane.conditioning_visible for lane in game_map.lanes + ) + assert divider_rows.num_rows == len(game_map.lane_dividers) + + +def test_road_joint_rejects_removed_curve_length(tmp_path: Path) -> None: + source = _road_joint_map() + nodes = cast(list[dict[str, Any]], source["nodes"]) + bend = next(node for node in nodes if node["id"] == "bend") + bend["curve_length_m"] = 8 + + with pytest.raises(GameMapError, match="unknown attributes.*curve_length_m"): + load_game_map(_write_map(tmp_path, source)) + + +def test_road_joint_rounds_ninety_degree_outside_boundary(tmp_path: Path) -> None: + source = _road_joint_map() + nodes = cast(list[dict[str, Any]], source["nodes"]) + east = next(node for node in nodes if node["id"] == "east_end") + east["pose"] = {"x_m": 0, "y_m": 35} + + game_map = load_game_map(_write_map(tmp_path, source)) + joint = next( + element for element in game_map.elements if element.element_id == "bend" + ) + outside = max(joint.curbs, key=lambda curb: len(curb.polyline_world)) + points = outside.polyline_world[:, :2] + chord = LineString((points[0], points[-1])) + + assert len(points) > 3 + assert max(chord.distance(Point(point)) for point in points[1:-1]) > 0.5 + + +def test_straight_road_joint_remains_straight(tmp_path: Path) -> None: + source = _road_joint_map() + nodes = cast(list[dict[str, Any]], source["nodes"]) + east = next(node for node in nodes if node["id"] == "east_end") + east["pose"] = {"x_m": 35, "y_m": 0} + + game_map = load_game_map(_write_map(tmp_path, source)) + joint_lane = next(lane for lane in game_map.lanes if lane.lane_id == "bend:lane:1") + + assert np.ptp(joint_lane.centerline_world[:, 1]) < 1.0e-4 + + +def test_road_joint_without_curbs_keeps_semantic_boundaries(tmp_path: Path) -> None: + source = _road_joint_map() + profiles = cast(dict[str, dict[str, Any]], source["profiles"]) + profiles["street"]["curb"] = False + + game_map = load_game_map(_write_map(tmp_path, source)) + joint = next( + element for element in game_map.elements if element.element_id == "bend" + ) + + assert joint.road_boundaries + assert not joint.curbs + + +def test_road_joint_trims_curved_approach_tangentially(tmp_path: Path) -> None: + game_map = load_game_map( + _write_map(tmp_path, _road_joint_map(curved_approach=True)) + ) + lanes = {lane.lane_id: lane for lane in game_map.lanes} + approach = lanes["approach:lane:1"].centerline_world + joint = lanes["bend:lane:1"].centerline_world + approach_tangent = next( + approach[-1, :2] - point[:2] + for point in approach[-2::-1] + if np.linalg.norm(approach[-1, :2] - point[:2]) > 0.05 + ) + joint_tangent = joint[1, :2] - joint[0, :2] + approach_tangent /= np.linalg.norm(approach_tangent) + joint_tangent /= np.linalg.norm(joint_tangent) + + np.testing.assert_allclose(joint_tangent, approach_tangent, atol=0.08) + + +def test_road_joint_infers_independent_trims_for_unequal_widths( + tmp_path: Path, +) -> None: + source = _road_joint_map() + roads = cast(list[dict[str, Any]], source["roads"]) + roads[0]["curb_offset_m"] = 0.2 + roads[1]["curb_offset_m"] = 1.2 + + game_map = load_game_map(_write_map(tmp_path, source)) + lanes = {lane.lane_id: lane for lane in game_map.lanes} + approach_cut = np.linalg.norm(lanes["approach:lane:1"].centerline_world[-1, :2]) + exit_cut = np.linalg.norm(lanes["exit:lane:1"].centerline_world[0, :2]) + + assert approach_cut != pytest.approx(exit_cut, abs=0.1) + + +def test_road_joint_normalizes_reversed_authored_road_direction(tmp_path: Path) -> None: + source = _road_joint_map() + roads = cast(list[dict[str, Any]], source["roads"]) + roads[1]["from"] = "east_end" + roads[1]["to"] = "bend" + + game_map = load_game_map(_write_map(tmp_path, source)) + lanes = {lane.lane_id: lane for lane in game_map.lanes} + + assert lanes["approach:lane:1"].successor_ids == ("bend:lane:1",) + assert lanes["bend:lane:1"].successor_ids == ("exit:lane:0",) + + +def test_road_joint_builds_visible_lane_count_and_width_transition( + tmp_path: Path, +) -> None: + source = _road_joint_map() + nodes = cast(list[dict[str, Any]], source["nodes"]) + bend = next(node for node in nodes if node["id"] == "bend") + bend["lane_transition_length_m"] = 16 + roads = cast(list[dict[str, Any]], source["roads"]) + roads[0]["lanes"] = ["backward", "backward", "forward", "forward"] + roads[0]["divider_markings"] = [ + {"style": "DASHED_SINGLE", "color": "WHITE"}, + {"style": "SOLID_GROUP", "color": "YELLOW"}, + {"style": "DASHED_SINGLE", "color": "WHITE"}, + ] + roads[1]["lane_width_m"] = 3.2 + + game_map = load_game_map(_write_map(tmp_path, source)) + lanes = {lane.lane_id: lane for lane in game_map.lanes} + transitions = [ + lane + for lane in game_map.lanes + if lane.lane_id.startswith("bend:transition:exit:") + ] + + assert len(transitions) == 4 + assert all(lane.conditioning_visible for lane in transitions) + assert lanes["exit:lane:0"].successor_ids == ( + "bend:transition:exit:lane:0", + "bend:transition:exit:lane:1", + ) + assert lanes["bend:transition:exit:lane:2"].successor_ids == ("exit:lane:1",) + assert lanes["bend:transition:exit:lane:3"].successor_ids == ("exit:lane:1",) + joint = _surface(game_map, "bend") + assert joint.is_valid + assert joint.bounds[2] > 15 + + +def test_road_joint_lane_change_requires_transition_length(tmp_path: Path) -> None: + source = _road_joint_map() + roads = cast(list[dict[str, Any]], source["roads"]) + roads[1]["lane_width_m"] = 4.2 + + with pytest.raises(GameMapError, match="positive lane_transition_length_m"): + load_game_map(_write_map(tmp_path, source)) + + +def test_road_joint_builds_lane_width_only_transition(tmp_path: Path) -> None: + source = _road_joint_map() + nodes = cast(list[dict[str, Any]], source["nodes"]) + bend = next(node for node in nodes if node["id"] == "bend") + bend["lane_transition_length_m"] = 12 + roads = cast(list[dict[str, Any]], source["roads"]) + roads[1]["lane_width_m"] = 4.2 + + game_map = load_game_map(_write_map(tmp_path, source)) + transitions = [ + lane + for lane in game_map.lanes + if lane.lane_id.startswith("bend:transition:approach:") + ] + + assert len(transitions) == 2 + for lane in transitions: + widths = np.linalg.norm( + lane.left_edge_world[:, :2] - lane.right_edge_world[:, :2], + axis=1, + ) + assert {round(float(widths[0]), 1), round(float(widths[-1]), 1)} == { + 3.6, + 4.2, + } + + +def test_road_joint_transition_preserves_each_curb_offset( + tmp_path: Path, +) -> None: + source = _road_joint_map() + nodes = cast(list[dict[str, Any]], source["nodes"]) + bend = next(node for node in nodes if node["id"] == "bend") + bend["lane_transition_length_m"] = 12 + roads = cast(list[dict[str, Any]], source["roads"]) + roads[0]["curb_offset_m"] = 0.4 + roads[1]["lane_width_m"] = 4.2 + roads[1]["curb_offset_m"] = 1.0 + + game_map = load_game_map(_write_map(tmp_path, source)) + transition = next( + lane + for lane in game_map.lanes + if lane.lane_id == "bend:transition:approach:lane:1" + ) + curb_offsets = np.linalg.norm( + transition.right_edge_world[:, :2] - transition.roadside_edge_world[:, :2], + axis=1, + ) + + np.testing.assert_allclose(curb_offsets, 0.4, atol=0.02) + + +def test_intersection_builds_transition_only_on_mismatched_through_arm( + tmp_path: Path, +) -> None: + game_map = load_game_map(_write_map(tmp_path, _intersection_transition_map())) + lanes = {lane.lane_id: lane for lane in game_map.lanes} + transition_ids = { + lane.lane_id for lane in game_map.lanes if ":transition:" in lane.lane_id + } + + assert transition_ids == { + f"center:transition:north_road:lane:{index}" for index in range(4) + } + assert lanes["north_road:lane:0"].successor_ids == ( + "center:transition:north_road:lane:0", + "center:transition:north_road:lane:1", + ) + assert lanes["center:transition:north_road:lane:2"].successor_ids == ( + "north_road:lane:1", + ) + assert lanes["center:transition:north_road:lane:3"].successor_ids == ( + "north_road:lane:1", + ) + assert not any("east_road" in lane_id for lane_id in transition_ids) + assert not any("west_road" in lane_id for lane_id in transition_ids) + center = _surface(game_map, "center") + assert center.bounds[3] == pytest.approx(23.71, abs=0.1) + assert center.bounds[0] == pytest.approx(-7.8, abs=0.1) + assert center.bounds[2] == pytest.approx(7.8, abs=0.1) + north_transition = lanes["center:transition:north_road:lane:3"] + curb_offsets = np.linalg.norm( + north_transition.right_edge_world[:, :2] + - north_transition.roadside_edge_world[:, :2], + axis=1, + ) + np.testing.assert_allclose(curb_offsets, 0.5, atol=0.02) + center_element = next( + element for element in game_map.elements if element.element_id == "center" + ) + assert max( + float(np.max(boundary.polyline_world[:, 1])) + for boundary in center_element.road_boundaries + ) == pytest.approx(23.71, abs=0.1) + assert ( + max(float(np.max(curb.polyline_world[:, 1])) for curb in center_element.curbs) + < 15 + ) + + +def test_intersection_lane_change_requires_transition_length(tmp_path: Path) -> None: + with pytest.raises(GameMapError, match="positive lane_transition_length_m"): + load_game_map(_write_map(tmp_path, _intersection_transition_map(0))) + + +def test_intersection_rejects_transition_that_consumes_road_arm( + tmp_path: Path, +) -> None: + with pytest.raises(GameMapError, match="consumes its road arm"): + load_game_map(_write_map(tmp_path, _intersection_transition_map(95))) + + +def test_road_joint_requires_exactly_two_roads(tmp_path: Path) -> None: + source = _road_joint_map() + roads = cast(list[dict[str, Any]], source["roads"]) + roads.pop() + + with pytest.raises(GameMapError, match="must connect exactly two distinct roads"): + load_game_map(_write_map(tmp_path, source)) + + +def test_road_rejects_combined_trims_from_both_endpoint_joints( + tmp_path: Path, +) -> None: + source = _road_joint_map() + nodes = cast(list[dict[str, Any]], source["nodes"]) + east = next(node for node in nodes if node["id"] == "east_end") + east["type"] = "road_joint" + east["pose"] = {"x_m": 2, "y_m": 2} + east.pop("culdesac_radius_m") + nodes.append( + { + "id": "far_end", + "type": "cul_de_sac", + "pose": {"x_m": 2, "y_m": 35}, + "culdesac_radius_m": 8, + } + ) + roads = cast(list[dict[str, Any]], source["roads"]) + roads.append( + { + "id": "tail", + "from": "east_end", + "to": "far_end", + "profile": "street", + } + ) + + with pytest.raises(GameMapError, match="too short for road-joint trims"): + load_game_map(_write_map(tmp_path, source)) + + +def test_path_road_is_one_topological_road(tmp_path: Path) -> None: + game_map = load_game_map(_write_map(tmp_path, _self_loop_map(), "self-loop")) + road = next( + item for item in game_map.topology.roads if item.road_id == "neighborhood_loop" + ) + road_lanes = [lane for lane in game_map.lanes if lane.element_id == road.road_id] + + assert road.from_node_id == road.to_node_id == "hub" + assert len(road.bezier_spans_world) == 5 + np.testing.assert_allclose( + road.bezier_spans_world[0][3], + np.asarray([45, 15, 0], dtype=np.float32), + ) + assert len(road_lanes) == 2 + assert ( + max( + np.max( + np.linalg.norm(np.diff(lane.centerline_world[:, :2], axis=0), axis=1) + ) + for lane in road_lanes + ) + < 4.0 + ) + lanes = {lane.lane_id: lane for lane in game_map.lanes} + assert any( + "neighborhood_loop:lane:1" in lanes[successor].successor_ids + for successor in lanes["neighborhood_loop:lane:1"].successor_ids + ) + + +def test_malformed_path_points_are_rejected(tmp_path: Path) -> None: + source = _self_loop_map() + loop = next(road for road in source["roads"] if road["id"] == "neighborhood_loop") + loop["path"][0] = {"x_m": 0, "y_m": 0} + + with pytest.raises(GameMapError, match="degenerate segment"): + load_game_map(_write_map(tmp_path, source)) + + +def test_explicit_bezier_is_supported(tmp_path: Path) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + road = next(item for item in source["roads"] if item["id"] == "dead_end_road") + road["bezier"] = [ + { + "control_points": [{"x_m": 0, "y_m": 10}, {"x_m": 0, "y_m": 20}], + "end": {"x_m": 0, "y_m": 30}, + } + ] + + game_map = load_game_map(_write_map(tmp_path, source)) + resolved = next( + item for item in game_map.topology.roads if item.road_id == "dead_end_road" + ) + + assert len(resolved.bezier_spans_world) == 1 + np.testing.assert_allclose( + resolved.bezier_spans_world[0], + np.asarray([[0, 0, 0], [0, 10, 0], [0, 20, 0], [0, 30, 0]], dtype=np.float32), + ) + + +@pytest.mark.parametrize("field", ["path", "bezier"]) +def test_road_geometry_fields_must_not_be_empty(tmp_path: Path, field: str) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + road = next(item for item in source["roads"] if item["id"] == "dead_end_road") + road[field] = [] + + with pytest.raises(GameMapError, match=rf"\.{field} must not be empty"): + load_game_map(_write_map(tmp_path, source)) + + +def test_path_rejects_bezier_spans(tmp_path: Path) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + road = next(item for item in source["roads"] if item["id"] == "dead_end_road") + road["path"] = [ + { + "control_points": [{"x_m": 0, "y_m": 15}, {"x_m": 0, "y_m": 20}], + "end": {"x_m": 0, "y_m": 30}, + } + ] + + with pytest.raises(GameMapError, match="put explicit spans under bezier"): + load_game_map(_write_map(tmp_path, source)) + + +def test_bezier_rejects_path_points(tmp_path: Path) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + road = next(item for item in source["roads"] if item["id"] == "dead_end_road") + road["bezier"] = [{"x_m": 0, "y_m": 15}] + + with pytest.raises(GameMapError, match="Bezier spans require"): + load_game_map(_write_map(tmp_path, source)) + + +def test_bezier_takes_precedence_over_path(tmp_path: Path) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + road = next(item for item in source["roads"] if item["id"] == "dead_end_road") + road["path"] = [{"x_m": 10, "y_m": 15}] + road["bezier"] = [ + { + "control_points": [{"x_m": 0, "y_m": 10}, {"x_m": 0, "y_m": 20}], + "end": {"x_m": 0, "y_m": 30}, + } + ] + + game_map = load_game_map(_write_map(tmp_path, source)) + resolved = next( + item for item in game_map.topology.roads if item.road_id == "dead_end_road" + ) + + np.testing.assert_allclose( + resolved.bezier_spans_world[0], + np.asarray([[0, 0, 0], [0, 10, 0], [0, 20, 0], [0, 30, 0]], dtype=np.float32), + ) + + +def test_path_is_validated_when_bezier_is_present(tmp_path: Path) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + road = next(item for item in source["roads"] if item["id"] == "dead_end_road") + road["path"] = [{"x_m": 10}] + road["bezier"] = [ + { + "control_points": [{"x_m": 0, "y_m": 10}, {"x_m": 0, "y_m": 20}], + "end": {"x_m": 0, "y_m": 30}, + } + ] + + with pytest.raises(GameMapError, match="requires exactly x_m and y_m"): + load_game_map(_write_map(tmp_path, source)) + + +def test_bezier_final_endpoint_must_match_to_node(tmp_path: Path) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + road = next(item for item in source["roads"] if item["id"] == "dead_end_road") + road["bezier"] = [ + { + "control_points": [{"x_m": 0, "y_m": 10}, {"x_m": 0, "y_m": 20}], + "end": {"x_m": 1, "y_m": 30}, + } + ] + + with pytest.raises(GameMapError, match="final Bezier endpoint"): + load_game_map(_write_map(tmp_path, source)) + + +def test_self_loop_supports_explicit_bezier(tmp_path: Path) -> None: + source = _self_loop_map() + loop = next(road for road in source["roads"] if road["id"] == "neighborhood_loop") + del loop["path"] + loop["bezier"] = [ + { + "control_points": [{"x_m": 30, "y_m": 0}, {"x_m": 45, "y_m": 0}], + "end": {"x_m": 45, "y_m": 15}, + }, + { + "control_points": [{"x_m": 45, "y_m": 35}, {"x_m": 45, "y_m": 50}], + "end": {"x_m": 30, "y_m": 50}, + }, + { + "control_points": [{"x_m": -30, "y_m": 50}, {"x_m": -45, "y_m": 35}], + "end": {"x_m": -45, "y_m": 15}, + }, + { + "control_points": [{"x_m": -45, "y_m": 0}, {"x_m": -30, "y_m": 0}], + "end": {"x_m": 0, "y_m": 0}, + }, + ] + + game_map = load_game_map(_write_map(tmp_path, source)) + resolved = next( + item for item in game_map.topology.roads if item.road_id == "neighborhood_loop" + ) + + assert len(resolved.bezier_spans_world) == 4 + + +def test_self_loop_requires_path_or_bezier(tmp_path: Path) -> None: + source = _self_loop_map() + loop = next(road for road in source["roads"] if road["id"] == "neighborhood_loop") + del loop["path"] + + with pytest.raises(GameMapError, match="requires path or bezier"): + load_game_map(_write_map(tmp_path, source)) + + +def test_roads_cannot_cross_without_a_connection_node(tmp_path: Path) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + road = next(item for item in source["roads"] if item["id"] == "dead_end_road") + road["path"] = [ + {"x_m": 45, "y_m": 0}, + {"x_m": 45, "y_m": 30}, + ] + + with pytest.raises( + GameMapError, + match=( + "Unrelated elements.*overlap|completely contained by its endpoint " + "footprints|invalid boundary ribbon" + ), + ): + load_game_map(_write_map(tmp_path, source)) + + +def test_cul_de_sac_must_terminate_exactly_one_road(tmp_path: Path) -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + source["roads"].append( + { + "id": "invalid_second_road", + "from": "hub", + "to": "dead_end", + "profile": "neighborhood", + } + ) + + with pytest.raises(GameMapError, match="terminate exactly one road"): + load_game_map(_write_map(tmp_path, source)) + + +def test_intersection_surface_is_inferred_from_incident_road_edges() -> None: + game_map = load_game_map(_INTERSECTION_GEOMETRY_MAP) + intersection = _surface(game_map, "center") + node = next(node for node in game_map.topology.nodes if node.node_id == "center") + + assert node.polygon_vertices_xy == () + assert intersection.bounds == pytest.approx((-3.45, -7.8, 7.67, 9.60), abs=0.02) + assert intersection.area == pytest.approx(161.47, abs=0.1) + assert intersection.convex_hull.area - intersection.area < 0.1 + + +def test_intersection_curbs_have_no_cap_fragments() -> None: + game_map = load_game_map(_INTERSECTION_GEOMETRY_MAP) + elements = {element.element_id: element for element in game_map.elements} + + curb_lengths = [ + LineString(curb.polyline_world[:, :2]).length + for curb in elements["center"].curbs + ] + assert curb_lengths + assert min(curb_lengths) > 1.0 + + +def test_parking_lot_uses_its_complete_authored_opening() -> None: + game_map = load_game_map(_PARKING_DRIVEWAY_MAP) + elements = {element.element_id: element for element in game_map.elements} + opening = LineString(((-6.0, -20.0), (6.0, -20.0))) + lot = elements["parking_lot"] + access = _surface(game_map, "parking_lot:access") + + assert access.boundary.intersection(opening).length == pytest.approx(12.0) + assert ( + sum( + LineString(curb.polyline_world[:, :2]).intersection(opening).length + for curb in lot.curbs + ) + <= 1.0e-4 + ) + + +def test_cul_de_sac_has_full_width_flat_road_connection() -> None: + game_map = load_game_map(_STARTER_MAP) + cul_de_sac = _surface(game_map, "dead_end") + road = _surface(game_map, "dead_end_road") + boundary = np.asarray(cul_de_sac.exterior.coords) + segment_lengths = np.linalg.norm(np.diff(boundary, axis=0), axis=1) + + assert cul_de_sac.distance(road) < 1.0e-3 + assert float(segment_lengths.max()) == pytest.approx(8.4, abs=0.05) + + +def test_parking_access_is_inferred_from_lot_node_and_not_authored_as_a_road() -> None: + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + lot = next(node for node in source["nodes"] if node["type"] == "parking_lot") + game_map = load_game_map(_STARTER_MAP) + road_ids = {road.road_id for road in game_map.topology.roads} + access_ids = {access.access_id for access in game_map.topology.parking_accesses} + inferred = { + element.element_id + for element in game_map.elements + if element.element_type == "parking_access" + } + + assert lot["connected_to"] == "hub" + assert lot["opening_vertex"] == 2 + assert "neighborhood_lot:access" not in road_ids + assert access_ids == {"neighborhood_lot:access"} + assert inferred == access_ids + width = np.ptp( + next( + element.surface_world[:, 0] + for element in game_map.elements + if element.element_id == "neighborhood_lot:access" + ) + ) + assert width == pytest.approx(6.0, abs=0.2) + lanes = [ + lane for lane in game_map.lanes if lane.element_id == "neighborhood_lot:access" + ] + assert len(lanes) == 2 + assert all(lane.speed_limit_mps == pytest.approx(5.5) for lane in lanes) + assert all(lane.marking_style == "VIRTUAL" for lane in lanes) + assert all(not lane.allows_taxi_stops for lane in lanes) + + +def test_degree_two_parking_sources_compile_as_driveway_nodes() -> None: + game_map = load_game_map(_PARKING_DRIVEWAY_MAP) + nodes = {node.node_id: node for node in game_map.topology.nodes} + + assert nodes["lot_driveway"].node_type == "driveway" + assert not any( + element.element_type == "intersection" + for element in game_map.elements + if element.element_id == "lot_driveway" + ) + + +def test_lane_graph_routes_to_and_from_parking_lot() -> None: + game_map = load_game_map(_STARTER_MAP) + outbound = _reachable(game_map, game_map.default_spawn.lane_id) + returning = _reachable(game_map, "neighborhood_lot:access:lane:1") + + assert "neighborhood_lot:access:lane:0" in outbound + assert "neighborhood_loop_southwest:lane:0" in returning + assert not any(lane.element_id == "neighborhood_lot" for lane in game_map.lanes) + + +def test_parking_lots_compile_as_green_roadnet_masks() -> None: + game_map = load_game_map(_PARKING_DRIVEWAY_MAP) + rows = game_map_compiler._road_marking_rows(game_map) + lots = [node for node in game_map.topology.nodes if node.node_type == "parking_lot"] + + assert len(rows) == len(lots) == 1 + assert all( + cast(dict[str, Any], row["road_marking"])["category"] + == "ROI_POLYGON_ROADNET_MASK" + for row in rows + ) + assert not game_map.line_markings + + +def test_routing_connectors_are_not_world_model_conditioning() -> None: + game_map = load_game_map(_STARTER_MAP) + connectors = [lane for lane in game_map.lanes if ":connector:" in lane.lane_id] + compiled_lane_ids = { + cast(dict[str, str], row["key"])["label_class_id"] + for row in game_map_compiler._lane_rows(game_map) + } + + assert connectors + assert all(not lane.conditioning_visible for lane in connectors) + assert compiled_lane_ids.isdisjoint(lane.lane_id for lane in connectors) + + +@pytest.mark.parametrize("source_path", _COMPILER_TEST_MAPS) +def test_every_authored_join_has_exact_non_overlapping_surfaces( + source_path: Path, +) -> None: + game_map = load_game_map(source_path) + surfaces = { + element.element_id: Polygon(element.surface_world[:, :2]).buffer(0) + for element in game_map.elements + } + pairs: list[tuple[str, str]] = [] + for road in game_map.topology.roads: + for node_id in (road.from_node_id, road.to_node_id): + pairs.append((road.road_id, node_id)) + for access in game_map.topology.parking_accesses: + for node_id in (access.source_node_id, access.parking_lot_node_id): + pairs.append((access.access_id, node_id)) + for first_id, second_id in pairs: + first, second = surfaces[first_id], surfaces[second_id] + assert first.intersection(second).area <= 1.0e-4, (first_id, second_id) + assert first.distance(second) <= 1.0e-4, (first_id, second_id) + + +@pytest.mark.parametrize("source_path", _COMPILER_TEST_MAPS) +def test_connected_surface_seams_do_not_emit_boundaries(source_path: Path) -> None: + game_map = load_game_map(source_path) + elements = {element.element_id: element for element in game_map.elements} + surfaces = { + element.element_id: Polygon(element.surface_world[:, :2]).buffer(0) + for element in game_map.elements + } + pairs: list[tuple[str, str]] = [] + for road in game_map.topology.roads: + for node_id in (road.from_node_id, road.to_node_id): + pairs.append((road.road_id, node_id)) + for access in game_map.topology.parking_accesses: + for node_id in (access.source_node_id, access.parking_lot_node_id): + pairs.append((access.access_id, node_id)) + + for first_id, second_id in pairs: + seam = surfaces[first_id].boundary.intersection(surfaces[second_id].boundary) + seam_boundaries = sum( + LineString(boundary.polyline_world[:, :2]).intersection(seam).length + for element_id in (first_id, second_id) + for boundary in elements[element_id].road_boundaries + ) + assert seam_boundaries <= 1.0e-4, (first_id, second_id, seam_boundaries) + + +@pytest.mark.parametrize("source_path", _COMPILER_TEST_MAPS) +def test_compiled_boundaries_belong_to_their_elements(source_path: Path) -> None: + game_map = load_game_map(source_path) + for element in game_map.elements: + surface_boundary = Polygon(element.surface_world[:, :2]).boundary + for boundary in element.road_boundaries: + line = LineString(boundary.polyline_world[:, :2]) + assert line.difference(surface_boundary.buffer(1.0e-4)).length < 1.0e-4 + for curb in element.curbs: + line = LineString(curb.polyline_world[:, :2]) + assert line.difference(surface_boundary.buffer(1.0e-4)).length < 1.0e-4 + if element.attributes.curb: + assert all(curb.polyline_world.shape[0] >= 2 for curb in element.curbs) + else: + assert not element.curbs + + +@pytest.mark.parametrize("source_path", _COMPILER_TEST_MAPS) +def test_every_authored_road_emits_every_profile_divider(source_path: Path) -> None: + document = yaml.safe_load(source_path.read_text(encoding="utf-8")) + game_map = load_game_map(source_path) + actual: dict[str, int] = {} + lane_elements = {lane.lane_id: lane.element_id for lane in game_map.lanes} + for divider in game_map.lane_dividers: + element_ids = {lane_elements[lane_id] for lane_id, _side in divider.lane_edges} + assert len(element_ids) == 1 + element_id = element_ids.pop() + actual[element_id] = actual.get(element_id, 0) + 1 + + for road in document["roads"]: + markings = document["profiles"][road["profile"]]["divider_markings"] + expected = sum(marking["style"].upper() != "VIRTUAL" for marking in markings) + assert actual.get(road["id"], 0) == expected, road["id"] + + +@pytest.mark.parametrize("source_path", _COMPILER_TEST_MAPS) +def test_final_clipgt_archive_contains_all_authored_map_geometry( + source_path: Path, tmp_path: Path +) -> None: + document = yaml.safe_load(source_path.read_text(encoding="utf-8")) + compiled = compile_game_map(source_path, cache_root=tmp_path / "cache") + + with zipfile.ZipFile(compiled.archive_path) as archive: + archive_names = set(archive.namelist()) + lane_lines = pq.read_table( + io.BytesIO(archive.read("clipgt/lane_line.parquet")) + ).to_pylist() + boundaries = pq.read_table( + io.BytesIO(archive.read("clipgt/road_boundary.parquet")) + ) + intersection_count = sum( + node.node_type == "intersection" + for node in compiled.game_map.topology.nodes + ) + intersections = ( + pq.read_table(io.BytesIO(archive.read("clipgt/intersection_area.parquet"))) + if intersection_count + else None + ) + + labels = [row["key"]["label_class_id"] for row in lane_lines] + for road in document["roads"]: + markings = document["profiles"][road["profile"]]["divider_markings"] + expected = sum(marking["style"].upper() != "VIRTUAL" for marking in markings) + actual = sum( + label.startswith(f"lane_line:{road['id']}:lane:") for label in labels + ) + assert actual == expected, road["id"] + assert boundaries.num_rows == sum( + len(element.road_boundaries) for element in compiled.game_map.elements + ) + if intersections is None: + assert "clipgt/intersection_area.parquet" not in archive_names + else: + assert intersections.num_rows == intersection_count + + +def test_compiler_settings_remain_map_local(tmp_path: Path) -> None: + baseline = load_game_map(_STARTER_MAP) + source = yaml.safe_load(_STARTER_MAP.read_text(encoding="utf-8")) + source["compiler"]["ground_margin_m"] = 35.0 + modified = load_game_map(_write_map(tmp_path, source)) + + assert baseline.compiler_settings["ground_margin_m"] == pytest.approx(20) + assert np.ptp(modified.ground_vertices[:, 0]) == pytest.approx( + np.ptp(baseline.ground_vertices[:, 0]) + 30 + ) + + +def test_missing_variant_image_uses_spawn_render(tmp_path: Path) -> None: + source = _road_joint_map() + variant = source["spawns"][0]["variants"]["default"] + del variant["image"] + source_path = _write_map(tmp_path, source) + + game_map = load_game_map(source_path) + assert game_map.default_spawn.variants[0].image is None + rendered = render_spawn_first_frame( + game_map, game_map.default_spawn, resolution_wh=(160, 88) + ) + assert rendered.shape == (88, 160, 3) + assert rendered.dtype == np.uint8 + assert len(np.unique(rendered.reshape(-1, 3), axis=0)) > 10 + + compiled = compile_game_map(source_path, cache_root=tmp_path / "cache") + with zipfile.ZipFile(compiled.archive_path) as archive: + with Image.open(io.BytesIO(archive.read("first_image.png"))) as image: + assert image.size == (1280, 704) + assert image.mode == "RGB" + + +def test_null_variant_image_selects_spawn_render(tmp_path: Path) -> None: + source = _road_joint_map() + source["spawns"][0]["variants"]["default"]["image"] = None + + game_map = load_game_map(_write_map(tmp_path, source)) + + assert game_map.default_spawn.variants[0].image is None + + +def test_spawn_preview_selects_authored_spawn(tmp_path: Path) -> None: + output = write_spawn_first_frame_preview( + _STARTER_MAP, + tmp_path / "spawn.png", + spawn_id="taxi_start", + ) + + assert output == (tmp_path / "spawn.png").resolve() + with Image.open(output) as image: + assert image.size == (1280, 704) + + +def test_spawn_preview_rejects_unknown_spawn(tmp_path: Path) -> None: + with pytest.raises(GameMapError, match="Unknown spawn 'missing'"): + write_spawn_first_frame_preview( + _STARTER_MAP, + tmp_path / "spawn.png", + spawn_id="missing", + ) + + +def test_lane_graph_initializes_navigation_and_gameplay() -> None: + game_map = load_game_map(_STARTER_MAP) + spawn = game_map.default_spawn + lanes = tuple( + NavigationLane( + centerline_world=lane.centerline_world, + road_edge_world=lane.roadside_edge_world + if lane.allows_taxi_stops + else None, + allows_taxi_stops=lane.allows_taxi_stops, + lane_id=lane.lane_id, + successor_ids=lane.successor_ids, + ) + for lane in game_map.lanes + ) + navigation = TaxiNavigationMap(lanes, endpoint_snap_tolerance_m=1.0e-6) + spawn_lane = next(lane for lane in lanes if lane.lane_id == spawn.lane_id) + state = VehicleState( + x_m=float(spawn.position_world[0]), + y_m=float(spawn.position_world[1]), + z_m=float(spawn.position_world[2]), + yaw_rad=spawn.yaw_rad, + speed_mps=0, + steer_rad=0, + ) + controller = TaxiGameController( + scene_id=game_map.map_id, + reference_route_world=spawn_lane.centerline_world, + navigation_lanes=lanes, + initial_state=state, + config=TaxiGameConfig(enabled=True, seed=17), + ) + + assert len(navigation.sample_waypoints(spacing_m=20, offset_m=0)) > 2 + assert controller.snapshot(state).pickup_targets_xyz_m + + +def test_compile_preview_and_scene_discovery(tmp_path: Path) -> None: + first = compile_game_map(_STARTER_MAP, cache_root=tmp_path / "cache") + second = compile_game_map(_STARTER_MAP, cache_root=tmp_path / "cache") + forced = compile_game_map(_STARTER_MAP, cache_root=tmp_path / "cache", force=True) + preview = write_game_map_preview(_STARTER_MAP, tmp_path / "preview.svg") + options = cli._discover_scene_options(_MAPS, _STARTER_MAP) + + assert not first.cache_hit and second.cache_hit + assert forced.cache_hit is False + assert first.archive_path == second.archive_path == forced.archive_path + preview_text = preview.read_text(encoding="utf-8") + assert preview_text.startswith(" WorldVehicleBBoxTrack: - timestamps = np.asarray([-1_000_000, 1_000_000], dtype=np.int64) - return WorldVehicleBBoxTrack( - track_id=f"{object_type.lower()}-1", - object_type=object_type, - timestamps_us=timestamps, - centers_world=np.asarray([[x_m, y_m, 0.8], [x_m, y_m, 0.8]], dtype=np.float32), - dimensions_lwh=np.asarray([[4.0, 1.9, 1.6], [4.0, 1.9, 1.6]], dtype=np.float32), - orientations_xyzw=np.asarray( - [[0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 1.0]], dtype=np.float32 - ), - max_extrapolation_us=2_000_000.0, - ) - - -def _moving_track() -> WorldVehicleBBoxTrack: - timestamps = np.asarray([-1_000_000, 12_000_000], dtype=np.int64) - return WorldVehicleBBoxTrack( - track_id="car-moving", - object_type="Car", - timestamps_us=timestamps, - centers_world=np.asarray([[5.0, 0.0, 0.8], [18.0, 0.0, 0.8]], dtype=np.float32), - dimensions_lwh=np.asarray([[4.0, 1.9, 1.6], [4.0, 1.9, 1.6]], dtype=np.float32), - orientations_xyzw=np.asarray( - [[0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 1.0]], dtype=np.float32 - ), - max_extrapolation_us=2_000_000.0, +def _scene(*, line_layers: tuple[WorldLineSegments, ...] = ()) -> SimpleNamespace: + return SimpleNamespace( + line_layers=line_layers, + polygon_layers=(), ) -def _fast_moving_track() -> WorldVehicleBBoxTrack: - timestamps = np.asarray([-1_000_000, 9_000_000], dtype=np.int64) - return WorldVehicleBBoxTrack( - track_id="car-fast", +def _test_scene_object() -> SceneObject: + return SceneObject( + object_id="test-car", object_type="Car", - timestamps_us=timestamps, - centers_world=np.asarray( - [[20.0, 0.0, 0.8], [220.0, 0.0, 0.8]], dtype=np.float32 - ), - dimensions_lwh=np.asarray([[4.0, 1.9, 1.6], [4.0, 1.9, 1.6]], dtype=np.float32), + model=rigid_body_model_for_object("Car", np.asarray([4.0, 1.9, 1.6])), + timestamps_us=np.asarray([-1_000_000, 1_000_000], dtype=np.int64), + positions_m=np.asarray([[5.0, 0.0, 0.8], [5.0, 0.0, 0.8]], dtype=np.float32), orientations_xyzw=np.asarray( [[0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 1.0]], dtype=np.float32 ), - max_extrapolation_us=2_000_000.0, - ) - - -def _scene( - *tracks: WorldVehicleBBoxTrack, - line_layers: tuple[WorldLineSegments, ...] = (), -) -> SimpleNamespace: - return SimpleNamespace( - vehicle_bbox_tracks=tracks, - line_layers=line_layers, - polygon_layers=(), ) @@ -224,138 +181,6 @@ def test_visual_flare_counts_external_hit_across_driving_direction() -> None: ) -def test_collision_detaches_actor_and_applies_physics_response() -> None: - world = GamePhysicsWorld(_scene(_track()), VehicleConfig()) - state = _moving_ego() - try: - for frame_index in range(16): - state, first_samples = world.step( - state, - timestamp_us=frame_index * 33_333, - dt_s=1.0 / 30.0, - ) - if world.last_step_actor_collision: - break - - actor = world.entities[0] - assert world.last_step_actor_collision is True - assert actor.detached_from_track is True - assert state.ragdoll_active is True - assert first_samples[0][3] is True - assert world._world._track_drive_enabled["car-1"] is False - impact_position = actor.transform.position_m.copy() - - next_timestamp_us = (frame_index + 1) * 33_333 - _, second_samples = world.step( - state, timestamp_us=next_timestamp_us, dt_s=1.0 / 30.0 - ) - - assert second_samples[0][3] is True - assert actor.transform.position_m[0] > impact_position[0] - trajectory = world.build_trajectories( - np.asarray([frame_index * 33_333, next_timestamp_us], dtype=np.int64), - [first_samples, second_samples], - )[0] - np.testing.assert_array_equal( - trajectory.timestamps_us, [frame_index * 33_333, next_timestamp_us] - ) - assert trajectory.detached_from_track is True - assert trajectory.is_simulated is True - finally: - world.close() - - -def test_minor_collision_does_not_latch_vehicle_drive_off() -> None: - world = GamePhysicsWorld(_scene(_track()), VehicleConfig()) - state = VehicleState( - x_m=2.0, - y_m=0.0, - z_m=0.0, - yaw_rad=0.0, - speed_mps=1.0, - steer_rad=0.0, - velocity_x_mps=1.0, - velocity_y_mps=0.0, - ) - - try: - for frame_index in range(17): - state, samples = world.step( - state, - timestamp_us=frame_index * 33_333, - dt_s=1.0 / 30.0, - ) - assert world.last_step_actor_collision is False - assert samples[0][3] is False - if frame_index == 0: - assert world._world._track_drive_enabled["car-1"] is False - assert world._traffic_ai._states["car-1"].drive_enabled is True - assert world._world._track_drive_enabled["car-1"] is True - finally: - world.close() - - -def test_recorded_renderer_trajectory_is_cached_and_holds_final_pose() -> None: - scene_object = GamePhysicsWorld._object_from_track( - _track(x_m=500.0), VehicleConfig() - ) - cached = _recorded_actor_trajectory(scene_object) - world = object.__new__(GamePhysicsWorld) - world.graph = PhysicsObjectGraph(objects=(scene_object,)) - world._recorded_trajectories_by_id = {scene_object.object_id: cached} - timestamps = np.asarray([0, 33_333], dtype=np.int64) - - first = world.build_trajectories(timestamps, [(), ()])[0] - second = world.build_trajectories(timestamps, [(), ()])[0] - - assert first is second - assert first.is_simulated is False - assert int(first.timestamps_us[-1]) > int(timestamps[-1]) - np.testing.assert_array_equal( - first.translations_world[-1], first.translations_world[-2] - ) - np.testing.assert_array_equal( - first.orientations_xyzw[-1], first.orientations_xyzw[-2] - ) - - -def test_side_impact_is_resolved_by_physx_contact() -> None: - world = GamePhysicsWorld(_scene(_track(x_m=0.0, y_m=3.0)), VehicleConfig()) - state = VehicleState( - x_m=0.0, - y_m=0.0, - z_m=0.0, - yaw_rad=np.pi / 2.0, - speed_mps=10.0, - steer_rad=0.0, - velocity_x_mps=0.0, - velocity_y_mps=10.0, - ) - - resolved, samples = world.step(state, timestamp_us=0, dt_s=1.0 / 30.0) - actor = world._world.body_state("car-1") - - assert samples[0][3] is True - assert resolved.ragdoll_active is True - assert actor.linear_velocity_mps[1] > 0.0 - actor_heights = [float(actor.position_m[2])] - actor_tilts = [float(np.linalg.norm(actor.orientation_xyzw[:2]))] - for frame_index in range(1, 121): - resolved, _ = world.step( - resolved, - timestamp_us=frame_index * 33_333, - dt_s=1.0 / 30.0, - ) - actor = world._world.body_state("car-1") - actor_heights.append(float(actor.position_m[2])) - actor_tilts.append(float(np.linalg.norm(actor.orientation_xyzw[:2]))) - - assert max(actor_heights) < 1.6 - assert max(actor_tilts) < 0.5 - assert actor_heights[-1] == pytest.approx(0.8, abs=0.08) - world.close() - - def test_physx_vehicle_yaw_is_free_away_from_road_boundaries() -> None: config = VehicleConfig() world = PhysXWorld( @@ -415,290 +240,6 @@ def test_physx_vehicle_yaw_is_limited_at_road_boundary() -> None: assert resolved.angular_velocity_radps[2] <= 1.0e-5 -def test_collision_yaw_impulse_stays_within_camera_continuity_envelope() -> None: - config = VehicleConfig(max_collision_yaw_rate_radps=0.35) - world = GamePhysicsWorld(_scene(_track(x_m=5.0, y_m=0.75)), config) - before = _moving_ego() - dt_s = 1.0 / 30.0 - - resolved, _ = world.step(before, timestamp_us=0, dt_s=dt_s) - - yaw_delta = math.atan2( - math.sin(resolved.yaw_rad - before.yaw_rad), - math.cos(resolved.yaw_rad - before.yaw_rad), - ) - assert resolved.ragdoll_active is True - assert abs(yaw_delta) <= config.max_collision_yaw_rate_radps * dt_s - assert abs(resolved.yaw_rate_radps) <= config.max_collision_yaw_rate_radps - world.close() - - -def test_held_throttle_cannot_drive_ego_inside_another_vehicle() -> None: - config = VehicleConfig() - ego_model = rigid_body_model_from_vehicle_config(config) - actor_model = rigid_body_model_for_object("Car", (4.0, 1.9, 1.6)) - assert ego_model.vehicle is not None - assert actor_model.vehicle is not None - chassis_contact_distance = ( - ego_model.vehicle.chassis_half_extents_m[0] - + actor_model.vehicle.chassis_half_extents_m[0] - ) - world = GamePhysicsWorld(_scene(_track(x_m=8.0)), config) - state = VehicleState( - x_m=0.0, - y_m=0.0, - z_m=0.0, - yaw_rad=0.0, - speed_mps=10.0, - steer_rad=0.0, - velocity_x_mps=10.0, - velocity_y_mps=0.0, - ) - command = DriverCommand(throttle=1.0, manual_control=True) - actor_slot = world._world._object_slots["car-1"] - minimum_center_separation = float("inf") - - try: - for frame_index in range(180): - state = integrate_vehicle(state, command, 1.0 / 30.0, config) - state, _ = world.step(state, frame_index * 33_333, 1.0 / 30.0) - actor_x = float(world._world.state_buffer[actor_slot, 0]) - center_separation = actor_x - state.x_m - minimum_center_separation = min( - minimum_center_separation, center_separation - ) - assert state.x_m < actor_x - finally: - world.close() - - assert minimum_center_separation >= chassis_contact_distance - 0.08 - - -def test_recorded_track_applies_force_without_prescribing_actor_pose() -> None: - track = _moving_track() - world = GamePhysicsWorld(_scene(track), VehicleConfig()) - parked_ego = VehicleState( - x_m=-20.0, - y_m=0.0, - z_m=0.0, - yaw_rad=0.0, - speed_mps=0.0, - steer_rad=0.0, - ) - timestamp_us = 5_000_000 - target_x = track.centers_world[0, 0] + ( - (track.centers_world[1, 0] - track.centers_world[0, 0]) - * (timestamp_us - track.timestamps_us[0]) - / (track.timestamps_us[1] - track.timestamps_us[0]) - ) - _, first_samples = world.step(parked_ego, timestamp_us, 1.0 / 30.0) - _, second_samples = world.step(parked_ego, timestamp_us + 33_333, 1.0 / 30.0) - - first_x = float(first_samples[0][1][0]) - second_x = float(second_samples[0][1][0]) - assert first_samples[0][3] is False - assert first_x < target_x - 1.0 - assert second_x > first_x - - trajectory = world.build_trajectories( - np.asarray([timestamp_us, timestamp_us + 33_333], dtype=np.int64), - [first_samples, second_samples], - )[0] - np.testing.assert_allclose(trajectory.translations_world[:, 0], [first_x, second_x]) - world.close() - - -def test_non_ego_track_drive_is_limited_to_fifteen_mph() -> None: - world = GamePhysicsWorld(_scene(_fast_moving_track()), VehicleConfig()) - parked_ego = VehicleState( - x_m=-100.0, - y_m=0.0, - z_m=0.0, - yaw_rad=0.0, - speed_mps=0.0, - steer_rad=0.0, - ) - actor_speeds_mps = [] - - try: - for frame_index in range(180): - world.step(parked_ego, frame_index * 33_333, 1.0 / 30.0) - actor_velocity = world._world.body_state("car-fast").linear_velocity_mps - actor_speeds_mps.append(float(np.linalg.norm(actor_velocity[:2]))) - finally: - world.close() - - max_drive_speed_mps = 15.0 * 0.44704 - assert max(actor_speeds_mps) <= max_drive_speed_mps + 0.05 - assert actor_speeds_mps[-1] == pytest.approx(max_drive_speed_mps, abs=0.20) - - -def test_struck_vehicle_ai_waits_until_stopped_for_one_second() -> None: - track = _track() - scene_object = GamePhysicsWorld._object_from_track(track, VehicleConfig()) - traffic_ai = TrafficDriverAI() - traffic_ai.synchronize((scene_object,)) - track_position, track_orientation, track_velocity = scene_object.sample(0) - - moving = BodyState( - position_m=track_position.copy(), - orientation_xyzw=track_orientation.copy(), - linear_velocity_mps=np.asarray([2.0, 0.0, 0.0], dtype=np.float32), - angular_velocity_radps=np.zeros(3, dtype=np.float32), - ) - decision = traffic_ai.update( - scene_object.object_id, - struck=True, - body=moving, - track_position=track_position, - track_orientation_xyzw=track_orientation, - track_velocity_mps=track_velocity, - dt_s=0.25, - ) - assert decision is not None - assert decision.drive_enabled is False - - stopped = BodyState( - position_m=track_position.copy(), - orientation_xyzw=track_orientation.copy(), - linear_velocity_mps=np.zeros(3, dtype=np.float32), - angular_velocity_radps=np.zeros(3, dtype=np.float32), - ) - for _ in range(2): - decision = traffic_ai.update( - scene_object.object_id, - struck=False, - body=stopped, - track_position=track_position, - track_orientation_xyzw=track_orientation, - track_velocity_mps=track_velocity, - dt_s=0.25, - ) - assert decision is not None - assert decision.drive_enabled is False - - decision = traffic_ai.update( - scene_object.object_id, - struck=False, - body=moving, - track_position=track_position, - track_orientation_xyzw=track_orientation, - track_velocity_mps=track_velocity, - dt_s=0.25, - ) - assert decision is not None - assert decision.drive_enabled is False - - for _ in range(3): - decision = traffic_ai.update( - scene_object.object_id, - struck=False, - body=stopped, - track_position=track_position, - track_orientation_xyzw=track_orientation, - track_velocity_mps=track_velocity, - dt_s=0.25, - ) - assert decision is not None - assert decision.drive_enabled is False - - decision = traffic_ai.update( - scene_object.object_id, - struck=False, - body=stopped, - track_position=track_position, - track_orientation_xyzw=track_orientation, - track_velocity_mps=track_velocity, - dt_s=0.25, - ) - assert decision is not None - assert decision.drive_enabled is True - assert decision.detached_from_track is True - - decision = traffic_ai.update( - scene_object.object_id, - struck=False, - body=stopped, - track_position=track_position, - track_orientation_xyzw=track_orientation, - track_velocity_mps=track_velocity, - dt_s=0.25, - ) - assert decision is not None - assert decision.detached_from_track is False - - -def test_ego_remains_driveable_after_collision_physics_takes_authority() -> None: - config = VehicleConfig() - world = GamePhysicsWorld(_scene(_track()), config) - state, _ = world.step(_moving_ego(), timestamp_us=0, dt_s=1.0 / 30.0) - impact_position = np.asarray([state.x_m, state.y_m]) - command = DriverCommand( - throttle=1.0, - steer=0.5, - steer_is_direct=True, - manual_control=True, - ) - - for frame_index in range(1, 61): - state = integrate_vehicle(state, command, 1.0 / 30.0, config) - state, _ = world.step(state, frame_index * 33_333, 1.0 / 30.0) - - assert state.ragdoll_active is False - assert np.linalg.norm(np.asarray([state.x_m, state.y_m]) - impact_position) > 3.0 - assert abs(state.y_m - impact_position[1]) > 0.5 - world.close() - - -def test_approaching_truck_triggers_flare_and_allows_reverse_after_impact() -> None: - config = VehicleConfig() - world = GamePhysicsWorld(_scene(_track("Truck", x_m=15.0)), config) - state = VehicleState( - x_m=0.0, - y_m=0.0, - z_m=0.0, - yaw_rad=0.0, - speed_mps=10.0, - steer_rad=0.0, - velocity_x_mps=10.0, - velocity_y_mps=0.0, - ) - forward = DriverCommand( - throttle=1.0, - steer_is_direct=True, - manual_control=True, - ) - collision_detected = False - frame_index = 0 - for frame_index in range(90): - state = integrate_vehicle(state, forward, 1.0 / 30.0, config) - state, _ = world.step(state, frame_index * 33_333, 1.0 / 30.0) - collision_detected |= world.last_step_actor_collision - if state.ragdoll_active: - break - - impact_x = state.x_m - reverse = DriverCommand( - throttle=1.0, - reverse=True, - steer_is_direct=True, - manual_control=True, - ) - for reverse_frame in range(120): - state = integrate_vehicle(state, reverse, 1.0 / 30.0, config) - state, _ = world.step( - state, - (frame_index + reverse_frame + 1) * 33_333, - 1.0 / 30.0, - ) - collision_detected |= world.last_step_actor_collision - - assert collision_detected is True - assert state.speed_mps < -1.0 - assert state.x_m < impact_x - 1.0 - world.close() - - def test_vehicle_instances_have_dimensioned_wheels_and_suspension() -> None: car = rigid_body_model_for_object("Car", (4.0, 1.9, 1.6)) truck = rigid_body_model_for_object("Truck", (8.0, 2.5, 3.2)) @@ -777,15 +318,7 @@ def test_physx_world_applies_incremental_graph_changes_in_stable_buffers() -> No state_pointer = world.state_buffer.__array_interface__["data"][0] track_state_pointer = world._track_state_buffer.__array_interface__["data"][0] active_pointer = world.active_buffer.__array_interface__["data"][0] - track = _track() - actor = SceneObject( - object_id=track.track_id, - object_type=track.object_type, - model=rigid_body_model_for_object(track.object_type, track.dimensions_lwh[0]), - timestamps_us=track.timestamps_us, - positions_m=track.centers_world, - orientations_xyzw=track.orientations_xyzw, - ) + actor = _test_scene_object() graph.upsert_object(actor) graph.upsert_barrier("road-edge", InvisibleBarrier((2.0, -5.0), (2.0, 5.0))) @@ -810,76 +343,42 @@ def test_physx_world_applies_incremental_graph_changes_in_stable_buffers() -> No world.close() -def test_game_world_reuses_native_track_samples_during_step() -> None: - world = GamePhysicsWorld(_scene(_track()), VehicleConfig()) - parked_ego = VehicleState( - x_m=-20.0, - y_m=0.0, - z_m=0.0, - yaw_rad=0.0, - speed_mps=0.0, - steer_rad=0.0, +def test_physx_world_uses_per_object_initial_track_timestamp() -> None: + ego_model = RigidBodyModel(1_550.0, (2.4, 1.0, 0.8)) + world = PhysXWorld(PhysicsObjectGraph(), ego_model, capacity=8) + actor = SceneObject( + object_id="procedural-car", + object_type="Car", + model=rigid_body_model_for_object("Car", np.asarray([4.0, 1.9, 1.6])), + timestamps_us=np.asarray([0, 1_000_000], dtype=np.int64), + positions_m=np.asarray([[0.0, 0.0, 0.8], [10.0, 0.0, 0.8]]), + orientations_xyzw=np.asarray( + [[0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 1.0]], dtype=np.float32 + ), ) - try: - with patch.object( - SceneObject, - "sample", - side_effect=AssertionError("track was sampled again in Python"), - ): - _, samples = world.step(parked_ego, timestamp_us=0, dt_s=1.0 / 30.0) - - assert len(samples) == 1 - finally: - world.close() - - -def test_game_world_recenters_topology_without_reallocating_state_buffers() -> None: - world = GamePhysicsWorld( - _scene(_track("Car", x_m=5.0), _track("Truck", x_m=500.0)), - VehicleConfig(), + world.synchronize( + PhysicsObjectGraph(objects=(actor,)), + timestamp_us=0, + initial_object_timestamps_us={actor.object_id: 1_000_000}, ) - state_pointer = world._world.state_buffer.__array_interface__["data"][0] - - assert [entity.entity_id for entity in world.entities] == ["car-1"] - assert world.synchronize_window(np.asarray([500.0, 0.0], dtype=np.float32)) - assert [entity.entity_id for entity in world.entities] == ["truck-1"] - assert world._world.state_buffer.__array_interface__["data"][0] == state_pointer - assert not world.synchronize_window(np.asarray([501.0, 0.0], dtype=np.float32)) - world.close() - -def test_game_world_bounds_physics_topology_around_ego() -> None: - world = GamePhysicsWorld( - _scene( - _track("Car", x_m=95.0), - _track("Truck", x_m=100.0), - ), - VehicleConfig(), + np.testing.assert_array_equal( + world.body_state(actor.object_id).position_m, + np.asarray([10.0, 0.0, 0.8], dtype=np.float32), ) - - assert [entity.entity_id for entity in world.entities] == ["car-1"] - assert world.synchronize_window(np.asarray([32.0, 0.0], dtype=np.float32)) - assert [entity.entity_id for entity in world.entities] == ["car-1", "truck-1"] - world.close() - - -def test_physics_topology_keeps_collider_crossing_window_boundary() -> None: - world = GamePhysicsWorld(_scene(_track("Truck", x_m=97.0)), VehicleConfig()) - - assert [entity.entity_id for entity in world.entities] == ["truck-1"] world.close() def test_physics_topology_indexes_sparse_track_segments() -> None: - track = _track() + source = _test_scene_object() actor = SceneObject( - object_id=track.track_id, - object_type=track.object_type, - model=rigid_body_model_for_object(track.object_type, track.dimensions_lwh[0]), + object_id=source.object_id, + object_type=source.object_type, + model=source.model, timestamps_us=np.asarray([0, 1_000_000], dtype=np.int64), positions_m=np.asarray([[-200.0, 0.0, 0.8], [200.0, 0.0, 0.8]]), - orientations_xyzw=track.orientations_xyzw, + orientations_xyzw=source.orientations_xyzw, ) culled = PhysicsObjectGraph(objects=(actor,)).copy_for_physx( @@ -889,166 +388,6 @@ def test_physics_topology_indexes_sparse_track_segments() -> None: assert culled.objects == (actor,) -def test_game_world_activates_actor_at_current_track_pose() -> None: - timestamps_us = np.asarray([0, 2_000_000], dtype=np.int64) - approaching_track = WorldVehicleBBoxTrack( - track_id="car-approaching", - object_type="Car", - timestamps_us=timestamps_us, - centers_world=np.asarray( - [[120.0, 0.0, 0.8], [90.0, 0.0, 0.8]], dtype=np.float32 - ), - dimensions_lwh=np.asarray([[4.0, 1.9, 1.6], [4.0, 1.9, 1.6]], dtype=np.float32), - orientations_xyzw=np.asarray( - [[0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 1.0]], dtype=np.float32 - ), - max_extrapolation_us=2_000_000.0, - ) - world = GamePhysicsWorld(_scene(approaching_track), VehicleConfig()) - - assert world.entities == () - assert world.synchronize_window( - np.asarray([0.0, 0.0], dtype=np.float32), timestamp_us=2_000_000 - ) - np.testing.assert_array_equal( - world._world.body_state("car-approaching").position_m, - np.asarray([90.0, 0.0, 0.8], dtype=np.float32), - ) - world.close() - - -def test_game_world_keeps_out_of_window_actor_on_recorded_trajectory() -> None: - world = GamePhysicsWorld( - _scene( - _track("Car", x_m=5.0), - _track("Truck", x_m=100.0), - ), - VehicleConfig(), - ) - _, samples = world.step(_moving_ego(), timestamp_us=0, dt_s=1.0 / 30.0) - - trajectories = { - trajectory.entity_id: trajectory - for trajectory in world.build_trajectories( - np.asarray([0], dtype=np.int64), [samples] - ) - } - - assert trajectories["car-1"].is_simulated is True - assert trajectories["truck-1"].is_simulated is False - np.testing.assert_array_equal( - trajectories["truck-1"].translations_world[0], - np.asarray([100.0, 0.0, 0.8], dtype=np.float32), - ) - world.close() - - -def test_actor_collision_can_be_disabled() -> None: - world = GamePhysicsWorld( - _scene(_track()), VehicleConfig(actor_collision_enabled=False) - ) - - state, samples = world.step(_moving_ego(), timestamp_us=0, dt_s=1.0 / 30.0) - - assert state.ragdoll_active is False - assert len(samples) == 1 - assert samples[0][3] is False - world.close() - - -def test_actor_collider_stays_enabled_while_hdmap_track_is_visible() -> None: - track = _track() - track = WorldVehicleBBoxTrack( - track_id=track.track_id, - object_type=track.object_type, - timestamps_us=track.timestamps_us, - centers_world=track.centers_world, - dimensions_lwh=track.dimensions_lwh, - orientations_xyzw=track.orientations_xyzw, - max_extrapolation_us=500_000.0, - ) - world = GamePhysicsWorld(_scene(track), VehicleConfig()) - - state, _ = world.step( - _moving_ego(), - timestamp_us=1_500_000, - dt_s=1.0 / 30.0, - ) - - assert state.ragdoll_active is True - assert world._active_collider_ids == {track.track_id} - assert world.debug_frame(state).actor_positions_m.shape == (1, 3) - world.close() - - -def test_actor_and_hdmap_hold_final_pose_after_track_ends() -> None: - track = _track() - track = WorldVehicleBBoxTrack( - track_id=track.track_id, - object_type=track.object_type, - timestamps_us=track.timestamps_us, - centers_world=track.centers_world, - dimensions_lwh=track.dimensions_lwh, - orientations_xyzw=track.orientations_xyzw, - max_extrapolation_us=500_000.0, - ) - world = GamePhysicsWorld(_scene(track), VehicleConfig()) - - parked_ego = VehicleState( - x_m=-20.0, - y_m=0.0, - z_m=0.0, - yaw_rad=0.0, - speed_mps=0.0, - steer_rad=0.0, - ) - state, first_samples = world.step( - parked_ego, - timestamp_us=1_500_001, - dt_s=1.0 / 30.0, - ) - _, second_samples = world.step( - state, - timestamp_us=1_533_334, - dt_s=1.0 / 30.0, - ) - - assert state.ragdoll_active is False - assert len(first_samples) == 1 - assert len(second_samples) == 1 - assert world._active_collider_ids == {track.track_id} - debug_frame = world.debug_frame(state) - assert debug_frame.actor_positions_m.shape == (1, 3) - np.testing.assert_allclose( - debug_frame.actor_positions_m[0], track.centers_world[-1], atol=0.05 - ) - trajectory = world.build_trajectories( - np.asarray([1_500_001, 1_533_334], dtype=np.int64), - [first_samples, second_samples], - )[0] - np.testing.assert_array_equal(trajectory.timestamps_us, [1_500_001, 1_533_334]) - np.testing.assert_allclose( - trajectory.translations_world, - np.stack((first_samples[0][1], second_samples[0][1])), - ) - assert trajectory.detached_from_track is False - world.close() - - collision_world = GamePhysicsWorld(_scene(track), VehicleConfig()) - collision_state, _ = collision_world.step( - _moving_ego(), - timestamp_us=3_000_000, - dt_s=1.0 / 30.0, - ) - assert collision_state.ragdoll_active is True - assert collision_world._active_collider_ids == {track.track_id} - assert collision_world.debug_frame(collision_state).actor_positions_m.shape == ( - 1, - 3, - ) - collision_world.close() - - def test_held_throttle_advances_ego_through_physx_world() -> None: config = VehicleConfig() world = GamePhysicsWorld(_scene(), config) @@ -1096,7 +435,7 @@ def test_held_s_reverses_runtime_ego_through_physx_world() -> None: boundary_x = [] for _ in range(16): chunk = simulation.pose_chunk( - command=input_backend.sample().command, + commands=(input_backend.sample().command,) * 8, chunk_size=8, frame_interval_s=1.0 / 30.0, extrapolation_offset_s=0.0, @@ -1143,7 +482,7 @@ def test_runtime_pose_chunks_keep_ground_anchored_ego_driving_forward() -> None: for _ in range(8): sampled = input_backend.sample() chunk = simulation.pose_chunk( - command=sampled.command, + commands=(sampled.command,) * 8, chunk_size=8, frame_interval_s=1.0 / 30.0, extrapolation_offset_s=0.0, @@ -1231,11 +570,7 @@ def test_physx_debug_view_packs_active_colliders_and_invisible_walls_for_ludus() layer_name="road_boundaries", ) world = GamePhysicsWorld( - _scene( - _track(), - _track("Truck", x_m=250.0), - line_layers=(boundary,), - ), + _scene(line_layers=(boundary,)), VehicleConfig(), ) state, _ = world.step(_moving_ego(), timestamp_us=0, dt_s=1.0 / 30.0) @@ -1245,17 +580,13 @@ def test_physx_debug_view_packs_active_colliders_and_invisible_walls_for_ludus() (snapshot,), np.asarray([0], dtype=np.int64), device=torch.device("cpu") ) - assert snapshot.actor_positions_m.shape == (1, 3) + assert snapshot.actor_positions_m.shape == (0, 3) assert snapshot.barrier_segments_xy_m.shape == (1, 2, 2) assert snapshot.barrier_thicknesses_m.tolist() == pytest.approx([0.3]) assert snapshot.barrier_heights_m.tolist() == pytest.approx([3.0]) - assert pool.translations.shape == (2, 3) - assert pool.quaternions.shape == (2, 4) - assert pool.scales.shape == (2, 3) - np.testing.assert_allclose( - pool.translations[0].numpy(), snapshot.actor_positions_m[0] - ) - np.testing.assert_allclose(pool.scales[0].numpy(), snapshot.actor_dimensions_lwh[0]) + assert pool.translations.shape == (1, 3) + assert pool.quaternions.shape == (1, 4) + assert pool.scales.shape == (1, 3) assert pool.render_flags == 0 lazy_debug = object() @@ -1273,6 +604,22 @@ def test_physx_debug_view_packs_active_colliders_and_invisible_walls_for_ludus() world.close() +def test_model_view_selects_generated_frame_during_impact() -> None: + raster = object() + model = object() + frame = PresentedFrame( + timestamp_us=0, + rgb_host_uint8=raster, + depth_host_f32=None, + model_rgb_host_uint8=model, + impact_kind="static", + ) + + selected = select_presented_rgb(frame, "model_rgb", width=320, height=180) + + assert selected is model + + def _debug_snapshot_at( positions_m: list[tuple[float, float, float]], ) -> PhysicsDebugFrame: @@ -1552,7 +899,7 @@ def _cube_pool(prim_type_id: int) -> CubePool: ) -def test_ludus_replacement_removes_recorded_actor_without_dropping_static_pools() -> ( +def test_ludus_replacement_removes_dynamic_actors_without_dropping_static_pools() -> ( None ): context = _FakeLudusContext() @@ -1581,8 +928,8 @@ def test_ludus_replacement_removes_recorded_actor_without_dropping_static_pools( detached_from_track=True, is_simulated=True, ) - recorded_actor = DynamicActorTrajectory( - entity_id="car-recorded", + second_actor = DynamicActorTrajectory( + entity_id="car-2", object_type="Car", timestamps_us=actor.timestamps_us, translations_world=actor.translations_world @@ -1591,7 +938,7 @@ def test_ludus_replacement_removes_recorded_actor_without_dropping_static_pools( dimensions_lwh=actor.dimensions_lwh, ) - renderer._replace_dynamic_actor_scene((recorded_actor, actor)) + renderer._replace_dynamic_actor_scene((second_actor, actor)) assert context.clear_count == 0 assert context.replace_count == 1 @@ -1605,13 +952,13 @@ def test_ludus_replacement_removes_recorded_actor_without_dropping_static_pools( ] assert len(obstacle_pools) == 2 np.testing.assert_allclose( - obstacle_pools[0].translations.numpy(), recorded_actor.translations_world + obstacle_pools[0].translations.numpy(), second_actor.translations_world ) np.testing.assert_allclose( obstacle_pools[1].translations.numpy(), actor.translations_world ) - renderer._replace_dynamic_actor_scene((recorded_actor, actor)) + renderer._replace_dynamic_actor_scene((second_actor, actor)) assert context.replace_count == 1 assert context.update_count == 1 diff --git a/apps/crazy_robotaxi/tests/test_latency_loop.py b/apps/crazy_robotaxi/tests/test_latency_loop.py index c551f7994..e1e3e0438 100644 --- a/apps/crazy_robotaxi/tests/test_latency_loop.py +++ b/apps/crazy_robotaxi/tests/test_latency_loop.py @@ -272,6 +272,30 @@ def sample(self) -> SampledInput: return SampledInput(command=DriverCommand(), sample_time=time.perf_counter()) +def test_command_timeline_preserves_short_press_and_release() -> None: + timeline = loop_module.CommandTimeline() + pressed = DriverCommand(steer=1.0) + released = DriverCommand() + timeline.observe(pressed, 10.0) + timeline.observe(released, 10.05) + + commands = timeline.commands_for_chunk(chunk_size=8, frame_interval_s=1.0 / 30.0) + + assert commands[:2] == (pressed, pressed) + assert commands[2:] == (released,) * 6 + + +def test_command_timeline_holds_latest_command_without_new_edges() -> None: + timeline = loop_module.CommandTimeline() + reverse = DriverCommand(throttle=1.0, reverse=True) + timeline.observe(reverse, 20.0) + first = timeline.commands_for_chunk(chunk_size=8, frame_interval_s=1.0 / 30.0) + second = timeline.commands_for_chunk(chunk_size=8, frame_interval_s=1.0 / 30.0) + + assert first == (reverse,) * 8 + assert second == (reverse,) * 8 + + class _TaxiRuntime: def __init__( self, controller: TaxiGameController, controls: _FakeRuntimeControls @@ -329,12 +353,13 @@ def current_state(self) -> VehicleState: def pose_chunk( self, - command: DriverCommand, + commands: tuple[DriverCommand, ...], chunk_size: int, frame_interval_s: float, extrapolation_offset_s: float, ) -> TrajectoryChunk: - del command, frame_interval_s, extrapolation_offset_s + assert len(commands) == chunk_size + del frame_interval_s, extrapolation_offset_s self.pose_chunk_calls += 1 return replace( make_trajectory(chunk_size), @@ -351,7 +376,7 @@ def test_chunk_request_gates_physx_debug_capture_on_view_mode( loop_module.make_chunk_request( state=loop_module.MainLoopState(), simulation=simulation, - command=DriverCommand(), + commands=(DriverCommand(),), input_sample_time=time.perf_counter(), chunk_history=loop_module.ChunkHistory(4), config=_loop_config(frame_interval_s=1.0 / 30.0), @@ -366,7 +391,7 @@ def test_chunk_request_can_force_physx_debug_capture_for_diagnostics() -> None: loop_module.make_chunk_request( state=loop_module.MainLoopState(), simulation=simulation, - command=DriverCommand(), + commands=(DriverCommand(),), input_sample_time=time.perf_counter(), chunk_history=loop_module.ChunkHistory(4), config=replace( diff --git a/apps/crazy_robotaxi/tests/test_latency_simulation.py b/apps/crazy_robotaxi/tests/test_latency_simulation.py index 291093aa7..5835b726b 100644 --- a/apps/crazy_robotaxi/tests/test_latency_simulation.py +++ b/apps/crazy_robotaxi/tests/test_latency_simulation.py @@ -20,6 +20,10 @@ def _initial_state() -> VehicleState: ) +def _held(command: DriverCommand, frames: int) -> tuple[DriverCommand, ...]: + return tuple(command for _ in range(frames)) + + def test_pose_chunk_rejects_nonzero_extrapolation_for_stage_one() -> None: simulation = EgoVehicleKinematics( initial_state=_initial_state(), @@ -29,7 +33,7 @@ def test_pose_chunk_rejects_nonzero_extrapolation_for_stage_one() -> None: ) with pytest.raises(NotImplementedError): simulation.pose_chunk( - command=DriverCommand(), + commands=_held(DriverCommand(), 4), chunk_size=4, frame_interval_s=1.0 / 30.0, extrapolation_offset_s=0.1, @@ -51,7 +55,7 @@ def test_pose_chunk_advances_state_to_chunk_boundary() -> None: initial_timestamp_us=0, ) chunk = simulation.pose_chunk( - command=DriverCommand(throttle=1.0), + commands=_held(DriverCommand(throttle=1.0), 4), chunk_size=4, frame_interval_s=1.0 / 30.0, extrapolation_offset_s=0.0, @@ -72,13 +76,13 @@ def test_pose_chunk_can_align_first_frame_with_rollout_initial_state() -> None: command = DriverCommand(throttle=1.0) first = simulation.pose_chunk( - command=command, + commands=_held(command, 5), chunk_size=5, frame_interval_s=1.0 / 30.0, extrapolation_offset_s=0.0, ) second = simulation.pose_chunk( - command=command, + commands=_held(command, 2), chunk_size=2, frame_interval_s=1.0 / 30.0, extrapolation_offset_s=0.0, @@ -101,7 +105,7 @@ def test_pose_chunk_default_still_simulates_before_first_frame() -> None: ) chunk = simulation.pose_chunk( - command=DriverCommand(throttle=1.0), + commands=_held(DriverCommand(throttle=1.0), 1), chunk_size=1, frame_interval_s=1.0 / 30.0, extrapolation_offset_s=0.0, @@ -110,6 +114,28 @@ def test_pose_chunk_default_still_simulates_before_first_frame() -> None: assert chunk.vehicle_states[0].speed_mps > 0.0 +def test_pose_chunk_applies_each_frame_command_in_order() -> None: + simulation = EgoVehicleKinematics( + initial_state=_initial_state(), + vehicle_config=VehicleConfig(), + ground_snapper=None, + initial_timestamp_us=0, + ) + forward = DriverCommand(throttle=1.0) + reverse = DriverCommand(throttle=1.0, reverse=True) + + chunk = simulation.pose_chunk( + commands=(forward, forward, reverse, reverse), + chunk_size=4, + frame_interval_s=0.1, + extrapolation_offset_s=0.0, + ) + + assert chunk.applied_commands == (forward, forward, reverse, reverse) + assert chunk.vehicle_states[1].speed_mps > chunk.vehicle_states[0].speed_mps + assert chunk.vehicle_states[-1].speed_mps < chunk.vehicle_states[1].speed_mps + + def test_pose_chunk_chains_across_calls() -> None: """Successive ``pose_chunk`` calls start from the previous boundary state. @@ -127,13 +153,13 @@ def test_pose_chunk_chains_across_calls() -> None: initial_timestamp_us=0, ) a.pose_chunk( - command=DriverCommand(throttle=1.0), + commands=_held(DriverCommand(throttle=1.0), chunk_size), chunk_size=chunk_size, frame_interval_s=frame_interval_s, extrapolation_offset_s=0.0, ) a.pose_chunk( - command=DriverCommand(throttle=1.0), + commands=_held(DriverCommand(throttle=1.0), chunk_size), chunk_size=chunk_size, frame_interval_s=frame_interval_s, extrapolation_offset_s=0.0, @@ -146,7 +172,7 @@ def test_pose_chunk_chains_across_calls() -> None: initial_timestamp_us=0, ) b.pose_chunk( - command=DriverCommand(throttle=1.0), + commands=_held(DriverCommand(throttle=1.0), chunk_size * 2), chunk_size=chunk_size * 2, frame_interval_s=frame_interval_s, extrapolation_offset_s=0.0, diff --git a/apps/crazy_robotaxi/tests/test_motion_conformance.py b/apps/crazy_robotaxi/tests/test_motion_conformance.py new file mode 100644 index 000000000..a9e7e3b3a --- /dev/null +++ b/apps/crazy_robotaxi/tests/test_motion_conformance.py @@ -0,0 +1,135 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +from __future__ import annotations + +import cv2 +import numpy as np +import pytest +import torch +from omnidreams_game_engine.motion_conformance import compare_motion + +pytestmark = pytest.mark.ci_cpu + + +def _translated_frames(step_px: int) -> tuple[np.ndarray, ...]: + rng = np.random.default_rng(42) + base = rng.integers(0, 256, size=(88, 160, 3), dtype=np.uint8) + return tuple( + cv2.warpAffine( + base, + np.asarray([[1.0, 0.0, float(index * step_px)], [0.0, 1.0, 0.0]]), + (160, 88), + borderMode=cv2.BORDER_REFLECT, + ) + for index in range(8) + ) + + +def _zoom_frames(scale_step: float) -> tuple[np.ndarray, ...]: + rng = np.random.default_rng(7) + base = rng.integers(0, 256, size=(88, 160, 3), dtype=np.uint8) + center = (79.5, 43.5) + return tuple( + cv2.warpAffine( + base, + cv2.getRotationMatrix2D(center, 0.0, 1.0 + index * scale_step), + (160, 88), + borderMode=cv2.BORDER_REFLECT, + ) + for index in range(8) + ) + + +def test_motion_conformance_accepts_matching_turn_flow() -> None: + frames = _translated_frames(2) + + result = compare_motion( + frames, + frames, + yaw_delta_rad=0.1, + longitudinal_delta_m=0.0, + ) + + assert result.axis == "turn" + assert result.mismatched is False + + +def test_motion_conformance_rejects_opposite_turn_flow() -> None: + result = compare_motion( + _translated_frames(2), + _translated_frames(-2), + yaw_delta_rad=0.1, + longitudinal_delta_m=0.0, + ) + + assert result.axis == "turn" + assert result.mismatched is True + + +def test_motion_conformance_rejects_missing_turn_flow() -> None: + stationary = _translated_frames(0) + result = compare_motion( + _translated_frames(2), + stationary, + yaw_delta_rad=0.1, + longitudinal_delta_m=0.0, + ) + + assert result.mismatched is True + + +def test_motion_conformance_rejects_opposite_longitudinal_flow() -> None: + result = compare_motion( + _zoom_frames(0.02), + _zoom_frames(-0.02), + yaw_delta_rad=0.0, + longitudinal_delta_m=1.0, + ) + + assert result.axis == "longitudinal" + assert result.mismatched is True + + +def test_motion_conformance_skips_low_motion_chunks() -> None: + result = compare_motion( + _translated_frames(0), + _translated_frames(-2), + yaw_delta_rad=0.0, + longitudinal_delta_m=0.0, + ) + + assert result.axis == "none" + assert result.mismatched is False + + +def test_motion_conformance_downsamples_lazy_tensor_without_host_materialization() -> ( + None +): + class _LazyFrame: + def __init__(self, value: np.ndarray) -> None: + self.tensor = torch.from_numpy(value) + self.host_materialized = False + + def to_cuda_tensor(self) -> torch.Tensor: + return self.tensor + + def to_cuda_event(self) -> None: + return None + + def to_numpy(self) -> np.ndarray: + self.host_materialized = True + return self.tensor.numpy() + + condition = tuple(_LazyFrame(frame) for frame in _translated_frames(2)) + generated = tuple(_LazyFrame(frame) for frame in _translated_frames(2)) + + result = compare_motion( + condition, + generated, + yaw_delta_rad=0.1, + longitudinal_delta_m=0.0, + ) + + assert result.mismatched is False + assert not any(frame.host_materialized for frame in condition + generated) diff --git a/apps/crazy_robotaxi/tests/test_physics.py b/apps/crazy_robotaxi/tests/test_physics.py index 3d0875cdd..0d1afc12c 100644 --- a/apps/crazy_robotaxi/tests/test_physics.py +++ b/apps/crazy_robotaxi/tests/test_physics.py @@ -291,7 +291,7 @@ def test_sample_chunk_trajectory_without_snapper_is_unchanged() -> None: chunk = sample_chunk_trajectory( start_state=state, start_timestamp_us=0, - command=DriverCommand(throttle=1.0), + commands=(DriverCommand(throttle=1.0),) * 10, chunk_size=10, chunk_config=ChunkConfig(fps=30), vehicle_config=VehicleConfig(), @@ -312,9 +312,12 @@ class _PhysicsWorld: def __init__(self) -> None: self._step_index = 0 self.last_step_actor_collision = False + self.last_step_static_barrier_impact = False def synchronize_window( - self, center_xy_m: np.ndarray, timestamp_us: int | None = None + self, + center_xy_m: np.ndarray, + timestamp_us: int | None = None, ) -> None: del center_xy_m, timestamp_us @@ -326,6 +329,7 @@ def step( ) -> tuple[VehicleState, tuple[object, ...]]: del timestamp_us, dt_s self.last_step_actor_collision = self._step_index == 1 + self.last_step_static_barrier_impact = self._step_index == 0 self._step_index += 1 return state, () @@ -344,7 +348,7 @@ def build_trajectories( chunk = sample_chunk_trajectory( start_state=_state(), start_timestamp_us=0, - command=DriverCommand(), + commands=(DriverCommand(),) * 2, chunk_size=2, chunk_config=ChunkConfig(fps=30), vehicle_config=VehicleConfig(), @@ -355,6 +359,8 @@ def build_trajectories( assert chunk.physx_elapsed_s == pytest.approx(0.010) assert chunk.actor_collision_detected is True assert chunk.actor_collision_frame_index == 1 + assert chunk.static_collision_detected is True + assert chunk.static_collision_frame_index == 0 def test_sample_chunk_trajectory_with_snapper_follows_slope() -> None: @@ -366,7 +372,7 @@ def test_sample_chunk_trajectory_with_snapper_follows_slope() -> None: chunk = sample_chunk_trajectory( start_state=state, start_timestamp_us=0, - command=DriverCommand(throttle=1.0), + commands=(DriverCommand(throttle=1.0),) * 30, chunk_size=30, # ~1s of driving chunk_config=chunk_cfg, vehicle_config=vehicle, @@ -397,7 +403,7 @@ def test_sample_chunk_trajectory_levels_stale_attitude_off_mesh() -> None: chunk = sample_chunk_trajectory( start_state=state, start_timestamp_us=0, - command=DriverCommand(), + commands=(DriverCommand(),) * 30, chunk_size=30, chunk_config=ChunkConfig(fps=30), vehicle_config=VehicleConfig(), diff --git a/apps/crazy_robotaxi/tests/test_presenter.py b/apps/crazy_robotaxi/tests/test_presenter.py index 1178bea28..5728d1733 100644 --- a/apps/crazy_robotaxi/tests/test_presenter.py +++ b/apps/crazy_robotaxi/tests/test_presenter.py @@ -868,31 +868,6 @@ def test_taxi_hud_bev_draws_nearby_targets_and_omits_distant_ones( assert not np.any(np.all(np.asarray(distant_canvas) == marker_color, axis=-1)) -def test_taxi_hud_bev_draws_visible_enclosure_segment() -> None: - presenter = CrazyRobotaxiHudPresenter.__new__(CrazyRobotaxiHudPresenter) - presenter._bev_config = BevConfig( - width=64, - height=64, - height_m=15.0, - fov_deg=60.0, - tilt_deg=0.0, - ) - presenter.configure_taxi_enclosure( - np.asarray([[[-100.0, 0.0, 0.0], [100.0, 0.0, 0.0]]], dtype=np.float32) - ) - presenter._latest_presented_frame = PresentedFrame( - timestamp_us=0, - rgb_host_uint8=np.zeros((1, 1, 3), dtype=np.uint8), - depth_host_f32=None, - bev_rig_to_world=np.eye(4, dtype=np.float32), - ) - canvas = Image.new("RGBA", (100, 80), (0, 0, 0, 0)) - - presenter._draw_bev_taxi_enclosure(ImageDraw.Draw(canvas), (20, 10, 80, 70)) - - assert np.any(np.all(np.asarray(canvas) == (235, 50, 50, 255), axis=-1)) - - def test_hud_bev_update_keeps_lazy_source_unmaterialized() -> None: presenter = _hud_presenter_without_window() lazy = _LazyFrame() @@ -1130,7 +1105,6 @@ def test_hud_postprocess_control_toggles_configured_preset() -> None: presenter._postprocess_rect = (10, 20, 110, 52) presenter._panel_chrome_cache_key = object() presenter._panel_chrome_cache = object() - presenter._scene_dropdown_open = False presenter._variant_dropdown_open = False presenter.set_postprocess_control( preset="rtx-super-resolution", @@ -1153,7 +1127,6 @@ def test_hud_postprocess_control_ignores_click_without_configured_preset() -> No presenter._postprocess_preset = "" presenter._postprocess_enabled = False presenter._postprocess_callback = calls.append - presenter._scene_dropdown_open = False presenter._variant_dropdown_open = False presenter._handle_click((20, 30)) @@ -1162,25 +1135,6 @@ def test_hud_postprocess_control_ignores_click_without_configured_preset() -> No assert presenter._postprocess_enabled is False -def test_hud_scene_dropdown_blocks_underlying_upsample_toggle() -> None: - presenter = _hud_presenter_without_window() - calls: list[bool] = [] - presenter._postprocess_rect = (10, 20, 110, 52) - presenter._postprocess_preset = "rtx-super-resolution-ultra" - presenter._postprocess_enabled = True - presenter._postprocess_callback = calls.append - presenter._scene_dropdown_open = True - presenter._variant_dropdown_open = False - presenter._scene_item_rects = [] - presenter._scene_header_rect = None - presenter._scene_selection_locked_probe = lambda: False - - presenter._handle_click((20, 30)) - - assert calls == [] - assert presenter._postprocess_enabled is True - - def test_hud_resize_uses_actual_window_size_without_model_resolution_clamp() -> None: presenter = _hud_presenter_without_window() presenter._pending_resize = None @@ -1415,7 +1369,6 @@ def _hud_presenter_for_exit(selected_variant: str) -> SlangPyHudPresenter: presenter._should_close_flag = True presenter._keyboard = _ExitSceneKeyboard() # State cleared by _reset_scene_view_state. - presenter._scene_dropdown_open = True presenter._variant_dropdown_open = True presenter._camera_resize_cache_key = object() presenter._camera_resize_cache = object() diff --git a/apps/crazy_robotaxi/tests/test_rasterizer.py b/apps/crazy_robotaxi/tests/test_rasterizer.py index 50e490e6c..76d0a0b6d 100644 --- a/apps/crazy_robotaxi/tests/test_rasterizer.py +++ b/apps/crazy_robotaxi/tests/test_rasterizer.py @@ -87,6 +87,44 @@ def set_line_widths(self, **kwargs) -> None: assert configured_cube_levels == [3] +def test_rasterizer_uses_thinner_linework_for_bev(monkeypatch) -> None: + configured_widths: list[dict[str, float]] = [] + + class _Context: + def __init__(self, *, device) -> None: + self.device = device + + def set_depth_scaling(self, enabled: bool) -> None: + pass + + def set_msaa_samples(self, samples: int) -> None: + pass + + def set_max_tessellation_levels(self, *, cube: int) -> None: + pass + + def set_line_widths(self, **kwargs: float) -> None: + configured_widths.append(kwargs) + + monkeypatch.setattr(torch.cuda, "is_available", lambda: True) + monkeypatch.setattr(rasterizer_module, "LudusCudaTimestampedContext", _Context) + + _LudusConditionRasterizerImpl( + RasterConfig(line_width_px=12.0, pole_width_px=8.0), None + ) + + assert len(configured_widths) == 1 + assert configured_widths[0] == pytest.approx( + { + "polyline_regular": 12.0, + "polyline_bev": 2.4, + "ego_traj_regular": 8.0, + "ego_traj_bev": 3.2, + "wireframe": 4.0, + } + ) + + def _impl_for_render_chunk(*, use_cuda_frames: bool) -> _LudusConditionRasterizerImpl: impl = _LudusConditionRasterizerImpl.__new__(_LudusConditionRasterizerImpl) impl._scene_data = _LoadedSceneData(clipgt_scene=object(), scene_adapter=object()) diff --git a/apps/crazy_robotaxi/tests/test_runner.py b/apps/crazy_robotaxi/tests/test_runner.py index 0ba1dfcd1..db8a6028a 100644 --- a/apps/crazy_robotaxi/tests/test_runner.py +++ b/apps/crazy_robotaxi/tests/test_runner.py @@ -21,7 +21,6 @@ from pathlib import Path import pytest - from crazy_robotaxi.runner import ( CRAZY_ROBOTAXI_RUNNER, CrazyRobotaxiRunner, @@ -37,7 +36,7 @@ def test_runner_registry_metadata_uses_public_slug() -> None: assert CRAZY_ROBOTAXI_RUNNER.description -def test_runner_delegates_to_standalone_legacy_cli( +def test_runner_delegates_to_standalone_cli( monkeypatch: pytest.MonkeyPatch, ) -> None: """Translate typed runner fields without constructing the GPU pipeline.""" @@ -48,12 +47,13 @@ def test_runner_delegates_to_standalone_legacy_cli( ) config = replace( CRAZY_ROBOTAXI_RUNNER, - scene=Path("city.usdz"), + map=Path("city.robotaxi.yaml"), world_model_manifest=Path("example_world_model_perf.yaml"), + renderer_config=Path("renderer.yaml"), + game_config=Path("game.yaml"), backend="raster", stream_mjpeg="127.0.0.1:8080", auto_start=True, - synthetic_scene=True, synthetic_model=False, taxi_seed=7, taxi_highscores=Path("scores.csv"), @@ -66,10 +66,14 @@ def test_runner_delegates_to_standalone_legacy_cli( [ "--camera", "front", - "--scene", - "city.usdz", + "--map", + "city.robotaxi.yaml", "--manifest", "example_world_model_perf.yaml", + "--renderer-config", + "renderer.yaml", + "--game-config", + "game.yaml", "--backend", "raster", "--stream-mjpeg", @@ -78,7 +82,6 @@ def test_runner_delegates_to_standalone_legacy_cli( "7", "--taxi-highscores", "scores.csv", - "--synthetic-scene", "--auto-start", "--no-synthetic-model", ] diff --git a/apps/crazy_robotaxi/tests/test_scene_fixture.py b/apps/crazy_robotaxi/tests/test_scene_fixture.py index 49fe2e93c..2e4cf3e0d 100644 --- a/apps/crazy_robotaxi/tests/test_scene_fixture.py +++ b/apps/crazy_robotaxi/tests/test_scene_fixture.py @@ -48,12 +48,3 @@ def test_build_synthetic_scene_usdz_round_trip() -> None: "road_islands", } assert all(len(layer.polygons_world) > 0 for layer in bundle.polygon_layers) - - # One track per BBOX_V3_COLORS category (Car, Truck, Pedestrian, Cyclist, - # Others). Each track has two samples covering the full trajectory span so - # ``interpolate_at_timestamp`` resolves at every render frame. - track_types = {track.object_type for track in bundle.vehicle_bbox_tracks} - assert track_types == {"Car", "Truck", "Pedestrian", "Cyclist", "Others"} - for track in bundle.vehicle_bbox_tracks: - assert len(track.timestamps_us) == 2 - assert track.interpolate_at_timestamp(bundle.initial_timestamp_us) is not None diff --git a/apps/crazy_robotaxi/tests/test_scene_loader.py b/apps/crazy_robotaxi/tests/test_scene_loader.py index 7a49ea576..f00c7b95b 100644 --- a/apps/crazy_robotaxi/tests/test_scene_loader.py +++ b/apps/crazy_robotaxi/tests/test_scene_loader.py @@ -5,280 +5,40 @@ import io import zipfile -from typing import Any -import numpy as np import pytest -from crazy_robotaxi.scene import ( - _build_fallback_perimeter, - _build_lane_centerlines, - _build_lane_network_perimeter, - _build_navigation_lanes, - load_scene_data, -) from omnidreams_game_engine._sample_assets import SAMPLE_SCENE -from omnidreams_game_engine.colors import BBOX_V3_COLORS from omnidreams_game_engine.config import RasterConfig from omnidreams_game_engine.scene_loader import ( _discover_prompts, load_scene_bundle, ) -from shapely.geometry import Point, Polygon - - -def _point(x_m: float, y_m: float, z_m: float = 0.0) -> dict[str, float]: - return {"x": x_m, "y": y_m, "z": z_m} - - -def _lane_row_from_rails( - left_rail: tuple[dict[str, float], ...], - right_rail: tuple[dict[str, float], ...], - *, - vehicle_types: tuple[str, ...] = ("CAR",), -) -> dict[str, Any]: - return { - "lane": { - "left_rail": list(left_rail), - "right_rail": list(right_rail), - "vehicle_types": list(vehicle_types), - } - } - - -def _lane_row( - *, - start_x_m: float, - end_x_m: float, - center_y_m: float, - map_end: str, - use_types: tuple[str, ...] = (), -) -> dict[str, Any]: - return { - "lane": { - "left_rail": [ - _point(start_x_m, center_y_m + 2.0), - _point(end_x_m, center_y_m + 2.0), - ], - "right_rail": [ - _point(start_x_m, center_y_m - 2.0), - _point(end_x_m, center_y_m - 2.0), - ], - "vehicle_types": ["CAR"], - "map_end": map_end, - "use_types": list(use_types), - } - } - - -def _boundary_row(*points: dict[str, float]) -> dict[str, Any]: - return {"road_boundary": {"location": list(points)}} - - -def _assert_segments_form_closed_rings(segments: np.ndarray) -> None: - discontinuities = np.flatnonzero( - np.linalg.norm(segments[:-1, 1] - segments[1:, 0], axis=1) > 1.0e-4 - ) - ring_starts = (0, *(int(index) + 1 for index in discontinuities)) - ring_stops = (*(int(index) + 1 for index in discontinuities), len(segments)) - for start, stop in zip(ring_starts, ring_stops, strict=True): - ring = segments[start:stop] - np.testing.assert_allclose(ring[:, 1], np.roll(ring[:, 0], -1, axis=0)) - - -def test_lane_centerlines_use_car_lane_rail_midpoints() -> None: - rows = [ - { - "lane": { - "left_rail": [_point(0.0, 2.0), _point(10.0, 2.0)], - "right_rail": [_point(10.0, -2.0), _point(0.0, -2.0)], - "vehicle_types": ["CAR"], - } - }, - { - "lane": { - "left_rail": [_point(0.0, 12.0), _point(10.0, 12.0)], - "right_rail": [_point(0.0, 8.0), _point(10.0, 8.0)], - "vehicle_types": ["BICYCLE"], - } - }, - ] - - centerlines = _build_lane_centerlines(rows) - - assert len(centerlines) == 1 - np.testing.assert_allclose( - centerlines[0], - np.array([[0.0, 0.0, 0.0], [10.0, 0.0, 0.0]], dtype=np.float32), - ) - - -def test_lane_network_perimeter_is_closed_beyond_lane_rails() -> None: - lanes = [_lane_row(start_x_m=0.0, end_x_m=30.0, center_y_m=0.0, map_end="NONE")] - - perimeter = _build_lane_network_perimeter( - lanes, - np.asarray([5.0, 0.0], dtype=np.float32), - ) - - assert len(perimeter) >= 4 - _assert_segments_form_closed_rings(perimeter) - assert float(perimeter[:, :, 0].min()) < 0.0 - assert float(perimeter[:, :, 0].max()) > 30.0 - assert float(perimeter[:, :, 1].min()) < -2.0 - assert float(perimeter[:, :, 1].max()) > 2.0 - - -def test_lane_network_perimeter_wraps_connected_branches_without_internal_caps() -> ( - None -): - lanes = [ - _lane_row(start_x_m=0.0, end_x_m=30.0, center_y_m=0.0, map_end="NONE"), - _lane_row_from_rails( - (_point(13.0, 0.0), _point(13.0, 20.0)), - (_point(17.0, 0.0), _point(17.0, 20.0)), - ), - ] - perimeter = _build_lane_network_perimeter( - lanes, - np.asarray([5.0, 0.0], dtype=np.float32), - ) - ring = Polygon(perimeter[:, 0, :2]) - - assert ring.is_valid - assert ring.covers(Point(15.0, 10.0)) - - -def test_lane_network_perimeter_encloses_inner_block_edge() -> None: - lanes = [ - _lane_row(start_x_m=0.0, end_x_m=30.0, center_y_m=0.0, map_end="NONE"), - _lane_row(start_x_m=0.0, end_x_m=30.0, center_y_m=30.0, map_end="NONE"), - _lane_row_from_rails( - (_point(-2.0, 0.0), _point(-2.0, 30.0)), - (_point(2.0, 0.0), _point(2.0, 30.0)), - ), - _lane_row_from_rails( - (_point(28.0, 0.0), _point(28.0, 30.0)), - (_point(32.0, 0.0), _point(32.0, 30.0)), - ), - ] - - perimeter = _build_lane_network_perimeter( - lanes, - np.asarray([0.0, 0.0], dtype=np.float32), - ) - inner_segments = perimeter[ - np.all( - (perimeter[:, :, :2] >= 4.0) & (perimeter[:, :, :2] <= 26.0), axis=(1, 2) - ) - ] - - _assert_segments_form_closed_rings(perimeter) - assert len(inner_segments) >= 4 - _assert_segments_form_closed_rings(inner_segments) - - -def test_lane_network_perimeter_excludes_disconnected_parking_area() -> None: - lanes = [ - _lane_row(start_x_m=0.0, end_x_m=30.0, center_y_m=0.0, map_end="NONE"), - _lane_row( - start_x_m=100.0, - end_x_m=120.0, - center_y_m=100.0, - map_end="NONE", - use_types=("SERVICE_ROAD",), - ), - ] - - perimeter = _build_lane_network_perimeter( - lanes, - np.asarray([5.0, 0.0], dtype=np.float32), - ) - - assert float(perimeter[:, :, 0].max()) < 100.0 - assert float(perimeter[:, :, 1].max()) < 100.0 +pytestmark = pytest.mark.ci_cpu -def test_fallback_perimeter_is_closed_outside_navigation_extent() -> None: - rows = [_lane_row(start_x_m=0.0, end_x_m=10.0, center_y_m=5.0, map_end="NONE")] - - boundary_rows = [_boundary_row(_point(-50.0, 0.0), _point(-40.0, 0.0))] - - perimeter = _build_fallback_perimeter( # type: ignore[arg-type] - rows, boundary_rows - ) - - assert perimeter.shape == (4, 2, 3) - np.testing.assert_allclose(perimeter[:, 1], np.roll(perimeter[:, 0], -1, axis=0)) - assert float(perimeter[:, :, 0].min()) < -50.0 - assert float(perimeter[:, :, 0].max()) > 10.0 - assert float(perimeter[:, :, 1].min()) < 3.0 - assert float(perimeter[:, :, 1].max()) > 7.0 - - -def test_navigation_lanes_keep_only_road_edges_as_stopping_surfaces() -> None: - rows = [ - { - "lane": { - "left_rail": [_point(0.0, 2.0), _point(10.0, 2.0)], - "right_rail": [_point(0.0, -2.0), _point(10.0, -2.0)], - "left_edge_styles": ["LONG_DASHED_SINGLE", "LONG_DASHED_SINGLE"], - "right_edge_styles": ["TALL_CURB", "TALL_CURB"], - "left_edge_colors": ["WHITE", "WHITE"], - "right_edge_colors": ["UNKNOWN", "UNKNOWN"], - "vehicle_types": ["CAR"], - } - }, - { - "lane": { - "left_rail": [_point(0.0, 6.0), _point(10.0, 6.0)], - "right_rail": [_point(0.0, 2.0), _point(10.0, 2.0)], - "left_edge_styles": ["LONG_DASHED_SINGLE", "LONG_DASHED_SINGLE"], - "right_edge_styles": ["TALL_CURB", "VIRTUAL"], - "left_edge_colors": ["WHITE", "WHITE"], - "right_edge_colors": ["UNKNOWN", "UNKNOWN"], - "vehicle_types": ["CAR"], - } - }, - ] - - lanes = _build_navigation_lanes(rows) - - assert len(lanes) == 2 - assert lanes[0].allows_taxi_stops - assert lanes[0].road_edge_world is not None - np.testing.assert_allclose( - lanes[0].road_edge_world, - np.array([[0.0, -2.0, 0.0], [10.0, -2.0, 0.0]], dtype=np.float32), - ) - assert not lanes[1].allows_taxi_stops - assert lanes[1].road_edge_world is None - - -def test_usdz_prompt_discovery_accepts_legacy_numeric_suffix() -> None: +def test_usdz_prompt_discovery_accepts_numeric_suffix() -> None: archive = io.BytesIO() with zipfile.ZipFile(archive, "w") as zf: - zf.writestr("prompt1.txt", "legacy one") - zf.writestr("prompt_2.txt", "canonical two") + zf.writestr("prompt1.txt", "one") + zf.writestr("prompt_2.txt", "two") zf.writestr("promptnight.txt", "ignored") archive.seek(0) with zipfile.ZipFile(archive, "r") as zf: prompts = _discover_prompts(zf) - assert prompts["default"] == "legacy one" - assert prompts["1"] == "legacy one" - assert prompts["2"] == "canonical two" + assert prompts["default"] == "one" + assert prompts["1"] == "one" + assert prompts["2"] == "two" assert "night" not in prompts -# Opportunistic: exercises the real USDZ loader, so this test is silently -# skipped on machines where ``prepare.py`` hasn't fetched the production asset. @pytest.mark.skipif( not SAMPLE_SCENE.exists(), reason="sample scene is not available on this workstation", ) -def test_load_scene_bundle_from_real_usdz() -> None: +def test_internal_scene_bundle_loader_reads_recorded_archive() -> None: bundle = load_scene_bundle( scene_path=SAMPLE_SCENE, camera_name="camera_front_wide_120fov", @@ -291,17 +51,6 @@ def test_load_scene_bundle_from_real_usdz() -> None: assert bundle.selected_camera.logical_name == "camera_front_wide_120fov" assert bundle.initial_rgb.shape == (352, 640, 3) assert bundle.initial_timestamp_us > 0 - scene_data = load_scene_data(bundle) - assert scene_data.reference_route_world.ndim == 2 - assert scene_data.reference_route_world.shape[1] == 3 - assert len(scene_data.reference_route_world) >= 2 - assert len(scene_data.navigation_routes_world) > 100 - assert len(scene_data.navigation_lanes) > 100 - assert len(scene_data.perimeter_segments_world) > 100 - _assert_segments_form_closed_rings(scene_data.perimeter_segments_world) - navigation_points = np.concatenate(scene_data.navigation_routes_world, axis=0) - assert np.ptp(navigation_points[:, 0]) > 200.0 - assert np.ptp(navigation_points[:, 1]) > 200.0 assert len(bundle.line_layers) > 0 assert any(layer.color_rgba == (1.0, 1.0, 0.0, 1.0) for layer in bundle.line_layers) assert any( @@ -312,9 +61,3 @@ def test_load_scene_bundle_from_real_usdz() -> None: layer.layer_name == "crosswalks" and len(layer.polygons_world) > 0 for layer in bundle.polygon_layers ) - assert len(bundle.vehicle_bbox_tracks) > 0 - sample_track = bundle.vehicle_bbox_tracks[0] - assert sample_track.object_type in BBOX_V3_COLORS - assert ( - sample_track.interpolate_at_timestamp(bundle.initial_timestamp_us) is not None - ) diff --git a/apps/crazy_robotaxi/tests/test_streaming_presenter_realtime.py b/apps/crazy_robotaxi/tests/test_streaming_presenter_realtime.py index a9c5bad1c..b43280d0e 100644 --- a/apps/crazy_robotaxi/tests/test_streaming_presenter_realtime.py +++ b/apps/crazy_robotaxi/tests/test_streaming_presenter_realtime.py @@ -41,7 +41,9 @@ def test_streaming_page_contains_taxi_name_and_leaderboard_controls() -> None: assert "'/taxi/name'" in _INDEX_HTML assert 'id="score-rows"' in _INDEX_HTML assert 'id="new-game"' in _INDEX_HTML - assert 'id="taxi-boundaries"' in _INDEX_HTML + assert 'id="taxi-boundaries"' not in _INDEX_HTML + assert 'id="scene-picker"' not in _INDEX_HTML + assert "fetchScenes" not in _INDEX_HTML def test_streaming_presenter_materializes_lazy_rgba_frames() -> None: @@ -155,9 +157,6 @@ def test_streaming_state_snapshot_includes_taxi_payload() -> None: presenter._keyboard = keyboard presenter._taxi_enabled = True presenter._bev_config = BevConfig(tilt_deg=0.0) - presenter._taxi_enclosure_segments_world = np.asarray( - [[[10.0, -100.0, 0.0], [10.0, 100.0, 0.0]]], dtype=np.float32 - ) presenter._latest_presented_frame = PresentedFrame( timestamp_us=0, rgb_host_uint8=np.zeros((1, 1, 3), dtype=np.uint8), @@ -177,11 +176,7 @@ def test_streaming_state_snapshot_includes_taxi_payload() -> None: assert snapshot["taxi"]["global_remaining_time_s"] == 0.0 assert len(snapshot["taxi"]["bev_targets"]) == 4 assert all(target["visible"] for target in snapshot["taxi"]["bev_targets"]) - assert len(snapshot["taxi"]["bev_enclosure_segments"]) == 1 - assert all( - 0.0 <= coordinate <= 1.0 - for coordinate in snapshot["taxi"]["bev_enclosure_segments"][0].values() - ) + assert "bev_enclosure_segments" not in snapshot["taxi"] def test_streaming_state_snapshot_keeps_upstream_shape_outside_taxi() -> None: diff --git a/apps/crazy_robotaxi/tests/test_taxi_driving.py b/apps/crazy_robotaxi/tests/test_taxi_driving.py index 8c2c49d91..a0c0ef2da 100644 --- a/apps/crazy_robotaxi/tests/test_taxi_driving.py +++ b/apps/crazy_robotaxi/tests/test_taxi_driving.py @@ -44,13 +44,19 @@ def test_taxi_config_does_not_enable_base_game_mode() -> None: assert config.vehicle == TaxiVehicleConfig() -def test_taxi_cli_keeps_base_mode_disabled_and_owns_traffic_density( +def test_taxi_cli_keeps_base_mode_disabled( monkeypatch: pytest.MonkeyPatch, ) -> None: monkeypatch.setattr(cli, "RasterRenderBackend", lambda **_kwargs: object()) config, _backend = cli.prepare_config_and_backend( - args := build_parser().parse_args(["--taxi-game", "--traffic-density", "0.25"]) + args := build_parser().parse_args( + [ + "--map", + "city.robotaxi.yaml", + "--taxi-game", + ] + ) ) taxi_config = taxi_config_from_args(args) @@ -58,7 +64,6 @@ def test_taxi_cli_keeps_base_mode_disabled_and_owns_traffic_density( assert config.vehicle.actor_collision_enabled is False assert config.visual_flare_enabled is False assert taxi_config.enabled is True - assert taxi_config.traffic_density == pytest.approx(0.25) assert taxi_config.vehicle.actor_collision_enabled is True @@ -128,9 +133,3 @@ def test_space_remains_upstream_stop_until_taxi_controls_are_enabled() -> None: assert taxi_command.stop is False assert taxi_command.handbrake is True - - -@pytest.mark.parametrize("density", [0.0, -0.1, 1.1]) -def test_taxi_config_rejects_invalid_traffic_density(density: float) -> None: - with pytest.raises(ValueError, match="traffic_density"): - TaxiGameConfig(traffic_density=density) diff --git a/apps/crazy_robotaxi/tests/test_taxi_game.py b/apps/crazy_robotaxi/tests/test_taxi_game.py index 3b4bcdd0d..472dfba3e 100644 --- a/apps/crazy_robotaxi/tests/test_taxi_game.py +++ b/apps/crazy_robotaxi/tests/test_taxi_game.py @@ -22,8 +22,8 @@ from crazy_robotaxi.navigation import NavigationLane from omnidreams_game_engine.camera import FThetaCameraModel from omnidreams_game_engine.config import BevConfig +from omnidreams_game_engine.game_map.vicinity import GameMapVicinity from omnidreams_game_engine.math3d import rig_pose_from_vehicle_state -from omnidreams_game_engine.simulation.map_bounds import MapBounds from omnidreams_game_engine.types import ( CameraCalibration, TrajectoryChunk, @@ -287,71 +287,86 @@ def test_pickup_markers_and_passengers_use_separate_roadside_positions() -> None ) -def test_pickups_and_dropoffs_exclude_map_boundary_margin() -> None: - bounds = MapBounds(x_min=0.0, y_min=0.0, x_max=300.0, y_max=100.0) - controller = TaxiGameController( - scene_id="bounded-targets", - reference_route_world=np.asarray( - [[0.0, 50.0, 0.0], [300.0, 50.0, 0.0]], dtype=np.float32 +def test_pickup_passengers_are_filtered_without_removing_pickup_targets() -> None: + lanes = ( + NavigationLane( + centerline_world=np.asarray( + [[0.0, 0.0, 0.0], [100.0, 0.0, 0.0]], dtype=np.float32 + ), + lane_id="lane-a", + successor_ids=("lane-b",), + element_id="road-a", ), - initial_state=_state(150.0, 50.0), + NavigationLane( + centerline_world=np.asarray( + [[100.0, 10.0, 0.0], [0.0, 10.0, 0.0]], dtype=np.float32 + ), + lane_id="lane-b", + successor_ids=("lane-a",), + element_id="road-b", + ), + ) + + class _Resolver: + def resolve(self, *_args: object, **_kwargs: object) -> GameMapVicinity: + return GameMapVicinity( + "road-a", frozenset({"road-a"}), frozenset({"road-a"}) + ) + + controller = TaxiGameController( + scene_id="local-passengers", + reference_route_world=lanes[0].centerline_world, + navigation_lanes=lanes, + initial_state=_state(), config=TaxiGameConfig( enabled=True, seed=17, - waypoint_spacing_m=10.0, + waypoint_spacing_m=20.0, pickup_grid_spacing_m=20.0, - waypoint_edge_margin_m=40.0, + pickup_min_distance_m=0.0, ), - map_bounds=bounds, - ) - - seeking = controller.snapshot(_state(150.0, 50.0)) - assert seeking.pickup_targets_xyz_m - assert all( - 40.0 <= target[0] <= 260.0 and 40.0 <= target[1] <= 60.0 - for target in seeking.pickup_targets_xyz_m + vicinity_resolver=_Resolver(), # type: ignore[arg-type] ) + controller._available_pickup_indices = tuple(range(len(controller._waypoints))) - pickup = seeking.target_xyz_m - controller.advance(_trajectory(pickup[:2]), 0.0) - dropoff = controller.snapshot(_state(*pickup[:2])) + snapshot = controller.snapshot(_state()) - assert dropoff.phase == "to_dropoff" - assert 40.0 <= dropoff.target_xyz_m[0] <= 260.0 - assert 40.0 <= dropoff.target_xyz_m[1] <= 60.0 + assert len(snapshot.pickup_targets_xyz_m) == len(controller._waypoints) + assert len(snapshot.pickup_passengers_xyz_m) < len(snapshot.pickup_targets_xyz_m) + visible = { + tuple( + float(value) + for value in ( + waypoint.passenger_xyz_m + if waypoint.passenger_xyz_m is not None + else waypoint.xyz_m + ) + ) + for waypoint in controller._waypoints + if waypoint.element_id == "road-a" + } + assert set(snapshot.pickup_passengers_xyz_m) == visible -def test_default_targets_stay_one_hundred_meters_inside_map_bounds() -> None: - bounds = MapBounds(x_min=0.0, y_min=0.0, x_max=500.0, y_max=500.0) +def test_targets_use_full_navigation_extent() -> None: controller = TaxiGameController( - scene_id="default-bounded-targets", + scene_id="full-extent-targets", reference_route_world=np.asarray( - [[0.0, 250.0, 0.0], [500.0, 250.0, 0.0]], dtype=np.float32 + [[0.0, 50.0, 0.0], [300.0, 50.0, 0.0]], dtype=np.float32 ), - initial_state=_state(250.0, 250.0), + initial_state=_state(150.0, 50.0), config=TaxiGameConfig( enabled=True, seed=17, waypoint_spacing_m=10.0, pickup_grid_spacing_m=20.0, ), - map_bounds=bounds, ) - seeking = controller.snapshot(_state(250.0, 250.0)) + seeking = controller.snapshot(_state(150.0, 50.0)) assert seeking.pickup_targets_xyz_m - assert all( - 100.0 <= target[0] <= 400.0 and 100.0 <= target[1] <= 400.0 - for target in seeking.pickup_targets_xyz_m - ) - - pickup = seeking.target_xyz_m - controller.advance(_trajectory(pickup[:2]), 0.0) - dropoff = controller.snapshot(_state(*pickup[:2])) - - assert dropoff.phase == "to_dropoff" - assert 100.0 <= dropoff.target_xyz_m[0] <= 400.0 - assert 100.0 <= dropoff.target_xyz_m[1] <= 400.0 + assert any(target[0] < 100.0 for target in seeking.pickup_targets_xyz_m) + assert any(target[0] > 200.0 for target in seeking.pickup_targets_xyz_m) def test_fare_completion_survives_no_directed_route_to_next_pickup( diff --git a/apps/crazy_robotaxi/tests/test_taxi_navigation.py b/apps/crazy_robotaxi/tests/test_taxi_navigation.py index dd2f9c317..df5082f46 100644 --- a/apps/crazy_robotaxi/tests/test_taxi_navigation.py +++ b/apps/crazy_robotaxi/tests/test_taxi_navigation.py @@ -11,6 +11,7 @@ import pytest from crazy_robotaxi.navigation import ( LanePosition, + NavigationFareRegion, NavigationLane, NavigationWaypoint, TaxiNavigationMap, @@ -65,6 +66,77 @@ def test_route_does_not_traverse_lane_against_its_direction() -> None: assert navigation.route(_position(0, 8.0), destination) is None +def test_parking_target_is_sampled_in_polygon_and_routes_only_to_entrance() -> None: + navigation = TaxiNavigationMap( + ( + NavigationLane( + np.asarray([[0, 0, 0], [10, 0, 0]], dtype=np.float32), + lane_id="arrival", + successor_ids=(), + ), + NavigationLane( + np.asarray([[10, 0, 0], [0, 0, 0]], dtype=np.float32), + lane_id="departure", + successor_ids=(), + ), + ) + ) + region = NavigationFareRegion( + region_id="lot", + kind="area", + geometry_world=( + np.asarray( + [[20, 20, 0], [20, 30, 0], [30, 30, 0], [30, 20, 0]], + dtype=np.float32, + ), + ), + arrival_lane_ids=("arrival",), + departure_lane_ids=("departure",), + ) + + waypoint = navigation.sample_fare_regions( + (region,), spacing_m=20, rng=np.random.default_rng(7) + )[0] + route = navigation.route(_position(0, 2), waypoint) + + assert 20 <= waypoint.xyz_m[0] <= 30 + assert 20 <= waypoint.xyz_m[1] <= 30 + assert waypoint.departure_anchors[0].lane_index == 1 + assert route is not None + assert route.distance_m == pytest.approx(8.0) + + +def test_concave_parking_targets_stay_inside_polygon() -> None: + navigation = TaxiNavigationMap( + ( + NavigationLane( + np.asarray([[0, 0, 0], [10, 0, 0]], dtype=np.float32), + lane_id="road", + successor_ids=(), + ), + ) + ) + region = NavigationFareRegion( + region_id="concave_lot", + kind="area", + geometry_world=( + np.asarray( + [[0, 0, 0], [0, 10, 0], [4, 10, 0], [4, 4, 0], [10, 4, 0], [10, 0, 0]], + dtype=np.float32, + ), + ), + arrival_lane_ids=("road",), + departure_lane_ids=("road",), + ) + + waypoints = navigation.sample_fare_regions( + (region,), spacing_m=2, rng=np.random.default_rng(19) + ) + + assert waypoints + assert all(point.xyz_m[0] <= 4 or point.xyz_m[1] <= 4 for point in waypoints) + + def test_lane_matching_prefers_vehicle_heading_on_overlapping_lanes() -> None: navigation = TaxiNavigationMap( ( diff --git a/apps/crazy_robotaxi/tests/test_taxi_physics.py b/apps/crazy_robotaxi/tests/test_taxi_physics.py index 3535ec2ac..fc2b9255c 100644 --- a/apps/crazy_robotaxi/tests/test_taxi_physics.py +++ b/apps/crazy_robotaxi/tests/test_taxi_physics.py @@ -19,10 +19,12 @@ from crazy_robotaxi.physics import ( TaxiPhysicsWorld, inset_vehicle_chassis, - select_traffic_tracks, step_taxi_physics_world, ) -from omnidreams_game_engine.config import ChunkConfig +from ludus_renderer import BodyState +from omnidreams_game_engine.config import ChunkConfig, VehicleConfig +from omnidreams_game_engine.game_map.types import GameMapTrafficVehicle +from omnidreams_game_engine.game_map.vicinity import GameMapVicinity from omnidreams_game_engine.simulation.components import ( rigid_body_model_from_vehicle_config, ) @@ -30,6 +32,10 @@ sample_chunk_trajectory, ) from omnidreams_game_engine.simulation.game_physics import GamePhysicsWorld +from omnidreams_game_engine.simulation.map_traffic import ( + MapTrafficController, + MapTrafficPhase, +) from omnidreams_game_engine.types import ( DriverCommand, PhysicsDebugFrame, @@ -43,7 +49,6 @@ @dataclass(frozen=True) class _Scene: scene_id: str = "taxi-physics-test" - vehicle_bbox_tracks: tuple[object, ...] = () line_layers: tuple[WorldLineSegments, ...] = () polygon_layers: tuple[object, ...] = () @@ -57,19 +62,6 @@ def _yaw_from_quaternion_xyzw(quaternion: np.ndarray) -> float: return math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)) -def test_taxi_traffic_filter_is_stable_and_keeps_non_motor_actors() -> None: - tracks = tuple( - SimpleNamespace(track_id=f"car-{index}", object_type="Car") - for index in range(10) - ) + (SimpleNamespace(track_id="person-1", object_type="Pedestrian"),) - - selected = select_traffic_tracks(tracks, 0.4, "scene-a") - - assert selected == select_traffic_tracks(tracks, 0.4, "scene-a") - assert len([track for track in selected if track.object_type == "Car"]) == 4 - assert tracks[-1] in selected - - def test_taxi_chassis_inset_does_not_change_visual_extents() -> None: model = rigid_body_model_from_vehicle_config(TaxiVehicleConfig()) assert model.vehicle is not None @@ -86,24 +78,431 @@ def test_taxi_chassis_inset_does_not_change_visual_extents() -> None: ) -def test_taxi_enclosure_is_added_only_to_private_physics_scene() -> None: +def test_map_traffic_controller_holds_a_car_for_same_lane_headway() -> None: + centerline = np.asarray( + [[0, 0, 0], [100, 0, 0], [100, 50, 0], [0, 50, 0], [0, 0, 0]], + dtype=np.float32, + ) + speed_limits = np.full(len(centerline), 10.0, dtype=np.float32) + + def definition(vehicle_id: str, start_distance_m: float) -> GameMapTrafficVehicle: + return GameMapTrafficVehicle( + vehicle_id=vehicle_id, + node_ids=("a", "b"), + end_behavior="wrap", + vehicle_type="car", + dimensions_lwh_m=(4.5, 1.8, 1.5), + speed_mps=None, + start_distance_m=start_distance_m, + centerline_world=centerline, + speed_limits_mps=speed_limits, + route_element_ids=("test-road",) * (len(centerline) - 1), + ) + + controller = MapTrafficController( + (definition("following", 0), definition("leading", 10)), VehicleConfig() + ) + controller.set_vicinity( + GameMapVicinity("test-road", frozenset({"test-road"}), frozenset({"test-road"})) + ) + assert controller.max_drive_speeds_mps == { + "map-traffic:following": 10.0, + "map-traffic:leading": 10.0, + } + bodies = { + "map-traffic:following": BodyState( + position_m=np.asarray([0, 0, 0.75], dtype=np.float32), + orientation_xyzw=np.asarray([0, 0, 0, 1], dtype=np.float32), + linear_velocity_mps=np.zeros(3, dtype=np.float32), + angular_velocity_radps=np.zeros(3, dtype=np.float32), + ), + "map-traffic:leading": BodyState( + position_m=np.asarray([10, 0, 0.75], dtype=np.float32), + orientation_xyzw=np.asarray([0, 0, 0, 1], dtype=np.float32), + linear_velocity_mps=np.zeros(3, dtype=np.float32), + angular_velocity_radps=np.zeros(3, dtype=np.float32), + ), + } + published: list[tuple[tuple[str, int, float], ...]] = [] + world = SimpleNamespace( + body_state=lambda object_id: bodies[object_id], + ego_model=SimpleNamespace(half_extents_m=(2.4, 1.0, 0.8)), + apply_track_progress=published.append, + ) + ego = BodyState( + position_m=np.asarray([-100, 0, 0.8], dtype=np.float32), + orientation_xyzw=np.asarray([0, 0, 0, 1], dtype=np.float32), + linear_velocity_mps=np.zeros(3, dtype=np.float32), + angular_velocity_radps=np.zeros(3, dtype=np.float32), + ) + + controller.prepare_step(world, ego, 0.0) # type: ignore[arg-type] + + scales = {object_id: scale for object_id, _, scale in published[-1]} + assert scales["map-traffic:following"] == 0.0 + assert scales["map-traffic:leading"] == 1.0 + + +def test_map_traffic_advances_only_inactive_car_without_publishing_it() -> None: + centerline = np.asarray( + [[0, 0, 0], [100, 0, 0], [100, 50, 0], [0, 50, 0], [0, 0, 0]], + dtype=np.float32, + ) + speed_limits = np.full(len(centerline), 10.0, dtype=np.float32) + + def definition(vehicle_id: str, element_id: str, start_m: float): + return GameMapTrafficVehicle( + vehicle_id=vehicle_id, + node_ids=("a", "b"), + end_behavior="wrap", + vehicle_type="car", + dimensions_lwh_m=(4.5, 1.8, 1.5), + speed_mps=None, + start_distance_m=start_m, + centerline_world=centerline, + speed_limits_mps=speed_limits, + route_element_ids=(element_id,) * (len(centerline) - 1), + ) + + controller = MapTrafficController( + (definition("near", "near-road", 1.0), definition("far", "far-road", 20.0)), + VehicleConfig(), + ) + controller.set_vicinity( + GameMapVicinity("near-road", frozenset({"near-road"}), frozenset({"near-road"})) + ) + near_id = "map-traffic:near" + far_id = "map-traffic:far" + near_timestamp_before = controller._states_by_id[near_id].timestamp_us + far_timestamp_before = controller._states_by_id[far_id].timestamp_us + published: list[tuple[tuple[str, int, float], ...]] = [] + near_position, near_orientation, _ = controller.active_objects[0].sample( + int(near_timestamp_before) + ) + near_body = BodyState( + position_m=near_position, + orientation_xyzw=near_orientation, + linear_velocity_mps=np.zeros(3, dtype=np.float32), + angular_velocity_radps=np.zeros(3, dtype=np.float32), + ) + world = SimpleNamespace( + body_state=lambda object_id: {near_id: near_body}[object_id], + ego_model=SimpleNamespace(half_extents_m=np.asarray([2.4, 1.0, 0.8])), + apply_track_progress=published.append, + ) + ego = BodyState( + position_m=np.asarray([-100, -100, 0.8], dtype=np.float32), + orientation_xyzw=np.asarray([0, 0, 0, 1], dtype=np.float32), + linear_velocity_mps=np.zeros(3, dtype=np.float32), + angular_velocity_radps=np.zeros(3, dtype=np.float32), + ) + + controller.prepare_step(world, ego, 1.0 / 30.0) + + assert controller.active_object_ids == {near_id} + assert tuple(item[0] for item in published[0]) == (near_id,) + assert controller._states_by_id[near_id].timestamp_us == pytest.approx( + near_timestamp_before, abs=1.0 + ) + assert controller._states_by_id[far_id].timestamp_us > far_timestamp_before + + +def _recovery_controller() -> tuple[MapTrafficController, str]: + centerline = np.asarray( + [[0, 0, 0], [20, 0, 0], [20, 20, 0], [0, 20, 0], [0, 0, 0]], + dtype=np.float32, + ) + definition = GameMapTrafficVehicle( + vehicle_id="recovering", + node_ids=("a", "b"), + end_behavior="wrap", + vehicle_type="car", + dimensions_lwh_m=(4.5, 1.8, 1.5), + speed_mps=None, + start_distance_m=2.0, + centerline_world=centerline, + speed_limits_mps=np.full(len(centerline), 10.0, dtype=np.float32), + route_element_ids=("south", "east", "north", "west"), + ) + controller = MapTrafficController((definition,), VehicleConfig()) + controller.set_vicinity( + GameMapVicinity( + "south", + frozenset({"south", "east", "north", "west"}), + frozenset({"south", "east", "north", "west"}), + ) + ) + return controller, "map-traffic:recovering" + + +def _body_at( + position_xy: tuple[float, float], + *, + yaw_rad: float = 0.0, + linear_velocity_xy: tuple[float, float] = (0.0, 0.0), + angular_speed_radps: float = 0.0, +) -> BodyState: + return BodyState( + position_m=np.asarray([*position_xy, 0.75], dtype=np.float32), + orientation_xyzw=np.asarray( + [0.0, 0.0, math.sin(yaw_rad * 0.5), math.cos(yaw_rad * 0.5)], + dtype=np.float32, + ), + linear_velocity_mps=np.asarray([*linear_velocity_xy, 0.0], dtype=np.float32), + angular_velocity_radps=np.asarray( + [0.0, 0.0, angular_speed_radps], dtype=np.float32 + ), + ) + + +def test_active_map_traffic_progress_is_anchored_to_its_physical_body() -> None: + controller, object_id = _recovery_controller() + state = controller.state(object_id) + assert state is not None + initial_timestamp_us = state.timestamp_us + initial_position, initial_orientation, _ = state.scene_object.sample( + int(initial_timestamp_us) + ) + body = BodyState( + position_m=initial_position.copy(), + orientation_xyzw=initial_orientation.copy(), + linear_velocity_mps=np.zeros(3, dtype=np.float32), + angular_velocity_radps=np.zeros(3, dtype=np.float32), + ) + published: list[tuple[tuple[str, int, float], ...]] = [] + world = SimpleNamespace( + ego_model=SimpleNamespace(half_extents_m=(2.4, 1.0, 0.8)), + apply_track_progress=published.append, + ) + ego = _body_at((-100.0, -100.0)) + + for _ in range(10): + controller.prepare_step(world, ego, 1.0) # type: ignore[arg-type] + + assert state.timestamp_us == pytest.approx(initial_timestamp_us, abs=1.0) + assert {batch[0][1] for batch in published} == {int(initial_timestamp_us + 350_000)} + + body.position_m[:2] = (5.0, 0.0) + controller.observe_physics( + object_id, + struck=False, + body=body, + dt_s=1.0 / 30.0, + ) + controller.prepare_step(world, ego, 10.0) # type: ignore[arg-type] + + assert state.timestamp_us == pytest.approx(500_000, abs=1.0) + assert published[-1][0][1] == pytest.approx(850_000, abs=1.0) + + +def test_active_map_traffic_walks_its_route_cursor_without_global_search() -> None: + centerline = np.asarray( + [[x, 0, 0] for x in range(21)] + [[20, 20, 0], [0, 20, 0], [0, 0, 0]], + dtype=np.float32, + ) + definition = GameMapTrafficVehicle( + vehicle_id="cursor", + node_ids=("a", "b"), + end_behavior="wrap", + vehicle_type="car", + dimensions_lwh_m=(4.5, 1.8, 1.5), + speed_mps=None, + start_distance_m=1.0, + centerline_world=centerline, + speed_limits_mps=np.full(len(centerline), 10.0, dtype=np.float32), + route_element_ids=("road",) * (len(centerline) - 1), + ) + controller = MapTrafficController((definition,), VehicleConfig()) + controller.set_vicinity( + GameMapVicinity("road", frozenset({"road"}), frozenset({"road"})) + ) + state = controller.state("map-traffic:cursor") + assert state is not None + body = _body_at((12.4, 0.0), linear_velocity_xy=(10.0, 0.0)) + controller.observe_physics( + state.object_id, + struck=False, + body=body, + dt_s=1.0 / 30.0, + ) + published: list[tuple[tuple[str, int, float], ...]] = [] + world = SimpleNamespace( + ego_model=SimpleNamespace(half_extents_m=(2.4, 1.0, 0.8)), + apply_track_progress=published.append, + ) + + with patch.object( + controller, + "_nearest_route_projection", + side_effect=AssertionError("normal traversal used a global route search"), + ): + controller.prepare_step(world, _body_at((-100.0, -100.0)), 1.0 / 30.0) + + assert state.route_segment_index == 12 + assert state.timestamp_us == pytest.approx(1_240_000, abs=1.0) + + +def test_map_traffic_collision_freezes_route_until_nearest_route_recovery() -> None: + controller, object_id = _recovery_controller() + state = controller.state(object_id) + assert state is not None + frozen_timestamp_us = state.timestamp_us + + decision = controller.observe_physics( + object_id, + struck=True, + body=_body_at((18.0, 8.0), yaw_rad=math.pi), + dt_s=0.25, + ) + assert decision is not None + assert decision.drive_enabled is False + assert decision.detached_from_track is True + assert state.phase is MapTrafficPhase.COLLISION + + published: list[tuple[tuple[str, int, float], ...]] = [] + world = SimpleNamespace( + body_state=lambda _: _body_at((18.0, 8.0), yaw_rad=math.pi), + ego_model=SimpleNamespace(half_extents_m=(2.4, 1.0, 0.8)), + apply_track_progress=published.append, + ) + controller.prepare_step( + world, # type: ignore[arg-type] + _body_at((-100.0, -100.0)), + 0.5, + ) + assert state.timestamp_us == frozen_timestamp_us + assert published[-1][0][2] == 0.0 + + stopped = _body_at((18.0, 8.0), yaw_rad=math.pi) + for _ in range(4): + decision = controller.observe_physics( + object_id, struck=False, body=stopped, dt_s=0.25 + ) + assert decision is not None + assert decision.drive_enabled is True + assert decision.detached_from_track is False + assert state.phase is MapTrafficPhase.RECOVERING + projected_position, projected_orientation, projected_velocity = ( + state.scene_object.sample(int(state.timestamp_us)) + ) + np.testing.assert_allclose(projected_position[:2], [20.0, 8.0], atol=1.0e-4) + assert state.route_segment_index == 1 + + # Track stabilization is reattached, but gameplay recovery continues until the + # physical pose and velocity match the route. + decision = controller.observe_physics( + object_id, + struck=False, + body=_body_at(tuple(projected_position[:2]), yaw_rad=math.pi), + dt_s=0.25, + ) + assert decision is not None + assert decision.detached_from_track is False + assert state.phase is MapTrafficPhase.RECOVERING + np.testing.assert_allclose(state.position_m[:2], projected_position[:2]) + + recovered = BodyState( + position_m=projected_position.copy(), + orientation_xyzw=projected_orientation.copy(), + linear_velocity_mps=projected_velocity.copy(), + angular_velocity_radps=np.zeros(3, dtype=np.float32), + ) + decision = controller.observe_physics( + object_id, struck=False, body=recovered, dt_s=0.25 + ) + assert decision is not None + assert decision.detached_from_track is False + assert state.phase is MapTrafficPhase.TRAVERSING + + +def test_map_traffic_collision_settling_has_a_maximum_duration() -> None: + controller, object_id = _recovery_controller() + unsettled = _body_at((18.0, 8.0), angular_speed_radps=1.0) + controller.observe_physics( + object_id, + struck=True, + body=unsettled, + dt_s=0.25, + ) + + for _ in range(11): + decision = controller.observe_physics( + object_id, + struck=False, + body=unsettled, + dt_s=0.25, + ) + assert decision is not None + assert decision.drive_enabled is False + assert decision.detached_from_track is True + + decision = controller.observe_physics( + object_id, + struck=False, + body=unsettled, + dt_s=0.25, + ) + + state = controller.state(object_id) + assert state is not None + assert decision is not None + assert decision.drive_enabled is True + assert decision.detached_from_track is False + assert state.phase is MapTrafficPhase.RECOVERING + + +def test_map_traffic_recollision_and_offscreen_reset_share_one_state() -> None: + controller, object_id = _recovery_controller() + state = controller.state(object_id) + assert state is not None + displaced = _body_at((19.0, 9.0)) + controller.observe_physics(object_id, struck=True, body=displaced, dt_s=0.25) + for _ in range(4): + controller.observe_physics(object_id, struck=False, body=displaced, dt_s=0.25) + assert state.phase is MapTrafficPhase.RECOVERING + + decision = controller.observe_physics( + object_id, struck=True, body=_body_at((17.0, 10.0)), dt_s=0.25 + ) + assert decision is not None + assert decision.drive_enabled is False + assert state.phase is MapTrafficPhase.COLLISION + + changed = controller.set_vicinity( + GameMapVicinity("other", frozenset({"other"}), frozenset({"other"})) + ) + assert changed is True + assert controller.active_object_ids == frozenset() + assert state.phase is MapTrafficPhase.TRAVERSING + assert state.decision.detached_from_track is False + route_position, route_orientation, route_velocity = state.scene_object.sample( + int(state.timestamp_us) + ) + np.testing.assert_array_equal(state.position_m, route_position) + np.testing.assert_array_equal(state.orientation_xyzw, route_orientation) + np.testing.assert_array_equal(state.linear_velocity_mps, route_velocity) + + +def test_taxi_curbs_are_physics_barriers_without_a_render_layer() -> None: scene = _scene() - enclosure = np.asarray([[[5.0, -3.0, 0.0], [5.0, 3.0, 0.0]]], dtype=np.float32) + curbs = np.asarray([[[5.0, -3.0, 0.0], [5.0, 3.0, 0.0]]], dtype=np.float32) + config = TaxiVehicleConfig() with patch.object(GamePhysicsWorld, "__init__", return_value=None) as initialize: TaxiPhysicsWorld( scene, - TaxiVehicleConfig(), - traffic_density=1.0, - enclosure_segments_world=enclosure, + config, + curb_segments_world=curbs, ) - physics_scene = initialize.call_args.args[0] - assert scene.line_layers == () - assert len(physics_scene.line_layers) == 1 - assert physics_scene.line_layers[0].layer_name == "crazy_robotaxi_enclosure_walls" - np.testing.assert_allclose(physics_scene.line_layers[0].segments_world, enclosure) - assert len(GamePhysicsWorld._build_barriers(physics_scene)) == 1 + physics_scene = initialize.call_args.args[0] + assert scene.line_layers == () + assert physics_scene.line_layers == () + np.testing.assert_array_equal( + initialize.call_args.kwargs["static_barrier_segments_world"], curbs + ) + assert initialize.call_args.kwargs["static_barrier_restitution"] == 0.45 + assert config.collision_restitution == VehicleConfig().collision_restitution def test_taxi_physics_keeps_app_heading_after_contact_resolution() -> None: @@ -239,7 +638,9 @@ class _PhysicsWorld: last_step_timings = None def synchronize_window( - self, center_xy_m: np.ndarray, timestamp_us: int | None = None + self, + center_xy_m: np.ndarray, + timestamp_us: int | None = None, ) -> None: del center_xy_m, timestamp_us @@ -269,7 +670,7 @@ def command_aware_step( sample_chunk_trajectory( start_state=VehicleState(0.0, 0.0, 0.0, 0.0, 5.0, 0.0), start_timestamp_us=0, - command=command, + commands=(command,) * 2, chunk_size=2, chunk_config=ChunkConfig(fps=30), vehicle_config=TaxiVehicleConfig(), @@ -295,9 +696,9 @@ def test_taxi_native_heading_matches_app_heading_after_boundary_contact() -> Non world = TaxiPhysicsWorld( _scene(line_layers=(boundary,)), config, - traffic_density=1.0, + curb_segments_world=boundary.segments_world, ) - initial_yaw = math.radians(15.0) + initial_yaw = math.radians(30.0) state = VehicleState( x_m=-5.0, y_m=-3.0, @@ -310,10 +711,14 @@ def test_taxi_native_heading_matches_app_heading_after_boundary_contact() -> Non ) command = DriverCommand(throttle=1.0, steer_is_direct=True, manual_control=True) contact_detected = False + contact_velocity_y_mps = 0.0 + contact_requested_speed_mps = 0.0 + contact_resolved_speed_mps = 0.0 try: for frame_index in range(90): state = integrate_taxi_vehicle(state, command, 1.0 / 30.0, config) + requested_speed_mps = state.speed_mps state, _ = world.step_with_command( state, command, @@ -323,19 +728,27 @@ def test_taxi_native_heading_matches_app_heading_after_boundary_contact() -> Non native_state = world._world.state_buffer[world._world._ego_slot] native_yaw = _yaw_from_quaternion_xyzw(native_state[3:7]) assert native_yaw == pytest.approx(state.yaw_rad, abs=1.0e-5) - contact_detected |= state.ragdoll_active + if state.ragdoll_active and not contact_detected: + contact_detected = True + contact_velocity_y_mps = float(state.velocity_y_mps or 0.0) + contact_requested_speed_mps = requested_speed_mps + contact_resolved_speed_mps = state.speed_mps if contact_detected and frame_index > 20: break finally: world.close() assert contact_detected is True + assert contact_velocity_y_mps < -0.75 + assert contact_resolved_speed_mps >= ( + contact_requested_speed_mps * config.curb_forward_momentum_retention - 1.0e-5 + ) assert state.yaw_rad == pytest.approx(initial_yaw, abs=1.0e-5) def test_taxi_handbrake_turn_remains_bounded_through_physx() -> None: config = TaxiVehicleConfig(drag_mps2=0.0) - world = TaxiPhysicsWorld(_scene(), config, traffic_density=1.0) + world = TaxiPhysicsWorld(_scene(), config) state = VehicleState( x_m=0.0, y_m=0.0, @@ -378,7 +791,7 @@ def test_taxi_handbrake_turn_remains_bounded_through_physx() -> None: def test_taxi_normal_steering_tracks_arcade_heading_through_physx() -> None: config = TaxiVehicleConfig(drag_mps2=0.0) - world = TaxiPhysicsWorld(_scene(), config, traffic_density=1.0) + world = TaxiPhysicsWorld(_scene(), config) state = VehicleState( x_m=0.0, y_m=0.0, @@ -409,7 +822,7 @@ def test_taxi_normal_steering_tracks_arcade_heading_through_physx() -> None: def test_taxi_acceleration_and_braking_remain_arcade_responsive_through_physx() -> None: config = TaxiVehicleConfig() - world = TaxiPhysicsWorld(_scene(), config, traffic_density=1.0) + world = TaxiPhysicsWorld(_scene(), config) state = VehicleState(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) throttle = DriverCommand(throttle=1.0, steer_is_direct=True, manual_control=True) @@ -446,7 +859,7 @@ def test_taxi_acceleration_and_braking_remain_arcade_responsive_through_physx() def test_taxi_pedal_brake_builds_reverse_speed_through_physx() -> None: config = TaxiVehicleConfig() - world = TaxiPhysicsWorld(_scene(), config, traffic_density=1.0) + world = TaxiPhysicsWorld(_scene(), config) state = VehicleState(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) reverse = DriverCommand(brake=1.0, steer_is_direct=True, manual_control=True) diff --git a/apps/crazy_robotaxi/tests/test_variant_thumbnails.py b/apps/crazy_robotaxi/tests/test_variant_thumbnails.py index 137588c5f..3f082aa89 100644 --- a/apps/crazy_robotaxi/tests/test_variant_thumbnails.py +++ b/apps/crazy_robotaxi/tests/test_variant_thumbnails.py @@ -1,72 +1,72 @@ # SPDX-License-Identifier: Apache-2.0 # SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -"""Per-variant thumbnail discovery feeding the HUD variant dropdown.""" +"""Map thumbnails feeding the HUD variant dropdown.""" from __future__ import annotations -import io -import zipfile +from collections.abc import Callable from pathlib import Path -from omnidreams_game_engine.demo import ( - SCENE_THUMB_SIZE, - _discover_variants, - _load_variant_thumbnails, +import pytest +import yaml +from crazy_robotaxi import cli as crazy_cli +from omnidreams_game_engine import demo as engine_demo + +pytestmark = pytest.mark.ci_cpu + +_MAP = ( + Path(__file__).parents[1] / "crazy_robotaxi" / "maps" / "minimal_loop.robotaxi.yaml" +) + + +@pytest.mark.parametrize( + "build_option", + [crazy_cli._scene_option_for_game_map, engine_demo._scene_option_for_game_map], +) +def test_map_option_uses_authored_seed_image( + build_option: Callable[[Path], object], +) -> None: + option = build_option(_MAP) + + assert option.label == "Minimal Loop and Parking Lot" + assert option.variants == ("default",) + assert option.thumbnail is not None + assert option.thumbnail.size == crazy_cli.SCENE_THUMB_SIZE + assert set(option.variant_thumbnails) == {"default"} + assert option.variant_paths == {"default": _MAP} + + +@pytest.mark.parametrize( + "build_option", + [crazy_cli._scene_option_for_game_map, engine_demo._scene_option_for_game_map], +) +def test_map_option_generates_thumbnail_when_seed_image_is_omitted( + build_option: Callable[[Path], object], tmp_path: Path +) -> None: + document = yaml.safe_load(_MAP.read_text(encoding="utf-8")) + del document["spawns"][0]["variants"]["default"]["image"] + path = tmp_path / "fallback.robotaxi.yaml" + path.write_text(yaml.safe_dump(document, sort_keys=False), encoding="utf-8") + + option = build_option(path) + + assert option.thumbnail is not None + assert option.thumbnail.size == crazy_cli.SCENE_THUMB_SIZE + assert set(option.variant_thumbnails) == {"default"} + + +@pytest.mark.parametrize( + "discover", + [crazy_cli._discover_scene_options, engine_demo._discover_scene_options], ) -from omnidreams_game_engine.scene_fixture import build_synthetic_scene_usdz -from PIL import Image - - -def test_load_variant_thumbnails_one_per_variant(tmp_path: Path) -> None: - # The synthetic bundle ships first_image.png + first_image_1/2.png, so - # each variant must get its own distinctly-rendered preview. - scene_path = build_synthetic_scene_usdz(tmp_path / "scene.usdz", length_frames=60) - # Numbered variants exist, so the bare "default" (a duplicate of "1") is - # dropped and "1" becomes the default selection. - variants = _discover_variants(scene_path) - assert variants == ("1", "2") - - thumbs = _load_variant_thumbnails(scene_path, variants) - assert set(thumbs) == {"1", "2"} - for thumb in thumbs.values(): - assert isinstance(thumb, Image.Image) - assert thumb.size == SCENE_THUMB_SIZE - # The per-variant first images are distinct (variant_1/2 are shifted), - # so the rendered thumbnails must not collapse to one shared image. - rendered = {variant: thumb.tobytes() for variant, thumb in thumbs.items()} - assert len(set(rendered.values())) == 2 - - -def test_discover_variants_default_only_when_unnumbered(tmp_path: Path) -> None: - # A scene with only the bare prompt / first_image (no numbered variants) - # exposes a single "default". - scene_path = tmp_path / "plain.usdz" - buf = io.BytesIO() - Image.new("RGB", (32, 32), color=(5, 5, 5)).save(buf, format="PNG") - with zipfile.ZipFile(scene_path, "w") as zf: - zf.writestr("first_image.png", buf.getvalue()) - zf.writestr("prompt.txt", "a plain scene") - assert _discover_variants(scene_path) == ("default",) - - -def test_load_variant_thumbnails_falls_back_to_default(tmp_path: Path) -> None: - # A bundle with only first_image.png (no per-variant images): every - # requested variant should reuse the single default thumbnail. - scene_path = tmp_path / "default_only.usdz" - buf = io.BytesIO() - Image.new("RGB", (64, 32), color=(10, 120, 200)).save(buf, format="PNG") - with zipfile.ZipFile(scene_path, "w") as zf: - zf.writestr("first_image.png", buf.getvalue()) - - thumbs = _load_variant_thumbnails(scene_path, ("default", "1")) - assert set(thumbs) == {"default", "1"} - assert thumbs["1"] is thumbs["default"] - - -def test_load_variant_thumbnails_missing_images_returns_empty(tmp_path: Path) -> None: - # No first_image*.png at all -> empty mapping (HUD draws text-only rows). - scene_path = tmp_path / "empty.usdz" - with zipfile.ZipFile(scene_path, "w") as zf: - zf.writestr("metadata.yaml", "scene_id: x\n") - assert _load_variant_thumbnails(scene_path, ("default",)) == {} +def test_map_discovery_ignores_archives( + discover: Callable[[Path, Path], tuple[object, ...]], tmp_path: Path +) -> None: + (tmp_path / "recorded.usdz").write_bytes(b"not a map") + + options = discover(tmp_path, _MAP) + + assert options + assert all(option.path.name.endswith(".robotaxi.yaml") for option in options) + assert _MAP.resolve() in {option.path for option in options} diff --git a/apps/crazy_robotaxi/tests/test_world_model_adapter.py b/apps/crazy_robotaxi/tests/test_world_model_adapter.py index 57917d364..cb0adf443 100644 --- a/apps/crazy_robotaxi/tests/test_world_model_adapter.py +++ b/apps/crazy_robotaxi/tests/test_world_model_adapter.py @@ -8,9 +8,11 @@ from types import SimpleNamespace import numpy as np +import omnidreams_game_engine.backends.world_model as world_backend_module import omnidreams_game_engine.world_model.flashdreams_adapter as adapter_module import pytest import torch +from omnidreams_game_engine._pipeline_fakes import make_trajectory from omnidreams_game_engine.backends.world_model import WorldModelRenderBackend from omnidreams_game_engine.config import WorldModelProfileConfig from omnidreams_game_engine.types import PresentedFrame @@ -132,6 +134,87 @@ def test_world_model_merge_preserves_bev_source_pose() -> None: assert merged[0].bev_rig_to_world is bev_pose +def test_world_model_diagnostics_disabled_does_not_check_or_replace_frames( + monkeypatch: pytest.MonkeyPatch, +) -> None: + backend = WorldModelRenderBackend.__new__(WorldModelRenderBackend) + backend._motion_conformance_diagnostics_enabled = False + trajectory = replace( + make_trajectory(3), + static_collision_detected=True, + static_collision_frame_index=1, + ) + model_frames = tuple( + np.full((2, 2, 3), index, dtype=np.uint8) for index in range(3) + ) + merged = tuple( + PresentedFrame( + timestamp_us=index, + rgb_host_uint8=np.zeros((2, 2, 3), dtype=np.uint8), + depth_host_f32=None, + model_rgb_host_uint8=model_frames[index], + ) + for index in range(3) + ) + monkeypatch.setattr( + world_backend_module, + "compare_motion", + lambda *args, **kwargs: pytest.fail("diagnostics should be disabled"), + ) + + frames = backend._annotate_motion_conformance( + trajectory=trajectory, + condition_frames=tuple(frame.rgb_host_uint8 for frame in merged), + model_frames=model_frames, + merged_frames=merged, + ) + + assert frames is merged + assert all( + frame.model_rgb_host_uint8 is model_frames[index] + for index, frame in enumerate(frames) + ) + + +def test_world_model_motion_mismatch_is_diagnostics_only( + monkeypatch: pytest.MonkeyPatch, +) -> None: + backend = WorldModelRenderBackend.__new__(WorldModelRenderBackend) + backend._motion_conformance_diagnostics_enabled = True + mismatch = SimpleNamespace( + mismatched=True, + as_metrics=lambda: {"mismatched": True, "axis": "turn"}, + ) + monkeypatch.setattr( + world_backend_module, "compare_motion", lambda *a, **k: mismatch + ) + trajectory = make_trajectory(3) + model_frames = tuple(np.zeros((2, 2, 3), dtype=np.uint8) for _ in range(3)) + merged = tuple( + PresentedFrame( + index, model_frames[index], None, model_rgb_host_uint8=model_frames[index] + ) + for index in range(3) + ) + + frames = backend._annotate_motion_conformance( + trajectory=trajectory, + condition_frames=model_frames, + model_frames=model_frames, + merged_frames=merged, + ) + + assert all( + frame.model_rgb_host_uint8 is model_frames[index] + for index, frame in enumerate(frames) + ) + assert all( + frame.model_motion_metrics is not None + and frame.model_motion_metrics["mismatched"] is True + for frame in frames + ) + + def test_select_config_name_uses_omnidreams_recipe_slugs() -> None: assert ( _select_config_name(_manifest()) diff --git a/apps/omnidreams_game_engine/GAME_MAP_FUTURE_CONDITIONING.md b/apps/omnidreams_game_engine/GAME_MAP_FUTURE_CONDITIONING.md new file mode 100644 index 000000000..b58c02576 --- /dev/null +++ b/apps/omnidreams_game_engine/GAME_MAP_FUTURE_CONDITIONING.md @@ -0,0 +1,33 @@ +# Future Game-Map Conditioning + +The node-graph map schema currently describes roads, lanes, intersections, +driveways, parking lots, curb barriers, spawns, and visual seed variants. The +following conditioning classes are candidates for future schema versions. +They are not committed additions to the format. + +## Roadside objects + +- Poles, including placement, height, radius, and visual category. +- Traffic signs, including sign type, facing direction, and supporting pole. +- Traffic lights, including signal heads, mounting, orientation, and controlled + approaches or lanes. + +## Road-surface features + +- Wait or stop lines associated with a lane or intersection approach. +- Crosswalk polygons and their relationship to intersection approaches. +- Road islands and medians, including traversability, curb treatment, and + whether they divide opposing traffic. + +## Traffic + +- Authored traffic sources, destinations, routes, spawn rates, and vehicle + classes. +- Rules for connecting authored traffic to lane successors, signals, and game + difficulty settings. +- A controller-owned runtime representation shared by routing, physics, and + rendering. + +Each addition should define authoring semantics first, then specify validation, +compiled conditioning output, preview rendering, collision behavior, and +runtime ownership. diff --git a/apps/omnidreams_game_engine/NODE_GRAPH_MAP_FORMAT.md b/apps/omnidreams_game_engine/NODE_GRAPH_MAP_FORMAT.md new file mode 100644 index 000000000..9eab88eb1 --- /dev/null +++ b/apps/omnidreams_game_engine/NODE_GRAPH_MAP_FORMAT.md @@ -0,0 +1,414 @@ +# Node-Graph Map Format + +Schema version 1 is the authoring format for standalone OmniDreams games. It +models structural places as nodes, public roads as graph edges, and parking +access through driveway relationships. + +## Document shape + +```yaml +schema_version: 1 +id: example-map +name: Example Map +compiler: + sample_spacing_m: 2.0 + ground_margin_m: 20.0 + intersection_connector_samples: 8 +profiles: {} +nodes: [] +roads: [] +traffic_count: 12 +traffic: [] +spawns: [] +``` + +`profiles`, `traffic_count`, and `traffic` are optional. All other root fields +are required, and unknown root fields are errors. + +The compiler settings control road sampling, ground extent, and routing-only +turn-connector resolution. They do not configure the renderer archive. + +## Attributes and profiles + +Profiles are optional, partial sets of defaults. An element may provide any +applicable attribute directly at its top level, reference a profile, or do +both. A directly supplied value wins over the profile value. Profile fields +that do not apply to an element are ignored. + +After combining direct values and profile defaults, every required attribute +must have a value or compilation fails. Identity, pose, topology, and road +geometry are not profile attributes. + +Linear elements use these attributes: + +```yaml +lane_width_m: 3.6 +curb_offset_m: 0.6 +lanes: [backward, forward] +speed_limit_mps: 13.4 +lane_marking: {style: SOLID_GROUP, color: YELLOW} +divider_markings: + - {style: SOLID_GROUP, color: YELLOW} +``` + +There must be one divider marking for every adjacent lane pair. The paved +surface width is `lane_width_m * len(lanes) + 2 * curb_offset_m`. + +Every element emits semantic road-boundary polylines around its surface, +excluding declared connections. Those boundaries are always included in HD-map +conditioning. `curb: true` also makes them physical collision barriers; +`curb: false` leaves them non-colliding. The `curb` attribute defaults to +`true` when neither the element nor its profile supplies it. + +For example, a road can inherit most values while overriding its width: + +```yaml +- id: oak_street + from: west_junction + to: east_junction + profile: neighborhood + lane_width_m: 4.0 +``` + +## Coordinates and topology + +Every node except a parking lot has an explicit map-space pose: + +```yaml +pose: {x_m: 12, y_m: -4} +``` + +`x_m` and `y_m` use metres. Connected road geometry determines every node's +approach directions and footprint orientation. + +The persisted `GameMapTopology` retains typed nodes, roads, derived parking +accesses, and adjacency. The compiler separately derives a +directed lane graph for routing. Routing-only turn connectors are not emitted +into ClipGT map conditioning. + +Each node and edge owns its surface and curb geometry. Connected elements meet +at equal-width openings without overlapping. Unrelated elements may not have +positive-area overlap or share a boundary edge; isolated point tangency is +allowed. Roads, parking lots, and other surfaces therefore cannot be layered +over one another to repair topology. + +## Nodes + +All non-parking nodes require `id`, `type`, and `pose`. Their remaining required +attributes may be supplied directly or by profile. + +### Intersections + +An intersection connects at least three incident road arms and has no required +attributes beyond its identity and pose: + +```yaml +- id: askew_junction + type: intersection + pose: {x_m: 0, y_m: 0} + lane_transition_length_m: 20 +``` + +Use a road joint for a degree-two connection and a cul-de-sac for a degree-one +road ending; one- and two-arm intersections are rejected. The compiler infers +the intersection footprint from its incident roads and access paths. Each +opening uses that element's paved width and endpoint tangent. +Adjacent road-edge lines determine how far each arm must reach, so orthogonal +roads form a compact rectangular junction while acute approaches extend far +enough to meet without gaps. Intersection dimensions and arm lengths are not +authored. Road centerlines determine their endpoint tangents independently of +node rotation. + +`lane_transition_length_m` is optional and defaults to zero. For each pair of +opposing through-road arms, the compiler independently selects the cross-section +with the greater lane count (or wider lanes when the counts match) at the +intersection. A narrower arm then widens over this distance: its incoming lane +splits before the intersection and its outgoing local lanes merge after it. +Perpendicular through roads are paired separately, so a north-south lane-count +change does not add lanes to a matching east-west street. The taper is part of +the intersection surface and its lanes and markings are conditioning-visible. +Each approach retains its own `curb_offset_m` outside the changing lane +envelope, and its authored curb mode controls the physical curb along the taper. + +Opposing arms are inferred from their endpoint tangents; authors do not label +through-road pairs. A positive transition length is required only when a pair +changes lane count or lane width. It is measured into the authored road from +the inferred intersection opening and must not consume the complete road arm. + +### Road joints + +A road joint connects exactly two compatible authored roads without creating an +intersection: + +```yaml +- id: diagonal_bend + type: road_joint + pose: {x_m: 40, y_m: 20} + lane_transition_length_m: 20 +``` + +The compiler independently infers the shortest trim on each incident road from +the roads' endpoint tangents and paved widths. It replaces those portions with +one tangent-continuous cubic Bézier and traces the joint surface from the +resulting roadside boundaries. The outside boundary remains curved rather than +forming a straight miter between the two approaches. Curved `path` and `bezier` +approaches are supported. + +To author a longer curve, place a `bezier` road between two road joints. The +joints provide the minimal tangent connections while the road owns the extended +curve geometry. + +`lane_transition_length_m` is optional and defaults to zero. It permits the two +roads to differ in lane count, lane width, or both. The joint uses the dominant +cross-section at the curve, then tapers into each narrower incident road over +the authored distance. As at an intersection, an incoming narrow lane splits +toward the joint and outgoing local lanes merge into the narrow road. The taper +is measured along the incident road after its inferred joint trim, including +across curved `path` or `bezier` approaches. + +When oriented through the joint, both roads must still have compatible +direction ordering and opposing dividers. Speed limits, outer markings, curb +offsets, and curb modes may differ. Each approach keeps its authored curb offset +outside the changing lane envelope throughout its taper; adding lanes never +widens or narrows that offset. A lane-count or lane-width change requires a +positive `lane_transition_length_m`; otherwise its default of zero preserves +the previous exact-width behavior. The joint and taper emit +conditioning-visible lanes and markings, and each directed joint lane inherits +its incoming road's speed. Inferred curve trims or lane transitions that consume +an entire road or produce invalid or overlapping geometry are errors. + +### Cul-de-sacs + +A cul-de-sac requires `culdesac_radius_m` and must terminate exactly one road: + +```yaml +- id: oak_court_end + type: cul_de_sac + pose: {x_m: 80, y_m: 20} + culdesac_radius_m: 10 +``` + +Its circular surface has a flat opening matching the incident road width. The +circle has no visible centerline or lane divisions and derives a routing-only +turnaround. + +### Parking lots + +A parking lot is an absolute map-space polygon. It has no pose, profile, or +linear attributes: + +```yaml +- id: market_lot + type: parking_lot + connected_to: market_west_driveway + opening_vertex: 3 + vertices: + - {x_m: 10, y_m: -30} + - {x_m: 10, y_m: -10} + - {x_m: 18, y_m: -10} + - {x_m: 26, y_m: -10} + - {x_m: 40, y_m: -10} + - {x_m: 40, y_m: -30} +``` + +Vertices must describe a simple clockwise polygon. Concave polygons are +supported; holes, self-intersections, duplicate vertices, and degenerate edges +are not. `connected_to` must name an intersection or driveway node. +`opening_vertex` is one-based and selects the complete polygon edge from that +vertex to the next, wrapping from the final vertex to the first. Authors may +insert vertices around a narrower opening. The lot has physical curbs and +semantic boundaries on every edge except its selected access opening. +It has no inferred aisle or turnaround lanes. Its surface becomes a green +ClipGT roadnet mask; parking-stall lines are not generated. + +### Driveways + +A driveway is a degree-two road node with one parking access: + +```yaml +- id: market_west_driveway + type: driveway + pose: {x_m: 17, y_m: -7} +``` + +Its two roads must have compatible cross-sections, markings, and curb modes. +The compiler infers a minimal through-road surface large enough to contain the +curb opening, preserves conditioning-visible through lanes, and adds hidden +turn connectors to the access. A driveway is not emitted as an intersection. +Its entrance width comes from the selected parking-lot polygon edge. + +## Road geometry + +An authored road is one topological edge between intersections, road joints, +driveways, and/or cul-de-sacs: + +```yaml +- id: oak_street + from: west_junction + to: east_junction + profile: neighborhood +``` + +It uses the linear attributes. Without `path` or `bezier`, its centerline is +the straight segment between node poses. A self-loop therefore requires one of +those fields. + +Each authored road has one uniform cross-section. To change lane count or lane +width along a contiguous street, end one road and begin another at a road joint, +then set the joint's `lane_transition_length_m`. Intersections provide the same +transition independently for each inferred through-road pair. + +For normal hand-authored maps, `path` is a list of map-space points the road +centerline passes through. The `from` node pose is the implicit first point and +the `to` node pose is the implicit final point. The compiler derives smooth +cubic spans through the authored points. + +```yaml +- id: river_road + from: west_junction + to: east_junction + profile: neighborhood + path: + - {x_m: 45, y_m: 15} + - {x_m: 70, y_m: 5} +``` + +The resulting centerline is: + +```text +west_junction pose -> (45, 15) -> (70, 5) -> east_junction pose +``` + +Intermediate path points are geometry only; they do not become graph nodes. + +For imported or precision-authored geometry, `bezier` supplies exact cubic +Bézier spans. Each span starts at the previous endpoint and has exactly two +control points plus an endpoint: + +```yaml +- id: imported_curve + from: west_junction + to: east_junction + profile: neighborhood + bezier: + - control_points: [{x_m: 20, y_m: 0}, {x_m: 35, y_m: 12}] + end: {x_m: 45, y_m: 15} + - control_points: [{x_m: 55, y_m: 18}, {x_m: 70, y_m: 5}] + end: {x_m: 80, y_m: 5} +``` + +An `end` closes its span and becomes the next span's implicit start. The final +`end` must match the `to` node pose within 0.05m. Control points pull the curve +toward themselves; the centerline does not generally pass through them. + +A road may include both fields. Both must be valid, and `bezier` determines the +compiled geometry when present. This lets a generated or precision-authored +curve override a simpler editable `path` without conflating the two formats. + +## Inferred parking access + +The parking lot's `connected_to` and `opening_vertex` fields generate a +boundary-to-boundary access span; authors do not declare a separate road or +top-level access object. The compiler derives its stable identifier from the +lot identifier. + +The compiler infers a tangent cubic to the opening midpoint and validates that +the connected node is outside the lot on the edge's exterior side. The exact +opening width becomes two equal opposing lanes with no shoulder, virtual white +markings, physical curbs, and a 5.5m/s speed limit. Intersection connections +include the access as an inferred footprint arm. Access lanes end at the lot +boundary; parking lots contain no internal routing lanes. + +## NPC traffic + +The optional `traffic` list defines vehicles that continuously follow the +compiled public-road lane graph: + +```yaml +traffic: + - id: neighborhood_car + nodes: [west_junction, central_intersection, east_junction] + end_behavior: reverse + vehicle_type: car + speed_mps: 11 + start_distance_m: 20 +``` + +`traffic_count` optionally fixes the final number of NPC vehicles. When it is +omitted, the compiler uses the authored `traffic` list unchanged. It must be a +nonnegative integer at least as large as the authored list; a smaller value is +a conflict and fails compilation. When it is larger, the compiler fills the +difference with deterministic default cars distributed across legal public-road +loops and cul-de-sac routes. Generated cars avoid playable spawns and unsafe +initial overlap. Compilation fails with the map's safe capacity when the +requested count cannot be placed. + +The compiled fleet advances logically across the full public-road graph, but +only graph-nearby vehicles enter PhysX and HD-map conditioning. While the ego +is on a road, nearby starts at both endpoint nodes; while the ego is on a node, +it starts at that node. The neighborhood includes public roads attached to +those nodes, the nodes at the other ends of those roads, and every public road +attached to that expanded node set. It stops before adding otherwise-unreached +nodes at the far ends of that final road ring. Leaving mapped road/node +surfaces retains the last valid neighborhood. Invisible vehicles continue +following their routes, speed limits, and same-direction headway. + +`nodes` requires at least two non-parking nodes. Consecutive nodes do not need +to be adjacent: the compiler selects the shortest routable sequence of public +roads and rejects routes that cannot be connected. Parking accesses and +parking-lot interiors are never considered. At each intersection, traffic uses +the rightmost lane for right turns, the leftmost lane for left turns, and +preserves its relative lane for straight travel. Lane-count changes are joined +with a smooth lateral transition. + +`end_behavior: wrap` routes from the final node back to the first without +teleporting. `reverse` traverses the waypoint list in the opposite order while +the vehicle continues to drive forward; the resulting route must have a legal +turnaround. A cul-de-sac endpoint supplies one automatically. + +`vehicle_type` is optional and defaults to `car`; accepted values are `car`, +`truck`, and `bus`. Their dimensions can be overridden with +`dimensions_lwh_m: [length, width, height]`. `speed_mps` is an optional cap on +road speed limits. `start_distance_m` offsets the initial position along the +compiled cyclic route and defaults to zero. Vehicles are physical, collidable, +and maintain simple same-lane headway; traffic signals and right-of-way are not +currently modeled. + +## Spawns and visual variants + +A spawn names an authored road lane and a distance along its directed +centerline. Lane indices follow the effective `lanes` order. + +```yaml +spawns: + - id: taxi_start + road: oak_street + lane: 1 + distance_m: 5 + variants: + default: + image: seed.png + prompt: A forward-facing taxi view in a quiet neighborhood at daylight. +``` + +Every spawn requires a `default` variant. `image` is optional; when omitted (or +set to `null`), the compiler generates a deterministic synthetic first-person +view by projecting the semantic map from that spawn through the runtime front +camera. This fallback shows aligned road surfaces, boundaries, curbs, and +markings, but does not synthesize scenery. Use it as a robust placeholder, not +as a photorealistic authoring result. + +Authored images may be map-relative paths or `package://package/resource` +references. Resolved geometry, compiler and fallback-renderer code, seed +images, and prompts participate in the compiled-map cache key. + +## Validation summary + +Compilation rejects unknown fields and references, missing effective +attributes, duplicate element identifiers, invalid endpoint types, malformed +or discontinuous road paths, invalid node degrees, invalid driveway +relationships, invalid parking polygons or openings, overlapping or +edge-sharing unrelated surfaces, overlapping connected surfaces, mismatched +connection openings, incompatible lane-transition cross-sections, transitions +that consume a road arm, and parking accesses placed on an opening's interior +side. diff --git a/apps/omnidreams_game_engine/README.md b/apps/omnidreams_game_engine/README.md index f263442b7..87ac394be 100644 --- a/apps/omnidreams_game_engine/README.md +++ b/apps/omnidreams_game_engine/README.md @@ -1,9 +1,10 @@ # OmniDreams Game Engine This package owns the reusable scene, simulation, input, conditioning, -presentation, and legacy inference runtime used by standalone OmniDreams games. -It is intentionally independent of `omnidreams.interactive_drive` so game -development can diverge without changing the enterprise demo. +presentation, and inference runtime used by standalone OmniDreams games. +Games inject an application policy into `InteractiveDriveApp`. -Games inject an application policy into `InteractiveDriveApp`; the historical -class name is retained temporarily to keep the proven runtime behavior stable. +Standalone games may author semantic road networks with the engine's +[node-graph map format](NODE_GRAPH_MAP_FORMAT.md). The schema keeps authored +topology, generated surfaces, curb collision geometry, and the derived directed +lane graph as separate runtime concepts. diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/README.md b/apps/omnidreams_game_engine/omnidreams_game_engine/README.md index f263442b7..16ae257f9 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/README.md +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/README.md @@ -1,9 +1,5 @@ # OmniDreams Game Engine This package owns the reusable scene, simulation, input, conditioning, -presentation, and legacy inference runtime used by standalone OmniDreams games. -It is intentionally independent of `omnidreams.interactive_drive` so game -development can diverge without changing the enterprise demo. - -Games inject an application policy into `InteractiveDriveApp`; the historical -class name is retained temporarily to keep the proven runtime behavior stable. +presentation, and inference runtime used by standalone OmniDreams games. +Games inject an application policy into `InteractiveDriveApp`. diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/_pipeline_fakes.py b/apps/omnidreams_game_engine/omnidreams_game_engine/_pipeline_fakes.py index d75769c5e..bdef9a807 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/_pipeline_fakes.py +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/_pipeline_fakes.py @@ -80,7 +80,11 @@ def make_trajectory(chunk_size: int) -> TrajectoryChunk: class FakeVideoModelBackend: """Deterministic backend stub used by pipeline and loop tests.""" - def __init__(self, frames_per_render: int, rgb_value: int = 0) -> None: + def __init__( + self, + frames_per_render: int, + rgb_value: int = 0, + ) -> None: self._frames_per_render = frames_per_render self._rgb_value = rgb_value self.warmup_model_calls = 0 diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/app.py b/apps/omnidreams_game_engine/omnidreams_game_engine/app.py index 92ac32a65..3b19fca8e 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/app.py +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/app.py @@ -14,6 +14,7 @@ from omnidreams_game_engine.application import InteractiveDriveApplication from omnidreams_game_engine.backends.base import RenderBackend from omnidreams_game_engine.config import AppConfig +from omnidreams_game_engine.game_map import compile_game_map from omnidreams_game_engine.input.keyboard import ( KeyboardInputBackend, KeyboardState, @@ -31,14 +32,12 @@ from omnidreams_game_engine.simulation.ego_vehicle_kinematics import ( EgoVehicleKinematics, build_ground_snapper, - build_map_bounds, integrate_vehicle, state_from_initial_pose, step_physics_world, ) from omnidreams_game_engine.simulation.game_physics import GamePhysicsWorld from omnidreams_game_engine.simulation.ground_snap import GroundSnapper -from omnidreams_game_engine.simulation.map_bounds import MapBounds from omnidreams_game_engine.streaming_presenter import ( MJPEGStreamingPresenter, parse_bind, @@ -58,8 +57,7 @@ class InteractiveDriveApp: The backend, video-model adapter and :class:`ChunkPipeline` are built once in :meth:`__init__`; the pipeline worker starts warming the - scene-independent model immediately, overlapping the model load with - any scene-selection wait. Each scene the user picks is bound via + scene-independent model immediately. Each scene is bound via :meth:`load_scene` and driven by :meth:`run_scene`. The warmed model stays resident across scene changes, so switching scenes never re-pays the warmup/compile cost (only the per-scene geometry upload). @@ -124,7 +122,6 @@ def __init__( ) self._pipeline = ChunkPipeline(self._adapter, trace_context=self._trace_context) self._scene: SceneBundle | None = None - self._map_bounds: MapBounds | None = None # Ground snapper for the current scene. Built once per scene (its # spatial grid is invariant across rollouts) and reused, so a reset # doesn't rebuild it -- that pure-Python grid build can take seconds @@ -141,14 +138,13 @@ def __init__( self._cache_scenes = False self._scene_cache: dict[ tuple[str, str, str | None], - tuple[SceneBundle, MapBounds | None, GroundSnapper | None], + tuple[SceneBundle, GroundSnapper | None], ] = {} # Parsed geometry per base scene path. Weather variants share all # geometry and differ only in seed frame + prompt, so we parse once and # re-seed per variant rather than re-parsing the USDZ each switch. - self._geometry_cache: dict[ - str, tuple[SceneBundle, MapBounds | None, GroundSnapper | None] - ] = {} + self._geometry_cache: dict[str, tuple[SceneBundle, GroundSnapper | None]] = {} + self._forced_map_recompiles: set[Path] = set() self._scene_cache_lock = threading.Lock() # Set while --preload-scenes is still parsing scenes in the # background; the presenter locks scene selection until it clears so @@ -204,9 +200,9 @@ def load_scene( # indicator still covers that part. cached = self._cached_scene(scene_path, variant, prompt_override) if cached is not None: - self._scene, self._map_bounds, self._ground_snapper = cached + self._scene, self._ground_snapper = cached if self._application is not None: - self._application.load_scene(self._scene, self._map_bounds) + self._application.load_scene(self._scene) self._pipeline.request_scene(self._scene) return True @@ -216,9 +212,8 @@ def load_scene( def _parse() -> None: try: - # Map bounds + ground snapper are geometry-derived; built here - # on the background thread and cached so resets and variant - # switches don't rebuild them. + # The ground snapper is geometry-derived; build it on the + # background thread and cache it across resets and variants. loaded.extend( self._resolve_scene_assets(scene_path, variant, prompt_override) ) @@ -241,17 +236,15 @@ def _parse() -> None: # one rollout from the previous selection before the outer loop sees # ``pending_scene_change``. return False - self._scene, self._map_bounds, self._ground_snapper = ( # type: ignore[assignment] + self._scene, self._ground_snapper = ( # type: ignore[assignment] loaded[0], loaded[1], - loaded[2], ) self._store_scene( scene_path, variant, prompt_override, self._scene, - self._map_bounds, self._ground_snapper, ) if self._presenter.should_close: @@ -260,7 +253,7 @@ def _parse() -> None: # close/requested state for the outer loop to consume. return False if self._application is not None: - self._application.load_scene(self._scene, self._map_bounds) + self._application.load_scene(self._scene) self._pipeline.request_scene(self._scene) return True @@ -305,7 +298,7 @@ def _preload_worker(self, pending: list[tuple[object, str, str | None]]) -> None try: # Reuses cached geometry, so only the first variant of each # scene pays the full parse; the rest are cheap re-seeds. - scene, bounds, snapper = self._resolve_scene_assets( + scene, snapper = self._resolve_scene_assets( scene_path, variant, prompt_override ) except BaseException as exc: # noqa: BLE001 - log & skip one scene @@ -315,7 +308,7 @@ def _preload_worker(self, pending: list[tuple[object, str, str | None]]) -> None ) continue with self._scene_cache_lock: - self._scene_cache[key] = (scene, bounds, snapper) + self._scene_cache[key] = (scene, snapper) logger.info( f"[interactive-drive] preloaded scene " f"{Path(str(scene_path)).name} variant={variant!r}", @@ -323,36 +316,52 @@ def _preload_worker(self, pending: list[tuple[object, str, str | None]]) -> None def _resolve_scene_assets( self, scene_path: object, variant: str, prompt_override: str | None - ) -> tuple[SceneBundle, MapBounds | None, GroundSnapper | None]: - """Resolve ``(scene, map_bounds, ground_snapper)``, caching geometry per scene. + ) -> tuple[SceneBundle, GroundSnapper | None]: + """Resolve ``(scene, ground_snapper)``, caching geometry per scene. - First variant of a scene: full parse + build bounds/snapper, then cache. + First variant of a scene: full parse + build snapper, then cache. Later variants: re-seed the cached bundle (frame + prompt only). """ + source_path = Path(str(scene_path)) + canonical_source = source_path.expanduser().resolve() + force_recompile = False + if self._config.force_map_recompile: + with self._scene_cache_lock: + if canonical_source not in self._forced_map_recompiles: + self._forced_map_recompiles.add(canonical_source) + force_recompile = True + try: + renderer_path = compile_game_map( + source_path, force=force_recompile + ).archive_path + except BaseException: + if force_recompile: + with self._scene_cache_lock: + self._forced_map_recompiles.discard(canonical_source) + raise geometry = self._cached_geometry(scene_path) if geometry is not None: - base_scene, map_bounds, ground_snapper = geometry + base_scene, ground_snapper = geometry scene = reseed_scene_bundle( base_scene, - Path(str(scene_path)), + renderer_path, self._config.camera_name, variant, prompt_override, self._config.raster, ) - return scene, map_bounds, ground_snapper + return scene, ground_snapper scene = load_scene_bundle( - scene_path=scene_path, + scene_path=renderer_path, camera_name=self._config.camera_name, variant=variant, prompt_override=prompt_override, raster=self._config.raster, ) - map_bounds = build_map_bounds(scene) ground_snapper = build_ground_snapper(scene) - self._store_geometry(scene_path, scene, map_bounds, ground_snapper) - return scene, map_bounds, ground_snapper + self._store_geometry(scene_path, scene, ground_snapper) + return scene, ground_snapper @staticmethod def _scene_cache_key( @@ -362,7 +371,7 @@ def _scene_cache_key( def _cached_scene( self, scene_path: object, variant: str, prompt_override: str | None - ) -> tuple[SceneBundle, MapBounds | None, GroundSnapper | None] | None: + ) -> tuple[SceneBundle, GroundSnapper | None] | None: if not self._cache_scenes: return None with self._scene_cache_lock: @@ -376,7 +385,6 @@ def _store_scene( variant: str, prompt_override: str | None, scene: SceneBundle, - map_bounds: MapBounds | None, ground_snapper: GroundSnapper | None, ) -> None: if not self._cache_scenes: @@ -384,11 +392,11 @@ def _store_scene( with self._scene_cache_lock: self._scene_cache[ self._scene_cache_key(scene_path, variant, prompt_override) - ] = (scene, map_bounds, ground_snapper) + ] = (scene, ground_snapper) def _cached_geometry( self, scene_path: object - ) -> tuple[SceneBundle, MapBounds | None, GroundSnapper | None] | None: + ) -> tuple[SceneBundle, GroundSnapper | None] | None: # Always on, unlike the --preload-scenes-gated ``_scene_cache``: one # bundle per scene lets a live variant switch re-seed, not re-parse. with self._scene_cache_lock: @@ -398,14 +406,11 @@ def _store_geometry( self, scene_path: object, scene: SceneBundle, - map_bounds: MapBounds | None, ground_snapper: GroundSnapper | None, ) -> None: # First parse of a scene wins; later variants re-seed off it. with self._scene_cache_lock: - self._geometry_cache.setdefault( - str(scene_path), (scene, map_bounds, ground_snapper) - ) + self._geometry_cache.setdefault(str(scene_path), (scene, ground_snapper)) def _pump_presenter_until(self, done: threading.Event) -> None: """Pump events + a loading overlay until ``done`` is set or we close. @@ -442,10 +447,10 @@ def run_scene(self) -> None: ``run_main_loop`` reports the presenter wants to close -- which the slangpy HUD also uses to signal a scene change -- so the caller inspects ``presenter.pending_scene_change`` to tell the two apart. - A manual reset / OOB respawn keeps the loop going with a fresh - simulation and ``pipeline.reset`` (the warmed model is kept). + A manual reset keeps the loop going with a fresh simulation and + ``pipeline.reset`` (the warmed model is kept). """ - if self._scene is None or self._map_bounds is None: + if self._scene is None: raise RuntimeError("load_scene() must be called before run_scene()") if self._application is not None: self._application.configure_scene_presenter(self._presenter, self._scene) @@ -460,8 +465,7 @@ def run_scene(self) -> None: depth_host_f32=None, ) # First rollout is the scene load ("Loading scene..." / "Loading - # world model..."); subsequent rollouts come from a manual reset or - # OOB respawn, so switch the indicator to "Resetting..." for those. + # world model..."); subsequent rollouts come from a manual reset. loading_status = self._loading_status_message while not self._presenter.should_close: if self._application is None: @@ -500,9 +504,6 @@ def run_scene(self) -> None: vehicle_config=vehicle_config, ground_snapper=ground_snapper, initial_timestamp_us=self._scene.initial_timestamp_us, - map_bounds=self._map_bounds, - oob_margin_m=self._config.oob_margin_m, - oob_warning_zone_m=self._config.oob_warning_zone_m, scene=self._scene, integrate_fn=integrate_fn, physics_world_factory=physics_world_factory, @@ -518,7 +519,7 @@ def run_scene(self) -> None: ) # Publish the freshly-built initial state up front so read-side # speed readouts (the HUD speed digit, the browser ``/state`` - # endpoint) reflect a reset / respawn immediately. Without this + # endpoint) reflect a reset immediately. Without this # the last telemetry from the previous rollout would linger on # screen through the "Resetting..." window until the new rollout # requested its first chunk -- the "reset doesn't reset the @@ -540,11 +541,6 @@ def run_scene(self) -> None: initial_chunk_size=self._config.chunk.initial_chunk_frames, chunk_size=self._config.chunk.chunk_frames, frame_interval_s=self._config.chunk.frame_interval_s, - oob_warn_proximity=self._config.oob_warn_proximity, - oob_respawn_proximity=self._config.oob_respawn_proximity, - oob_respawn_debounce_chunks=( - self._config.oob_respawn_debounce_chunks - ), stop_after_consumed_chunks=( self._config.stop_after_consumed_chunks ), @@ -596,7 +592,7 @@ def _loading_status_message(self) -> str: return "Loading scene..." def _resetting_status_message(self) -> str: - """Phase text shown while a reset / respawn re-primes the rollout.""" + """Phase text shown while a reset re-primes the rollout.""" return "Resetting..." def run(self) -> None: diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/application.py b/apps/omnidreams_game_engine/omnidreams_game_engine/application.py index 85b953238..1e1696bb0 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/application.py +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/application.py @@ -27,7 +27,6 @@ ) from omnidreams_game_engine.simulation.game_physics import GamePhysicsWorld from omnidreams_game_engine.simulation.ground_snap import GroundSnapper -from omnidreams_game_engine.simulation.map_bounds import MapBounds from omnidreams_game_engine.types import ( DriverCommand, SceneBundle, @@ -120,7 +119,7 @@ def configure_presenter(self, presenter: Any) -> None: """Configure application-aware presentation before a scene loads.""" ... - def load_scene(self, scene: SceneBundle, map_bounds: MapBounds | None) -> None: + def load_scene(self, scene: SceneBundle) -> None: """Load application-specific data for ``scene``.""" ... diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/assets/README.md b/apps/omnidreams_game_engine/omnidreams_game_engine/assets/README.md index d939b3f10..7a392f24a 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/assets/README.md +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/assets/README.md @@ -1,8 +1,6 @@ # Assets -This directory holds the unpacked-scene-bundle loader -(`scene_bundle.py`) plus the bundled HUD control sprites under -`wheel_and_pedals/`. +This directory holds packaged assets used by the game engine. ## `wheel_and_pedals/` @@ -20,10 +18,8 @@ the brake PNGs are also accepted under AlpaSim's `break_*.png` spelling. When a sprite is missing, the HUD falls back to a CPU-rendered vector wheel / fill-bar pedals. -## Scenes +## Game-map seed images -Scene USDZs themselves are staged into the shared `omnidreams` scene -cache under `$FLASHDREAMS_CACHE_DIR/omnidreams-scenes/`, **not** here. -See `omnidreams.scenes` and `omnidreams-prepare` for how staging -works; both the desktop demo and the WebRTC server consume from the -same cache root. +Authored maps can reference packaged images with +`package://omnidreams_game_engine/path/to/image`. The compiler embeds the +selected images and prompts in its private runtime archive. diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/backends/base.py b/apps/omnidreams_game_engine/omnidreams_game_engine/backends/base.py index 1c34c0bf3..4cb0947a8 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/backends/base.py +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/backends/base.py @@ -30,9 +30,8 @@ def chunk_frames(self) -> int: def can_prewarm(self) -> bool: """Whether :meth:`warmup_model` does its heavy build without a scene. - ``True`` lets the demo start loading the model immediately at - launch, overlapping warmup with the scene-selection wait. ``False`` - means the build is deferred until the first :meth:`load_scene` + ``True`` lets the demo start loading the model immediately at launch. + ``False`` means the build is deferred until the first :meth:`load_scene` (e.g. the world model under ``--offload-text-encoder``, which must precompute per-scene embeddings and free the one-shot encoders before allocating the diffusion pipeline to keep peak VRAM low). diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/backends/world_model.py b/apps/omnidreams_game_engine/omnidreams_game_engine/backends/world_model.py index dbecf2618..b811c8725 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/backends/world_model.py +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/backends/world_model.py @@ -4,8 +4,10 @@ from __future__ import annotations import hashlib +import math import time from collections.abc import Sequence +from dataclasses import replace from pathlib import Path import numpy as np @@ -21,6 +23,7 @@ RasterConfig, WorldModelProfileConfig, ) +from omnidreams_game_engine.motion_conformance import compare_motion from omnidreams_game_engine.rasterizer import LudusConditionRasterizer from omnidreams_game_engine.types import ( FrameChunk, @@ -49,6 +52,7 @@ def __init__( postprocess: VideoPostprocessChainConfig | None = None, *, synchronize_bev_with_rgb: bool = False, + motion_conformance_diagnostics_enabled: bool = False, ) -> None: super().__init__(chunk=chunk, raster=raster) self._manifest = manifest @@ -65,6 +69,9 @@ def __init__( ) self._scene: SceneBundle | None = None self._next_chunk_count = 0 + self._motion_conformance_diagnostics_enabled = bool( + motion_conformance_diagnostics_enabled + ) self._debug_first_chunk_condition_frames: tuple[np.ndarray, ...] | None = None @property @@ -230,6 +237,12 @@ def render_next_chunk(self, trajectory: TrajectoryChunk) -> FrameChunk: model_frames = self._session.continue_generation(condition_frames) model_end = time.perf_counter() merged_frames = self._merge_frames(raster_chunk.frames, model_frames) + merged_frames = self._annotate_motion_conformance( + trajectory=trajectory, + condition_frames=condition_frames, + model_frames=model_frames, + merged_frames=merged_frames, + ) merge_end = time.perf_counter() self._next_chunk_count += 1 total_ms = (merge_end - chunk_start) * 1000.0 @@ -280,6 +293,42 @@ def render_next_chunk(self, trajectory: TrajectoryChunk) -> FrameChunk: ), ) + def _annotate_motion_conformance( + self, + *, + trajectory: TrajectoryChunk, + condition_frames: Sequence[object], + model_frames: Sequence[object], + merged_frames: tuple[PresentedFrame, ...], + ) -> tuple[PresentedFrame, ...]: + """Attach opt-in diagnostics without changing generated presentation.""" + if not self._motion_conformance_diagnostics_enabled: + return merged_frames + start = trajectory.vehicle_states[0] + end = trajectory.vehicle_states[-1] + yaw_delta = (end.yaw_rad - start.yaw_rad + math.pi) % (2.0 * math.pi) - math.pi + longitudinal_delta = (end.x_m - start.x_m) * math.cos(start.yaw_rad) + ( + end.y_m - start.y_m + ) * math.sin(start.yaw_rad) + try: + result = compare_motion( + condition_frames, + model_frames, + yaw_delta_rad=yaw_delta, + longitudinal_delta_m=longitudinal_delta, + ) + metrics = result.as_metrics() + if result.mismatched: + logger.warning( + "[world-model] diagnostic motion mismatch metrics={}", metrics + ) + except Exception as exc: # noqa: BLE001 - diagnostics must not stop play + logger.warning(f"[world-model] motion conformance skipped: {exc!r}") + metrics = {"mismatched": False, "axis": "error"} + return tuple( + replace(frame, model_motion_metrics=metrics) for frame in merged_frames + ) + def reset(self) -> None: self._session.reset() self._next_chunk_count = 0 diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/camera_defaults.py b/apps/omnidreams_game_engine/omnidreams_game_engine/camera_defaults.py new file mode 100644 index 000000000..bcd16b7f7 --- /dev/null +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/camera_defaults.py @@ -0,0 +1,137 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Canonical front-camera calibration for compiled semantic maps.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import numpy as np + +from omnidreams_game_engine.math3d import ( + euler_xyz_degrees_to_matrix, + transform_from_rt, +) + +if TYPE_CHECKING: + from omnidreams_game_engine.types import CameraCalibration + +DEFAULT_FRONT_CAMERA_CLIPGT_NAME = "camera:front:wide:120fov" +"""ClipGT sensor name embedded in compiled semantic-map archives.""" + +DEFAULT_FRONT_CAMERA_LOGICAL_NAME = "camera_front_wide_120fov" +"""Filesystem-safe name for the canonical front camera.""" + +DEFAULT_FIRST_FRAME_RESOLUTION_WH = (1280, 704) +"""Pixel resolution used by generated and authored first frames.""" + +_NATIVE_RESOLUTION_WH = (3848, 2168) +_PRINCIPAL_POINT_XY = (1921.318705874846, 1076.978854184438) +_POLYNOMIAL = ( + 0.0, + 0.0005385247479413695, + -1.598462177407655e-09, + 6.250864794463573e-12, + -2.194585699335322e-15, + 4.525222700710391e-19, +) +_POLYNOMIAL_TEXT = ( + "0 0.0005385247479413695 -1.598462177407655e-09 " + "6.250864794463573e-12 -2.194585699335322e-15 " + "4.525222700710391e-19" +) +_NOMINAL_RPY_DEG = ( + 0.292217969894409, + 0.464194804430008, + -0.191304489970207, +) +_CORRECTION_RPY_DEG = ( + -0.1592078059911728, + 0.11539523303508759, + 0.5026581287384033, +) +_NOMINAL_TRANSLATION_M = ( + 1.69035196304321, + 0.00553808081895113, + 1.45306670665741, +) +_CORRECTION_TRANSLATION_M = ( + -0.057110343128442764, + -0.0032010308932513, + 0.008508340455591679, +) + + +def default_front_camera_calibration() -> CameraCalibration: + """Build the canonical compiled-map front-camera calibration.""" + from omnidreams_game_engine.types import CameraCalibration + + nominal_rotation = euler_xyz_degrees_to_matrix(_NOMINAL_RPY_DEG) + correction_rotation = euler_xyz_degrees_to_matrix(_CORRECTION_RPY_DEG) + rotation = (nominal_rotation @ correction_rotation).astype(np.float32) + translation = np.asarray(_NOMINAL_TRANSLATION_M, dtype=np.float32) + np.asarray( + _CORRECTION_TRANSLATION_M, dtype=np.float32 + ) + return CameraCalibration( + clipgt_name=DEFAULT_FRONT_CAMERA_CLIPGT_NAME, + logical_name=DEFAULT_FRONT_CAMERA_LOGICAL_NAME, + width=_NATIVE_RESOLUTION_WH[0], + height=_NATIVE_RESOLUTION_WH[1], + cx=_PRINCIPAL_POINT_XY[0], + cy=_PRINCIPAL_POINT_XY[1], + polynomial=np.asarray(_POLYNOMIAL, dtype=np.float32), + is_backward_polynomial=True, + linear_cde=np.asarray([1.0, 0.0, 0.0], dtype=np.float32), + sensor_to_rig_flu=transform_from_rt(rotation, translation.tolist()), + ) + + +def default_front_camera_rig() -> dict[str, object]: + """Build the ClipGT rig record for the canonical front camera.""" + return { + "rig": { + "properties": {}, + "vehicle": {}, + "vehicleio": {}, + "sensors": [ + { + "name": DEFAULT_FRONT_CAMERA_CLIPGT_NAME, + "protocol": "camera.virtual", + "parameter": ("video=synthetic/camera_front_wide_120fov.mp4"), + "nominalSensor2Rig_FLU": { + "roll-pitch-yaw": list(_NOMINAL_RPY_DEG), + "t": list(_NOMINAL_TRANSLATION_M), + }, + "correction_sensor_R_FLU": { + "roll-pitch-yaw": list(_CORRECTION_RPY_DEG), + }, + "correction_rig_T": list(_CORRECTION_TRANSLATION_M), + "properties": { + "width": str(_NATIVE_RESOLUTION_WH[0]), + "height": str(_NATIVE_RESOLUTION_WH[1]), + "cx": str(_PRINCIPAL_POINT_XY[0]), + "cy": str(_PRINCIPAL_POINT_XY[1]), + "Model": "ftheta", + "polynomial-type": "pixeldistance-to-angle", + "polynomial": _POLYNOMIAL_TEXT, + "linear-c": "1.000000", + "linear-d": "0.000000", + "linear-e": "0.000000", + }, + } + ], + } + } diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/cli.py b/apps/omnidreams_game_engine/omnidreams_game_engine/cli.py index 4b5e04c93..8ba4eed8c 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/cli.py +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/cli.py @@ -10,7 +10,6 @@ from loguru import logger from omnidreams.hf_org import DEFAULT_HF_ORG, apply_cli_to_env from omnidreams.hf_org import ENV_VAR as _HF_ORG_ENV_VAR -from omnidreams.scenes import local_scene_archive_path from flashdreams.infra.postprocess import VideoPostprocessChainConfig from flashdreams.plugins.registry import discover_postprocess_presets @@ -27,27 +26,16 @@ WorldModelProfileConfig, ) from omnidreams_game_engine.log import configure_logging -from omnidreams_game_engine.synthetic_scene import build_synthetic_scene_to_temp from omnidreams_game_engine.world_model.manifest import ( load_world_model_manifest, resolve_world_model_manifest_path, ) -# Package root (from this file's location) so packaged-asset defaults below -# resolve relative to the install, not the user's cwd. Bundled configs live at -# ``interactive_drive/configs/``; scene USDZs are staged into -# ``$FLASHDREAMS_CACHE_DIR/omnidreams-scenes/`` (shared with the webrtc server). +# Package root (from this file's location) so packaged config paths resolve +# relative to the install, not the user's cwd. _PACKAGE_ROOT = Path(__file__).resolve().parent _CONFIGS_ROOT = _PACKAGE_ROOT / "configs" -# Default scene UUID staged by ``omnidreams-prepare`` (clear-weather base -# archive in nvidia/omni-dreams-scenes). -DEFAULT_SCENE_UUID = "0d404ff7-2b66-498c-b047-1ed8cded60d4" - -# Default scene path under the shared ``$FLASHDREAMS_CACHE_DIR/omnidreams-scenes/`` -# cache, so a scene staged by the desktop demo or webrtc server is visible to both. -DEFAULT_SCENE = local_scene_archive_path(DEFAULT_SCENE_UUID) - def resolve_manifest_path(path: str | Path) -> Path: """Resolve a CLI manifest value. @@ -65,49 +53,17 @@ def build_parser() -> argparse.ArgumentParser: description="Single-process flashdreams driving demo" ) parser.add_argument( - "--scene", - type=Path, - default=DEFAULT_SCENE, - help=( - "Path to the input USDZ scene. Defaults to the scene staged by " - f"prepare.py at {DEFAULT_SCENE}; any UUID from " - "nvidia/omni-dreams-scenes/scenes/ works once staged." - ), - ) - parser.add_argument( - "--synthetic-scene", - action="store_true", - help=( - "Skip the USDZ download / staging and build a procedural," - " HD-map-data-free scene at startup instead. Useful for" - " demos in territories where the real-world scenes can't be" - " distributed. The generated scene is a wavy 2-lane road" - " with a single intersection; pair with --synthetic-initial-rgb" - " to supply a natural-looking starting camera frame." - ), - ) - parser.add_argument( - "--synthetic-initial-rgb", + "--map", + dest="scene", type=Path, default=None, - help=( - "Path to a JPG / PNG used as the initial camera frame when" - " --synthetic-scene is set. The world model is trained on" - " natural driving frames, so a real photo (any forward-facing" - " roadway) gives noticeably better generation than the" - " default debug gradient. Resized to the raster resolution" - " automatically." - ), + metavar="PATH", + help="Path to a .robotaxi.yaml game map.", ) parser.add_argument( - "--synthetic-prompt", - default=None, - help=( - "Optional text prompt embedded in the synthetic scene." - " Mutually overridable by --prompt at run time. When omitted," - " the synthetic-scene builder uses a generic forward-driving" - " caption." - ), + "--force-map-recompile", + action="store_true", + help="Rebuild each selected map's compiled cache once in this process.", ) # ``--backend`` exists primarily for the test suite, which exercises # the raster path (~30s warmup) instead of the full omnidreams pipeline @@ -128,10 +84,7 @@ def build_parser() -> argparse.ArgumentParser: parser.add_argument( "--variant", default="default", - help=( - "Scene variant to load: weather siblings (default, rain, snow) or " - "legacy in-archive numbered variants (1, 2, 3)." - ), + help="Visual variant defined by the game map.", ) parser.add_argument("--prompt", default=None, help="Optional prompt override") parser.add_argument( @@ -203,8 +156,8 @@ def build_parser() -> argparse.ArgumentParser: default=None, metavar="ORG", help=( - "Hugging Face org that hosts the omni-dreams repos (models /" - f" samples / scenes). Defaults to {DEFAULT_HF_ORG!r}." + "Hugging Face org that hosts the omni-dreams model and sample" + f" repos. Defaults to {DEFAULT_HF_ORG!r}." f" Equivalent to setting {_HF_ORG_ENV_VAR}; the flag wins when" " both are present. Stamped into the env var early in main()" " so every downstream HF lookup -- including URLs read from" @@ -301,74 +254,6 @@ def build_parser() -> argparse.ArgumentParser: " below ``bev-fov-deg / 2``." ), ) - parser.add_argument( - "--oob-warn-proximity", - type=float, - default=None, - metavar="FLOAT", - help=( - "Proximity at which the loop overlays " - "'Approaching map edge, turn back to avoid respawn' on the " - "frame. Mirrors alpasim's ``oob_proximity``: 0.0 is solidly " - "inside the navigable AABB+margin, 1.0 is at the AABB+margin " - "edge (the warning band ramps linearly across a 100 m zone " - "inside the edge), 2.0 is the off-map sentinel. Default 0.6, " - "matching alpasim's 'approaching' threshold." - ), - ) - parser.add_argument( - "--oob-respawn-proximity", - type=float, - default=None, - metavar="FLOAT", - help=( - "Proximity above which the loop fires the auto-respawn (after " - "``--oob-respawn-debounce-chunks`` consecutive chunks at this " - "level). Default 2.0, matching alpasim: a hard binary trigger " - "that only fires when the ego has actually crossed the " - "AABB+margin boundary. Set to 2.5 (or any value > 2.0) to " - "disable auto-respawn entirely while keeping the warning " - "overlay." - ), - ) - parser.add_argument( - "--oob-respawn-debounce-chunks", - type=int, - default=None, - metavar="N", - help=( - "Number of consecutive chunks the proximity must stay at or " - "above ``--oob-respawn-proximity`` before the auto-respawn " - "fires. Default 1, matching alpasim's immediate-on-step " - "behaviour. Raise this for an added buffer; useful mainly " - "if you've lowered the respawn threshold below 2.0." - ), - ) - parser.add_argument( - "--oob-margin-m", - type=float, - default=None, - metavar="METERS", - help=( - "Margin (in metres) added around the scene's spatial-content " - "AABB before any in-bounds check. The respawn fires only " - "once the ego is past AABB+margin, so larger values give " - "more room to leave the explicitly mapped area. Default 50, " - "matching alpasim. Bump to 200+ on scenes whose geometry " - "layers don't cover the full driveable area." - ), - ) - parser.add_argument( - "--oob-warning-zone-m", - type=float, - default=None, - metavar="METERS", - help=( - "Depth of the linear warning-ramp band inside the AABB+margin " - "edge. Default 100, matching alpasim. Set to 0 to disable the " - "ramp and only ever show the binary on/off respawn signal." - ), - ) return parser @@ -387,28 +272,6 @@ def _parse_resolution(value: str) -> tuple[int, int]: return width, height -def _oob_kwargs(args: argparse.Namespace) -> dict[str, float | int]: - """Forward only the OOB flags the user actually passed. - - Each ``--oob-*`` flag defaults to ``None`` so the - :class:`AppConfig` field defaults stay authoritative; we only add - a kwarg to the ``AppConfig(**kwargs)`` call when the user passed - an explicit value. - """ - overrides: dict[str, float | int] = {} - if args.oob_warn_proximity is not None: - overrides["oob_warn_proximity"] = float(args.oob_warn_proximity) - if args.oob_respawn_proximity is not None: - overrides["oob_respawn_proximity"] = float(args.oob_respawn_proximity) - if args.oob_respawn_debounce_chunks is not None: - overrides["oob_respawn_debounce_chunks"] = int(args.oob_respawn_debounce_chunks) - if args.oob_margin_m is not None: - overrides["oob_margin_m"] = float(args.oob_margin_m) - if args.oob_warning_zone_m is not None: - overrides["oob_warning_zone_m"] = float(args.oob_warning_zone_m) - return overrides - - def main() -> None: """Stand-alone entry point for ``python -m omnidreams_game_engine.cli``. @@ -428,8 +291,7 @@ def prepare_config_and_backend( hand it to a long-lived :class:`InteractiveDriveApp` that switches scenes in place (keeping the warmed model resident). """ - # Stamp the resolved HF org into the env var before anything fetches - # (manifest, scene staging, model build read it lazily). + # Stamp the resolved HF org before manifest and model artifact resolution. resolved_org = apply_cli_to_env(args.hf_org) if resolved_org != DEFAULT_HF_ORG: logger.info( @@ -437,21 +299,8 @@ def prepare_config_and_backend( ) scene_path = args.scene - if args.synthetic_scene: - # Materialise a procedural USDZ to a temp dir for this process. - # The scene loader treats it like any other USDZ; downstream code - # paths (rasterizer, world model, presenter) need no changes. - scene_path = build_synthetic_scene_to_temp( - initial_rgb_path=args.synthetic_initial_rgb, - prompt=args.synthetic_prompt, - ) - logger.info( - f"[interactive-drive] synthetic scene materialised at {scene_path}", - ) - elif args.synthetic_initial_rgb is not None or args.synthetic_prompt is not None: - raise SystemExit( - "--synthetic-initial-rgb / --synthetic-prompt require --synthetic-scene" - ) + if scene_path is None: + raise SystemExit("--map is required") bev_width, bev_height = _parse_resolution(args.bev_resolution) bev_config = BevConfig( @@ -472,6 +321,7 @@ def prepare_config_and_backend( camera_name=args.camera, variant=args.variant, prompt_override=args.prompt, + force_map_recompile=bool(args.force_map_recompile), manifest_path=manifest_path, raster=RasterConfig( compute_device=args.compute_device, @@ -487,7 +337,6 @@ def prepare_config_and_backend( stream_mjpeg_bind=args.stream_mjpeg, stop_after_consumed_chunks=args.stop_after_chunks, visual_flare_enabled=False if args.disable_visual_flare else None, - **_oob_kwargs(args), ) backend: RenderBackend diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/config.py b/apps/omnidreams_game_engine/omnidreams_game_engine/config.py index 205fe2f48..040cf85c0 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/config.py +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/config.py @@ -129,6 +129,7 @@ class AppConfig: camera_name: str = "camera_front_wide_120fov" variant: str = "default" prompt_override: str | None = None + force_map_recompile: bool = False manifest_path: Path | None = None chunk: ChunkConfig = ChunkConfig() raster: RasterConfig = RasterConfig() @@ -139,17 +140,6 @@ class AppConfig: default_factory=VideoPostprocessChainConfig ) bev: BevConfig = BevConfig() - # OOB thresholds plumbed to LoopConfig (overridable via CLI --oob-*). - # Match alpasim's driver-side proximity: warn > 0.6, respawn >= 2.0 - # against the AABB-distance proximity. - oob_warn_proximity: float = 0.6 - oob_respawn_proximity: float = 2.0 - oob_respawn_debounce_chunks: int = 1 - # OOB AABB geometry: oob_margin_m (50 m, matching alpasim) expands the - # scene's spatial-content AABB before any in-bounds check; - # oob_warning_zone_m is the depth of the linear warning ramp inside it. - oob_margin_m: float = 50.0 - oob_warning_zone_m: float = 100.0 # When set ("HOST:PORT" or bare ":PORT"), swap the Vulkan presenter for # the MJPEG streaming presenter (HTTP frames + keyboard) -- needed on # compute-only boxes with no Vulkan-capable GPU. diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/control.py b/apps/omnidreams_game_engine/omnidreams_game_engine/control.py index cd35c7ffb..a535d58df 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/control.py +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/control.py @@ -48,7 +48,7 @@ def iterate_frame_chunks( trajectory = sample_chunk_trajectory( start_state=state, start_timestamp_us=next_timestamp_us, - command=command_source(), + commands=tuple(command_source() for _ in range(chunk_size)), chunk_size=chunk_size, chunk_config=chunk_config, vehicle_config=vehicle_config, diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/demo.py b/apps/omnidreams_game_engine/omnidreams_game_engine/demo.py index 631966406..3293241de 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/demo.py +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/demo.py @@ -11,21 +11,24 @@ import struct import threading import time -import zipfile -from collections.abc import Iterable from dataclasses import dataclass, field, replace from pathlib import Path from typing import Any import numpy as np from loguru import logger -from omnidreams import scenes as _scenes -from omnidreams.scenes import normalise_scene_uuid, scenes_cache_root from PIL import Image from omnidreams_game_engine import cli as _cli from omnidreams_game_engine.app import InteractiveDriveApp from omnidreams_game_engine.config import BevConfig, RasterConfig +from omnidreams_game_engine.game_map import ( + GAME_MAP_SUFFIX, + load_game_map, + load_game_map_header, + render_spawn_first_frame, + resolve_seed_asset, +) from omnidreams_game_engine.input.wheel_profiles import ( EV_ABS, EV_KEY, @@ -46,7 +49,6 @@ user_wheel_profiles_dir, ) from omnidreams_game_engine.log import configure_logging -from omnidreams_game_engine.synthetic_scene import build_synthetic_scene_to_temp # Private aliases for the evdev helpers (canonical defs in # ``input/wheel_profiles.py``, shared with the configuration tool). @@ -109,8 +111,7 @@ class SceneOption: # dropdown. Variants without a dedicated preview map to the default image # so every row still shows a preview. variant_thumbnails: dict[str, Image.Image] = field(default_factory=dict) - # Variant slug -> its USDZ archive. Distinct sibling files for the current - # per-weather dataset; the single ``path`` for legacy in-zip-variant scenes. + # Variant slug -> the authored map containing that variant. variant_paths: dict[str, Path] = field(default_factory=dict) @@ -492,7 +493,7 @@ def build_parser() -> argparse.ArgumentParser: Union of: the backend args from :func:`omnidreams_game_engine.cli.build_parser`; HUD args - (``--scene-dir``, ``--wheel-*``, ...) ignored under ``--no-hud`` / + (``--map-dir``, ``--wheel-*``, ...) ignored under ``--no-hud`` / ``--stream-mjpeg``; and the ``--no-hud`` toggle (bare Vulkan window). """ parser = _cli.build_parser() @@ -521,20 +522,16 @@ def build_parser() -> argparse.ArgumentParser: "--no-hud", action="store_true", help=( - "Skip the HUD chrome and run the backend with a bare slangpy" - " Vulkan window (matching the legacy lightweight demo)." + "Skip the HUD chrome and run the backend with a bare slangpy Vulkan window." ), ) parser.add_argument( - "--scene-dir", + "--map-dir", + dest="scene_dir", type=Path, - default=scenes_cache_root(), - help=( - "Directory of USDZ scenes shown in the HUD scene selector. " - "Defaults to ``$FLASHDREAMS_CACHE_DIR/omnidreams-scenes/``, " - "the shared cache root used by both this demo and the " - "centralized ``webrtc`` scene pipeline." - ), + default=Path.cwd(), + metavar="DIRECTORY", + help="Directory of .robotaxi.yaml maps available for scene switching.", ) parser.add_argument( "--auto-start", @@ -542,7 +539,7 @@ def build_parser() -> argparse.ArgumentParser: action=argparse.BooleanOptionalAction, default=False, help=( - "Start loading --scene immediately instead of opening the HUD on" + "Start loading --map immediately instead of opening the HUD on" " Load Scene. Distinct from --preload-scenes (which only warms the" " parse cache in the background)." ), @@ -561,10 +558,10 @@ def build_parser() -> argparse.ArgumentParser: action=argparse.BooleanOptionalAction, default=False, help=( - "Parse every scene in --scene-dir in the background at startup so" - " switching scenes skips the USDZ parse (the per-scene geometry" + "Parse every map in --map-dir in the background at startup so" + " switching scenes skips map compilation and archive parsing (geometry" " upload and first-chunk generation still happen on switch)." - " Off by default; uses more memory the more scenes are staged." + " Off by default; uses more memory as more maps are loaded." ), ) parser.add_argument( @@ -620,86 +617,8 @@ def build_parser() -> argparse.ArgumentParser: return parser -def _has_discoverable_scenes(scene_dir: Path, scene: Path) -> bool: - """Whether the scene picker would find any staged USDZ to offer. - - Mirrors :func:`_discover_scene_options`'s directory sweep -- the - ``--scene-dir`` cache plus the requested scene's own folder -- so the - default-scene autostage can be skipped when a curated set of scenes is - already present. - """ - for directory in (scene_dir, scene.parent): - resolved = _project_path(directory) - if resolved.is_dir() and any(resolved.glob("*.usdz")): - return True - return False - - -def _maybe_autostage_scene(scene: Path, *, scene_dir: Path, allow_skip: bool) -> Path: - """Auto-download the default scene UUID on first launch. - - Triggers only for a missing ``clipgt-.usdz`` under the shared scenes - cache root; external / non-clipgt paths are returned unchanged. With - ``allow_skip`` (any scene-picker mode), a missing default is skipped when - the picker already has staged scenes, so a curated set never blocks on the - default UUID. ``omnidreams-prepare`` remains the way to pre-stage arbitrary - UUIDs. - """ - if scene.exists(): - return scene - if allow_skip and _has_discoverable_scenes(scene_dir, scene): - logger.info( - f"[interactive-drive] default scene '{scene.name}' is not staged; " - f"using the scenes already present under {scene_dir} instead.", - ) - return scene - cache_dir = scenes_cache_root().resolve() - if scene.resolve().parent != cache_dir: - return scene - stem = scene.stem - if not stem.startswith("clipgt-"): - return scene - bare_uuid = normalise_scene_uuid(stem) - if not os.environ.get("HF_TOKEN"): - raise SystemExit( - f"Scene '{scene.name}' is not staged yet and HF_TOKEN is not set.\n" - "Either export HF_TOKEN to enable auto-staging on launch, or run:\n" - f" uv run --package flashdreams-omnidreams omnidreams-prepare --scene-uuid {bare_uuid}" - ) - logger.info( - f"[interactive-drive] Scene '{stem}' not found locally; " - "auto-staging from Hugging Face (one-time download)..." - ) - from omnidreams.prepare import stage_scene - - staged_default = stage_scene(bare_uuid, force=False) - # Also stage the scene's other weather variants so the HUD shows a - # Default/Rain/Snow selector; discovery globs the cache dir for them. - try: - sibling_variants = [ - variant - for uuid, variant in _scenes.list_available_scene_files() - if uuid == bare_uuid and variant != _scenes.SCENE_VARIANT_DEFAULT - ] - except Exception as exc: # noqa: BLE001 - best-effort; base scene already staged - logger.info( - f"[interactive-drive] could not enumerate scene variants ({exc}); " - "staged the base scene only.", - ) - sibling_variants = [] - for variant in sibling_variants: - try: - stage_scene(bare_uuid, variant=variant, force=False) - except Exception as exc: # noqa: BLE001 - skip a variant, keep the rest - logger.info( - f"[interactive-drive] failed to stage variant {variant!r} " - f"({exc}); skipping.", - ) - return staged_default - - def main() -> None: - """Run the legacy parser entry point used by internal development tools.""" + """Run the parser entry point used by internal development tools.""" _run_namespace(build_parser().parse_args()) @@ -739,21 +658,9 @@ def _coerce_launch_path(key: str, value: object) -> object: def _run_namespace(args: argparse.Namespace) -> None: """Execute one already-resolved local-window namespace.""" configure_logging() - if not args.synthetic_scene: - # Only the bare ``--no-hud`` backend has no scene picker; the HUD - # and MJPEG paths both let the user pick from ``--scene-dir``, so a - # missing default scene there is fine as long as the directory - # already has other scenes staged (see _maybe_autostage_scene). - uses_scene_picker = args.stream_mjpeg is not None or not args.no_hud - args.scene = _maybe_autostage_scene( - args.scene, scene_dir=args.scene_dir, allow_skip=uses_scene_picker - ) # ``--stream-mjpeg`` runs through ``_run_streaming`` so the long-lived - # MJPEG presenter (HTTP server, browser session) survives across - # scene-change requests posted by the in-page picker. ``--no-hud`` - # without MJPEG drops straight through to the bare CLI's Vulkan - # window, which has no scene picker UI of its own. The default path - # is the slangpy HUD with full chrome. + # MJPEG presenter survives scene changes. ``--no-hud`` without MJPEG + # drops straight through to the bare CLI's Vulkan window. if args.stream_mjpeg is not None: _run_streaming(args) return @@ -768,11 +675,11 @@ def _run_slangpy_hud(args: argparse.Namespace) -> None: """Run the engine with the slangpy + PIL HUD presenter in one process. Builds one ``SlangPyHudPresenter`` and one long-lived - :class:`InteractiveDriveApp` at startup (model warmup overlaps the - scene-selection wait), then loops over scene-change requests calling - ``app.load_scene`` / ``app.run_scene`` per scene. The warmed model and the - window stay alive across switches (``close_presenter_on_exit=False``); the - wheel binds once to the app's single ``KeyboardState``. + :class:`InteractiveDriveApp` at startup, then loops over scene-change + requests calling ``app.load_scene`` / ``app.run_scene`` per scene. The + warmed model and the window stay alive across switches + (``close_presenter_on_exit=False``); the wheel binds once to the app's + single ``KeyboardState``. """ from omnidreams_game_engine.input.keyboard import KeyboardState from omnidreams_game_engine.slangpy_hud_presenter import ( @@ -782,17 +689,16 @@ def _run_slangpy_hud(args: argparse.Namespace) -> None: _apply_cuda_visible_devices_inplace(args.cuda_visible_devices) _resolve_demo_paths(args) - _materialize_synthetic_scene_for_picker(args) scene_options = _discover_scene_options(args.scene_dir, args.scene) - if not args.scene.exists() and scene_options: + if (args.scene is None or not args.scene.exists()) and scene_options: args.scene = scene_options[0].path # Validate paths up front so a typo in ``--manifest`` / - # ``--scene-dir`` / ``--control-assets-dir`` fails immediately, + # ``--map-dir`` / ``--control-assets-dir`` fails immediately, # before we open the slangpy window and the user wastes 30s on # world-model warmup that's about to ENOENT. Scene path is # validated lazily because ``_discover_scene_options`` already # backfills ``args.scene`` from the directory, so a missing - # ``--scene`` is only fatal if the directory is empty too. + # ``--map`` is only fatal if the directory is empty too. if args.backend == "omnidreams": if args.manifest is None: raise SystemExit("--manifest is required for the omnidreams backend") @@ -802,21 +708,16 @@ def _run_slangpy_hud(args: argparse.Namespace) -> None: " (typo? expected a path or bundled config name like " "example_world_model.yaml)" ) + if args.scene is None: + raise SystemExit("--map is required when --map-dir contains no maps") if not scene_options and not args.scene.exists(): - raise SystemExit( - f"--scene path does not exist and --scene-dir contains no scenes: {args.scene}" - ) + raise SystemExit(f"--map path does not exist: {args.scene}") control_assets = _load_control_assets(args.control_assets_dir) wheel_selection = None if args.no_wheel else _select_wheel(args) - # Construct the presenter UPFRONT, before any backend, so the demo - # can open the HUD window in "Load Scene" mode and wait for the - # user to pick a scene from the dropdown when ``--auto-start`` - # is off. The placeholder ``KeyboardState`` is rebound to each - # successive ``InteractiveDriveApp``'s real keyboard via - # ``presenter.bind_keyboard`` in the factory below; no engine is - # listening to the placeholder, so events are harmlessly dropped - # during the initial wait. + # Construct the presenter before the backend. The placeholder + # ``KeyboardState`` is rebound to the app's real keyboard via + # ``presenter.bind_keyboard`` in the factory below. placeholder_keyboard = KeyboardState() presenter = SlangPyHudPresenter( raster=RasterConfig(), @@ -827,13 +728,9 @@ def _run_slangpy_hud(args: argparse.Namespace) -> None: wheel=None, ) - # Build the backend + engine ONCE, up front. Constructing the app - # starts the (scene-independent) model warmup on the pipeline worker - # thread immediately, so the long weight-load + compile overlaps with - # the user's scene-selection wait below instead of starting only after - # the first pick. The app owns one long-lived KeyboardState and rebinds - # the presenter to it; scenes are switched in place via - # ``app.load_scene`` so the warmed model is never rebuilt. + # Build the backend + engine once. The app owns one long-lived + # KeyboardState and rebinds the presenter to it; scenes are switched in + # place via ``app.load_scene`` so the warmed model is never rebuilt. config, backend = _cli.prepare_config_and_backend(args) app = InteractiveDriveApp( config=config, @@ -848,11 +745,9 @@ def _run_slangpy_hud(args: argparse.Namespace) -> None: callback=app.set_postprocess_enabled, ) - # Attach the wheel up front, bound to the app's long-lived keyboard, so - # the HUD's steering / pedal chrome reacts to the physical device during - # the initial scene-selection wait -- not only once a scene is running. - # The evdev reader thread starts now and runs for the process lifetime; - # the single keyboard means it never needs rebinding across scenes. + # Attach the wheel up front, bound to the app's long-lived keyboard. The + # evdev reader thread starts now and runs for the process lifetime; the + # single keyboard means it never needs rebinding across scenes. wheel: Any = None if wheel_selection is not None: profile, device_paths = wheel_selection @@ -870,40 +765,19 @@ def _run_slangpy_hud(args: argparse.Namespace) -> None: for opt in scene_options for variant in (opt.variants or ("default",)) ) - # Lock scene selection until every scene is cached so the user only + # Lock scene changes until every scene is cached so the user only # ever hits the instant (cache-hit) switch path. presenter.set_scene_selection_locked(app.preload_in_progress) - # First scene: prefer the resolved ``config.scene_path`` so - # ``--synthetic-scene`` (materialised to a temp USDZ) and any autostaged - # default are honoured; a dropdown selection overrides it below. scene_path: Any = config.scene_path variant = _resolve_scene_variant(scene_options, scene_path, config.variant) - presenter.acknowledge_scene_change(scene_path, variant) try: - # ``need_selection`` drives the scene-selection wait: True on first - # launch (unless ``--auto-start``) and again every time the user - # exits a scene back to the selector. While waiting the engine is - # idle, so the video model stops generating -- the whole point of the - # exit-scene affordance for long-running demos -- without closing the - # window or dropping the warmed model. - need_selection = not args.auto_start - # --auto-start + --preload-scenes: wait for the preloader to finish - # before the auto-load below so it hits the cache instead of racing - # the background thread with a second parse of the same USDZ. - if args.auto_start and app.preload_in_progress(): + if app.preload_in_progress(): presenter.wait_while_preloading(app.preload_in_progress) + presenter.acknowledge_scene_change(scene_path, variant) while True: - if need_selection: - request = presenter.wait_for_scene_selection() - if request is None: - break # window closed before any scene was loaded - scene_path, variant = request - presenter.acknowledge_scene_change(scene_path, variant) - need_selection = False - presenter.set_engine_active(True) - # load_scene parses the USDZ on a background thread while keeping + # load_scene compiles the map on a background thread while keeping # the window responsive; it returns False if the window closed # (or a new scene was requested) before the parse finished, so # we skip run_scene and let the pending checks below decide @@ -912,11 +786,8 @@ def _run_slangpy_hud(args: argparse.Namespace) -> None: app.run_scene() presenter.set_engine_active(False) if presenter.pending_exit_scene: - # ``x`` / bound exit button: tear down the rollout and go - # back to the selector over the same presenter. presenter.acknowledge_exit_scene() - need_selection = True - continue + break requested = presenter.pending_scene_change if requested is None: # Window closed (X / ESC) during load or run; we're done. @@ -933,8 +804,7 @@ def _run_streaming(args: argparse.Namespace) -> None: Like :func:`_run_slangpy_hud` but with a long-lived :class:`MJPEGStreamingPresenter`: the HTTP server / browser sessions stay - alive across scene swaps while only the scene is rebuilt. Scene options are - serialised to JSON for the in-browser ``/scenes`` dropdown. + alive across scene swaps while only the scene is rebuilt. """ from omnidreams_game_engine.input.keyboard import KeyboardState from omnidreams_game_engine.streaming_presenter import ( @@ -944,9 +814,8 @@ def _run_streaming(args: argparse.Namespace) -> None: _apply_cuda_visible_devices_inplace(args.cuda_visible_devices) _resolve_demo_paths(args) - _materialize_synthetic_scene_for_picker(args) scene_options = _discover_scene_options(args.scene_dir, args.scene) - if not args.scene.exists() and scene_options: + if (args.scene is None or not args.scene.exists()) and scene_options: args.scene = scene_options[0].path if args.backend == "omnidreams": if args.manifest is None: @@ -957,10 +826,10 @@ def _run_streaming(args: argparse.Namespace) -> None: " (typo? expected a path or bundled config name like " "example_world_model.yaml)" ) + if args.scene is None: + raise SystemExit("--map is required when --map-dir contains no maps") if not scene_options and not args.scene.exists(): - raise SystemExit( - f"--scene path does not exist and --scene-dir contains no scenes: {args.scene}" - ) + raise SystemExit(f"--map path does not exist: {args.scene}") # JSON-serialisable form of the discovered scenes for the browser # ``/scenes`` endpoint. Thumbnails are JPEG-encoded once at startup @@ -1003,11 +872,9 @@ def _run_streaming(args: argparse.Namespace) -> None: thumbnails=thumbnails, ) - # Build the backend + engine once so the model warms up (on the - # pipeline worker thread) while the browser is still choosing the first - # scene. The app rebinds the presenter to its long-lived keyboard and - # switches scenes in place via ``app.load_scene``, keeping the warmed - # model resident across scene changes. + # Build the backend + engine once. The app rebinds the presenter to its + # long-lived keyboard and switches scenes in place via ``app.load_scene``, + # keeping the warmed model resident across scene changes. config, backend = _cli.prepare_config_and_backend(args) app = InteractiveDriveApp( config=config, @@ -1028,39 +895,14 @@ def _run_streaming(args: argparse.Namespace) -> None: presenter.set_scene_selection_locked(app.preload_in_progress) try: - if args.auto_start: - # Headless / scriptable start: skip the browser scene picker and - # load the resolved ``--scene`` (or the first discovered scene) - # immediately. This lets the demo run with no GUI/browser. - # --auto-start + --preload-scenes: let the preloader finish first - # so the auto-load hits the cache instead of racing a second parse. - if app.preload_in_progress(): - presenter.wait_while_preloading(app.preload_in_progress) - scene_path = config.scene_path - variant = _resolve_scene_variant(scene_options, scene_path, config.variant) - presenter.acknowledge_scene_change(scene_path, variant) - logger.info( - f"[demo] streaming auto-start scene -> {scene_path.name} " - f"variant={variant!r}", - ) - else: - # Don't auto-load: always wait for the browser to pick the first - # scene. There's no Vulkan window to show progress in, so the - # presenter publishes an idle overlay frame ("Loading world - # model..." while warmup runs in the background, then "Select a - # scene to begin") so connected browsers have something to render - # while the wait spins. - logger.info( - "[demo] streaming presenter waiting for first scene selection...", - ) - request = presenter.wait_for_scene_selection() - if request is None: - return # presenter closed before any selection (Ctrl-C) - scene_path, variant = request - presenter.acknowledge_scene_change(scene_path, variant) - logger.info( - f"[demo] streaming initial scene -> {scene_path.name} variant={variant!r}", - ) + if app.preload_in_progress(): + presenter.wait_while_preloading(app.preload_in_progress) + scene_path = config.scene_path + variant = _resolve_scene_variant(scene_options, scene_path, config.variant) + presenter.acknowledge_scene_change(scene_path, variant) + logger.info( + f"[demo] streaming initial scene -> {scene_path.name} variant={variant!r}", + ) while True: # load_scene parses the USDZ on a background thread while the @@ -1113,32 +955,6 @@ def _resolve_demo_paths(args: argparse.Namespace) -> None: args.control_assets_dir = _project_path(args.control_assets_dir) -def _materialize_synthetic_scene_for_picker(args: argparse.Namespace) -> None: - """Build ``--synthetic-scene`` before scene-picker discovery. - - The single-scene ``--no-hud`` path lets ``cli.prepare_config_and_backend`` - materialize the synthetic USDZ. HUD and MJPEG modes discover scenes first - so the picker can show options before a scene is loaded; those modes need - the temporary USDZ to exist before discovery runs. - """ - if not args.synthetic_scene: - return - scene_path = build_synthetic_scene_to_temp( - initial_rgb_path=args.synthetic_initial_rgb, - prompt=args.synthetic_prompt, - ) - logger.info( - "[interactive-drive] synthetic scene materialised at {}", - scene_path, - ) - args.scene = scene_path - # The synthetic inputs have been consumed into the temp USDZ. Clear them so - # the later shared backend builder treats the scene as a normal archive. - args.synthetic_scene = False - args.synthetic_initial_rgb = None - args.synthetic_prompt = None - - def _project_path(path: Path) -> Path: path = Path(path).expanduser() if path.is_absolute(): @@ -1149,28 +965,19 @@ def _project_path(path: Path) -> Path: def _discover_scene_options( - scene_dir: Path, selected_scene: Path + scene_dir: Path, selected_scene: Path | None ) -> tuple[SceneOption, ...]: paths: set[Path] = set() - if selected_scene.exists(): + if selected_scene is not None and selected_scene.exists(): paths.add(selected_scene.resolve()) if scene_dir.is_dir(): - paths.update(path.resolve() for path in scene_dir.glob("*.usdz")) - if selected_scene.parent.is_dir(): - paths.update(path.resolve() for path in selected_scene.parent.glob("*.usdz")) - - # Group archives by scene UUID so the per-weather sibling files - # (``clipgt--.usdz``) collapse into one scene with a variant - # selector. Single-archive scenes stay a group of one. - grouped: dict[str, dict[str, Path]] = {} - for path in sorted(paths): - uuid, variant = _scenes.parse_scene_stem(path.stem) - grouped.setdefault(uuid, {})[variant] = path - - options = tuple( - _scene_option_for_group(variant_paths) - for _uuid, variant_paths in sorted(grouped.items()) - ) + paths.update(path.resolve() for path in scene_dir.glob(f"*{GAME_MAP_SUFFIX}")) + if selected_scene is not None and selected_scene.parent.is_dir(): + paths.update( + path.resolve() for path in selected_scene.parent.glob(f"*{GAME_MAP_SUFFIX}") + ) + + options = tuple(_scene_option_for_game_map(path) for path in sorted(paths)) logger.info( "[demo] discovered scenes: " + ( @@ -1184,99 +991,46 @@ def _discover_scene_options( return options -def _order_variants(variants: Iterable[str]) -> tuple[str, ...]: - """Order variant slugs with ``default`` first, then the rest sorted.""" - unique = set(variants) - ordered = ["default"] if "default" in unique else [] - ordered.extend(sorted(unique - {"default"})) - return tuple(ordered) - - -def _scene_option_for_group(variant_paths: dict[str, Path]) -> SceneOption: - """Build one :class:`SceneOption` from a scene's variant archive(s). - - Multiple siblings => the weather variants are the files. A single archive - => fall back to in-zip variant discovery (legacy / synthetic scenes). - """ - if len(variant_paths) > 1: - variants = _order_variants(variant_paths.keys()) - base_path = variant_paths.get("default") or variant_paths[variants[0]] - resolved_paths = dict(variant_paths) - variant_thumbnails = _load_variant_file_thumbnails(resolved_paths, variants) - else: - base_path = next(iter(variant_paths.values())) - variants = _discover_variants(base_path) - resolved_paths = {variant: base_path for variant in variants} - variant_thumbnails = _load_variant_thumbnails(base_path, variants) - # Use the first variant's preview for the scene row so the scene and - # variant dropdowns agree, falling back to the standalone loader. - thumbnail = ( - variant_thumbnails.get(variants[0]) - or variant_thumbnails.get("default") - or _load_scene_thumbnail(base_path) - ) +def _scene_option_for_game_map(path: Path) -> SceneOption: + """Build scene metadata from the authored game map.""" + header = load_game_map_header(path) + variants = tuple(variant.name for variant in header.variants) + thumbnails: dict[str, Image.Image] = {} + generated_thumbnail: Image.Image | None = None + for variant in header.variants: + if variant.image is None: + if generated_thumbnail is None: + game_map = load_game_map(path) + generated_thumbnail = _make_thumbnail( + Image.fromarray( + render_spawn_first_frame(game_map, game_map.default_spawn) + ), + SCENE_THUMB_SIZE, + ) + thumbnails[variant.name] = generated_thumbnail.copy() + continue + try: + with Image.open(resolve_seed_asset(path, variant.image)) as image: + thumbnails[variant.name] = _make_thumbnail( + image.convert("RGB"), SCENE_THUMB_SIZE + ) + except OSError: + continue + thumbnail = thumbnails.get("default") or next(iter(thumbnails.values()), None) return SceneOption( - label=_scene_label(base_path), - path=base_path, + label=header.name, + path=path, variants=variants, thumbnail=thumbnail, - variant_thumbnails=variant_thumbnails, - variant_paths=resolved_paths, + variant_thumbnails=thumbnails, + variant_paths={variant: path for variant in variants}, ) -def _scene_label(path: Path) -> str: - scene_names = { - "0d404ff7-2b66-498c-b047-1ed8cded60d4": "Quiet Suburban Boulevard", - "7bd1eb2f-c375-44ee-b4ca-55473e0773a9": "Late Night Arrival in the Neighborhood", - "e2993759-36e1-4d97-868f-e2a737f1eb68": "Afternoon Commute Past the Park", - } - # Key by bare UUID so the label is stable across weather variant archives. - uuid, _variant = _scenes.parse_scene_stem(path.stem) - return scene_names.get(uuid, path.stem) - - -def _discover_variants(scene_path: Path) -> tuple[str, ...]: - variants: set[str] = set() - try: - with zipfile.ZipFile(scene_path, "r") as zf: - for name in zf.namelist(): - if "/" in name: - continue - stem = Path(name).stem - if name.startswith("first_image") and name.endswith(".png"): - variant = _scenes.variant_from_stem(stem, "first_image") - elif name.startswith("prompt") and name.endswith(".txt"): - variant = _scenes.variant_from_stem(stem, "prompt") - else: - continue - if variant is not None: - variants.add(variant) - except (OSError, zipfile.BadZipFile): - return ("default",) - # A bare ``default`` (prompt.txt / first_image.png) duplicates the first - # numbered variant, so when numbered variants exist we expose just those -- - # "1" is then the default selection. Scenes with no numbered variants show - # a single "default". - numbered = [value for value in variants if value != "default"] - if numbered: - numbered.sort(key=lambda v: (not v.isdigit(), int(v) if v.isdigit() else v)) - return tuple(numbered) - return ("default",) - - def _resolve_scene_variant( scene_options: tuple[SceneOption, ...], scene_path: Any, variant: str ) -> str: - """Return a variant that actually exists for *scene_path*. - - Numbered scenes no longer carry a bare ``default`` entry, so a configured - ``--variant default`` (or anything the scene lacks) falls back to the - scene's first variant rather than a selection the dropdown can't show. - For weather sibling archives, the path itself is also a source of truth: - ``clipgt-...-snow.usdz`` with the default CLI variant should start as - ``snow``, not silently load the clear/base archive. - """ + """Return a variant that exists for *scene_path*.""" for option in scene_options: path_variant = _scene_option_variant_for_path(option, scene_path) if path_variant is None: @@ -1298,9 +1052,6 @@ def _scene_option_variant_for_path(option: SceneOption, scene_path: Any) -> str resolved = None raw = str(scene_path) - # ``variant_paths`` is the authoritative map for weather sibling archives. - # For legacy single-archive scenes it maps every in-zip variant to the same - # path, so the first variant intentionally matches the old fallback. for variant, path in option.variant_paths.items(): if _same_scene_path(path, raw, resolved): return variant @@ -1315,86 +1066,6 @@ def _same_scene_path(path: Path, raw: str, resolved: Path | None) -> bool: return (resolved is not None and path == resolved) or str(path) == raw -def _load_scene_thumbnail(scene_path: Path) -> Image.Image | None: - try: - with zipfile.ZipFile(scene_path, "r") as zf: - names = [ - name - for name in zf.namelist() - if "/" not in name - and name.startswith("first_image") - and name.endswith(".png") - ] - if not names: - return None - name = "first_image.png" if "first_image.png" in names else sorted(names)[0] - with Image.open(io.BytesIO(zf.read(name))) as image: - return _make_thumbnail(image.convert("RGB"), SCENE_THUMB_SIZE) - except (OSError, zipfile.BadZipFile): - return None - - -def _load_variant_thumbnails( - scene_path: Path, variants: tuple[str, ...] -) -> dict[str, Image.Image]: - """Per-variant preview thumbnails for the HUD variant dropdown. - - Mirrors :func:`scene_loader._discover_first_images`: a bundle may ship - ``first_image_.png`` per variant alongside ``first_image.png`` - (the ``"default"`` variant). Each referenced image is decoded once; - variants without a dedicated image fall back to the default so every - dropdown row still shows a preview. Returns an empty mapping when the - archive has no parseable first images. - """ - decoded: dict[str, Image.Image] = {} - try: - with zipfile.ZipFile(scene_path, "r") as zf: - names_by_variant: dict[str, str] = {} - for name in zf.namelist(): - if ( - "/" in name - or not name.startswith("first_image") - or not name.endswith(".png") - ): - continue - variant = _scenes.variant_from_stem(Path(name).stem, "first_image") - if variant is not None: - names_by_variant[variant] = name - for variant, name in names_by_variant.items(): - with Image.open(io.BytesIO(zf.read(name))) as image: - decoded[variant] = _make_thumbnail( - image.convert("RGB"), SCENE_THUMB_SIZE - ) - except (OSError, zipfile.BadZipFile): - return {} - if not decoded: - return {} - default = decoded.get("default") or next(iter(decoded.values())) - return {variant: decoded.get(variant, default) for variant in variants} - - -def _load_variant_file_thumbnails( - variant_paths: dict[str, Path], variants: tuple[str, ...] -) -> dict[str, Image.Image]: - """Per-variant thumbnails when each variant is its own archive. - - Each preview comes from that variant file's ``first_image.png``; variants - with no usable preview reuse the default. Empty mapping if nothing decoded. - """ - decoded: dict[str, Image.Image] = {} - for variant in variants: - path = variant_paths.get(variant) - if path is None: - continue - thumb = _load_scene_thumbnail(path) - if thumb is not None: - decoded[variant] = thumb - if not decoded: - return {} - fallback = decoded.get("default") or next(iter(decoded.values())) - return {variant: decoded.get(variant, fallback) for variant in variants} - - def _make_thumbnail(image: Image.Image, size: tuple[int, int]) -> Image.Image: thumb = Image.new("RGB", size, (20, 20, 30)) fitted = _fit_image(image, size) @@ -1404,15 +1075,10 @@ def _make_thumbnail(image: Image.Image, size: tuple[int, int]) -> Image.Image: def _variant_label(variant: str) -> str: labels = { - # Per-weather variant archives. "default": "Default (Clear)", "clear": "Clear", "snow": "Snowstorm", "rain": "Night Rain", - # Legacy in-archive numbered variants. - "1": "Bright Midday Sun", - "2": "Snowstorm", - "3": "Night with Heavy Rain", } return labels.get(variant, variant) diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/__init__.py b/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/__init__.py new file mode 100644 index 000000000..0726e064f --- /dev/null +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/__init__.py @@ -0,0 +1,76 @@ +# SPDX-License-Identifier: Apache-2.0 +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. + +"""Semantic game-map loading, compilation, and previews.""" + +from omnidreams_game_engine.game_map._schema import ( + GAME_MAP_SUFFIX, + GameMapError, + GameMapHeader, + load_game_map_header, + resolve_seed_asset, +) +from omnidreams_game_engine.game_map.compiler import ( + CompiledGameMap, + compile_game_map, +) +from omnidreams_game_engine.game_map.loader import load_game_map +from omnidreams_game_engine.game_map.preview import write_game_map_preview +from omnidreams_game_engine.game_map.spawn_render import ( + SPAWN_RENDERER_VERSION, + render_spawn_first_frame, + write_spawn_first_frame_preview, +) +from omnidreams_game_engine.game_map.types import ( + GameMapBoundaryAttributes, + GameMapCurb, + GameMapElement, + GameMapLane, + GameMapLaneDivider, + GameMapLinearAttributes, + GameMapLineMarking, + GameMapNode, + GameMapParkingAccess, + GameMapRoad, + GameMapRoadBoundary, + GameMapSpawn, + GameMapTopology, + GameMapTrafficVehicle, + ResolvedGameMap, +) +from omnidreams_game_engine.game_map.vicinity import ( + GameMapVicinity, + GameMapVicinityResolver, +) + +__all__ = [ + "CompiledGameMap", + "GAME_MAP_SUFFIX", + "GameMapError", + "GameMapBoundaryAttributes", + "GameMapCurb", + "GameMapElement", + "GameMapHeader", + "GameMapLane", + "GameMapLaneDivider", + "GameMapLinearAttributes", + "GameMapLineMarking", + "GameMapNode", + "GameMapParkingAccess", + "GameMapRoad", + "GameMapRoadBoundary", + "GameMapSpawn", + "GameMapTrafficVehicle", + "GameMapTopology", + "GameMapVicinity", + "GameMapVicinityResolver", + "ResolvedGameMap", + "SPAWN_RENDERER_VERSION", + "compile_game_map", + "load_game_map", + "load_game_map_header", + "resolve_seed_asset", + "render_spawn_first_frame", + "write_game_map_preview", + "write_spawn_first_frame_preview", +] diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/_schema.py b/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/_schema.py new file mode 100644 index 000000000..9b587db5a --- /dev/null +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/_schema.py @@ -0,0 +1,389 @@ +# SPDX-License-Identifier: Apache-2.0 +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. + +"""Strict field, profile, and shared configuration parsing for game maps.""" + +from __future__ import annotations + +import math +from dataclasses import dataclass +from importlib import resources +from pathlib import Path +from typing import Any + +import numpy as np +import yaml + +from omnidreams_game_engine.game_map.types import ( + GameMapLinearAttributes, + GameMapVisualVariant, +) + +GAME_MAP_SUFFIX = ".robotaxi.yaml" +"""Filename suffix for authored node-graph game maps.""" + +_SCHEMA_VERSION = 1 +_REQUIRED_ROOT_FIELDS = frozenset( + { + "schema_version", + "id", + "name", + "compiler", + "nodes", + "roads", + "spawns", + } +) +_OPTIONAL_ROOT_FIELDS = frozenset({"profiles", "traffic", "traffic_count"}) + +_PROFILE_ATTRIBUTE_FIELDS = frozenset( + { + "lane_width_m", + "curb_offset_m", + "lanes", + "speed_limit_mps", + "curb", + "lane_marking", + "divider_markings", + "culdesac_radius_m", + } +) + + +class GameMapError(ValueError): + """Invalid semantic game-map definition.""" + + +@dataclass(frozen=True) +class GameMapHeader: + """Game-map metadata read without compiling geometry.""" + + map_id: str + name: str + variants: tuple[GameMapVisualVariant, ...] + source_path: Path + + +@dataclass(frozen=True) +class _CompilerSettings: + sample_spacing_m: float + ground_margin_m: float + intersection_connector_samples: int + + def as_dict(self) -> dict[str, object]: + """Return settings as stable cache metadata.""" + return dict(self.__dict__) + + +@dataclass(frozen=True) +class _Profile: + """Partial reusable defaults for resolved element attributes.""" + + profile_id: str + """Stable author-defined profile identifier.""" + + values: dict[str, object] + """Validated partial attribute values.""" + + +@dataclass +class _LaneBuild: + lane_id: str + element_id: str + centerline: np.ndarray + left_edge: np.ndarray + right_edge: np.ndarray + roadside_edge: np.ndarray + speed_limit_mps: float + marking_style: str + marking_color: str + start_endpoint: str + end_endpoint: str + successors: list[str] + allows_taxi_stops: bool + left_marking_style: str | None = None + left_marking_color: str | None = None + right_marking_style: str | None = None + right_marking_color: str | None = None + conditioning_visible: bool = True + + +def _mapping(value: object, context: str) -> dict[str, Any]: + if not isinstance(value, dict): + raise GameMapError(f"{context} must be a mapping") + if any(not isinstance(key, str) for key in value): + raise GameMapError(f"{context} keys must be strings") + return dict(value) + + +def _sequence(value: object, context: str) -> list[Any]: + if not isinstance(value, list): + raise GameMapError(f"{context} must be a sequence") + return value + + +def _positive_float(value: object, context: str) -> float: + number = _finite_float(value, context) + if number <= 0.0: + raise GameMapError(f"{context} must be positive") + return number + + +def _nonnegative_float(value: object, context: str) -> float: + number = _finite_float(value, context) + if number < 0.0: + raise GameMapError(f"{context} must be nonnegative") + return number + + +def _finite_float(value: object, context: str) -> float: + if isinstance(value, bool): + raise GameMapError(f"{context} must be a number") + try: + number = float(value) + except (TypeError, ValueError) as exc: + raise GameMapError(f"{context} must be a number") from exc + if not math.isfinite(number): + raise GameMapError(f"{context} must be finite") + return number + + +def _read_document(path: Path) -> dict[str, Any]: + path = Path(path).expanduser().resolve() + if not path.is_file(): + raise GameMapError(f"Game-map path does not exist or is not a file: {path}") + if not path.name.endswith(GAME_MAP_SUFFIX): + raise GameMapError(f"Game maps must use the {GAME_MAP_SUFFIX} suffix") + try: + raw = yaml.safe_load(path.read_text(encoding="utf-8")) + except yaml.YAMLError as exc: + raise GameMapError(f"Could not parse {path}: {exc}") from exc + return _mapping(raw, "map document") + + +def _parse_map_identity(doc: dict[str, Any]) -> tuple[str, str]: + version = doc.get("schema_version") + if version != _SCHEMA_VERSION: + raise GameMapError( + f"Unsupported schema_version {version!r}; expected {_SCHEMA_VERSION}" + ) + fields = set(doc) + missing = _REQUIRED_ROOT_FIELDS - fields + if missing: + raise GameMapError(f"Map is missing required fields {sorted(missing)}") + unknown = fields - (_REQUIRED_ROOT_FIELDS | _OPTIONAL_ROOT_FIELDS) + if unknown: + raise GameMapError(f"Map has unknown fields {sorted(unknown)}") + map_id = str(doc["id"]).strip() + name = str(doc["name"]).strip() + if not map_id: + raise GameMapError("Map id must not be empty") + if not name: + raise GameMapError("Map name must not be empty") + return map_id, name + + +def _parse_variants( + raw_spawn: dict[str, Any], source_path: Path +) -> tuple[GameMapVisualVariant, ...]: + variants_raw = _mapping(raw_spawn.get("variants"), "spawn.variants") + if "default" not in variants_raw: + raise GameMapError("Every spawn must define a default visual variant") + variants: list[GameMapVisualVariant] = [] + for name, raw_variant in variants_raw.items(): + variant = _mapping(raw_variant, f"variant {name!r}") + unknown = set(variant) - {"image", "prompt"} + if unknown: + raise GameMapError(f"Variant {name!r} has unknown fields {sorted(unknown)}") + image_value = variant.get("image") + image = None if image_value is None else str(image_value).strip() + prompt = str(variant.get("prompt", "")).strip() + if not prompt: + raise GameMapError(f"Variant {name!r} requires a non-empty prompt") + if image_value is not None and not image: + raise GameMapError(f"Variant {name!r} image must not be empty") + if image is not None: + resolve_seed_asset(source_path, image) + variants.append(GameMapVisualVariant(name=name, image=image, prompt=prompt)) + variants.sort(key=lambda item: (item.name != "default", item.name)) + return tuple(variants) + + +def load_game_map_header(path: Path) -> GameMapHeader: + """Load map name and default-spawn variants without resolving geometry.""" + source_path = Path(path).expanduser().resolve() + doc = _read_document(source_path) + map_id, name = _parse_map_identity(doc) + spawns = _sequence(doc.get("spawns"), "spawns") + if not spawns: + raise GameMapError("Map must define at least one spawn") + first_spawn = _mapping(spawns[0], "spawns[0]") + return GameMapHeader( + map_id=map_id, + name=name, + variants=_parse_variants(first_spawn, source_path), + source_path=source_path, + ) + + +def resolve_seed_asset(source_path: Path, reference: str) -> Path: + """Resolve a map-relative or package seed-image reference.""" + if reference.startswith("package://"): + location = reference.removeprefix("package://") + package, separator, resource = location.partition("/") + if not separator or not package or not resource: + raise GameMapError( + "Package assets must use package://package/path/to/resource" + ) + traversable = resources.files(package).joinpath(resource) + if not traversable.is_file(): + raise GameMapError(f"Seed image does not exist: {reference}") + return Path(str(traversable)) + path = Path(reference).expanduser() + if not path.is_absolute(): + path = source_path.parent / path + path = path.resolve() + if not path.is_file(): + raise GameMapError(f"Seed image does not exist: {path}") + return path + + +def _parse_attribute_values(raw: dict[str, Any], context: str) -> dict[str, object]: + """Validate and normalize partial profile-compatible attributes.""" + unknown = set(raw) - _PROFILE_ATTRIBUTE_FIELDS + if unknown: + raise GameMapError(f"{context} has unknown attributes {sorted(unknown)}") + result: dict[str, object] = {} + for key in ( + "lane_width_m", + "speed_limit_mps", + "culdesac_radius_m", + ): + if key in raw: + result[key] = _positive_float(raw[key], f"{context}.{key}") + if "curb_offset_m" in raw: + result["curb_offset_m"] = _nonnegative_float( + raw["curb_offset_m"], f"{context}.curb_offset_m" + ) + if "curb" in raw: + if type(raw["curb"]) is not bool: + raise GameMapError(f"{context}.curb must be a boolean") + result["curb"] = raw["curb"] + if "lanes" in raw: + directions = tuple( + str(value).lower() for value in _sequence(raw["lanes"], f"{context}.lanes") + ) + if not directions or any( + value not in {"forward", "backward"} for value in directions + ): + raise GameMapError(f"{context}.lanes must contain forward/backward values") + result["lanes"] = directions + if "lane_marking" in raw: + marking = _mapping(raw["lane_marking"], f"{context}.lane_marking") + if set(marking) != {"style", "color"}: + raise GameMapError(f"{context}.lane_marking requires style and color") + result["lane_marking"] = ( + str(marking["style"]).upper(), + str(marking["color"]).upper(), + ) + if "divider_markings" in raw: + dividers: list[tuple[str, str]] = [] + for index, value in enumerate( + _sequence(raw["divider_markings"], f"{context}.divider_markings") + ): + divider = _mapping(value, f"{context}.divider_markings[{index}]") + if set(divider) != {"style", "color"}: + raise GameMapError( + f"{context}.divider_markings[{index}] requires style and color" + ) + dividers.append( + (str(divider["style"]).upper(), str(divider["color"]).upper()) + ) + result["divider_markings"] = tuple(dividers) + return result + + +def _parse_profiles(doc: dict[str, Any]) -> dict[str, _Profile]: + """Parse optional partial profile defaults.""" + raw_profiles = _mapping(doc.get("profiles", {}), "profiles") + profiles: dict[str, _Profile] = {} + for profile_id, raw_value in raw_profiles.items(): + if not profile_id: + raise GameMapError("Profile ids must not be empty") + raw = _mapping(raw_value, f"profile {profile_id!r}") + profiles[profile_id] = _Profile( + profile_id=profile_id, + values=_parse_attribute_values(raw, f"profile {profile_id!r}"), + ) + return profiles + + +def _parse_compiler_settings(doc: dict[str, Any]) -> _CompilerSettings: + raw = _mapping(doc.get("compiler"), "compiler") + expected = { + "sample_spacing_m", + "ground_margin_m", + "intersection_connector_samples", + } + if set(raw) != expected: + raise GameMapError(f"compiler must contain exactly {sorted(expected)}") + samples = raw["intersection_connector_samples"] + if type(samples) is not int or samples < 2: + raise GameMapError( + "compiler.intersection_connector_samples must be an integer >= 2" + ) + return _CompilerSettings( + sample_spacing_m=_positive_float( + raw["sample_spacing_m"], "compiler.sample_spacing_m" + ), + ground_margin_m=_nonnegative_float( + raw["ground_margin_m"], "compiler.ground_margin_m" + ), + intersection_connector_samples=samples, + ) + + +def _offset_polyline(points: np.ndarray, offset_m: float) -> np.ndarray: + tangents = np.gradient(points, axis=0) + lengths = np.linalg.norm(tangents, axis=1) + tangents = tangents / np.maximum(lengths[:, None], 1.0e-9) + normals = np.column_stack((-tangents[:, 1], tangents[:, 0])) + return points + normals * offset_m + + +def _xyz(points_xy: np.ndarray) -> np.ndarray: + return np.column_stack((points_xy, np.zeros(len(points_xy)))).astype(np.float32) + + +def _surface_for_road(centerline: np.ndarray, width_m: float) -> np.ndarray: + left = _offset_polyline(centerline, width_m * 0.5) + right = _offset_polyline(centerline, -width_m * 0.5) + return _xyz(np.concatenate((left, right[::-1], left[:1]), axis=0)) + + +def _segments(points: np.ndarray) -> np.ndarray: + if len(points) < 2: + return np.empty((0, 2, 3), dtype=np.float32) + return np.stack((points[:-1], points[1:]), axis=1).astype(np.float32) + + +def _lane_edge_markings( + attributes: GameMapLinearAttributes, index: int, direction: str +) -> tuple[tuple[str, str], tuple[str, str]]: + virtual = ("VIRTUAL", "WHITE") + above = attributes.divider_markings[index - 1] if index > 0 else virtual + below = ( + attributes.divider_markings[index] + if index < len(attributes.directions) - 1 + else virtual + ) + return (below, above) if direction == "backward" else (above, below) + + +def _bezier( + start: np.ndarray, control: np.ndarray, end: np.ndarray, samples: int +) -> np.ndarray: + t = np.linspace(0.0, 1.0, samples, dtype=np.float32)[:, None] + return ((1.0 - t) ** 2 * start + 2.0 * (1.0 - t) * t * control + t**2 * end).astype( + np.float32 + ) diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/compiler.py b/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/compiler.py new file mode 100644 index 000000000..3a5b9b3dd --- /dev/null +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/compiler.py @@ -0,0 +1,401 @@ +# SPDX-License-Identifier: Apache-2.0 +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. + +"""Content-addressed ClipGT compilation for semantic game maps.""" + +from __future__ import annotations + +import hashlib +import io +import json +import os +import tempfile +import zipfile +from dataclasses import dataclass +from pathlib import Path + +import numpy as np +import pyarrow as pa +import pyarrow.parquet as pq +import yaml +from filelock import FileLock +from PIL import Image + +from omnidreams_game_engine import camera_defaults +from omnidreams_game_engine.camera_defaults import DEFAULT_FRONT_CAMERA_LOGICAL_NAME +from omnidreams_game_engine.game_map import spawn_render +from omnidreams_game_engine.game_map._schema import resolve_seed_asset +from omnidreams_game_engine.game_map.loader import load_game_map +from omnidreams_game_engine.game_map.types import ( + ResolvedGameMap, + game_map_to_dict, +) +from omnidreams_game_engine.math3d import rig_pose_from_state +from omnidreams_game_engine.ply_io import save_mesh_vf +from omnidreams_game_engine.scene_fixture import _calibration_row + +# Pre-release maps and compiler output stay at version 1. Do not increment this +# during development; a future release process owns version changes. +_COMPILER_VERSION = "1" +_START_TIMESTAMP_US = 1_700_000_000_000_000 + + +@dataclass(frozen=True) +class CompiledGameMap: + """Resolved map and its private renderer archive.""" + + source_path: Path + """Canonical semantic YAML path.""" + + archive_path: Path + """Content-addressed private USDZ/ClipGT archive.""" + + game_map: ResolvedGameMap + """Resolved semantic runtime map.""" + + cache_hit: bool + """Whether compilation reused an existing archive.""" + + +def _cache_root() -> Path: + return ( + Path( + os.path.expanduser( + os.environ.get("FLASHDREAMS_CACHE_DIR", "~/.cache/flashdreams") + ) + ) + / "omnidreams-game-engine" + / "game-maps" + ) + + +def _digest(game_map: ResolvedGameMap) -> str: + hasher = hashlib.sha256() + hasher.update(_COMPILER_VERSION.encode()) + hasher.update(Path(__file__).read_bytes()) + hasher.update(game_map.source_path.read_bytes()) + resolved = game_map_to_dict(game_map) + resolved.pop("source_path", None) + hasher.update(json.dumps(resolved, sort_keys=True, separators=(",", ":")).encode()) + for spawn in game_map.spawns: + for variant in spawn.variants: + hasher.update(variant.name.encode()) + hasher.update(variant.prompt.encode()) + if variant.image is None: + hasher.update(b"generated-spawn-first-frame") + hasher.update(spawn_render.SPAWN_RENDERER_VERSION.encode()) + hasher.update(Path(spawn_render.__file__).read_bytes()) + hasher.update(Path(camera_defaults.__file__).read_bytes()) + else: + asset = resolve_seed_asset(game_map.source_path, variant.image) + hasher.update(asset.read_bytes()) + return hasher.hexdigest() + + +def _point(point: np.ndarray) -> dict[str, float]: + return {"x": float(point[0]), "y": float(point[1]), "z": float(point[2])} + + +def _key(game_map: ResolvedGameMap, label: str) -> dict[str, str]: + return { + "clip_id": game_map.map_id, + "label_class_id": label, + "map_id": game_map.map_id, + "map_id_version": f"v{game_map.schema_version}", + } + + +def _lane_rows(game_map: ResolvedGameMap) -> list[dict[str, object]]: + shared_edges = { + lane_edge + for divider in game_map.lane_dividers + for lane_edge in divider.lane_edges + } + rows: list[dict[str, object]] = [] + for lane in game_map.lanes: + if not lane.conditioning_visible: + continue + left_shared = (lane.lane_id, "left") in shared_edges + right_shared = (lane.lane_id, "right") in shared_edges + left_style, left_color = lane.left_marking_style, lane.left_marking_color + right_style, right_color = lane.right_marking_style, lane.right_marking_color + rows.append( + { + "key": _key(game_map, lane.lane_id), + "lane": { + "left_rail": [_point(point) for point in lane.left_edge_world], + "right_rail": [_point(point) for point in lane.right_edge_world], + "vehicle_types": ["CAR"], + "map_end": "NONE", + "use_types": [], + "left_edge_styles": ( + [left_style if left_shared else "VIRTUAL"] + if lane.allows_taxi_stops + else [] + ), + "right_edge_styles": ( + [right_style if right_shared else "VIRTUAL"] + if lane.allows_taxi_stops + else [] + ), + "left_edge_colors": [left_color if left_shared else "WHITE"], + "right_edge_colors": [right_color if right_shared else "WHITE"], + "egomotion_label_class_id": "ego", + }, + "version": 1, + } + ) + return rows + + +def _lane_line_rows(game_map: ResolvedGameMap) -> list[dict[str, object]]: + rows: list[dict[str, object]] = [] + for divider in game_map.lane_dividers: + rows.append( + { + "key": _key(game_map, f"lane_line:{divider.divider_id}"), + "lane_line": { + "line_rail": [_point(point) for point in divider.polyline_world], + "styles": [divider.style], + "colors": [divider.color], + "left_driving_direction": ["FORWARD"], + "right_driving_direction": ["FORWARD"], + "is_first_point_physical_end": "false", + "is_last_point_physical_end": "false", + "egomotion_label_class_id": "ego", + }, + "version": 1, + } + ) + for marking in game_map.line_markings: + rows.append( + { + "key": _key(game_map, f"lane_line:{marking.marking_id}"), + "lane_line": { + "line_rail": [_point(point) for point in marking.polyline_world], + "styles": [marking.style], + "colors": [marking.color], + "left_driving_direction": ["FORWARD"], + "right_driving_direction": ["FORWARD"], + "is_first_point_physical_end": "true", + "is_last_point_physical_end": "true", + "egomotion_label_class_id": "ego", + }, + "version": 1, + } + ) + return rows + + +def _boundary_rows(game_map: ResolvedGameMap) -> list[dict[str, object]]: + return [ + { + "key": _key(game_map, boundary.boundary_id), + "road_boundary": { + "location": [_point(point) for point in boundary.polyline_world], + "category": "road_boundary", + "egomotion_label_class_id": "ego", + }, + "version": 1, + } + for element in game_map.elements + for boundary in element.road_boundaries + ] + + +def _intersection_rows(game_map: ResolvedGameMap) -> list[dict[str, object]]: + return [ + { + "key": _key(game_map, f"intersection:{element.element_id}"), + "intersection_area": { + "location": [_point(point) for point in element.surface_world], + "category": "intersection", + "egomotion_label_class_id": "ego", + }, + "version": 1, + } + for element in game_map.elements + if element.element_type == "intersection" + ] + + +def _road_marking_rows(game_map: ResolvedGameMap) -> list[dict[str, object]]: + roadnet_masks = [ + { + "key": _key(game_map, f"roadnet_mask:{element.element_id}"), + "road_marking": { + "location": [_point(point) for point in element.surface_world], + "category": "ROI_POLYGON_ROADNET_MASK", + "egomotion_label_class_id": "ego", + }, + "version": 1, + } + for element in game_map.elements + if element.element_type == "parking_lot" + ] + parking_space_markings = [ + { + "key": _key(game_map, f"road_marking:{index}"), + "road_marking": { + "location": [_point(point) for point in polygon], + "category": "ROI_POLYGON_ROAD_MARKING", + "egomotion_label_class_id": "ego", + }, + "version": 1, + } + for index, polygon in enumerate(game_map.road_marking_polygons_world) + ] + return roadnet_masks + parking_space_markings + + +def _write_parquet( + archive: zipfile.ZipFile, name: str, rows: list[dict[str, object]] +) -> None: + if not rows: + return + buffer = io.BytesIO() + pq.write_table(pa.Table.from_pylist(rows), buffer) + archive.writestr(name, buffer.getvalue()) + + +def _write_image(archive: zipfile.ZipFile, name: str, source: Path) -> None: + buffer = io.BytesIO() + with Image.open(source) as image: + image.convert("RGB").save(buffer, format="PNG") + archive.writestr(name, buffer.getvalue()) + + +def _write_image_array( + archive: zipfile.ZipFile, name: str, image_array: np.ndarray +) -> None: + buffer = io.BytesIO() + Image.fromarray(image_array).save(buffer, format="PNG") + archive.writestr(name, buffer.getvalue()) + + +def _metadata(game_map: ResolvedGameMap) -> dict[str, object]: + return { + "scene_id": game_map.map_id, + "dataset_hash": "semantic-game-map", + "is_resumable": False, + "sensors": { + "camera_ids": [DEFAULT_FRONT_CAMERA_LOGICAL_NAME], + "lidar_ids": [], + }, + "time_range": { + "start": _START_TIMESTAMP_US, + "end": _START_TIMESTAMP_US + 33_333, + }, + "version_string": f"omnidreams-game-map-{_COMPILER_VERSION}", + } + + +def _trajectory(game_map: ResolvedGameMap) -> dict[str, object]: + spawn = game_map.default_spawn + pose = rig_pose_from_state( + float(spawn.position_world[0]), + float(spawn.position_world[1]), + float(spawn.position_world[2]), + spawn.yaw_rad, + ).tolist() + return { + "rig_trajectories": [ + { + "T_rig_worlds": [pose, pose], + "T_rig_world_timestamps_us": [ + _START_TIMESTAMP_US, + _START_TIMESTAMP_US + 33_333, + ], + } + ] + } + + +def _write_archive(path: Path, game_map: ResolvedGameMap) -> None: + spawn = game_map.default_spawn + generated_image: np.ndarray | None = None + with zipfile.ZipFile(path, mode="w", compression=zipfile.ZIP_STORED) as archive: + archive.writestr( + "metadata.yaml", yaml.safe_dump(_metadata(game_map), sort_keys=True) + ) + archive.writestr("rig_trajectories.json", json.dumps(_trajectory(game_map))) + archive.writestr( + "game_map.json", + json.dumps(game_map_to_dict(game_map), separators=(",", ":")), + ) + archive.writestr( + "mesh_ground.ply", + save_mesh_vf(game_map.ground_vertices, game_map.ground_faces), + ) + for variant in spawn.variants: + suffix = "" if variant.name == "default" else f"_{variant.name}" + archive.writestr(f"prompt{suffix}.txt", variant.prompt) + image_name = f"first_image{suffix}.png" + if variant.image is None: + if generated_image is None: + generated_image = spawn_render.render_spawn_first_frame( + game_map, spawn + ) + _write_image_array( + archive, + image_name, + generated_image, + ) + else: + _write_image( + archive, + image_name, + resolve_seed_asset(game_map.source_path, variant.image), + ) + _write_parquet( + archive, "clipgt/calibration_estimate.parquet", _calibration_row() + ) + _write_parquet(archive, "clipgt/lane.parquet", _lane_rows(game_map)) + _write_parquet(archive, "clipgt/lane_line.parquet", _lane_line_rows(game_map)) + _write_parquet( + archive, "clipgt/road_boundary.parquet", _boundary_rows(game_map) + ) + _write_parquet( + archive, "clipgt/intersection_area.parquet", _intersection_rows(game_map) + ) + _write_parquet( + archive, "clipgt/road_marking.parquet", _road_marking_rows(game_map) + ) + + +def compile_game_map( + path: Path, + *, + cache_root: Path | None = None, + force: bool = False, +) -> CompiledGameMap: + """Compile a map, optionally replacing its valid cached archive.""" + game_map = load_game_map(path) + digest = _digest(game_map) + root = _cache_root() if cache_root is None else Path(cache_root) + output_dir = root / digest + archive_path = output_dir / f"{game_map.map_id}.usdz" + lock = FileLock(str(root / f"{digest}.lock")) + root.mkdir(parents=True, exist_ok=True) + with lock: + if archive_path.is_file() and not force: + try: + with zipfile.ZipFile(archive_path, "r") as archive: + if "game_map.json" in archive.namelist(): + return CompiledGameMap( + game_map.source_path, archive_path, game_map, True + ) + except (OSError, zipfile.BadZipFile): + pass + output_dir.mkdir(parents=True, exist_ok=True) + file_descriptor, temporary_name = tempfile.mkstemp( + dir=output_dir, prefix=".map-", suffix=".usdz" + ) + os.close(file_descriptor) + temporary = Path(temporary_name) + try: + _write_archive(temporary, game_map) + temporary.replace(archive_path) + finally: + temporary.unlink(missing_ok=True) + return CompiledGameMap(game_map.source_path, archive_path, game_map, False) diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/loader.py b/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/loader.py new file mode 100644 index 000000000..f756ec97c --- /dev/null +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/loader.py @@ -0,0 +1,3029 @@ +# SPDX-License-Identifier: Apache-2.0 +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. + +"""Node-graph game-map loading and geometry compilation.""" + +from __future__ import annotations + +import math +from dataclasses import dataclass, replace +from pathlib import Path +from typing import Any + +import numpy as np +from shapely import is_valid_reason +from shapely.geometry import LineString, Point, Polygon +from shapely.geometry.base import BaseGeometry +from shapely.ops import polygonize, substring, unary_union + +from omnidreams_game_engine.game_map._schema import ( + _SCHEMA_VERSION, + GameMapError, + _bezier, + _finite_float, + _lane_edge_markings, + _LaneBuild, + _mapping, + _nonnegative_float, + _offset_polyline, + _parse_attribute_values, + _parse_compiler_settings, + _parse_map_identity, + _parse_profiles, + _parse_variants, + _positive_float, + _Profile, + _read_document, + _sequence, + _xyz, +) +from omnidreams_game_engine.game_map.traffic import compile_traffic +from omnidreams_game_engine.game_map.types import ( + GameMapBoundaryAttributes, + GameMapCurb, + GameMapElement, + GameMapLane, + GameMapLaneDivider, + GameMapLinearAttributes, + GameMapNode, + GameMapParkingAccess, + GameMapRoad, + GameMapRoadBoundary, + GameMapSpawn, + GameMapTopology, + ResolvedGameMap, +) + +_POSITION_TOLERANCE_M = 0.05 +_AREA_TOLERANCE_M2 = 1.0e-4 +_LINE_TOLERANCE_M = 1.0e-4 +_OPENING_TOLERANCE_M = 1.0e-2 +"""Maximum numeric drift when matching separately materialized seam polylines.""" + +_BOUNDARY_CLEARANCE_M = 1.0e-2 +"""Outward clearance that keeps sampled roadside corners behind their openings.""" + +_INTERSECTION_TURN_HANDLE_RATIO = 0.4 +"""Cubic handle length as a fraction of the connector endpoint chord.""" + +_LINEAR_ATTRIBUTE_FIELDS = frozenset( + { + "lane_width_m", + "curb_offset_m", + "lanes", + "speed_limit_mps", + "curb", + "lane_marking", + "divider_markings", + } +) +_REQUIRED_LINEAR_ATTRIBUTE_FIELDS = _LINEAR_ATTRIBUTE_FIELDS - {"curb"} + +_NODE_ATTRIBUTE_FIELDS = { + "intersection": frozenset({"curb"}), + "road_joint": frozenset(), + "cul_de_sac": frozenset({"curb", "culdesac_radius_m"}), + "parking_lot": frozenset(), + "driveway": frozenset(), +} + + +@dataclass(frozen=True) +class _RoadSpec: + road: GameMapRoad + spans_xy: tuple[np.ndarray, ...] + + +@dataclass +class _LaneIncidence: + lane: _LaneBuild + node_id: str + kind: str + edge_ref: str + + +@dataclass(frozen=True) +class _Connection: + """Exact shared opening between two resolved surface elements.""" + + connection_id: str + """Stable topology-derived connection identifier.""" + + first_element_id: str + """First connected surface element.""" + + second_element_id: str + """Second connected surface element.""" + + opening_xy: np.ndarray + """Shared boundary polyline with shape ``[N, 2]``.""" + + +@dataclass(frozen=True) +class _RoadArm: + """One road cross-section oriented outward from an incident node.""" + + node_id: str + """Identifier of the node that owns the arm.""" + + road: GameMapRoad + """Authored road incident to the node.""" + + path_xy: np.ndarray + """Sampled road centerline oriented outward from the node.""" + + attributes: GameMapLinearAttributes + """Authored cross-section oriented outward from the node.""" + + +@dataclass(frozen=True) +class _ArmTransition: + """One node-owned transition from a local to an authored cross-section.""" + + arm: _RoadArm + """Road arm whose authored cross-section differs from the node.""" + + local_attributes: GameMapLinearAttributes + """Dominant cross-section used at the node opening.""" + + length_m: float + """Distance over which the node cross-section becomes the road cross-section.""" + + +@dataclass(frozen=True) +class _TransitionGeometry: + """Resolved centerline and cross-sections for one tapered node arm.""" + + transition: _ArmTransition + """Semantic transition resolved for this arm.""" + + path_xy: np.ndarray + """Sampled centerline from the node opening to the authored road.""" + + +@dataclass(frozen=True) +class _BoundaryArmGeometry: + """Boundary rails from a node core to one connected surface.""" + + reference_id: str + """Identifier of the road or inferred parking access.""" + + left_xy: np.ndarray + """Left roadside rail oriented from the core to the opening.""" + + right_xy: np.ndarray + """Right roadside rail oriented from the core to the opening.""" + + +def _point(value: object, context: str) -> np.ndarray: + raw = _mapping(value, context) + if set(raw) != {"x_m", "y_m"}: + raise GameMapError(f"{context} requires exactly x_m and y_m") + return np.asarray( + [ + _finite_float(raw["x_m"], f"{context}.x_m"), + _finite_float(raw["y_m"], f"{context}.y_m"), + ], + dtype=np.float64, + ) + + +def _resolve_attribute_values( + raw: dict[str, Any], + profiles: dict[str, _Profile], + *, + structural_fields: set[str], + allowed_fields: frozenset[str], + required_fields: frozenset[str], + context: str, +) -> tuple[str | None, dict[str, object]]: + """Resolve direct attributes over optional partial profile defaults.""" + profile_id = None if "profile" not in raw else str(raw["profile"]).strip() + if profile_id is not None and profile_id not in profiles: + raise GameMapError(f"{context} references unknown profile {profile_id!r}") + direct_raw = { + key: value + for key, value in raw.items() + if key not in structural_fields and key != "profile" + } + unknown = set(direct_raw) - allowed_fields + if unknown: + raise GameMapError(f"{context} has unknown attributes {sorted(unknown)}") + direct = _parse_attribute_values(direct_raw, context) + values = { + key: value + for key, value in ( + profiles[profile_id].values.items() if profile_id is not None else () + ) + if key in allowed_fields + } + values.update(direct) + if "curb" in allowed_fields: + values.setdefault("curb", True) + missing = required_fields - set(values) + if missing: + raise GameMapError(f"{context} is missing attributes {sorted(missing)}") + return profile_id, values + + +def _linear_attributes( + values: dict[str, object], context: str +) -> GameMapLinearAttributes: + """Build a complete linear attribute bundle.""" + directions = tuple(str(value) for value in values["lanes"]) + dividers = tuple( + (str(value[0]), str(value[1])) for value in values["divider_markings"] + ) + if len(dividers) != len(directions) - 1: + raise GameMapError( + f"{context}.divider_markings must contain one entry per adjacent lane pair" + ) + marking = tuple(str(value) for value in values["lane_marking"]) + return GameMapLinearAttributes( + curb=bool(values["curb"]), + lane_width_m=float(values["lane_width_m"]), + curb_offset_m=float(values["curb_offset_m"]), + directions=directions, + speed_limit_mps=float(values["speed_limit_mps"]), + marking_style=marking[0], + marking_color=marking[1], + divider_markings=dividers, + ) + + +def _parse_nodes( + doc: dict[str, Any], profiles: dict[str, _Profile] +) -> tuple[GameMapNode, ...]: + nodes: list[GameMapNode] = [] + ids: set[str] = set() + for index, value in enumerate(_sequence(doc.get("nodes"), "nodes")): + raw = _mapping(value, f"nodes[{index}]") + node_type = str(raw.get("type", "")) + if node_type not in _NODE_ATTRIBUTE_FIELDS: + raise GameMapError(f"nodes[{index}] has unsupported type {node_type!r}") + node_id = str(raw["id"]).strip() + if not node_id or node_id in ids: + raise GameMapError(f"Node id {node_id!r} is empty or duplicated") + ids.add(node_id) + context = f"node {node_id!r}" + if node_type == "parking_lot": + expected = { + "id", + "type", + "vertices", + "connected_to", + "opening_vertex", + } + if set(raw) != expected: + raise GameMapError( + f"{context} requires exactly id, type, vertices, " + "connected_to, and opening_vertex" + ) + vertices = tuple( + tuple( + float(item) + for item in _point(value, f"{context}.vertices[{vertex_index}]") + ) + for vertex_index, value in enumerate( + _sequence(raw["vertices"], f"{context}.vertices") + ) + ) + if len(vertices) < 3: + raise GameMapError(f"{context}.vertices requires at least three points") + polygon = Polygon(vertices) + if not polygon.is_valid or polygon.area <= _AREA_TOLERANCE_M2: + raise GameMapError(f"{context}.vertices must form a simple polygon") + if polygon.exterior.is_ccw: + raise GameMapError(f"{context}.vertices must be clockwise") + if len(set(vertices)) != len(vertices): + raise GameMapError(f"{context}.vertices contains duplicate points") + if not str(raw["connected_to"]).strip(): + raise GameMapError(f"{context}.connected_to must not be empty") + opening_value = raw["opening_vertex"] + if type(opening_value) is not int: + raise GameMapError(f"{context}.opening_vertex must be an integer") + if opening_value < 1 or opening_value > len(vertices): + raise GameMapError( + f"{context}.opening_vertex must be between 1 and {len(vertices)}" + ) + centroid = polygon.centroid + nodes.append( + GameMapNode( + node_id=node_id, + node_type=node_type, + x_m=float(centroid.x), + y_m=float(centroid.y), + profile_id=None, + attributes=GameMapBoundaryAttributes(curb=True), + geometry={}, + polygon_vertices_xy=vertices, + ) + ) + continue + if not {"id", "type", "pose"} <= set(raw): + raise GameMapError(f"nodes[{index}] requires id, type, and pose") + pose = _mapping(raw["pose"], f"node {node_id!r}.pose") + if set(pose) != {"x_m", "y_m"}: + raise GameMapError(f"Node {node_id!r}.pose requires x_m and y_m") + if node_type in {"road_joint", "driveway"}: + expected = {"id", "type", "pose"} + allowed = set(expected) + if node_type == "road_joint": + allowed.add("lane_transition_length_m") + missing = expected - set(raw) + unknown = set(raw) - allowed + if missing: + raise GameMapError(f"{context} is missing attributes {sorted(missing)}") + if unknown: + raise GameMapError( + f"{context} has unknown attributes {sorted(unknown)}" + ) + profile_id = None + geometry = ( + { + "lane_transition_length_m": _nonnegative_float( + raw.get("lane_transition_length_m", 0.0), + f"{context}.lane_transition_length_m", + ), + } + if node_type == "road_joint" + else {} + ) + attributes: GameMapBoundaryAttributes | GameMapLinearAttributes + attributes = GameMapBoundaryAttributes(curb=False) + else: + allowed = _NODE_ATTRIBUTE_FIELDS[node_type] + required = { + "intersection": frozenset(), + "cul_de_sac": frozenset({"culdesac_radius_m"}), + }[node_type] + profile_id, values = _resolve_attribute_values( + raw, + profiles, + structural_fields={"id", "type", "pose"} + | ( + {"lane_transition_length_m"} + if node_type == "intersection" + else set() + ), + allowed_fields=allowed, + required_fields=required, + context=context, + ) + geometry = { + key: float(item) + for key, item in values.items() + if key in {"culdesac_radius_m"} + } + if node_type == "intersection": + geometry["lane_transition_length_m"] = _nonnegative_float( + raw.get("lane_transition_length_m", 0.0), + f"{context}.lane_transition_length_m", + ) + attributes = GameMapBoundaryAttributes(curb=bool(values["curb"])) + nodes.append( + GameMapNode( + node_id=node_id, + node_type=node_type, + x_m=_finite_float(pose["x_m"], f"node {node_id!r}.pose.x_m"), + y_m=_finite_float(pose["y_m"], f"node {node_id!r}.pose.y_m"), + profile_id=profile_id, + attributes=attributes, + geometry=geometry, + ) + ) + if not nodes: + raise GameMapError("Map must define at least one node") + return tuple(nodes) + + +def _path_point_spans( + start: np.ndarray, path_points: list[np.ndarray], end: np.ndarray, road_id: str +) -> tuple[np.ndarray, ...]: + points = np.asarray([start, *path_points, end], dtype=np.float64) + segment_lengths = np.linalg.norm(np.diff(points, axis=0), axis=1) + for index, length in enumerate(segment_lengths): + if length <= _POSITION_TOLERANCE_M: + raise GameMapError( + f"Road {road_id!r}.path creates a degenerate segment at index {index}" + ) + + tangents = np.empty_like(points) + is_closed = np.linalg.norm(start - end) <= _POSITION_TOLERANCE_M + if is_closed: + if len(path_points) < 2: + raise GameMapError( + f"Self-loop road {road_id!r} requires at least two path points" + ) + loop_tangent = 0.5 * (points[1] - points[-2]) + tangents[0] = loop_tangent + tangents[-1] = loop_tangent + else: + tangents[0] = points[1] - points[0] + tangents[-1] = points[-1] - points[-2] + if len(points) > 2: + tangents[1:-1] = 0.5 * (points[2:] - points[:-2]) + + for index, tangent in enumerate(tangents): + if np.linalg.norm(tangent) <= _POSITION_TOLERANCE_M: + raise GameMapError( + f"Road {road_id!r}.path creates a degenerate tangent at point {index}" + ) + + spans: list[np.ndarray] = [] + for index in range(len(points) - 1): + control_1 = points[index] + tangents[index] / 3.0 + control_2 = points[index + 1] - tangents[index + 1] / 3.0 + spans.append( + np.vstack((points[index], control_1, control_2, points[index + 1])) + ) + return tuple(spans) + + +def _bezier_spans( + value: object, start: np.ndarray, end: np.ndarray, road_id: str +) -> tuple[np.ndarray, ...]: + bezier = _sequence(value, f"road {road_id!r}.bezier") + if not bezier: + raise GameMapError(f"Road {road_id!r}.bezier must not be empty") + spans: list[np.ndarray] = [] + cursor = start + for span_index, span_value in enumerate(bezier): + span = _mapping(span_value, f"road {road_id!r}.bezier[{span_index}]") + if set(span) != {"control_points", "end"}: + raise GameMapError( + f"Road {road_id!r} Bezier spans require control_points and end" + ) + controls = _sequence( + span["control_points"], + f"road {road_id!r}.bezier[{span_index}].control_points", + ) + if len(controls) != 2: + raise GameMapError( + f"Road {road_id!r} Bezier spans require exactly two control points" + ) + control_1 = _point( + controls[0], + f"road {road_id!r}.bezier[{span_index}].control_points[0]", + ) + control_2 = _point( + controls[1], + f"road {road_id!r}.bezier[{span_index}].control_points[1]", + ) + span_end = _point(span["end"], f"road {road_id!r}.bezier[{span_index}].end") + if np.linalg.norm(control_1 - cursor) <= _POSITION_TOLERANCE_M: + raise GameMapError( + f"Road {road_id!r} span {span_index} has a degenerate start tangent" + ) + if np.linalg.norm(span_end - control_2) <= _POSITION_TOLERANCE_M: + raise GameMapError( + f"Road {road_id!r} span {span_index} has a degenerate end tangent" + ) + spans.append(np.vstack((cursor, control_1, control_2, span_end))) + cursor = span_end + if np.linalg.norm(cursor - end) > _POSITION_TOLERANCE_M: + raise GameMapError( + f"Road {road_id!r} final Bezier endpoint must match its to-node pose " + f"within {_POSITION_TOLERANCE_M:g}m" + ) + return tuple(spans) + + +def _path_spans( + value: object, start: np.ndarray, end: np.ndarray, road_id: str +) -> tuple[np.ndarray, ...]: + path = _sequence(value, f"road {road_id!r}.path") + if not path: + raise GameMapError(f"Road {road_id!r}.path must not be empty") + path_points: list[np.ndarray] = [] + for index, item in enumerate(path): + raw = _mapping(item, f"road {road_id!r}.path[{index}]") + if "control_points" in raw or "end" in raw: + raise GameMapError( + f"Road {road_id!r}.path accepts path points only; " + "put explicit spans under bezier" + ) + path_points.append(_point(raw, f"road {road_id!r}.path[{index}]")) + return _path_point_spans(start, path_points, end, road_id) + + +def _parse_roads( + doc: dict[str, Any], nodes: dict[str, GameMapNode], profiles: dict[str, _Profile] +) -> tuple[_RoadSpec, ...]: + roads: list[_RoadSpec] = [] + ids: set[str] = set() + for index, value in enumerate(_sequence(doc.get("roads"), "roads")): + raw = _mapping(value, f"roads[{index}]") + if not {"id", "from", "to"} <= set(raw): + raise GameMapError(f"roads[{index}] requires id, from, and to") + road_id = str(raw["id"]).strip() + if not road_id or road_id in ids: + raise GameMapError(f"Road id {road_id!r} is empty or duplicated") + ids.add(road_id) + from_id, to_id = str(raw["from"]), str(raw["to"]) + for endpoint in (from_id, to_id): + if endpoint not in nodes: + raise GameMapError( + f"Road {road_id!r} references unknown node {endpoint!r}" + ) + if nodes[endpoint].node_type not in { + "intersection", + "road_joint", + "driveway", + "cul_de_sac", + }: + raise GameMapError( + f"Road {road_id!r} may connect only intersections, road joints, " + "driveways, and cul-de-sacs" + ) + context = f"road {road_id!r}" + profile_id, values = _resolve_attribute_values( + raw, + profiles, + structural_fields={"id", "from", "to", "path", "bezier"}, + allowed_fields=_LINEAR_ATTRIBUTE_FIELDS, + required_fields=_REQUIRED_LINEAR_ATTRIBUTE_FIELDS, + context=context, + ) + attributes = _linear_attributes(values, context) + start = np.asarray([nodes[from_id].x_m, nodes[from_id].y_m]) + end = np.asarray([nodes[to_id].x_m, nodes[to_id].y_m]) + path_spans: tuple[np.ndarray, ...] = () + if "path" in raw: + path_spans = _path_spans(raw["path"], start, end, road_id) + bezier_spans: tuple[np.ndarray, ...] = () + if "bezier" in raw: + bezier_spans = _bezier_spans(raw["bezier"], start, end, road_id) + if "bezier" in raw: + spans = bezier_spans + elif "path" in raw: + spans = path_spans + else: + spans = () + if np.linalg.norm(start - end) <= _POSITION_TOLERANCE_M: + raise GameMapError( + f"Self-loop road {road_id!r} requires path or bezier" + ) + runtime_spans = tuple( + np.column_stack((span, np.zeros(4))).astype(np.float32) for span in spans + ) + roads.append( + _RoadSpec( + GameMapRoad( + road_id=road_id, + from_node_id=from_id, + to_node_id=to_id, + profile_id=profile_id, + attributes=attributes, + bezier_spans_world=runtime_spans, + ), + tuple(spans), + ) + ) + if not roads: + raise GameMapError("Map must define at least one road") + return tuple(roads) + + +def _reverse_direction(direction: str) -> str: + return "backward" if direction == "forward" else "forward" + + +def _oriented_joint_attributes( + road: GameMapRoad, + *, + reverse: bool, +) -> GameMapLinearAttributes: + """Orient road attributes along the canonical path through a joint.""" + attributes = road.attributes + return _reversed_attributes(attributes) if reverse else attributes + + +def _reversed_attributes( + attributes: GameMapLinearAttributes, +) -> GameMapLinearAttributes: + """Reverse linear attributes with their physical cross-section order.""" + return replace( + attributes, + directions=tuple( + _reverse_direction(direction) + for direction in reversed(attributes.directions) + ), + divider_markings=tuple(reversed(attributes.divider_markings)), + ) + + +def _outward_attributes(road: GameMapRoad, node_id: str) -> GameMapLinearAttributes: + """Orient road attributes along its centerline away from ``node_id``.""" + return _oriented_joint_attributes( + road, + reverse=road.to_node_id == node_id, + ) + + +def _direction_block_count(attributes: GameMapLinearAttributes) -> int: + return 1 + sum( + first != second + for first, second in zip( + attributes.directions[:-1], + attributes.directions[1:], + strict=True, + ) + ) + + +def _opposing_divider( + attributes: GameMapLinearAttributes, +) -> tuple[str, str] | None: + indices = [ + index + for index, (first, second) in enumerate( + zip( + attributes.directions[:-1], + attributes.directions[1:], + strict=True, + ) + ) + if first != second + ] + return None if not indices else attributes.divider_markings[indices[0]] + + +def _dominant_cross_section( + first: GameMapLinearAttributes, + second: GameMapLinearAttributes, + context: str, +) -> GameMapLinearAttributes: + """Select the node-side profile that can contain both road profiles.""" + direction_set = {"backward", "forward"} + compatible = ( + set(first.directions) == set(second.directions) + and set(first.directions) <= direction_set + and _direction_block_count(first) <= 2 + and _direction_block_count(second) <= 2 + and _opposing_divider(first) == _opposing_divider(second) + ) + if not compatible: + raise GameMapError( + f"{context} requires compatible direction ordering and opposing dividers" + ) + + first_counts = { + direction: first.directions.count(direction) for direction in direction_set + } + second_counts = { + direction: second.directions.count(direction) for direction in direction_set + } + first_dominates = all( + first_counts[direction] >= second_counts[direction] + for direction in direction_set + ) + second_dominates = all( + second_counts[direction] >= first_counts[direction] + for direction in direction_set + ) + if not first_dominates and not second_dominates: + raise GameMapError( + f"{context} has conflicting directional lane counts; one road must " + "have at least as many lanes in both directions" + ) + if first_dominates and not second_dominates: + dominant = first + elif second_dominates and not first_dominates: + dominant = second + else: + dominant = first if first.lane_width_m >= second.lane_width_m else second + return replace( + dominant, + speed_limit_mps=min(first.speed_limit_mps, second.speed_limit_mps), + ) + + +def _cross_section_changes( + first: GameMapLinearAttributes, + second: GameMapLinearAttributes, +) -> bool: + return ( + first.directions != second.directions + or first.lane_width_m != second.lane_width_m + ) + + +def _lane_layout_for_arm( + layout: GameMapLinearAttributes, + arm: GameMapLinearAttributes, +) -> GameMapLinearAttributes: + return replace( + layout, + curb_offset_m=arm.curb_offset_m, + curb=arm.curb, + speed_limit_mps=arm.speed_limit_mps, + ) + + +def _resolve_linear_joint_nodes( + nodes: tuple[GameMapNode, ...], + road_specs: tuple[_RoadSpec, ...], +) -> tuple[GameMapNode, ...]: + """Infer linear attributes for every degree-two road joint and driveway.""" + incident: dict[str, list[GameMapRoad]] = {node.node_id: [] for node in nodes} + for spec in road_specs: + incident[spec.road.from_node_id].append(spec.road) + incident[spec.road.to_node_id].append(spec.road) + + resolved: list[GameMapNode] = [] + for node in nodes: + if node.node_type not in {"road_joint", "driveway"}: + resolved.append(node) + continue + roads = sorted(incident[node.node_id], key=lambda road: road.road_id) + if len(roads) != 2 or any( + road.from_node_id == road.to_node_id for road in roads + ): + raise GameMapError( + f"{node.node_type.replace('_', ' ').title()} {node.node_id!r} " + "must connect exactly two distinct roads" + ) + first = _oriented_joint_attributes( + roads[0], reverse=roads[0].from_node_id == node.node_id + ) + second = _oriented_joint_attributes( + roads[1], reverse=roads[1].to_node_id == node.node_id + ) + context = f"{node.node_type.replace('_', ' ').title()} {node.node_id!r}" + if node.node_type == "driveway": + compatible = ( + first.lane_width_m == second.lane_width_m + and first.curb_offset_m == second.curb_offset_m + and first.directions == second.directions + and first.curb == second.curb + and first.marking_style == second.marking_style + and first.marking_color == second.marking_color + and first.divider_markings == second.divider_markings + ) + if not compatible: + raise GameMapError( + f"{context} requires compatible road cross-sections, " + "markings, and curb modes" + ) + dominant = replace( + first, + speed_limit_mps=min(first.speed_limit_mps, second.speed_limit_mps), + ) + else: + dominant = _dominant_cross_section(first, second, context) + if ( + _cross_section_changes(first, second) + and node.geometry["lane_transition_length_m"] <= 0.0 + ): + raise GameMapError( + f"{context} changes lane count or width and requires a " + "positive lane_transition_length_m" + ) + resolved.append( + replace( + node, + attributes=dominant, + ) + ) + return tuple(resolved) + + +def _arm_for_road( + road: GameMapRoad, + node_id: str, + raw_roads: dict[str, np.ndarray], +) -> _RoadArm: + return _RoadArm( + node_id=node_id, + road=road, + path_xy=_road_path_from_node(road, raw_roads[road.road_id], node_id), + attributes=_outward_attributes(road, node_id), + ) + + +def _mutual_opposite_pairs(arms: list[_RoadArm]) -> list[tuple[_RoadArm, _RoadArm]]: + """Pair mutually straightest intersection arms within 45 degrees.""" + if len(arms) < 2: + return [] + directions: list[np.ndarray] = [] + for arm in arms: + vector = arm.path_xy[1] - arm.path_xy[0] + directions.append(vector / max(float(np.linalg.norm(vector)), 1.0e-9)) + best: dict[int, int] = {} + for first_index, first_direction in enumerate(directions): + candidates = [ + (float(np.dot(first_direction, second_direction)), second_index) + for second_index, second_direction in enumerate(directions) + if second_index != first_index + ] + dot, second_index = min(candidates) + if dot <= -math.cos(math.radians(45.0)): + best[first_index] = second_index + return [ + (arms[first_index], arms[second_index]) + for first_index, second_index in sorted(best.items()) + if first_index < second_index and best.get(second_index) == first_index + ] + + +def _cross_section_transitions( + topology: GameMapTopology, + raw_roads: dict[str, np.ndarray], +) -> dict[tuple[str, str], _ArmTransition]: + """Plan every node arm that must taper to its authored road profile.""" + incident: dict[str, list[GameMapRoad]] = { + node.node_id: [] for node in topology.nodes + } + for road in topology.roads: + incident[road.from_node_id].append(road) + if road.to_node_id != road.from_node_id: + incident[road.to_node_id].append(road) + + transitions: dict[tuple[str, str], _ArmTransition] = {} + for node in topology.nodes: + if node.node_type == "road_joint": + assert isinstance(node.attributes, GameMapLinearAttributes) + roads = sorted(incident[node.node_id], key=lambda road: road.road_id) + arms = [_arm_for_road(road, node.node_id, raw_roads) for road in roads] + local_attributes = ( + _reversed_attributes(node.attributes), + node.attributes, + ) + for arm, local in zip(arms, local_attributes, strict=True): + local = _lane_layout_for_arm(local, arm.attributes) + if _cross_section_changes(local, arm.attributes): + transitions[(node.node_id, arm.road.road_id)] = _ArmTransition( + arm, + local, + node.geometry["lane_transition_length_m"], + ) + continue + if node.node_type != "intersection": + continue + arms = [ + _arm_for_road(road, node.node_id, raw_roads) + for road in incident[node.node_id] + if road.from_node_id != road.to_node_id + ] + for first, second in _mutual_opposite_pairs(arms): + second_through = _reversed_attributes(second.attributes) + if not _cross_section_changes(first.attributes, second_through): + continue + context = ( + f"Intersection {node.node_id!r} through roads " + f"{first.road.road_id!r} and {second.road.road_id!r}" + ) + dominant = _dominant_cross_section( + first.attributes, + second_through, + context, + ) + length = node.geometry["lane_transition_length_m"] + if length <= 0.0: + raise GameMapError( + f"{context} changes lane count or width and requires a " + "positive lane_transition_length_m" + ) + local_values = ( + dominant, + _reversed_attributes(dominant), + ) + for arm, local in zip((first, second), local_values, strict=True): + local = _lane_layout_for_arm(local, arm.attributes) + if _cross_section_changes(local, arm.attributes): + transitions[(node.node_id, arm.road.road_id)] = _ArmTransition( + arm, + local, + length, + ) + return transitions + + +def _parking_accesses_from_nodes( + doc: dict[str, Any], nodes: dict[str, GameMapNode] +) -> tuple[GameMapParkingAccess, ...]: + accesses: list[GameMapParkingAccess] = [] + for index, value in enumerate(_sequence(doc.get("nodes"), "nodes")): + raw = _mapping(value, f"nodes[{index}]") + if raw.get("type") != "parking_lot": + continue + lot_id = str(raw["id"]) + source_id = str(raw["connected_to"]) + opening_value = raw["opening_vertex"] + if source_id not in nodes or nodes[source_id].node_type not in { + "intersection", + "driveway", + }: + raise GameMapError( + f"Parking lot {lot_id!r}.connected_to must reference an " + "intersection or driveway" + ) + opening_index = opening_value - 1 + accesses.append( + GameMapParkingAccess(f"{lot_id}:access", source_id, lot_id, opening_index) + ) + return tuple(accesses) + + +def _validate_element_ids(topology: GameMapTopology) -> None: + owners: dict[str, str] = {} + identifiers = ( + *((node.node_id, "node") for node in topology.nodes), + *((road.road_id, "road") for road in topology.roads), + *((access.access_id, "parking access") for access in topology.parking_accesses), + ) + for identifier, kind in identifiers: + previous = owners.setdefault(identifier, kind) + if previous != kind: + raise GameMapError( + f"Map element id {identifier!r} is shared by a {previous} and {kind}" + ) + + +def _validate_topology(topology: GameMapTopology) -> None: + _validate_element_ids(topology) + nodes = {node.node_id: node for node in topology.nodes} + road_degree = {node_id: 0 for node_id in nodes} + for road in topology.roads: + road_degree[road.from_node_id] += 1 + road_degree[road.to_node_id] += 1 + source_accesses: dict[str, list[GameMapParkingAccess]] = { + node_id: [] for node_id in nodes + } + lot_accesses: dict[str, list[GameMapParkingAccess]] = { + node_id: [] for node_id in nodes + } + for access in topology.parking_accesses: + source_accesses[access.source_node_id].append(access) + lot_accesses[access.parking_lot_node_id].append(access) + for node in topology.nodes: + if node.node_type == "intersection" and road_degree[node.node_id] < 3: + raise GameMapError( + f"Intersection {node.node_id!r} must connect at least three road " + f"arms (found {road_degree[node.node_id]})" + ) + if node.node_type == "cul_de_sac" and road_degree[node.node_id] != 1: + raise GameMapError( + f"Cul-de-sac {node.node_id!r} must terminate exactly one road" + ) + if node.node_type == "parking_lot" and road_degree[node.node_id]: + raise GameMapError( + f"Parking lot {node.node_id!r} cannot be an authored road endpoint" + ) + if node.node_type == "parking_lot" and not lot_accesses[node.node_id]: + raise GameMapError( + f"Parking lot {node.node_id!r} must have at least one parking access" + ) + if node.node_type == "cul_de_sac" and road_degree[node.node_id] == 1: + road = next( + road + for road in topology.roads + if node.node_id in {road.from_node_id, road.to_node_id} + ) + minimum_radius = road.attributes.surface_width_m * 0.5 + if node.geometry["culdesac_radius_m"] <= minimum_radius: + raise GameMapError( + f"Cul-de-sac {node.node_id!r} culdesac_radius_m must exceed " + f"half the incident road width ({minimum_radius:.2f} m)" + ) + if node.node_type == "driveway": + if road_degree[node.node_id] != 2: + raise GameMapError( + f"Driveway {node.node_id!r} must connect exactly two roads" + ) + if len(source_accesses[node.node_id]) != 1: + raise GameMapError( + f"Driveway {node.node_id!r} must have exactly one parking access" + ) + + +def _sample_road(spec: _RoadSpec, spacing_m: float) -> np.ndarray: + if not spec.spans_xy: + raise AssertionError("Straight road sampling requires node positions") + groups: list[np.ndarray] = [] + for span in spec.spans_xy: + estimate = sum( + float(np.linalg.norm(span[index + 1] - span[index])) for index in range(3) + ) + samples = max(3, int(math.ceil(estimate / spacing_m)) + 1) + t = np.linspace(0.0, 1.0, samples)[:, None] + points = ( + (1.0 - t) ** 3 * span[0] + + 3.0 * (1.0 - t) ** 2 * t * span[1] + + 3.0 * (1.0 - t) * t**2 * span[2] + + t**3 * span[3] + ) + groups.append(points if not groups else points[1:]) + return np.concatenate(groups, axis=0) + + +def _road_path_from_node( + road: GameMapRoad, + points: np.ndarray, + node_id: str, +) -> np.ndarray: + """Orient a road centerline outward from one endpoint node.""" + return points if road.from_node_id == node_id else points[::-1] + + +def _trimmed_road_paths_and_joints( + topology: GameMapTopology, + raw_roads: dict[str, np.ndarray], + spacing_m: float, +) -> tuple[dict[str, np.ndarray], dict[str, np.ndarray]]: + """Trim incident roads and build compact tangent joint centerlines.""" + nodes = {node.node_id: node for node in topology.nodes} + incident: dict[str, list[GameMapRoad]] = { + node.node_id: [] for node in topology.nodes + } + for road in topology.roads: + incident[road.from_node_id].append(road) + incident[road.to_node_id].append(road) + + joint_trims: dict[tuple[str, str], float] = {} + for node in topology.nodes: + if node.node_type != "road_joint": + continue + assert isinstance(node.attributes, GameMapLinearAttributes) + roads = sorted(incident[node.node_id], key=lambda road: road.road_id) + layouts = (_reversed_attributes(node.attributes), node.attributes) + arms: list[tuple[np.ndarray, float]] = [] + for road, layout in zip(roads, layouts, strict=True): + path = _road_path_from_node(road, raw_roads[road.road_id], node.node_id) + attributes = _lane_layout_for_arm( + layout, + _outward_attributes(road, node.node_id), + ) + arms.append((path, attributes.surface_width_m)) + reaches, _order, _corners = _inferred_intersection_arm_reaches(arms) + for road, reach in zip(roads, reaches, strict=True): + joint_trims[(node.node_id, road.road_id)] = reach + + trimmed: dict[str, np.ndarray] = {} + for road in topology.roads: + line = LineString(raw_roads[road.road_id]) + start_node = nodes[road.from_node_id] + end_node = nodes[road.to_node_id] + + def trim_for(node: GameMapNode) -> float: + if node.node_type == "road_joint": + return joint_trims[(node.node_id, road.road_id)] + if node.node_type == "driveway": + access = next( + item + for item in topology.parking_accesses + if item.source_node_id == node.node_id + ) + lot = nodes[access.parking_lot_node_id] + vertices = np.asarray(lot.polygon_vertices_xy) + return 0.5 * float( + np.linalg.norm( + vertices[(access.opening_vertex_index + 1) % len(vertices)] + - vertices[access.opening_vertex_index] + ) + ) + return 0.0 + + start_trim = trim_for(start_node) + end_trim = trim_for(end_node) + if start_trim + end_trim >= line.length - _POSITION_TOLERANCE_M: + raise GameMapError( + f"Road {road.road_id!r} is too short for road-joint trims " + f"{start_trim:g} m and {end_trim:g} m " + f"(centerline length {line.length:.3f} m)" + ) + remaining = substring(line, start_trim, line.length - end_trim) + if remaining.geom_type != "LineString": + raise GameMapError( + f"Road {road.road_id!r} does not retain one centerline after trimming" + ) + trimmed[road.road_id] = np.asarray(remaining.coords, dtype=np.float64) + + joints: dict[str, np.ndarray] = {} + for node in topology.nodes: + if node.node_type not in {"road_joint", "driveway"}: + continue + if node.node_type == "driveway": + access = next( + item + for item in topology.parking_accesses + if item.source_node_id == node.node_id + ) + lot = nodes[access.parking_lot_node_id] + vertices = np.asarray(lot.polygon_vertices_xy) + length = 0.5 * float( + np.linalg.norm( + vertices[(access.opening_vertex_index + 1) % len(vertices)] + - vertices[access.opening_vertex_index] + ) + ) + first_road, second_road = sorted( + incident[node.node_id], key=lambda road: road.road_id + ) + if node.node_type == "driveway": + lengths = (length, length) + else: + lengths = ( + joint_trims[(node.node_id, first_road.road_id)], + joint_trims[(node.node_id, second_road.road_id)], + ) + first_path = _road_path_from_node( + first_road, raw_roads[first_road.road_id], node.node_id + ) + second_path = _road_path_from_node( + second_road, raw_roads[second_road.road_id], node.node_id + ) + prefixes = [ + _polyline_prefix(path, trim) + for path, trim in zip((first_path, second_path), lengths, strict=True) + ] + cuts = [prefix[-1] for prefix in prefixes] + outward_tangents: list[np.ndarray] = [] + for prefix in prefixes: + tangent = prefix[-1] - prefix[-2] + tangent /= max(float(np.linalg.norm(tangent)), 1.0e-9) + outward_tangents.append(tangent) + if node.node_type == "driveway": + centerline = np.asarray( + [cuts[0], [node.x_m, node.y_m], cuts[1]], dtype=np.float64 + ) + if not LineString(centerline).is_simple: + raise GameMapError( + f"Driveway {node.node_id!r} produces a self-intersecting join" + ) + joints[node.node_id] = centerline + continue + incoming_tangent = -outward_tangents[0] + outgoing_tangent = outward_tangents[1] + turn_angle = math.acos( + float(np.clip(np.dot(incoming_tangent, outgoing_tangent), -1.0, 1.0)) + ) + if turn_angle >= math.pi - 1.0e-6: + raise GameMapError( + f"Road joint {node.node_id!r} cannot form a tangent U-turn" + ) + if turn_angle <= 1.0e-6: + handle_ratio = 2.0 / 3.0 + else: + handle_ratio = ( + 4.0 / 3.0 * math.tan(turn_angle * 0.25) / math.tan(turn_angle * 0.5) + ) + handle_lengths = [trim * handle_ratio for trim in lengths] + controls = [ + cut - outward_tangent * handle + for cut, outward_tangent, handle in zip( + cuts, outward_tangents, handle_lengths, strict=True + ) + ] + span = np.asarray( + [cuts[0], controls[0], controls[1], cuts[1]], dtype=np.float64 + ) + estimate = sum( + float(np.linalg.norm(span[index + 1] - span[index])) for index in range(3) + ) + samples = max(3, int(math.ceil(estimate / spacing_m)) + 1) + t = np.linspace(0.0, 1.0, samples)[:, None] + centerline = ( + (1.0 - t) ** 3 * span[0] + + 3.0 * (1.0 - t) ** 2 * t * span[1] + + 3.0 * (1.0 - t) * t**2 * span[2] + + t**3 * span[3] + ) + tangent_epsilons = [ + min(spacing_m * 0.5, handle * 0.25) for handle in handle_lengths + ] + centerline = np.concatenate( + ( + centerline[:1], + (cuts[0] - outward_tangents[0] * tangent_epsilons[0])[None, :], + centerline[1:-1], + (cuts[1] - outward_tangents[1] * tangent_epsilons[1])[None, :], + centerline[-1:], + ), + axis=0, + ) + line = LineString(centerline) + if line.length <= _POSITION_TOLERANCE_M or not line.is_simple: + raise GameMapError( + f"Road joint {node.node_id!r} produces a degenerate or " + "self-intersecting curve" + ) + joints[node.node_id] = centerline + return trimmed, joints + + +def _line_parts(geometry: BaseGeometry) -> list[np.ndarray]: + if geometry.is_empty: + return [] + if geometry.geom_type == "LineString": + values = [geometry] + elif geometry.geom_type == "MultiLineString": + values = list(geometry.geoms) + elif geometry.geom_type == "GeometryCollection": + values = [item for item in geometry.geoms if item.geom_type == "LineString"] + else: + return [] + return [np.asarray(item.coords, dtype=np.float64) for item in values if item.length] + + +def _trim_line( + points: np.ndarray, + start: Polygon | None, + end: Polygon | None, + context: str, +) -> np.ndarray: + remaining: BaseGeometry = LineString(points) + if start is not None: + remaining = remaining.difference(start.buffer(1.0e-5)) + if end is not None: + remaining = remaining.difference(end.buffer(1.0e-5)) + parts = _line_parts(remaining) + if not parts: + raise GameMapError( + f"{context} is completely contained by its endpoint footprints" + ) + result = max(parts, key=lambda item: LineString(item).length) + original_start = points[0] + if np.linalg.norm(result[0] - original_start) > np.linalg.norm( + result[-1] - original_start + ): + result = result[::-1] + return result + + +def _polyline_prefix(points: np.ndarray, length_m: float) -> np.ndarray: + """Return the exact prefix of a polyline through ``length_m``.""" + line = LineString(points) + prefix = substring(line, 0.0, min(length_m, line.length)) + if prefix.geom_type != "LineString" or prefix.length <= 0.0: + raise GameMapError("Intersection arm path is degenerate") + return np.asarray(prefix.coords, dtype=np.float64) + + +def _polyline_end_tangent(points: np.ndarray) -> np.ndarray: + """Return a stable unit tangent at the end of a sampled polyline.""" + for index in range(len(points) - 1, 0, -1): + vector = points[-1] - points[index - 1] + length = float(np.linalg.norm(vector)) + if length > _POSITION_TOLERANCE_M: + return vector / length + raise GameMapError("Polyline endpoint has no stable tangent") + + +def _inferred_intersection_arm_reaches( + incident: list[tuple[np.ndarray, float]], +) -> tuple[list[float], list[int], list[np.ndarray | None]]: + """Infer arm openings and roadside corners from approach geometry.""" + directions: list[np.ndarray] = [] + left_normals: list[np.ndarray] = [] + centerlines: list[LineString] = [] + center_distances: list[np.ndarray] = [] + left_boundary_distances: list[np.ndarray] = [] + right_boundary_distances: list[np.ndarray] = [] + left_boundaries: list[LineString] = [] + right_boundaries: list[LineString] = [] + for path, _width in incident: + direction = path[1] - path[0] + direction /= max(float(np.linalg.norm(direction)), 1.0e-9) + directions.append(direction) + left_normals.append(np.asarray([-direction[1], direction[0]])) + for path, width in incident: + widths = np.full(len(path), width, dtype=np.float64) + left = _variable_offset_polyline(path, widths * 0.5) + right = _variable_offset_polyline(path, -widths * 0.5) + centerlines.append(LineString(path)) + center_distances.append( + np.concatenate( + ([0.0], np.cumsum(np.linalg.norm(np.diff(path, axis=0), axis=1))) + ) + ) + left_boundary_distances.append( + np.concatenate( + ([0.0], np.cumsum(np.linalg.norm(np.diff(left, axis=0), axis=1))) + ) + ) + right_boundary_distances.append( + np.concatenate( + ([0.0], np.cumsum(np.linalg.norm(np.diff(right, axis=0), axis=1))) + ) + ) + left_boundaries.append(LineString(left)) + right_boundaries.append(LineString(right)) + + reaches = [0.0 for _path, _width in incident] + order = sorted( + range(len(incident)), + key=lambda index: math.atan2(directions[index][1], directions[index][0]), + ) + bearings = [math.atan2(direction[1], direction[0]) for direction in directions] + corners: list[np.ndarray | None] = [] + for order_index, first_index in enumerate(order): + second_index = order[(order_index + 1) % len(order)] + first_direction = directions[first_index] + second_direction = directions[second_index] + sector_angle = (bearings[second_index] - bearings[first_index]) % ( + 2.0 * math.pi + ) + crossing: BaseGeometry = Point() + if sector_angle < math.pi - 1.0e-6: + crossing = left_boundaries[first_index].intersection( + right_boundaries[second_index] + ) + crossing_points: list[np.ndarray] = [] + if crossing.geom_type == "Point" and not crossing.is_empty: + crossing_points.append(np.asarray(crossing.coords[0], dtype=np.float64)) + elif crossing.geom_type in {"MultiPoint", "GeometryCollection"}: + crossing_points.extend( + np.asarray(part.coords[0], dtype=np.float64) + for part in crossing.geoms + if part.geom_type == "Point" + ) + if crossing_points: + corner = min( + crossing_points, + key=lambda point: centerlines[first_index].project(Point(point)) + + centerlines[second_index].project(Point(point)), + ) + first_boundary_distance = left_boundaries[first_index].project( + Point(corner) + ) + second_boundary_distance = right_boundaries[second_index].project( + Point(corner) + ) + first_center_distance = float( + np.interp( + first_boundary_distance, + left_boundary_distances[first_index], + center_distances[first_index], + ) + ) + second_center_distance = float( + np.interp( + second_boundary_distance, + right_boundary_distances[second_index], + center_distances[second_index], + ) + ) + if ( + first_center_distance + < centerlines[first_index].length - _POSITION_TOLERANCE_M + and second_center_distance + < centerlines[second_index].length - _POSITION_TOLERANCE_M + ): + corners.append(corner) + reaches[first_index] = max(reaches[first_index], first_center_distance) + reaches[second_index] = max( + reaches[second_index], second_center_distance + ) + continue + if sector_angle >= math.pi - 1.0e-6: + corners.append(None) + continue + matrix = np.column_stack((first_direction, -second_direction)) + if abs(float(np.linalg.det(matrix))) <= 1.0e-9: + corners.append(None) + continue + first_width = incident[first_index][1] + second_width = incident[second_index][1] + first_edge = left_normals[first_index] * first_width * 0.5 + second_edge = -left_normals[second_index] * second_width * 0.5 + first_reach, second_reach = np.linalg.solve(matrix, second_edge - first_edge) + if ( + first_reach < -_POSITION_TOLERANCE_M + or second_reach < -_POSITION_TOLERANCE_M + or first_reach >= centerlines[first_index].length - _POSITION_TOLERANCE_M + or second_reach >= centerlines[second_index].length - _POSITION_TOLERANCE_M + ): + corners.append(None) + continue + corner = ( + incident[first_index][0][0] + first_edge + (first_direction * first_reach) + ) + corners.append(corner) + if first_reach > 0.0: + reaches[first_index] = max(reaches[first_index], float(first_reach)) + if second_reach > 0.0: + reaches[second_index] = max(reaches[second_index], float(second_reach)) + + if len(incident) == 2 and max(reaches) <= _POSITION_TOLERANCE_M: + fallback = max(width for _path, width in incident) * 0.5 + reaches = [fallback, fallback] + reaches = [ + reach + _BOUNDARY_CLEARANCE_M if reach > 0.0 else reach for reach in reaches + ] + return reaches, order, corners + + +def _parking_access_path( + access: GameMapParkingAccess, + nodes: dict[str, GameMapNode], + spacing_m: float, +) -> tuple[np.ndarray, float]: + """Infer a tangent cubic from a road node to one authored lot edge.""" + source = nodes[access.source_node_id] + lot = nodes[access.parking_lot_node_id] + vertices = np.asarray(lot.polygon_vertices_xy, dtype=np.float64) + first = vertices[access.opening_vertex_index] + second = vertices[(access.opening_vertex_index + 1) % len(vertices)] + edge = second - first + width = float(np.linalg.norm(edge)) + if width <= _POSITION_TOLERANCE_M: + raise GameMapError( + f"Parking access {access.access_id!r} has a degenerate opening edge" + ) + edge_direction = edge / width + outward = np.asarray([-edge_direction[1], edge_direction[0]]) + end = 0.5 * (first + second) + start = np.asarray([source.x_m, source.y_m], dtype=np.float64) + chord = end - start + chord_length = float(np.linalg.norm(chord)) + if chord_length <= _POSITION_TOLERANCE_M: + raise GameMapError(f"Parking access {access.access_id!r} is degenerate") + if float(np.dot(start - end, outward)) <= _POSITION_TOLERANCE_M: + raise GameMapError( + f"Parking access {access.access_id!r} source must be outside the lot " + "on the opening edge's exterior side" + ) + handle = chord_length / 3.0 + control_1 = start + chord / chord_length * handle + inward = -outward + control_2 = end - inward * handle + span = np.asarray([start, control_1, control_2, end]) + estimate = sum( + float(np.linalg.norm(span[index + 1] - span[index])) for index in range(3) + ) + samples = max(8, int(math.ceil(estimate / spacing_m)) + 1) + t = np.linspace(0.0, 1.0, samples)[:, None] + path = ( + (1.0 - t) ** 3 * span[0] + + 3.0 * (1.0 - t) ** 2 * t * span[1] + + 3.0 * (1.0 - t) * t**2 * span[2] + + t**3 * span[3] + ) + if not LineString(path).is_simple: + raise GameMapError( + f"Parking access {access.access_id!r} produces a self-intersecting curve" + ) + return path, width + + +def _polyline_section( + points: np.ndarray, + start_m: float, + end_m: float, + context: str, +) -> np.ndarray: + line = LineString(points) + if end_m >= line.length - _POSITION_TOLERANCE_M: + raise GameMapError( + f"{context} transition length {end_m - start_m:g} m consumes its " + f"road arm (available length {max(0.0, line.length - start_m):.3f} m)" + ) + section = substring(line, start_m, end_m) + if section.geom_type != "LineString" or section.length <= _POSITION_TOLERANCE_M: + raise GameMapError(f"{context} produces a degenerate lane transition") + coordinates = np.asarray(section.coords, dtype=np.float64) + cleaned = [coordinates[0]] + for index, point in enumerate(coordinates[1:], start=1): + if np.linalg.norm(point - cleaned[-1]) > _POSITION_TOLERANCE_M: + cleaned.append(point) + elif index == len(coordinates) - 1 and len(cleaned) > 1: + cleaned[-1] = point + if len(cleaned) < 2: + raise GameMapError(f"{context} produces a degenerate lane transition") + return np.asarray(cleaned, dtype=np.float64) + + +def _variable_offset_polyline( + points: np.ndarray, + offsets: np.ndarray, +) -> np.ndarray: + tangents = np.empty_like(points) + tangents[0] = points[1] - points[0] + tangents[-1] = points[-1] - points[-2] + if len(points) > 2: + tangents[1:-1] = points[2:] - points[:-2] + lengths = np.linalg.norm(tangents, axis=1) + if np.any(lengths <= 1.0e-9): + raise GameMapError("Lane transition has a degenerate centerline tangent") + normals = np.column_stack((-tangents[:, 1], tangents[:, 0])) / lengths[:, None] + return points + normals * offsets[:, None] + + +def _ribbon_sides( + points: np.ndarray, + widths_m: np.ndarray, + context: str, + start_opening_xy: np.ndarray | None = None, + end_opening_xy: np.ndarray | None = None, +) -> tuple[np.ndarray, np.ndarray]: + """Offset both sides of a centerline and optionally pin its openings.""" + if len(points) != len(widths_m): + raise AssertionError(f"{context} has mismatched path and width samples") + left = _remove_rail_loops(_variable_offset_polyline(points, widths_m * 0.5)) + right = _remove_rail_loops(_variable_offset_polyline(points, -widths_m * 0.5)) + if start_opening_xy is not None: + first = start_opening_xy[0].copy() + second = start_opening_xy[1].copy() + direct = np.linalg.norm(left[0] - first) + np.linalg.norm(right[0] - second) + reverse = np.linalg.norm(left[0] - second) + np.linalg.norm(right[0] - first) + if direct <= reverse: + left[0], right[0] = first, second + else: + left[0], right[0] = second, first + if end_opening_xy is not None: + first = end_opening_xy[0].copy() + second = end_opening_xy[1].copy() + direct = np.linalg.norm(left[-1] - first) + np.linalg.norm(right[-1] - second) + reverse = np.linalg.norm(left[-1] - second) + np.linalg.norm(right[-1] - first) + if direct <= reverse: + left[-1], right[-1] = first, second + else: + left[-1], right[-1] = second, first + return left, right + + +def _remove_rail_loops(points: np.ndarray) -> np.ndarray: + """Trim self-intersecting loops from an offset boundary rail.""" + cleaned: list[np.ndarray] = [points[0], points[1]] + for point in points[2:]: + current = LineString((cleaned[-1], point)) + crossing_index: int | None = None + crossing_point: np.ndarray | None = None + for index in range(len(cleaned) - 2): + crossing = current.intersection( + LineString((cleaned[index], cleaned[index + 1])) + ) + if crossing.geom_type == "Point" and not crossing.is_empty: + crossing_index = index + crossing_point = np.asarray(crossing.coords[0], dtype=np.float64) + break + if crossing_index is not None and crossing_point is not None: + cleaned = [*cleaned[: crossing_index + 1], crossing_point, point] + else: + cleaned.append(point) + return np.asarray(cleaned) + + +def _polygon_from_ribbon( + left: np.ndarray, + right: np.ndarray, + context: str, +) -> Polygon: + """Build one explicit surface from paired boundary rails.""" + polygon = Polygon(np.vstack((left, right[::-1]))) + if not polygon.is_valid or polygon.area <= _AREA_TOLERANCE_M2 or polygon.interiors: + raise GameMapError( + f"{context} produces an invalid boundary ribbon: {is_valid_reason(polygon)}" + ) + return polygon + + +def _road_joint_ribbon( + points: np.ndarray, + widths_m: np.ndarray, + context: str, +) -> tuple[np.ndarray, np.ndarray, Polygon]: + """Build a compact joint ribbon while preserving its curved outside rail. + + Args: + points: Sampled joint centerline. + widths_m: Paved width at each centerline sample. + context: Element description used in validation errors. + + Returns: + Left and right roadside rails with their enclosed surface polygon. + + Raises: + GameMapError: The rails cannot form one valid surface. + """ + left, right = _ribbon_sides(points, widths_m, context) + polygon = Polygon(np.vstack((left, right[::-1]))) + if polygon.is_valid and polygon.area > _AREA_TOLERANCE_M2: + return left, right, polygon + + start_direction = points[1] - points[0] + end_direction = points[-1] - points[-2] + turn = float( + start_direction[0] * end_direction[1] - start_direction[1] * end_direction[0] + ) + inner = left if turn > 0.0 else right + first_tangent = inner[1] - inner[0] + last_tangent = inner[-1] - inner[-2] + matrix = np.column_stack((first_tangent, -last_tangent)) + if abs(float(np.linalg.det(matrix))) <= 1.0e-9: + return left, right, _polygon_from_ribbon(left, right, context) + first_distance, _last_distance = np.linalg.solve( + matrix, + inner[-1] - inner[0], + ) + vertex = inner[0] + first_tangent * first_distance + mitered = np.asarray((inner[0], vertex, inner[-1]), dtype=np.float64) + if turn > 0.0: + left = mitered + else: + right = mitered + return left, right, _polygon_from_ribbon(left, right, context) + + +def _taper_polygon( + points: np.ndarray, + start_width_m: float, + end_width_m: float, + context: str, +) -> Polygon: + segment_lengths = np.linalg.norm(np.diff(points, axis=0), axis=1) + distances = np.concatenate(([0.0], np.cumsum(segment_lengths))) + alpha = distances / max(float(distances[-1]), 1.0e-9) + widths = start_width_m + alpha * (end_width_m - start_width_m) + left, right = _ribbon_sides(points, widths, context) + return _polygon_from_ribbon(left, right, context) + + +def _linear_width_samples( + points: np.ndarray, + start_width_m: float, + end_width_m: float, +) -> np.ndarray: + """Interpolate surface widths by distance along a sampled centerline.""" + distances = np.concatenate( + ([0.0], np.cumsum(np.linalg.norm(np.diff(points, axis=0), axis=1))) + ) + alpha = distances / max(float(distances[-1]), 1.0e-9) + return start_width_m + alpha * (end_width_m - start_width_m) + + +def _multiarm_node_polygon( + node: GameMapNode, + incident: list[tuple[np.ndarray, float, str]], + transitions: dict[tuple[str, str], _ArmTransition], +) -> tuple[ + Polygon, + dict[str, np.ndarray], + dict[tuple[str, str], _TransitionGeometry], +]: + """Trace a multi-arm node from connected roadside boundaries.""" + reaches, order, corners = _inferred_intersection_arm_reaches( + [(path, width) for path, width, _reference_id in incident] + ) + arms: dict[int, _BoundaryArmGeometry] = {} + openings: dict[str, np.ndarray] = {} + transition_geometry: dict[tuple[str, str], _TransitionGeometry] = {} + for index, (path, width, reference_id) in enumerate(incident): + reach = reaches[index] + path_length = LineString(path).length + if reach >= path_length - _POSITION_TOLERANCE_M: + raise GameMapError( + f"Node {node.node_id!r} opening for {reference_id!r} consumes " + f"its approach ({reach:.3f} m required, {path_length:.3f} m available)" + ) + core_path = _polyline_prefix(path, max(reach, _POSITION_TOLERANCE_M * 2.0)) + tangent = _polyline_end_tangent(core_path) + normal = np.asarray([-tangent[1], tangent[0]]) + core_left = core_path[-1] + normal * width * 0.5 + core_right = core_path[-1] - normal * width * 0.5 + + transition = transitions.get((node.node_id, reference_id)) + if transition is None: + left = core_left[None, :] + right = core_right[None, :] + else: + context = f"Node {node.node_id!r} road {reference_id!r}" + transition_path = _polyline_section( + path, + reach, + reach + transition.length_m, + context, + ) + widths = _linear_width_samples( + transition_path, + transition.local_attributes.surface_width_m, + transition.arm.attributes.surface_width_m, + ) + left, right = _ribbon_sides(transition_path, widths, context) + left[0] = core_left + right[0] = core_right + transition_geometry[(node.node_id, reference_id)] = _TransitionGeometry( + transition, + transition_path, + ) + arms[index] = _BoundaryArmGeometry(reference_id, left, right) + opening = np.asarray([right[-1], left[-1]]) + if np.linalg.norm(opening[1] - opening[0]) <= _LINE_TOLERANCE_M: + raise GameMapError( + f"Node {node.node_id!r} produces a degenerate opening for " + f"{reference_id!r}" + ) + openings[reference_id] = opening + + first = arms[order[0]] + perimeter: list[np.ndarray] = [first.right_xy[-1], first.left_xy[-1]] + perimeter.extend(first.left_xy[-2::-1]) + for order_index in range(len(order)): + corner = corners[order_index] + if corner is not None: + perimeter.append(corner) + next_index = order[(order_index + 1) % len(order)] + next_arm = arms[next_index] + perimeter.extend(next_arm.right_xy) + if next_index == order[0]: + break + perimeter.append(next_arm.left_xy[-1]) + perimeter.extend(next_arm.left_xy[-2::-1]) + + cleaned = [perimeter[0]] + for point in perimeter[1:]: + if np.linalg.norm(point - cleaned[-1]) > _POSITION_TOLERANCE_M: + cleaned.append(point) + if len(cleaned) > 1 and np.linalg.norm(cleaned[0] - cleaned[-1]) <= ( + _POSITION_TOLERANCE_M + ): + cleaned.pop() + polygon = Polygon(cleaned) + if not polygon.is_valid: + linework = unary_union(LineString(np.vstack((cleaned, cleaned[0])))) + candidates = list(polygonize(linework)) + resolved = unary_union(candidates) + if isinstance(resolved, Polygon): + polygon = resolved + extent = max( + 100.0, + max(float(np.linalg.norm(point - [node.x_m, node.y_m])) for point in cleaned) + * 4.0, + ) + for opening in openings.values(): + opening_center = np.mean(opening, axis=0) + opening_tangent = opening[1] - opening[0] + opening_tangent /= float(np.linalg.norm(opening_tangent)) + outward = np.asarray([opening_tangent[1], -opening_tangent[0]]) + if np.dot(outward, opening_center - [node.x_m, node.y_m]) < 0.0: + outward *= -1.0 + clip = Polygon( + ( + opening_center + opening_tangent * extent, + opening_center - opening_tangent * extent, + opening_center - opening_tangent * extent - outward * extent, + opening_center + opening_tangent * extent - outward * extent, + ) + ) + polygon = polygon.intersection(clip) + support = Polygon( + ( + opening[0], + opening[1], + opening[1] - outward * _POSITION_TOLERANCE_M, + opening[0] - outward * _POSITION_TOLERANCE_M, + ) + ) + supported = polygon.union(support) + if isinstance(supported, Polygon): + polygon = supported + if ( + not isinstance(polygon, Polygon) + or not polygon.is_valid + or polygon.area <= _AREA_TOLERANCE_M2 + or polygon.interiors + ): + raise GameMapError( + f"Node {node.node_id!r} produces an invalid boundary-driven footprint: " + f"{is_valid_reason(polygon)}" + ) + return polygon, openings, transition_geometry + + +def _node_polygons( + topology: GameMapTopology, + raw_roads: dict[str, np.ndarray], + road_joint_centerlines: dict[str, np.ndarray], + parking_access_paths: dict[str, tuple[np.ndarray, float]], + transitions: dict[tuple[str, str], _ArmTransition], +) -> tuple[ + dict[str, Polygon], + dict[tuple[str, str], _TransitionGeometry], + dict[tuple[str, str], np.ndarray], +]: + incidences: dict[str, list[tuple[np.ndarray, float, str | None]]] = { + node.node_id: [] for node in topology.nodes + } + for road in topology.roads: + points = raw_roads[road.road_id] + for endpoint, node_id, path in ( + ("from", road.from_node_id, points), + ("to", road.to_node_id, points[::-1]), + ): + transition = transitions.get((node_id, road.road_id)) + width = ( + transition.local_attributes.surface_width_m + if transition is not None + else road.attributes.surface_width_m + ) + reference_id = ( + f"{road.road_id}:{endpoint}" + if road.from_node_id == road.to_node_id + else road.road_id + ) + incidences[node_id].append((path, width, reference_id)) + for access in topology.parking_accesses: + path, width = parking_access_paths[access.access_id] + incidences[access.source_node_id].append((path, width, access.access_id)) + polygons: dict[str, Polygon] = {} + transition_geometry: dict[tuple[str, str], _TransitionGeometry] = {} + node_openings: dict[tuple[str, str], np.ndarray] = {} + for node in topology.nodes: + center = np.asarray([node.x_m, node.y_m]) + if node.node_type == "driveway": + assert isinstance(node.attributes, GameMapLinearAttributes) + joint_roads = sorted( + ( + road + for road in topology.roads + if node.node_id in {road.from_node_id, road.to_node_id} + ), + key=lambda road: road.road_id, + ) + centerline = road_joint_centerlines[node.node_id] + driveway_incident: list[tuple[np.ndarray, float, str]] = [] + for road, cut in zip( + joint_roads, (centerline[0], centerline[-1]), strict=True + ): + outward = _road_path_from_node( + road, raw_roads[road.road_id], node.node_id + ) + branch = np.vstack((center, cut, outward[1:])) + driveway_incident.append( + (branch, node.attributes.surface_width_m, road.road_id) + ) + access = next( + access + for access in topology.parking_accesses + if access.source_node_id == node.node_id + ) + access_path, access_width = parking_access_paths[access.access_id] + driveway_incident.append((access_path, access_width, access.access_id)) + polygon, openings, resolved_transitions = _multiarm_node_polygon( + node, + driveway_incident, + transitions, + ) + node_openings.update( + ((node.node_id, reference_id), opening) + for reference_id, opening in openings.items() + ) + transition_geometry.update(resolved_transitions) + elif node.node_type == "intersection": + incident = incidences[node.node_id] + if not incident: + raise GameMapError( + f"Node {node.node_id!r} must have at least one incidence" + ) + polygon, openings, resolved_transitions = _multiarm_node_polygon( + node, + [ + (path, width, reference_id) + for path, width, reference_id in incident + if reference_id is not None + ], + transitions, + ) + node_openings.update( + ((node.node_id, reference_id), opening) + for reference_id, opening in openings.items() + ) + transition_geometry.update(resolved_transitions) + elif node.node_type == "road_joint": + assert isinstance(node.attributes, GameMapLinearAttributes) + centerline = road_joint_centerlines[node.node_id] + joint_roads = sorted( + ( + road + for road in topology.roads + if node.node_id in {road.from_node_id, road.to_node_id} + ), + key=lambda road: road.road_id, + ) + endpoint_layouts = ( + _reversed_attributes(node.attributes), + node.attributes, + ) + endpoint_widths = [ + _lane_layout_for_arm( + layout, + _outward_attributes(road, node.node_id), + ).surface_width_m + for road, layout in zip( + joint_roads, + endpoint_layouts, + strict=True, + ) + ] + path_parts: list[np.ndarray] = [] + width_parts: list[np.ndarray] = [] + first_transition = transitions.get((node.node_id, joint_roads[0].road_id)) + if first_transition is not None: + context = f"Road joint {node.node_id!r} road {joint_roads[0].road_id!r}" + outward = _road_path_from_node( + joint_roads[0], raw_roads[joint_roads[0].road_id], node.node_id + ) + transition_path = _polyline_section( + outward, 0.0, first_transition.length_m, context + ) + transition_geometry[(node.node_id, joint_roads[0].road_id)] = ( + _TransitionGeometry(first_transition, transition_path) + ) + path_parts.append(transition_path[::-1]) + width_parts.append( + _linear_width_samples( + transition_path, + first_transition.local_attributes.surface_width_m, + first_transition.arm.attributes.surface_width_m, + )[::-1] + ) + path_parts.append(centerline) + width_parts.append( + _linear_width_samples( + centerline, endpoint_widths[0], endpoint_widths[1] + ) + ) + second_transition = transitions.get((node.node_id, joint_roads[1].road_id)) + if second_transition is not None: + context = f"Road joint {node.node_id!r} road {joint_roads[1].road_id!r}" + outward = _road_path_from_node( + joint_roads[1], raw_roads[joint_roads[1].road_id], node.node_id + ) + transition_path = _polyline_section( + outward, 0.0, second_transition.length_m, context + ) + transition_geometry[(node.node_id, joint_roads[1].road_id)] = ( + _TransitionGeometry(second_transition, transition_path) + ) + path_parts.append(transition_path) + width_parts.append( + _linear_width_samples( + transition_path, + second_transition.local_attributes.surface_width_m, + second_transition.arm.attributes.surface_width_m, + ) + ) + combined_path = path_parts[0] + combined_widths = width_parts[0] + for path_part, width_part in zip( + path_parts[1:], width_parts[1:], strict=True + ): + combined_path = np.vstack((combined_path, path_part[1:])) + combined_widths = np.concatenate((combined_widths, width_part[1:])) + context = f"Road joint {node.node_id!r}" + left, right, polygon = _road_joint_ribbon( + combined_path, + combined_widths, + context, + ) + node_openings[(node.node_id, joint_roads[0].road_id)] = np.asarray( + [right[0], left[0]] + ) + node_openings[(node.node_id, joint_roads[1].road_id)] = np.asarray( + [right[-1], left[-1]] + ) + elif node.node_type == "cul_de_sac": + radius = node.geometry["culdesac_radius_m"] + path, opening_width, road_id = incidences[node.node_id][0] + assert road_id is not None + vector = path[1] - path[0] + direction = vector / max(float(np.linalg.norm(vector)), 1.0e-9) + normal = np.asarray([-direction[1], direction[0]]) + chord_distance = math.sqrt(radius**2 - (opening_width * 0.5) ** 2) + opening_center = center + direction * chord_distance + right = opening_center - normal * opening_width * 0.5 + left = opening_center + normal * opening_width * 0.5 + bearing = math.atan2(direction[1], direction[0]) + half_angle = math.asin(opening_width * 0.5 / radius) + angles = np.linspace( + bearing + half_angle, + bearing + 2.0 * math.pi - half_angle, + 129, + ) + arc = center + radius * np.column_stack((np.cos(angles), np.sin(angles))) + polygon = Polygon(np.vstack((right, left, arc[1:-1]))) + node_openings[(node.node_id, road_id)] = np.asarray([right, left]) + elif node.node_type == "parking_lot": + polygon = Polygon(node.polygon_vertices_xy) + vertices = np.asarray(node.polygon_vertices_xy, dtype=np.float64) + for access in topology.parking_accesses: + if access.parking_lot_node_id != node.node_id: + continue + first = vertices[access.opening_vertex_index] + second = vertices[(access.opening_vertex_index + 1) % len(vertices)] + node_openings[(node.node_id, access.access_id)] = np.asarray( + [first, second] + ) + else: + raise AssertionError(f"Unsupported footprint node {node.node_type!r}") + if polygon.geom_type == "MultiPolygon": + parts = [part for part in polygon.geoms if part.area > _AREA_TOLERANCE_M2] + if len(parts) == 1: + polygon = parts[0] + if not isinstance(polygon, Polygon) or polygon.area <= 0.0: + raise GameMapError(f"Node {node.node_id!r} has an invalid footprint") + polygons[node.node_id] = polygon + return polygons, transition_geometry, node_openings + + +def _surface_array(polygon: Polygon) -> np.ndarray: + """Convert a resolved surface polygon to world coordinates.""" + points = np.asarray(polygon.exterior.coords, dtype=np.float64) + return np.column_stack((points, np.zeros(len(points), dtype=np.float64))) + + +def _exclude_connected_footprints( + surface: Polygon, + excluded: tuple[Polygon, ...], + context: str, +) -> Polygon: + """Trim numeric seam overlap from an explicit corridor ribbon.""" + geometry: BaseGeometry = surface + for footprint in excluded: + geometry = geometry.difference(footprint) + if isinstance(geometry, Polygon): + return geometry + parts = [ + part + for part in getattr(geometry, "geoms", ()) + if isinstance(part, Polygon) and part.area > _AREA_TOLERANCE_M2 + ] + if len(parts) != 1: + raise GameMapError(f"{context} does not retain one connected surface") + return parts[0] + + +def _boundaries_for_elements( + elements: list[GameMapElement], + connections: list[_Connection], + permitted_boundary_contacts: set[tuple[str, str]] | None = None, + curb_regions: dict[str, list[tuple[BaseGeometry, bool]]] | None = None, +) -> list[GameMapElement]: + """Validate contacts and attach semantic boundaries and physical curbs.""" + polygons = { + element.element_id: Polygon(element.surface_world[:, :2]) + for element in elements + } + for element_id, polygon in polygons.items(): + if not polygon.is_valid: + raise GameMapError( + f"Element {element_id!r} has an invalid surface: " + f"{is_valid_reason(polygon)}" + ) + connection_groups: dict[tuple[str, str], list[_Connection]] = {} + for connection in connections: + pair = tuple( + sorted((connection.first_element_id, connection.second_element_id)) + ) + connection_groups.setdefault(pair, []).append(connection) + + openings: dict[str, list[BaseGeometry]] = { + element.element_id: [] for element in elements + } + element_ids = [element.element_id for element in elements] + for first_index, first_id in enumerate(element_ids): + first = polygons[first_id] + for second_id in element_ids[first_index + 1 :]: + second = polygons[second_id] + pair = tuple(sorted((first_id, second_id))) + overlap_area = first.intersection(second).area + declared = connection_groups.get(pair) + if declared is None: + if overlap_area > _AREA_TOLERANCE_M2: + raise GameMapError( + f"Unrelated elements {first_id!r} and {second_id!r} overlap " + f"by {overlap_area:.6f} m^2" + ) + if pair not in (permitted_boundary_contacts or set()) and ( + first.boundary.intersection(second.boundary).length + > _LINE_TOLERANCE_M + ): + raise GameMapError( + f"Unrelated elements {first_id!r} and {second_id!r} " + "share a boundary" + ) + continue + if overlap_area > _AREA_TOLERANCE_M2: + labels = ", ".join(item.connection_id for item in declared) + raise GameMapError( + f"Connected elements {first_id!r} and {second_id!r} overlap " + f"by {overlap_area:.6f} m^2 at {labels}" + ) + for connection in declared: + opening = LineString(connection.opening_xy) + first_error = opening.difference( + first.boundary.buffer(_OPENING_TOLERANCE_M) + ).length + second_error = opening.difference( + second.boundary.buffer(_OPENING_TOLERANCE_M) + ).length + if ( + opening.length <= _LINE_TOLERANCE_M + or first_error > _OPENING_TOLERANCE_M + or second_error > _OPENING_TOLERANCE_M + ): + raise GameMapError( + f"Connection {connection.connection_id!r} between " + f"{first_id!r} and {second_id!r} has mismatched openings " + f"({first_error:.9f}/{second_error:.9f} m outside boundaries, " + f"{opening.length:.9f} m long)" + ) + openings[first_id].append(opening) + openings[second_id].append(opening) + + resolved: list[GameMapElement] = [] + for element in elements: + boundary: BaseGeometry = polygons[element.element_id].boundary + for opening in openings[element.element_id]: + boundary = boundary.difference( + opening.buffer( + _OPENING_TOLERANCE_M, + cap_style=2, + join_style=2, + ) + ) + parts = sorted( + ( + points + for points in _line_parts(boundary) + if LineString(points).length > _OPENING_TOLERANCE_M * 2.0 + ), + key=lambda points: ( + round(float(np.min(points[:, 0])), 6), + round(float(np.min(points[:, 1])), 6), + round(float(np.max(points[:, 0])), 6), + round(float(np.max(points[:, 1])), 6), + ), + ) + road_boundaries = tuple( + GameMapRoadBoundary( + boundary_id=f"{element.element_id}:road_boundary:{index}", + polyline_world=_xyz(points), + ) + for index, points in enumerate(parts) + if len(points) >= 2 + ) + remaining_curb_boundary = boundary + selected_curb_parts: list[np.ndarray] = [] + for region, enabled in (curb_regions or {}).get(element.element_id, []): + selected = remaining_curb_boundary.intersection(region) + if enabled: + selected_curb_parts.extend(_line_parts(selected)) + remaining_curb_boundary = remaining_curb_boundary.difference(region) + if element.attributes.curb: + selected_curb_parts.extend(_line_parts(remaining_curb_boundary)) + selected_curb_parts.sort( + key=lambda points: ( + round(float(np.min(points[:, 0])), 6), + round(float(np.min(points[:, 1])), 6), + round(float(np.max(points[:, 0])), 6), + round(float(np.max(points[:, 1])), 6), + ) + ) + curbs = tuple( + GameMapCurb( + curb_id=f"{element.element_id}:curb:{index}", + polyline_world=_xyz(points), + ) + for index, points in enumerate(selected_curb_parts) + if len(points) >= 2 + and LineString(points).length > _OPENING_TOLERANCE_M * 2.0 + ) + resolved.append(replace(element, road_boundaries=road_boundaries, curbs=curbs)) + return resolved + + +def _build_linear_lanes( + element_id: str, + points: np.ndarray, + attributes: GameMapLinearAttributes, + allows_taxi_stops: bool, +) -> list[_LaneBuild]: + lanes: list[_LaneBuild] = [] + for index, direction in enumerate(attributes.directions): + left_marking, right_marking = _lane_edge_markings(attributes, index, direction) + offset = ( + len(attributes.directions) - 1 + ) * attributes.lane_width_m * 0.5 - index * attributes.lane_width_m + center = _offset_polyline(points, offset) + start_endpoint, end_endpoint = "from", "to" + if direction == "backward": + center = center[::-1] + start_endpoint, end_endpoint = end_endpoint, start_endpoint + left = _offset_polyline(center, attributes.lane_width_m * 0.5) + right = _offset_polyline(center, -attributes.lane_width_m * 0.5) + roadside = _offset_polyline( + center, + -(attributes.lane_width_m * 0.5 + attributes.curb_offset_m), + ) + lanes.append( + _LaneBuild( + lane_id=f"{element_id}:lane:{index}", + element_id=element_id, + centerline=_xyz(center), + left_edge=_xyz(left), + right_edge=_xyz(right), + roadside_edge=_xyz(roadside), + speed_limit_mps=attributes.speed_limit_mps, + marking_style=attributes.marking_style, + marking_color=attributes.marking_color, + start_endpoint=start_endpoint, + end_endpoint=end_endpoint, + successors=[], + allows_taxi_stops=allows_taxi_stops, + left_marking_style=left_marking[0], + left_marking_color=left_marking[1], + right_marking_style=right_marking[0], + right_marking_color=right_marking[1], + ) + ) + return lanes + + +def _lane_boundary_offsets(attributes: GameMapLinearAttributes) -> np.ndarray: + lane_count = len(attributes.directions) + return np.linspace( + lane_count * attributes.lane_width_m * 0.5, + -lane_count * attributes.lane_width_m * 0.5, + lane_count + 1, + ) + + +def _direction_groups(directions: tuple[str, ...]) -> list[tuple[str, int, int]]: + groups: list[tuple[str, int, int]] = [] + start = 0 + for index in range(1, len(directions) + 1): + if index == len(directions) or directions[index] != directions[start]: + groups.append((directions[start], start, index - start)) + start = index + return groups + + +def _transition_boundary_mapping( + local: GameMapLinearAttributes, + road: GameMapLinearAttributes, +) -> list[int]: + local_groups = _direction_groups(local.directions) + road_groups = _direction_groups(road.directions) + if [group[0] for group in local_groups] != [group[0] for group in road_groups]: + raise GameMapError("Lane transition changes directional lane ordering") + mapping = [0] * (len(local.directions) + 1) + for group_index, (local_group, road_group) in enumerate( + zip(local_groups, road_groups, strict=True) + ): + _direction, local_start, local_count = local_group + _road_direction, road_start, road_count = road_group + extra = local_count - road_count + if extra < 0: + raise GameMapError("Lane transition local profile is not dominant") + for boundary in range(local_count + 1): + if group_index == 0: + road_boundary = max(0, boundary - extra) + else: + road_boundary = min(boundary, road_count) + mapping[local_start + boundary] = road_start + road_boundary + return mapping + + +def _build_transition_lanes( + geometry: _TransitionGeometry, +) -> list[_LaneBuild]: + transition = geometry.transition + local = transition.local_attributes + road = transition.arm.attributes + path = geometry.path_xy + segment_lengths = np.linalg.norm(np.diff(path, axis=0), axis=1) + distances = np.concatenate(([0.0], np.cumsum(segment_lengths))) + alpha = distances / max(float(distances[-1]), 1.0e-9) + local_offsets = _lane_boundary_offsets(local) + road_offsets = _lane_boundary_offsets(road) + mapping = _transition_boundary_mapping(local, road) + boundaries = [ + _variable_offset_polyline( + path, + local_offsets[index] + + alpha * (road_offsets[remote_index] - local_offsets[index]), + ) + for index, remote_index in enumerate(mapping) + ] + + lanes: list[_LaneBuild] = [] + for index, direction in enumerate(local.directions): + upper = boundaries[index] + lower = boundaries[index + 1] + center = 0.5 * (upper + lower) + left, right = upper, lower + roadside_offsets = ( + local_offsets[index + 1] + + alpha * (road_offsets[mapping[index + 1]] - local_offsets[index + 1]) + - road.curb_offset_m + ) + roadside = _variable_offset_polyline(path, roadside_offsets) + kind = "start" + if direction == "backward": + center = center[::-1] + left, right = lower[::-1], upper[::-1] + roadside_offsets = ( + local_offsets[index] + + alpha * (road_offsets[mapping[index]] - local_offsets[index]) + + road.curb_offset_m + ) + roadside = _variable_offset_polyline(path, roadside_offsets)[::-1] + kind = "end" + left_marking, right_marking = _lane_edge_markings(local, index, direction) + lanes.append( + _LaneBuild( + lane_id=( + f"{transition.arm.node_id}:transition:" + f"{transition.arm.road.road_id}:lane:{index}" + ), + element_id=transition.arm.node_id, + centerline=_xyz(center), + left_edge=_xyz(left), + right_edge=_xyz(right), + roadside_edge=_xyz(roadside), + speed_limit_mps=road.speed_limit_mps, + marking_style=local.marking_style, + marking_color=local.marking_color, + start_endpoint="from" if kind == "start" else "to", + end_endpoint="to" if kind == "start" else "from", + successors=[], + allows_taxi_stops=False, + left_marking_style=left_marking[0], + left_marking_color=left_marking[1], + right_marking_style=right_marking[0], + right_marking_color=right_marking[1], + ) + ) + return lanes + + +def _splice_transition_lanes( + transition_geometry: dict[tuple[str, str], _TransitionGeometry], + incidences: dict[str, list[_LaneIncidence]], + lanes: list[_LaneBuild], + lane_dividers: list[GameMapLaneDivider], +) -> None: + """Replace narrow road incidences with visible node transition lanes.""" + for (node_id, road_id), geometry in sorted(transition_geometry.items()): + road_incidences = [ + incidence + for incidence in incidences[node_id] + if incidence.lane.element_id == road_id + ] + if not road_incidences: + raise AssertionError(f"Missing road incidences for {node_id!r}/{road_id!r}") + incidences[node_id] = [ + incidence + for incidence in incidences[node_id] + if incidence.lane.element_id != road_id + ] + built = _build_transition_lanes(geometry) + for lane, direction in zip( + built, + geometry.transition.local_attributes.directions, + strict=True, + ): + kind = "start" if direction == "forward" else "end" + candidates = [item for item in road_incidences if item.kind == kind] + transition_far = ( + lane.centerline[-1, :2] + if direction == "forward" + else lane.centerline[0, :2] + ) + target = min( + candidates, + key=lambda incidence: float( + np.linalg.norm( + ( + incidence.lane.centerline[0, :2] + if kind == "start" + else incidence.lane.centerline[-1, :2] + ) + - transition_far + ) + ), + ) + if direction == "forward": + lane.successors.append(target.lane.lane_id) + else: + target.lane.successors.append(lane.lane_id) + incidences[node_id].append( + _LaneIncidence(lane, node_id, kind, target.edge_ref) + ) + lanes.extend(built) + lane_dividers.extend( + _build_lane_dividers(built, geometry.transition.local_attributes) + ) + + +def _build_lane_dividers( + lanes: list[_LaneBuild], attributes: GameMapLinearAttributes +) -> list[GameMapLaneDivider]: + """Resolve authored profile dividers without rediscovering them geometrically.""" + dividers: list[GameMapLaneDivider] = [] + for index, (style, color) in enumerate(attributes.divider_markings): + if style == "VIRTUAL": + continue + first = lanes[index] + second = lanes[index + 1] + first_side = "right" if attributes.directions[index] == "forward" else "left" + second_side = ( + "left" if attributes.directions[index + 1] == "forward" else "right" + ) + first_edge = first.right_edge if first_side == "right" else first.left_edge + second_edge = second.right_edge if second_side == "right" else second.left_edge + if first_edge.shape != second_edge.shape: + raise GameMapError( + f"Adjacent lanes in {first.element_id!r} have mismatched samples" + ) + direct_error = float(np.linalg.norm(first_edge - second_edge, axis=1).max()) + reverse_error = float( + np.linalg.norm(first_edge - second_edge[::-1], axis=1).max() + ) + aligned_second = ( + second_edge if direct_error <= reverse_error else second_edge[::-1] + ) + lane_edges = ((first.lane_id, first_side), (second.lane_id, second_side)) + dividers.append( + GameMapLaneDivider( + divider_id=":".join(sorted((first.lane_id, second.lane_id))), + lane_edges=lane_edges, + polyline_world=np.mean((first_edge, aligned_second), axis=0).astype( + np.float32 + ), + style=style, + color=color, + ) + ) + return dividers + + +def _incidences_for_lanes( + lanes: list[_LaneBuild], a: str, b: str, edge_ref: str +) -> list[_LaneIncidence]: + result: list[_LaneIncidence] = [] + for lane in lanes: + if lane.start_endpoint == "from": + result.extend( + ( + _LaneIncidence(lane, a, "start", f"{edge_ref}:a"), + _LaneIncidence(lane, b, "end", f"{edge_ref}:b"), + ) + ) + else: + result.extend( + ( + _LaneIncidence(lane, b, "start", f"{edge_ref}:b"), + _LaneIncidence(lane, a, "end", f"{edge_ref}:a"), + ) + ) + return result + + +def _wire_node( + node: GameMapNode, + incidences: list[_LaneIncidence], + lanes: list[_LaneBuild], + connector_samples: int, + *, + access_turns_only: bool = False, +) -> None: + incoming = [item for item in incidences if item.kind == "end"] + outgoing = [item for item in incidences if item.kind == "start"] + connector_count = 0 + for source in incoming: + for target in outgoing: + source_is_access = source.edge_ref.startswith("parking_access:") + target_is_access = target.edge_ref.startswith("parking_access:") + if access_turns_only and source_is_access == target_is_access: + continue + if source.edge_ref == target.edge_ref and node.node_type != "cul_de_sac": + continue + if node.node_type == "parking_lot": + source.lane.successors.append(target.lane.lane_id) + continue + center = np.asarray([node.x_m, node.y_m, 0.0], dtype=np.float32) + if node.node_type == "cul_de_sac": + centerline = _bezier( + source.lane.centerline[-1], + center, + target.lane.centerline[0], + connector_samples, + ) + else: + start = source.lane.centerline[-1] + end = target.lane.centerline[0] + incoming_tangent = start - source.lane.centerline[-2] + outgoing_tangent = target.lane.centerline[1] - end + incoming_tangent /= max(float(np.linalg.norm(incoming_tangent)), 1.0e-9) + outgoing_tangent /= max(float(np.linalg.norm(outgoing_tangent)), 1.0e-9) + chord_length = float(np.linalg.norm(end - start)) + handle_length = chord_length * _INTERSECTION_TURN_HANDLE_RATIO + first_control = start + incoming_tangent * handle_length + second_control = end - outgoing_tangent * handle_length + t = np.linspace(0.0, 1.0, connector_samples, dtype=np.float32)[:, None] + centerline = ( + (1.0 - t) ** 3 * start + + 3.0 * (1.0 - t) ** 2 * t * first_control + + 3.0 * (1.0 - t) * t**2 * second_control + + t**3 * end + ).astype(np.float32) + width = float( + np.linalg.norm(source.lane.left_edge[-1] - source.lane.right_edge[-1]) + ) + left = _xyz(_offset_polyline(centerline[:, :2], width * 0.5)) + right = _xyz(_offset_polyline(centerline[:, :2], -width * 0.5)) + connector_id = f"{node.node_id}:connector:{connector_count}" + connector_count += 1 + connector = _LaneBuild( + lane_id=connector_id, + element_id=node.node_id, + centerline=centerline, + left_edge=left, + right_edge=right, + roadside_edge=right, + speed_limit_mps=source.lane.speed_limit_mps, + marking_style="VIRTUAL", + marking_color="WHITE", + start_endpoint="", + end_endpoint="", + successors=[target.lane.lane_id], + allows_taxi_stops=False, + conditioning_visible=False, + ) + lanes.append(connector) + source.lane.successors.append(connector_id) + + +def _wire_road_joint( + node: GameMapNode, + incidences: list[_LaneIncidence], + centerline_xy: np.ndarray, + lanes: list[_LaneBuild], + lane_dividers: list[GameMapLaneDivider], +) -> None: + """Build conditioning-visible lanes through one road joint.""" + assert isinstance(node.attributes, GameMapLinearAttributes) + joint_lanes = _build_linear_lanes( + node.node_id, + centerline_xy, + node.attributes, + False, + ) + incoming = [incidence for incidence in incidences if incidence.kind == "end"] + outgoing = [incidence for incidence in incidences if incidence.kind == "start"] + if len(incoming) != len(joint_lanes) or len(outgoing) != len(joint_lanes): + raise GameMapError( + f"Road joint {node.node_id!r} cannot pair its directed road lanes" + ) + + unused_incoming = list(incoming) + unused_outgoing = list(outgoing) + for joint_lane in joint_lanes: + source = min( + unused_incoming, + key=lambda incidence: float( + np.linalg.norm( + incidence.lane.centerline[-1, :2] - joint_lane.centerline[0, :2] + ) + ), + ) + target = min( + unused_outgoing, + key=lambda incidence: float( + np.linalg.norm( + incidence.lane.centerline[0, :2] - joint_lane.centerline[-1, :2] + ) + ), + ) + unused_incoming.remove(source) + unused_outgoing.remove(target) + joint_lane.speed_limit_mps = source.lane.speed_limit_mps + joint_lane.successors.append(target.lane.lane_id) + source.lane.successors.append(joint_lane.lane_id) + + lanes.extend(joint_lanes) + lane_dividers.extend(_build_lane_dividers(joint_lanes, node.attributes)) + + +def _spawn( + raw: dict[str, Any], + source_path: Path, + lane_by_id: dict[str, _LaneBuild], +) -> GameMapSpawn: + if set(raw) != {"id", "road", "lane", "distance_m", "variants"}: + raise GameMapError( + "Spawns require exactly id, road, lane, distance_m, and variants" + ) + spawn_id = str(raw["id"]).strip() + if not spawn_id: + raise GameMapError("Spawn id must not be empty") + lane_index = raw["lane"] + if type(lane_index) is not int or lane_index < 0: + raise GameMapError("spawn.lane must be a nonnegative integer") + lane_id = f"{str(raw['road'])}:lane:{lane_index}" + if lane_id not in lane_by_id or not lane_by_id[lane_id].allows_taxi_stops: + raise GameMapError(f"Spawn references unavailable road lane {lane_id!r}") + lane = lane_by_id[lane_id] + distance = _positive_float(raw["distance_m"], "spawn.distance_m") + points = lane.centerline + lengths = np.linalg.norm(np.diff(points[:, :2], axis=0), axis=1) + total = float(np.sum(lengths)) + if distance >= total: + raise GameMapError( + f"Spawn distance {distance} must be below lane length {total}" + ) + cumulative = np.concatenate(([0.0], np.cumsum(lengths))) + segment = min( + int(np.searchsorted(cumulative, distance, side="right") - 1), len(lengths) - 1 + ) + alpha = (distance - cumulative[segment]) / max(float(lengths[segment]), 1.0e-9) + position = points[segment] + alpha * (points[segment + 1] - points[segment]) + direction = points[segment + 1] - points[segment] + return GameMapSpawn( + spawn_id=spawn_id, + lane_id=lane_id, + distance_m=distance, + position_world=position.astype(np.float32), + yaw_rad=math.atan2(float(direction[1]), float(direction[0])), + variants=_parse_variants(raw, source_path), + ) + + +def load_game_map(path: Path) -> ResolvedGameMap: + """Parse and compile the current node-graph schema into runtime geometry.""" + source_path = Path(path).expanduser().resolve() + doc = _read_document(source_path) + map_id, map_name = _parse_map_identity(doc) + settings = _parse_compiler_settings(doc) + profiles = _parse_profiles(doc) + node_values = _parse_nodes(doc, profiles) + nodes = {node.node_id: node for node in node_values} + road_specs = _parse_roads(doc, nodes, profiles) + node_values = _resolve_linear_joint_nodes(node_values, road_specs) + nodes = {node.node_id: node for node in node_values} + parking_accesses = _parking_accesses_from_nodes(doc, nodes) + adjacency: dict[str, list[str]] = {node_id: [] for node_id in nodes} + for spec in road_specs: + adjacency[spec.road.from_node_id].append(f"road:{spec.road.road_id}") + adjacency[spec.road.to_node_id].append(f"road:{spec.road.road_id}") + for access in parking_accesses: + reference = f"parking_access:{access.access_id}" + adjacency[access.source_node_id].append(reference) + adjacency[access.parking_lot_node_id].append(reference) + topology = GameMapTopology( + nodes=node_values, + roads=tuple(spec.road for spec in road_specs), + parking_accesses=parking_accesses, + adjacency=tuple( + (node_id, tuple(sorted(references))) + for node_id, references in adjacency.items() + ), + ) + _validate_topology(topology) + + raw_roads: dict[str, np.ndarray] = {} + for spec in road_specs: + if spec.spans_xy: + raw_roads[spec.road.road_id] = _sample_road(spec, settings.sample_spacing_m) + else: + start = nodes[spec.road.from_node_id] + end = nodes[spec.road.to_node_id] + length = math.hypot(end.x_m - start.x_m, end.y_m - start.y_m) + samples = max(2, int(math.ceil(length / settings.sample_spacing_m)) + 1) + raw_roads[spec.road.road_id] = np.linspace( + [start.x_m, start.y_m], [end.x_m, end.y_m], samples + ) + parking_access_paths = { + access.access_id: _parking_access_path(access, nodes, settings.sample_spacing_m) + for access in parking_accesses + } + transitions = _cross_section_transitions(topology, raw_roads) + raw_roads, road_joint_centerlines = _trimmed_road_paths_and_joints( + topology, raw_roads, settings.sample_spacing_m + ) + polygons, transition_geometry, node_openings = _node_polygons( + topology, + raw_roads, + road_joint_centerlines, + parking_access_paths, + transitions, + ) + elements: list[GameMapElement] = [] + connections: list[_Connection] = [] + lanes: list[_LaneBuild] = [] + lane_dividers: list[GameMapLaneDivider] = [] + incidences: dict[str, list[_LaneIncidence]] = {node_id: [] for node_id in nodes} + + for spec in road_specs: + road = spec.road + attributes = road.attributes + from_reference = ( + f"{road.road_id}:from" + if road.from_node_id == road.to_node_id + else road.road_id + ) + to_reference = ( + f"{road.road_id}:to" + if road.from_node_id == road.to_node_id + else road.road_id + ) + try: + points = _trim_line( + raw_roads[road.road_id], + polygons[road.from_node_id], + polygons[road.to_node_id], + f"Road {road.road_id!r}", + ) + except GameMapError as error: + transition_nodes = [ + node_id + for node_id in (road.from_node_id, road.to_node_id) + if (node_id, road.road_id) in transition_geometry + ] + if transition_nodes and "completely contained" in str(error): + raise GameMapError( + f"Node {transition_nodes[0]!r} transition consumes its road arm " + f"on {road.road_id!r}" + ) from error + raise + built = _build_linear_lanes(road.road_id, points, attributes, True) + lanes.extend(built) + lane_dividers.extend(_build_lane_dividers(built, attributes)) + for incidence in _incidences_for_lanes( + built, road.from_node_id, road.to_node_id, f"road:{road.road_id}" + ): + incidences[incidence.node_id].append(incidence) + context = f"Road {road.road_id!r}" + widths = np.full(len(points), attributes.surface_width_m, dtype=np.float64) + left, right = _ribbon_sides( + points, + widths, + context, + start_opening_xy=node_openings[(road.from_node_id, from_reference)], + end_opening_xy=node_openings[(road.to_node_id, to_reference)], + ) + surface = _polygon_from_ribbon(left, right, context) + surface = _exclude_connected_footprints( + surface, + (polygons[road.from_node_id], polygons[road.to_node_id]), + context, + ) + elements.append( + GameMapElement( + element_id=road.road_id, + element_type="road", + profile_id=road.profile_id, + attributes=attributes, + surface_world=_surface_array(surface), + road_boundaries=(), + curbs=(), + ) + ) + connections.extend( + _Connection( + connection_id=f"road:{road.road_id}:{endpoint}", + first_element_id=road.road_id, + second_element_id=node_id, + opening_xy=node_openings[ + ( + node_id, + f"{road.road_id}:{endpoint}" + if road.from_node_id == road.to_node_id + else road.road_id, + ) + ], + ) + for endpoint, node_id in ( + ("from", road.from_node_id), + ("to", road.to_node_id), + ) + ) + for access in parking_accesses: + source = nodes[access.source_node_id] + lot = nodes[access.parking_lot_node_id] + centerline, opening_width = parking_access_paths[access.access_id] + attributes = GameMapLinearAttributes( + curb=True, + lane_width_m=opening_width * 0.5, + curb_offset_m=0.0, + directions=("forward", "backward"), + speed_limit_mps=5.5, + marking_style="VIRTUAL", + marking_color="WHITE", + divider_markings=(("VIRTUAL", "WHITE"),), + ) + points = _trim_line( + centerline, + polygons[source.node_id], + polygons[lot.node_id], + f"Parking access {access.access_id!r}", + ) + built = _build_linear_lanes(access.access_id, points, attributes, False) + lanes.extend(built) + lane_dividers.extend(_build_lane_dividers(built, attributes)) + for incidence in _incidences_for_lanes( + built, + source.node_id, + lot.node_id, + f"parking_access:{access.access_id}", + ): + if incidence.node_id == source.node_id: + incidences[incidence.node_id].append(incidence) + context = f"Parking access {access.access_id!r}" + widths = np.full(len(points), opening_width, dtype=np.float64) + left, right = _ribbon_sides( + points, + widths, + context, + start_opening_xy=node_openings[(source.node_id, access.access_id)], + end_opening_xy=node_openings[(lot.node_id, access.access_id)], + ) + surface = _polygon_from_ribbon(left, right, context) + surface = _exclude_connected_footprints( + surface, + (polygons[source.node_id], polygons[lot.node_id]), + context, + ) + elements.append( + GameMapElement( + element_id=access.access_id, + element_type="parking_access", + profile_id=None, + attributes=attributes, + surface_world=_surface_array(surface), + road_boundaries=(), + curbs=(), + ) + ) + connections.extend( + ( + _Connection( + connection_id=f"parking_access:{access.access_id}:source", + first_element_id=access.access_id, + second_element_id=source.node_id, + opening_xy=node_openings[(source.node_id, access.access_id)], + ), + _Connection( + connection_id=f"parking_access:{access.access_id}:lot", + first_element_id=access.access_id, + second_element_id=lot.node_id, + opening_xy=node_openings[(lot.node_id, access.access_id)], + ), + ) + ) + + _splice_transition_lanes( + transition_geometry, + incidences, + lanes, + lane_dividers, + ) + + for node in node_values: + polygon = polygons[node.node_id] + elements.append( + GameMapElement( + element_id=node.node_id, + element_type=node.node_type, + profile_id=node.profile_id, + attributes=node.attributes, + surface_world=_surface_array(polygon), + road_boundaries=(), + curbs=(), + ) + ) + + for node in node_values: + if node.node_type in {"road_joint", "driveway"}: + centerline = road_joint_centerlines[node.node_id] + _wire_road_joint( + node, + [ + incidence + for incidence in incidences[node.node_id] + if incidence.edge_ref.startswith("road:") + ], + centerline, + lanes, + lane_dividers, + ) + if node.node_type == "driveway": + _wire_node( + node, + incidences[node.node_id], + lanes, + settings.intersection_connector_samples, + access_turns_only=True, + ) + else: + _wire_node( + node, + incidences[node.node_id], + lanes, + settings.intersection_connector_samples, + ) + + lane_by_id = {lane.lane_id: lane for lane in lanes} + spawn_values = _sequence(doc["spawns"], "spawns") + if not spawn_values: + raise GameMapError("Map must define at least one spawn") + spawns = tuple( + _spawn(_mapping(value, f"spawns[{index}]"), source_path, lane_by_id) + for index, value in enumerate(spawn_values) + ) + spawn_ids = [spawn.spawn_id for spawn in spawns] + if len(set(spawn_ids)) != len(spawn_ids): + raise GameMapError("Spawn ids must be non-empty and unique") + runtime_lanes = tuple( + GameMapLane( + lane_id=lane.lane_id, + element_id=lane.element_id, + centerline_world=lane.centerline, + left_edge_world=lane.left_edge, + right_edge_world=lane.right_edge, + roadside_edge_world=lane.roadside_edge, + speed_limit_mps=lane.speed_limit_mps, + marking_style=lane.marking_style, + marking_color=lane.marking_color, + left_marking_style=lane.left_marking_style or lane.marking_style, + left_marking_color=lane.left_marking_color or lane.marking_color, + right_marking_style=lane.right_marking_style or lane.marking_style, + right_marking_color=lane.right_marking_color or lane.marking_color, + successor_ids=tuple(dict.fromkeys(lane.successors)), + allows_taxi_stops=lane.allows_taxi_stops, + conditioning_visible=lane.conditioning_visible, + ) + for lane in lanes + ) + traffic = compile_traffic( + doc.get("traffic"), + topology, + runtime_lanes, + traffic_count=doc.get("traffic_count"), + map_id=map_id, + spawns=spawns, + ) + permitted_boundary_contacts = { + tuple(sorted((access.access_id, road.road_id))) + for access in parking_accesses + for road in topology.roads + if access.source_node_id in {road.from_node_id, road.to_node_id} + } + transition_curb_regions: dict[str, list[tuple[BaseGeometry, bool]]] = {} + for (node_id, _road_id), geometry in transition_geometry.items(): + transition = geometry.transition + region = _taper_polygon( + geometry.path_xy, + transition.local_attributes.surface_width_m, + transition.arm.attributes.surface_width_m, + f"Node {node_id!r} curb region", + ).buffer(_POSITION_TOLERANCE_M) + transition_curb_regions.setdefault(node_id, []).append( + (region, transition.arm.attributes.curb) + ) + elements = _boundaries_for_elements( + elements, + connections, + permitted_boundary_contacts, + transition_curb_regions, + ) + elements = [ + replace( + element, + surface_world=np.asarray(element.surface_world, dtype=np.float32), + ) + for element in elements + ] + all_points = np.concatenate([element.surface_world for element in elements]) + minimum = np.min(all_points[:, :2], axis=0) - settings.ground_margin_m + maximum = np.max(all_points[:, :2], axis=0) + settings.ground_margin_m + ground_vertices = np.asarray( + [ + [minimum[0], minimum[1], 0.0], + [maximum[0], minimum[1], 0.0], + [maximum[0], maximum[1], 0.0], + [minimum[0], maximum[1], 0.0], + ], + dtype=np.float32, + ) + return ResolvedGameMap( + schema_version=_SCHEMA_VERSION, + map_id=map_id, + name=map_name, + source_path=source_path, + compiler_settings=settings.as_dict(), + topology=topology, + lanes=runtime_lanes, + elements=tuple(elements), + road_marking_polygons_world=(), + lane_dividers=tuple(lane_dividers), + line_markings=(), + ground_vertices=ground_vertices, + ground_faces=np.asarray([[0, 1, 2], [0, 2, 3]], dtype=np.int32), + spawns=spawns, + traffic=traffic, + ) diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/preview.py b/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/preview.py new file mode 100644 index 000000000..f3198b343 --- /dev/null +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/preview.py @@ -0,0 +1,190 @@ +# SPDX-License-Identifier: Apache-2.0 +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. + +"""SVG previews for semantic game maps.""" + +from __future__ import annotations + +import html +from pathlib import Path + +import numpy as np + +from omnidreams_game_engine.game_map.loader import load_game_map + + +def _points(points: np.ndarray, transform: object) -> str: + convert = transform + return " ".join(f"{x:.2f},{y:.2f}" for x, y in (convert(point) for point in points)) + + +def _label(text: str, point: np.ndarray, transform: object, color: str) -> str: + x, y = transform(point) + return ( + f'{html.escape(text)}' + ) + + +def _point_at_distance( + points: np.ndarray, distance_m: float +) -> tuple[np.ndarray, np.ndarray]: + lengths = np.linalg.norm(np.diff(points[:, :2], axis=0), axis=1) + cumulative = np.concatenate((np.zeros(1), np.cumsum(lengths))) + index = min( + int(np.searchsorted(cumulative, distance_m, side="right") - 1), + len(lengths) - 1, + ) + index = max(0, index) + alpha = (distance_m - cumulative[index]) / max(float(lengths[index]), 1.0e-9) + point = points[index] + alpha * (points[index + 1] - points[index]) + tangent = points[index + 1, :2] - points[index, :2] + tangent /= max(float(np.linalg.norm(tangent)), 1.0e-9) + return point, tangent + + +def write_game_map_preview( + source: Path, destination: Path, *, include_annotations: bool = True +) -> Path: + """Render a top-down semantic-map preview as SVG.""" + game_map = load_game_map(source) + points = np.concatenate( + [element.surface_world[:, :2] for element in game_map.elements], axis=0 + ) + x_min, y_min = np.min(points, axis=0) - 8.0 + x_max, y_max = np.max(points, axis=0) + 8.0 + width = max(1.0, float(x_max - x_min)) + height = max(1.0, float(y_max - y_min)) + scale = min(1000.0 / width, 800.0 / height) + + def convert(point: np.ndarray) -> tuple[float, float]: + return ( + (float(point[0]) - float(x_min)) * scale, + (float(y_max) - float(point[1])) * scale, + ) + + lines = [ + '', + '', + ] + for element in game_map.elements: + fill = "#14a878" if element.element_type == "parking_lot" else "#4b4f55" + lines.append( + f'' + ) + for polygon in game_map.road_marking_polygons_world: + lines.append( + f'' + ) + for marking in game_map.line_markings: + color = "#ffd60a" if marking.color == "YELLOW" else "#f4f4f4" + lines.append( + f'' + ) + for divider in game_map.lane_dividers: + color = "#ffd60a" if divider.color == "YELLOW" else "#f4f4f4" + lines.append( + f'' + ) + for element in game_map.elements: + for boundary in element.road_boundaries: + lines.append( + f'' + ) + for curb in element.curbs: + lines.append( + f'' + ) + for traffic in game_map.traffic: + lines.append( + f'' + ) + point, forward = _point_at_distance( + traffic.centerline_world, traffic.start_distance_m + ) + left = np.asarray([-forward[1], forward[0]]) + half_length = traffic.dimensions_lwh_m[0] * 0.5 + half_width = traffic.dimensions_lwh_m[1] * 0.5 + corners = np.asarray( + [ + point[:2] + forward * half_length + left * half_width, + point[:2] + forward * half_length - left * half_width, + point[:2] - forward * half_length - left * half_width, + point[:2] - forward * half_length + left * half_width, + ] + ) + lines.append( + f'' + ) + if include_annotations: + lines.append(_label(traffic.vehicle_id, point, convert, "#9f1239")) + if include_annotations: + lane_by_element = { + lane.element_id: lane + for lane in game_map.lanes + if lane.conditioning_visible and ":connector:" not in lane.lane_id + } + for road in game_map.topology.roads: + lane = lane_by_element[road.road_id] + point = lane.centerline_world[len(lane.centerline_world) // 2, :2] + lines.append( + _label( + f"{road.road_id} [road:{road.profile_id}; " + f"{road.from_node_id}→{road.to_node_id}]", + point, + convert, + "#17233d", + ) + ) + for access in game_map.topology.parking_accesses: + lane = lane_by_element[access.access_id] + point = lane.centerline_world[len(lane.centerline_world) // 2, :2] + lines.append( + _label( + f"{access.access_id} [parking access; " + f"{access.source_node_id}→{access.parking_lot_node_id}]", + point, + convert, + "#064e3b", + ) + ) + node_colors = { + "intersection": "#2d6cdf", + "road_joint": "#0891b2", + "cul_de_sac": "#8b5cf6", + "driveway": "#f59e0b", + "parking_lot": "#059669", + } + for node in game_map.topology.nodes: + point = np.asarray([node.x_m, node.y_m]) + x, y = convert(point) + lines.append( + f'' + ) + lines.append( + _label( + f"{node.node_id} [node:{node.node_type}]", + point, + convert, + "#111827", + ) + ) + lines.append("") + destination = Path(destination) + destination.parent.mkdir(parents=True, exist_ok=True) + destination.write_text("\n".join(lines) + "\n", encoding="utf-8") + return destination diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/spawn_render.py b/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/spawn_render.py new file mode 100644 index 000000000..4cf701905 --- /dev/null +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/spawn_render.py @@ -0,0 +1,342 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Deterministic first-person fallback renders for semantic-map spawns.""" + +from __future__ import annotations + +from pathlib import Path + +import numpy as np +import shapely +from PIL import Image +from shapely.geometry import LineString, Polygon +from shapely.geometry.base import BaseGeometry +from shapely.ops import unary_union + +from omnidreams_game_engine.camera_defaults import ( + DEFAULT_FIRST_FRAME_RESOLUTION_WH, + default_front_camera_calibration, +) +from omnidreams_game_engine.game_map.types import ( + GameMapSpawn, + ResolvedGameMap, +) +from omnidreams_game_engine.math3d import rig_pose_from_state + +SPAWN_RENDERER_VERSION = "1" +"""Version included in compiled-map cache keys for fallback rendering.""" + +_MAX_GROUND_DISTANCE_M = 600.0 +_PAINT_WIDTH_M = 0.12 +_BOUNDARY_WIDTH_M = 0.10 +_CURB_WIDTH_M = 0.28 + +_SKY_TOP_RGB = np.asarray([104, 154, 202], dtype=np.float32) +_SKY_HORIZON_RGB = np.asarray([208, 222, 226], dtype=np.float32) +_TERRAIN_RGB = np.asarray([116, 125, 83], dtype=np.float32) +_ROAD_RGB = np.asarray([64, 67, 69], dtype=np.uint8) +_PARKING_RGB = np.asarray([72, 75, 76], dtype=np.uint8) +_BOUNDARY_RGB = np.asarray([52, 54, 55], dtype=np.uint8) +_CURB_RGB = np.asarray([151, 151, 145], dtype=np.uint8) +_WHITE_PAINT_RGB = np.asarray([226, 225, 213], dtype=np.uint8) +_YELLOW_PAINT_RGB = np.asarray([222, 177, 47], dtype=np.uint8) + + +def _camera_ground_intersections( + spawn: GameMapSpawn, width: int, height: int +) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]: + calibration = default_front_camera_calibration() + u, v = np.meshgrid( + np.arange(width, dtype=np.float32) + np.float32(0.5), + np.arange(height, dtype=np.float32) + np.float32(0.5), + ) + pixels = np.column_stack((u.reshape(-1), v.reshape(-1))) + + scale = np.asarray( + [width / calibration.width, height / calibration.height], dtype=np.float32 + ) + native_pixels = pixels / scale + relative = native_pixels - np.asarray( + [calibration.cx, calibration.cy], dtype=np.float32 + ) + relative = ( + relative + @ np.linalg.inv( + np.asarray( + [ + [calibration.linear_cde[0], calibration.linear_cde[1]], + [calibration.linear_cde[2], 1.0], + ], + dtype=np.float32, + ) + ).T + ) + radius = np.linalg.norm(relative, axis=1).astype(np.float32) + angle = np.zeros_like(radius) + for power, coefficient in enumerate(calibration.polynomial): + angle += np.float32(coefficient) * np.power(radius, power, dtype=np.float32) + sin_angle = np.sin(angle).astype(np.float32) + radial_scale = np.divide( + sin_angle, + np.maximum(radius, np.float32(1.0e-6)), + out=np.zeros_like(radius), + where=radius > np.float32(1.0e-6), + ) + directions_rdf = np.column_stack( + ( + relative[:, 0] * radial_scale, + relative[:, 1] * radial_scale, + np.cos(angle), + ) + ).astype(np.float32) + directions_sensor_flu = np.column_stack( + ( + directions_rdf[:, 2], + -directions_rdf[:, 0], + -directions_rdf[:, 1], + ) + ).astype(np.float32) + + rig_to_world = rig_pose_from_state( + float(spawn.position_world[0]), + float(spawn.position_world[1]), + float(spawn.position_world[2]), + spawn.yaw_rad, + ) + sensor_to_world = rig_to_world @ calibration.sensor_to_rig_flu + directions_world = directions_sensor_flu @ sensor_to_world[:3, :3].T + origin_world = sensor_to_world[:3, 3] + ground_z = float(spawn.position_world[2]) + distance_along_ray = np.divide( + np.float32(ground_z) - origin_world[2], + directions_world[:, 2], + out=np.full(len(directions_world), np.float32(-1.0)), + where=np.abs(directions_world[:, 2]) > np.float32(1.0e-6), + ) + valid = ( + (directions_sensor_flu[:, 0] > 0.0) + & (distance_along_ray > 0.0) + & (distance_along_ray <= _MAX_GROUND_DISTANCE_M) + ) + points_world = origin_world[None, :] + ( + directions_world * distance_along_ray[:, None] + ) + planar_distance = np.linalg.norm(points_world[:, :2] - origin_world[:2], axis=1) + return points_world[:, 0], points_world[:, 1], planar_distance, valid + + +def _polygon_geometry(polygons: list[np.ndarray]) -> BaseGeometry: + geometries = [ + Polygon(np.asarray(points, dtype=np.float64)[:, :2]) + for points in polygons + if len(points) >= 3 + ] + return unary_union(geometries) if geometries else Polygon() + + +def _line_geometry(polylines: list[np.ndarray], width_m: float) -> BaseGeometry: + geometries = [ + LineString(np.asarray(points, dtype=np.float64)[:, :2]).buffer( + width_m * 0.5, cap_style="flat", join_style="round" + ) + for points in polylines + if len(points) >= 2 + ] + return unary_union(geometries) if geometries else Polygon() + + +def _paint( + image_flat: np.ndarray, + ground_indices: np.ndarray, + ground_x: np.ndarray, + ground_y: np.ndarray, + geometry: BaseGeometry, + color: np.ndarray, +) -> None: + if geometry.is_empty: + return + covered = shapely.intersects_xy(geometry, ground_x, ground_y) + image_flat[ground_indices[covered]] = color + + +def render_spawn_first_frame( + game_map: ResolvedGameMap, + spawn: GameMapSpawn, + *, + resolution_wh: tuple[int, int] = DEFAULT_FIRST_FRAME_RESOLUTION_WH, +) -> np.ndarray: + """Render a deterministic synthetic first frame aligned to ``spawn``. + + Args: + game_map: Resolved semantic map containing drawable surfaces and lines. + spawn: Spawn supplying the camera position and heading. + resolution_wh: Output resolution as ``(width, height)``. + + Returns: + RGB image with shape ``[height, width, 3]`` and dtype ``uint8``. + """ + width, height = (int(resolution_wh[0]), int(resolution_wh[1])) + if width <= 0 or height <= 0: + raise ValueError(f"resolution_wh must be positive, got {resolution_wh!r}") + + vertical = np.linspace(0.0, 1.0, height, dtype=np.float32)[:, None, None] + sky = ( + _SKY_TOP_RGB[None, None, :] * (1.0 - vertical) + + _SKY_HORIZON_RGB[None, None, :] * vertical + ) + image = np.broadcast_to(sky, (height, width, 3)).copy() + + ground_x_all, ground_y_all, distance_all, valid_ground = ( + _camera_ground_intersections(spawn, width, height) + ) + flat = image.reshape(-1, 3) + ground_indices = np.flatnonzero(valid_ground) + ground_x = ground_x_all[valid_ground] + ground_y = ground_y_all[valid_ground] + distance = distance_all[valid_ground] + terrain_shade = np.clip(1.0 - distance / 1400.0, 0.72, 1.0)[:, None] + texture = 3.0 * np.sin(ground_x[:, None] * 0.37) * np.cos(ground_y[:, None] * 0.29) + flat[ground_indices] = np.clip( + _TERRAIN_RGB[None, :] * terrain_shade + texture, 0.0, 255.0 + ) + + parking_surfaces = [ + element.surface_world + for element in game_map.elements + if element.element_type == "parking_lot" + ] + road_surfaces = [ + element.surface_world + for element in game_map.elements + if element.element_type != "parking_lot" + ] + _paint( + flat, + ground_indices, + ground_x, + ground_y, + _polygon_geometry(road_surfaces), + _ROAD_RGB, + ) + _paint( + flat, + ground_indices, + ground_x, + ground_y, + _polygon_geometry(parking_surfaces), + _PARKING_RGB, + ) + + boundaries = [ + boundary.polyline_world + for element in game_map.elements + for boundary in element.road_boundaries + ] + curbs = [ + curb.polyline_world for element in game_map.elements for curb in element.curbs + ] + _paint( + flat, + ground_indices, + ground_x, + ground_y, + _line_geometry(boundaries, _BOUNDARY_WIDTH_M), + _BOUNDARY_RGB, + ) + _paint( + flat, + ground_indices, + ground_x, + ground_y, + _line_geometry(curbs, _CURB_WIDTH_M), + _CURB_RGB, + ) + + _paint( + flat, + ground_indices, + ground_x, + ground_y, + _polygon_geometry(list(game_map.road_marking_polygons_world)), + _WHITE_PAINT_RGB, + ) + for color_name, color in ( + ("WHITE", _WHITE_PAINT_RGB), + ("YELLOW", _YELLOW_PAINT_RGB), + ): + polylines = [ + divider.polyline_world + for divider in game_map.lane_dividers + if divider.color == color_name + ] + [ + marking.polyline_world + for marking in game_map.line_markings + if marking.color == color_name + ] + _paint( + flat, + ground_indices, + ground_x, + ground_y, + _line_geometry(polylines, _PAINT_WIDTH_M), + color, + ) + return np.clip(image, 0.0, 255.0).astype(np.uint8) + + +def write_spawn_first_frame_preview( + source: Path, + destination: Path, + *, + spawn_id: str | None = None, +) -> Path: + """Write the deterministic fallback render for one authored spawn. + + Args: + source: Semantic map YAML path. + destination: PNG path to create. + spawn_id: Spawn identifier; ``None`` selects the first spawn. + + Returns: + Resolved output path. + + Raises: + GameMapError: ``spawn_id`` does not identify a map spawn. + """ + from omnidreams_game_engine.game_map._schema import GameMapError + from omnidreams_game_engine.game_map.loader import load_game_map + + game_map = load_game_map(source) + if spawn_id is None: + spawn = game_map.default_spawn + else: + spawn = next( + ( + candidate + for candidate in game_map.spawns + if candidate.spawn_id == spawn_id + ), + None, + ) + if spawn is None: + available = ", ".join(item.spawn_id for item in game_map.spawns) + raise GameMapError( + f"Unknown spawn {spawn_id!r}; available spawns: {available}" + ) + output = Path(destination).expanduser().resolve() + output.parent.mkdir(parents=True, exist_ok=True) + Image.fromarray(render_spawn_first_frame(game_map, spawn)).save(output) + return output diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/traffic.py b/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/traffic.py new file mode 100644 index 000000000..552e97ce9 --- /dev/null +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/traffic.py @@ -0,0 +1,963 @@ +# SPDX-License-Identifier: Apache-2.0 +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. + +"""Compile authored traffic waypoints onto the directed public-road graph.""" + +from __future__ import annotations + +import hashlib +import heapq +import math +from collections import deque +from dataclasses import dataclass + +import numpy as np + +from omnidreams_game_engine.game_map._schema import GameMapError +from omnidreams_game_engine.game_map.types import ( + GameMapLane, + GameMapSpawn, + GameMapTopology, + GameMapTrafficVehicle, +) + +_VEHICLE_DIMENSIONS_LWH_M = { + "car": (4.5, 1.8, 1.5), + "truck": (7.0, 2.5, 3.0), + "bus": (12.0, 2.55, 3.2), +} +_TURN_THRESHOLD_RAD = math.radians(35.0) +_GENERATED_SLOT_SPACING_M = 2.0 +_GENERATED_FOOTPRINT_BUFFER_M = 0.5 +_GENERATED_SPAWN_CLEARANCE_M = 8.0 +_HEADWAY_MIN_CLEARANCE_M = 2.0 +_HEADWAY_TIME_S = 1.25 +_HEADWAY_LANE_CORRIDOR_M = 2.25 +_HEADWAY_MAX_ANGLE_RAD = math.radians(40.0) +_MIN_ROUTE_POINT_SPACING_M = 0.25 +_LANE_SEAM_TOLERANCE_M = 0.05 +_MAX_ROUTE_YAW_RATE_RADPS = 1.2 +_MAX_ROUTE_LATERAL_ACCEL_MPS2 = 2.5 +_MAX_ROUTE_ACCEL_MPS2 = 2.5 +_MAX_ROUTE_BRAKING_MPS2 = 4.0 + + +@dataclass(frozen=True) +class _RouteTemplate: + node_ids: tuple[str, ...] + end_behavior: str + centerline_world: np.ndarray + speed_limits_mps: np.ndarray + route_element_ids: tuple[str, ...] + + +@dataclass(frozen=True) +class _Placement: + position_xy: np.ndarray + forward_xy: np.ndarray + speed_mps: float + half_length_m: float + half_width_m: float + + +def _polyline_length(points: np.ndarray) -> float: + return float(np.linalg.norm(np.diff(points[:, :2], axis=0), axis=1).sum()) + + +def _resample(points: np.ndarray, count: int) -> np.ndarray: + lengths = np.linalg.norm(np.diff(points[:, :2], axis=0), axis=1) + cumulative = np.concatenate(([0.0], np.cumsum(lengths))) + total = float(cumulative[-1]) + distances = np.linspace(0.0, total, count) + result = np.empty((count, 3), dtype=np.float64) + for index, distance in enumerate(distances): + segment = min( + int(np.searchsorted(cumulative, distance, side="right") - 1), + len(lengths) - 1, + ) + segment = max(0, segment) + alpha = (distance - cumulative[segment]) / max(float(lengths[segment]), 1.0e-9) + result[index] = points[segment] + alpha * ( + points[segment + 1] - points[segment] + ) + return result + + +def _append_path( + points: list[np.ndarray], + speeds: list[float], + element_ids: list[str], + path: np.ndarray, + speed_mps: float, + element_id: str, +) -> None: + for point in path: + if ( + points + and float(np.linalg.norm(points[-1][:2] - point[:2])) + <= _MIN_ROUTE_POINT_SPACING_M + ): + speeds[-1] = min(speeds[-1], speed_mps) + continue + if points: + element_ids.append(element_id) + points.append(np.asarray(point, dtype=np.float64)) + speeds.append(speed_mps) + + +def _curve_limited_speeds(points: np.ndarray, speeds: np.ndarray) -> np.ndarray: + """Limit a closed route to speeds its physical follower can turn through.""" + if len(points) < 4: + return speeds + closed = float(np.linalg.norm(points[-1, :2] - points[0, :2])) <= 1.0e-4 + if not closed: + return speeds + route_points = points[:-1] + route_speeds = speeds[:-1].astype(np.float64, copy=True) + count = len(route_points) + if count < 3: + return speeds + + segment_vectors = np.roll(route_points[:, :2], -1, axis=0) - route_points[:, :2] + segment_lengths = np.linalg.norm(segment_vectors, axis=1) + headings = np.arctan2(segment_vectors[:, 1], segment_vectors[:, 0]) + heading_changes = np.abs( + (headings - np.roll(headings, 1) + math.pi) % (2.0 * math.pi) - math.pi + ) + previous_lengths = np.roll(segment_lengths, 1) + local_lengths = np.maximum( + 0.5 * (previous_lengths + segment_lengths), _MIN_ROUTE_POINT_SPACING_M + ) + curvature = heading_changes / local_lengths + turning = curvature > 1.0e-9 + lateral_caps = np.full(count, math.inf, dtype=np.float64) + lateral_caps[turning] = np.sqrt(_MAX_ROUTE_LATERAL_ACCEL_MPS2 / curvature[turning]) + yaw_caps = np.full(count, math.inf, dtype=np.float64) + changing_heading = heading_changes > 1.0e-9 + yaw_caps[changing_heading] = ( + _MAX_ROUTE_YAW_RATE_RADPS + * previous_lengths[changing_heading] + / heading_changes[changing_heading] + ) + route_speeds = np.minimum(route_speeds, np.minimum(lateral_caps, yaw_caps)) + + # Propagate each turn's limit backward through its braking distance and + # forward through acceleration, including across the cyclic route seam. + for _ in range(count): + previous_speeds = route_speeds.copy() + for index in range(count): + following = (index + 1) % count + acceleration_cap = math.sqrt( + route_speeds[index] ** 2 + + 2.0 * _MAX_ROUTE_ACCEL_MPS2 * segment_lengths[index] + ) + route_speeds[following] = min(route_speeds[following], acceleration_cap) + for index in range(count - 1, -1, -1): + following = (index + 1) % count + braking_cap = math.sqrt( + route_speeds[following] ** 2 + + 2.0 * _MAX_ROUTE_BRAKING_MPS2 * segment_lengths[index] + ) + route_speeds[index] = min(route_speeds[index], braking_cap) + if np.array_equal(route_speeds, previous_speeds): + break + + route_speeds = np.concatenate((route_speeds, route_speeds[:1])) + return route_speeds.astype(np.float32) + + +def _directed_road_lanes( + topology: GameMapTopology, lanes: tuple[GameMapLane, ...] +) -> dict[tuple[str, str, str], list[GameMapLane]]: + nodes = {node.node_id: node for node in topology.nodes} + result: dict[tuple[str, str, str], list[GameMapLane]] = {} + for road in topology.roads: + road_lanes = [lane for lane in lanes if lane.element_id == road.road_id] + for start_id, end_id in ( + (road.from_node_id, road.to_node_id), + (road.to_node_id, road.from_node_id), + ): + start = np.asarray([nodes[start_id].x_m, nodes[start_id].y_m]) + end = np.asarray([nodes[end_id].x_m, nodes[end_id].y_m]) + directed = [ + lane + for lane in road_lanes + if float(np.linalg.norm(lane.centerline_world[0, :2] - start)) + < float(np.linalg.norm(lane.centerline_world[-1, :2] - start)) + and float(np.linalg.norm(lane.centerline_world[-1, :2] - end)) + < float(np.linalg.norm(lane.centerline_world[0, :2] - end)) + ] + if not directed: + continue + tangent = ( + directed[0].centerline_world[-1, :2] + - directed[0].centerline_world[0, :2] + ) + tangent /= max(float(np.linalg.norm(tangent)), 1.0e-9) + right = np.asarray([tangent[1], -tangent[0]]) + directed.sort( + key=lambda lane: -float( + np.dot( + lane.centerline_world[len(lane.centerline_world) // 2, :2], + right, + ) + ) + ) + result[(road.road_id, start_id, end_id)] = directed + return result + + +def _shortest_roads( + start_id: str, + end_id: str, + topology: GameMapTopology, + directed: dict[tuple[str, str, str], list[GameMapLane]], +) -> list[tuple[str, str, str]]: + if start_id == end_id: + return [] + outgoing: dict[str, list[tuple[str, str, float]]] = {} + for road in topology.roads: + for a, b in ( + (road.from_node_id, road.to_node_id), + (road.to_node_id, road.from_node_id), + ): + road_lanes = directed.get((road.road_id, a, b)) + if not road_lanes: + continue + weight = min(_polyline_length(lane.centerline_world) for lane in road_lanes) + outgoing.setdefault(a, []).append((b, road.road_id, weight)) + queue: list[tuple[float, str]] = [(0.0, start_id)] + distance = {start_id: 0.0} + previous: dict[str, tuple[str, str]] = {} + while queue: + cost, node_id = heapq.heappop(queue) + if cost != distance.get(node_id): + continue + if node_id == end_id: + break + for target_id, road_id, weight in sorted(outgoing.get(node_id, ())): + candidate = cost + weight + if candidate + 1.0e-9 < distance.get(target_id, math.inf): + distance[target_id] = candidate + previous[target_id] = (node_id, road_id) + heapq.heappush(queue, (candidate, target_id)) + if end_id not in previous: + raise GameMapError( + f"Traffic route cannot reach node {end_id!r} from {start_id!r}" + ) + reversed_path: list[tuple[str, str, str]] = [] + node_id = end_id + while node_id != start_id: + source_id, road_id = previous[node_id] + reversed_path.append((road_id, source_id, node_id)) + node_id = source_id + return list(reversed(reversed_path)) + + +def _turn_kind(current: list[GameMapLane], following: list[GameMapLane]) -> str: + incoming = current[0].centerline_world + outgoing = following[0].centerline_world + first = incoming[-1, :2] - incoming[-2, :2] + second = outgoing[1, :2] - outgoing[0, :2] + first /= max(float(np.linalg.norm(first)), 1.0e-9) + second /= max(float(np.linalg.norm(second)), 1.0e-9) + angle = math.atan2( + float(first[0] * second[1] - first[1] * second[0]), float(np.dot(first, second)) + ) + if abs(angle) <= _TURN_THRESHOLD_RAD: + return "straight" + return "left" if angle > 0.0 else "right" + + +def _connector_path( + source_id: str, + target_id: str, + lane_by_id: dict[str, GameMapLane], + public_road_ids: set[str], + parking_access_ids: set[str], +) -> list[GameMapLane]: + queue: list[tuple[float, float, float, str, tuple[str, ...]]] = [ + (0.0, 0.0, 0.0, source_id, (source_id,)) + ] + best = {source_id: (0.0, 0.0, 0.0)} + while queue: + seam_cost, heading_cost, length_cost, lane_id, path = heapq.heappop(queue) + if (seam_cost, heading_cost, length_cost) != best.get(lane_id): + continue + if lane_id == target_id: + return [lane_by_id[item] for item in path] + lane = lane_by_id[lane_id] + source_tangent = lane.centerline_world[-1, :2] - lane.centerline_world[-2, :2] + source_tangent /= max(float(np.linalg.norm(source_tangent)), 1.0e-9) + for successor in lane.successor_ids: + if successor not in lane_by_id: + continue + following = lane_by_id[successor] + if following.element_id in public_road_ids and successor != target_id: + continue + if following.element_id in parking_access_ids: + continue + seam_distance = float( + np.linalg.norm( + lane.centerline_world[-1, :2] - following.centerline_world[0, :2] + ) + ) + target_tangent = ( + following.centerline_world[1, :2] - following.centerline_world[0, :2] + ) + target_tangent /= max(float(np.linalg.norm(target_tangent)), 1.0e-9) + heading_delta = math.acos( + float(np.clip(np.dot(source_tangent, target_tangent), -1.0, 1.0)) + ) + cost = ( + seam_cost + max(0.0, seam_distance - _LANE_SEAM_TOLERANCE_M), + heading_cost + heading_delta, + length_cost + + ( + 0.0 + if successor == target_id + else _polyline_length(following.centerline_world) + ), + ) + if cost >= best.get(successor, (math.inf, math.inf, math.inf)): + continue + best[successor] = cost + heapq.heappush(queue, (*cost, successor, (*path, successor))) + raise GameMapError( + f"Traffic route has no legal lane connection from {source_id!r} to {target_id!r}" + ) + + +def _compile_route( + traversals: list[tuple[str, str, str]], + topology: GameMapTopology, + lanes: tuple[GameMapLane, ...], + speed_cap_mps: float | None, +) -> tuple[np.ndarray, np.ndarray, tuple[str, ...]]: + if not traversals: + raise GameMapError("Traffic routes must contain travel between distinct nodes") + directed = _directed_road_lanes(topology, lanes) + candidates = [directed[item] for item in traversals] + node_types = {node.node_id: node.node_type for node in topology.nodes} + entry: list[int | None] = [None] * len(traversals) + exit_lane: list[int | None] = [None] * len(traversals) + for index, current in enumerate(candidates): + following_index = (index + 1) % len(candidates) + following = candidates[following_index] + node_id = traversals[index][2] + kind = ( + "straight" + if node_types[node_id] in {"road_joint", "driveway"} + else _turn_kind(current, following) + ) + if kind == "right": + exit_lane[index] = 0 + entry[following_index] = 0 + elif kind == "left": + exit_lane[index] = len(current) - 1 + entry[following_index] = len(following) - 1 + for _ in range(2): + for index, current in enumerate(candidates): + following_index = (index + 1) % len(candidates) + following = candidates[following_index] + if exit_lane[index] is None: + exit_lane[index] = entry[index] if entry[index] is not None else 0 + if entry[following_index] is None: + rank = ( + 0.0 if len(current) == 1 else exit_lane[index] / (len(current) - 1) + ) + entry[following_index] = round(rank * (len(following) - 1)) + lane_by_id = {lane.lane_id: lane for lane in lanes} + public_road_ids = {road.road_id for road in topology.roads} + parking_access_ids = {access.access_id for access in topology.parking_accesses} + points: list[np.ndarray] = [] + speeds: list[float] = [] + element_ids: list[str] = [] + for index, road_candidates in enumerate(candidates): + incoming_lane = road_candidates[int(entry[index] or 0)] + outgoing_lane = road_candidates[int(exit_lane[index] or 0)] + count = max( + len(incoming_lane.centerline_world), len(outgoing_lane.centerline_world), 8 + ) + incoming_points = _resample(incoming_lane.centerline_world, count) + outgoing_points = _resample(outgoing_lane.centerline_world, count) + alpha = np.linspace(0.0, 1.0, count) + smooth = alpha * alpha * alpha * (10.0 + alpha * (-15.0 + 6.0 * alpha)) + road_path = ( + incoming_points * (1.0 - smooth[:, None]) + + outgoing_points * smooth[:, None] + ) + road_speed = min(incoming_lane.speed_limit_mps, outgoing_lane.speed_limit_mps) + if speed_cap_mps is not None: + road_speed = min(road_speed, speed_cap_mps) + _append_path( + points, + speeds, + element_ids, + road_path, + road_speed, + traversals[index][0], + ) + + following_index = (index + 1) % len(candidates) + target_lane = candidates[following_index][int(entry[following_index] or 0)] + connector = _connector_path( + outgoing_lane.lane_id, + target_lane.lane_id, + lane_by_id, + public_road_ids, + parking_access_ids, + ) + for lane in connector[1:-1]: + connector_speed = lane.speed_limit_mps + if speed_cap_mps is not None: + connector_speed = min(connector_speed, speed_cap_mps) + _append_path( + points, + speeds, + element_ids, + lane.centerline_world, + connector_speed, + lane.element_id, + ) + closure_distance = float(np.linalg.norm(points[-1][:2] - points[0][:2])) + if closure_distance > _MIN_ROUTE_POINT_SPACING_M: + element_ids.append(element_ids[-1]) + points.append(points[0].copy()) + speeds.append(speeds[0]) + elif closure_distance > 1.0e-4: + points[-1] = points[0].copy() + speeds[-1] = min(speeds[-1], speeds[0]) + if len(points) < 3 or _polyline_length(np.asarray(points)) <= 1.0: + raise GameMapError("Traffic route resolves to degenerate geometry") + if len(element_ids) != len(points) - 1: + raise GameMapError("Traffic route element metadata is misaligned") + route_points = np.asarray(points, dtype=np.float32) + route_speeds = _curve_limited_speeds( + route_points, np.asarray(speeds, dtype=np.float32) + ) + return route_points, route_speeds, tuple(element_ids) + + +def _insert_turnarounds( + traversals: list[tuple[str, str, str]], + topology: GameMapTopology, + directed: dict[tuple[str, str, str], list[GameMapLane]], +) -> list[tuple[str, str, str]]: + """Route immediate reversals through an incident cul-de-sac arm.""" + node_types = {node.node_id: node.node_type for node in topology.nodes} + roads = list(topology.roads) + result: list[tuple[str, str, str]] = [] + for index, current in enumerate(traversals): + result.append(current) + following = traversals[(index + 1) % len(traversals)] + if current[2] != following[1] or current[0] != following[0]: + continue + node_id = current[2] + if node_types[node_id] == "cul_de_sac": + continue + candidates: list[tuple[str, str]] = [] + for road in roads: + if road.road_id == current[0]: + continue + if road.from_node_id == node_id: + remote = road.to_node_id + elif road.to_node_id == node_id: + remote = road.from_node_id + else: + continue + if ( + node_types[remote] == "cul_de_sac" + and (road.road_id, node_id, remote) in directed + and (road.road_id, remote, node_id) in directed + ): + candidates.append((road.road_id, remote)) + if not candidates: + raise GameMapError( + f"Traffic route cannot reverse direction at node {node_id!r}; " + "add a waypoint loop or use a cul-de-sac endpoint" + ) + road_id, remote = sorted(candidates)[0] + result.extend(((road_id, node_id, remote), (road_id, remote, node_id))) + return result + + +def _compile_waypoint_route( + node_ids: tuple[str, ...], + end_behavior: str, + topology: GameMapTopology, + lanes: tuple[GameMapLane, ...], + directed: dict[tuple[str, str, str], list[GameMapLane]], + speed_mps: float | None, +) -> tuple[np.ndarray, np.ndarray, tuple[str, ...]]: + waypoint_cycle = list(node_ids) + if end_behavior == "reverse": + waypoint_cycle.extend(reversed(node_ids[:-1])) + legs = list(zip(waypoint_cycle, waypoint_cycle[1:])) + if end_behavior == "wrap": + legs.append((waypoint_cycle[-1], waypoint_cycle[0])) + traversals: list[tuple[str, str, str]] = [] + for source_id, target_id in legs: + traversals.extend(_shortest_roads(source_id, target_id, topology, directed)) + traversals = _insert_turnarounds(traversals, topology, directed) + return _compile_route(traversals, topology, lanes, speed_mps) + + +def _tree_path( + start_id: str, + end_id: str, + adjacency: dict[str, list[tuple[str, str]]], +) -> list[str]: + queue = deque([start_id]) + previous: dict[str, str | None] = {start_id: None} + while queue: + node_id = queue.popleft() + if node_id == end_id: + break + for neighbor_id, _ in adjacency.get(node_id, ()): + if neighbor_id in previous: + continue + previous[neighbor_id] = node_id + queue.append(neighbor_id) + if end_id not in previous: + return [] + path: list[str] = [] + node_id: str | None = end_id + while node_id is not None: + path.append(node_id) + node_id = previous[node_id] + return list(reversed(path)) + + +def _fundamental_cycles(topology: GameMapTopology) -> list[tuple[str, ...]]: + parent: dict[str, str] = {} + + def find(node_id: str) -> str: + parent.setdefault(node_id, node_id) + while parent[node_id] != node_id: + parent[node_id] = parent[parent[node_id]] + node_id = parent[node_id] + return node_id + + tree: dict[str, list[tuple[str, str]]] = {} + cycles: list[tuple[str, ...]] = [] + for road in sorted(topology.roads, key=lambda value: value.road_id): + first = road.from_node_id + second = road.to_node_id + first_root = find(first) + second_root = find(second) + if first_root != second_root: + parent[second_root] = first_root + tree.setdefault(first, []).append((second, road.road_id)) + tree.setdefault(second, []).append((first, road.road_id)) + continue + path = _tree_path(second, first, tree) + if len(path) >= 3: + cycles.append(tuple([first, *path[:-1]])) + return cycles + + +def _nearest_cul_de_sac_pairs( + topology: GameMapTopology, + directed: dict[tuple[str, str, str], list[GameMapLane]], +) -> list[tuple[str, str]]: + cul_de_sacs = sorted( + node.node_id for node in topology.nodes if node.node_type == "cul_de_sac" + ) + pairs: set[tuple[str, str]] = set() + for source_id in cul_de_sacs: + candidates: list[tuple[float, str]] = [] + for target_id in cul_de_sacs: + if target_id == source_id: + continue + try: + route = _shortest_roads(source_id, target_id, topology, directed) + except GameMapError: + continue + length = sum( + min( + _polyline_length(lane.centerline_world) + for lane in directed[traversal] + ) + for traversal in route + ) + candidates.append((length, target_id)) + if candidates: + target_id = min(candidates)[1] + pairs.add(tuple(sorted((source_id, target_id)))) + return sorted(pairs) + + +def _generated_route_templates( + topology: GameMapTopology, + lanes: tuple[GameMapLane, ...], + directed: dict[tuple[str, str, str], list[GameMapLane]], +) -> list[_RouteTemplate]: + candidates: list[tuple[tuple[str, ...], str]] = [] + for cycle in _fundamental_cycles(topology): + candidates.append((cycle, "wrap")) + candidates.append((tuple([cycle[0], *reversed(cycle[1:])]), "wrap")) + candidates.extend( + ((pair, "reverse") for pair in _nearest_cul_de_sac_pairs(topology, directed)) + ) + templates: list[_RouteTemplate] = [] + seen_geometry: set[bytes] = set() + for node_ids, end_behavior in candidates: + try: + centerline, speed_limits, route_element_ids = _compile_waypoint_route( + node_ids, + end_behavior, + topology, + lanes, + directed, + None, + ) + except GameMapError: + continue + fingerprint = hashlib.sha256(centerline.tobytes()).digest() + if fingerprint in seen_geometry: + continue + seen_geometry.add(fingerprint) + templates.append( + _RouteTemplate( + node_ids=node_ids, + end_behavior=end_behavior, + centerline_world=centerline, + speed_limits_mps=speed_limits, + route_element_ids=route_element_ids, + ) + ) + return templates + + +def _placement_at_distance( + centerline: np.ndarray, + speeds: np.ndarray, + distance_m: float, + dimensions_lwh_m: tuple[float, float, float], +) -> _Placement: + lengths = np.linalg.norm(np.diff(centerline[:, :2], axis=0), axis=1) + cumulative = np.concatenate(([0.0], np.cumsum(lengths))) + segment = min( + max(int(np.searchsorted(cumulative, distance_m, side="right") - 1), 0), + len(lengths) - 1, + ) + alpha = (distance_m - cumulative[segment]) / max(float(lengths[segment]), 1e-9) + position = centerline[segment, :2] + alpha * ( + centerline[segment + 1, :2] - centerline[segment, :2] + ) + forward = centerline[segment + 1, :2] - centerline[segment, :2] + forward /= max(float(np.linalg.norm(forward)), 1e-9) + speed = float(speeds[segment] * (1.0 - alpha) + speeds[segment + 1] * alpha) + return _Placement( + position_xy=np.asarray(position, dtype=np.float64), + forward_xy=np.asarray(forward, dtype=np.float64), + speed_mps=speed, + half_length_m=dimensions_lwh_m[0] * 0.5, + half_width_m=dimensions_lwh_m[1] * 0.5, + ) + + +def _footprints_overlap(first: _Placement, second: _Placement) -> bool: + first_left = np.asarray([-first.forward_xy[1], first.forward_xy[0]]) + second_left = np.asarray([-second.forward_xy[1], second.forward_xy[0]]) + delta = second.position_xy - first.position_xy + axes = (first.forward_xy, first_left, second.forward_xy, second_left) + first_extents = ( + first.half_length_m + _GENERATED_FOOTPRINT_BUFFER_M, + first.half_width_m + _GENERATED_FOOTPRINT_BUFFER_M, + ) + second_extents = ( + second.half_length_m + _GENERATED_FOOTPRINT_BUFFER_M, + second.half_width_m + _GENERATED_FOOTPRINT_BUFFER_M, + ) + for axis in axes: + first_radius = first_extents[0] * abs(float(np.dot(first.forward_xy, axis))) + first_radius += first_extents[1] * abs(float(np.dot(first_left, axis))) + second_radius = second_extents[0] * abs(float(np.dot(second.forward_xy, axis))) + second_radius += second_extents[1] * abs(float(np.dot(second_left, axis))) + if abs(float(np.dot(delta, axis))) >= first_radius + second_radius: + return False + return True + + +def _placement_is_safe( + candidate: _Placement, + occupied: list[_Placement], + spawn_positions: tuple[np.ndarray, ...], +) -> bool: + if any( + float(np.linalg.norm(candidate.position_xy - spawn_position)) + < _GENERATED_SPAWN_CLEARANCE_M + for spawn_position in spawn_positions + ): + return False + for other in occupied: + if _footprints_overlap(candidate, other): + return False + heading_dot = float(np.dot(candidate.forward_xy, other.forward_xy)) + heading_dot = float(np.clip(heading_dot, -1.0, 1.0)) + if math.acos(heading_dot) > _HEADWAY_MAX_ANGLE_RAD: + continue + delta = other.position_xy - candidate.position_xy + lateral = abs( + float( + candidate.forward_xy[0] * delta[1] - candidate.forward_xy[1] * delta[0] + ) + ) + if lateral > _HEADWAY_LANE_CORRIDOR_M: + continue + longitudinal = abs(float(np.dot(delta, candidate.forward_xy))) + required = ( + candidate.half_length_m + + other.half_length_m + + _HEADWAY_MIN_CLEARANCE_M + + _HEADWAY_TIME_S * max(candidate.speed_mps, other.speed_mps) + ) + if longitudinal < required: + return False + return True + + +def _generate_traffic( + count: int, + authored: list[GameMapTrafficVehicle], + topology: GameMapTopology, + lanes: tuple[GameMapLane, ...], + directed: dict[tuple[str, str, str], list[GameMapLane]], + map_id: str, + spawns: tuple[GameMapSpawn, ...], +) -> list[GameMapTrafficVehicle]: + templates = _generated_route_templates(topology, lanes, directed) + dimensions = _VEHICLE_DIMENSIONS_LWH_M["car"] + occupied = [ + _placement_at_distance( + vehicle.centerline_world, + vehicle.speed_limits_mps, + vehicle.start_distance_m, + vehicle.dimensions_lwh_m, + ) + for vehicle in authored + ] + spawn_positions = tuple( + np.asarray(spawn.position_world[:2], dtype=np.float64) for spawn in spawns + ) + slots: list[tuple[bytes, _RouteTemplate, float]] = [] + for template in templates: + route_length = _polyline_length(template.centerline_world) + signature = "|".join((*template.node_ids, template.end_behavior)).encode() + for offset_m in np.arange(0.0, route_length, _GENERATED_SLOT_SPACING_M): + key = hashlib.sha256( + map_id.encode() + + b"|" + + signature + + b"|" + + f"{float(offset_m):.3f}".encode() + ).digest() + slots.append((key, template, float(offset_m))) + accepted: list[tuple[_RouteTemplate, float]] = [] + for _, template, offset_m in sorted(slots, key=lambda item: item[0]): + placement = _placement_at_distance( + template.centerline_world, + template.speed_limits_mps, + offset_m, + dimensions, + ) + if not _placement_is_safe(placement, occupied, spawn_positions): + continue + occupied.append(placement) + accepted.append((template, offset_m)) + if len(accepted) == count: + break + if len(accepted) < count: + maximum = len(authored) + len(accepted) + raise GameMapError( + f"traffic_count requests {len(authored) + count} vehicles, but this " + f"map has safe capacity for {maximum}" + ) + used_ids = {vehicle.vehicle_id for vehicle in authored} + generated: list[GameMapTrafficVehicle] = [] + next_id = 1 + for template, offset_m in accepted[:count]: + while True: + vehicle_id = f"generated-traffic-{next_id:04d}" + next_id += 1 + if vehicle_id not in used_ids: + break + used_ids.add(vehicle_id) + generated.append( + GameMapTrafficVehicle( + vehicle_id=vehicle_id, + node_ids=template.node_ids, + end_behavior=template.end_behavior, + vehicle_type="car", + dimensions_lwh_m=dimensions, + speed_mps=None, + start_distance_m=offset_m, + centerline_world=template.centerline_world, + speed_limits_mps=template.speed_limits_mps, + route_element_ids=template.route_element_ids, + ) + ) + return generated + + +def compile_traffic( + raw_values: object, + topology: GameMapTopology, + lanes: tuple[GameMapLane, ...], + *, + traffic_count: object = None, + map_id: str = "", + spawns: tuple[GameMapSpawn, ...] = (), +) -> tuple[GameMapTrafficVehicle, ...]: + """Validate and compile optional traffic definitions.""" + if traffic_count is not None and ( + isinstance(traffic_count, bool) + or not isinstance(traffic_count, int) + or traffic_count < 0 + ): + raise GameMapError("traffic_count must be a nonnegative integer") + if raw_values is None: + raw_values = [] + if not isinstance(raw_values, list): + raise GameMapError("traffic must be a sequence") + nodes = {node.node_id: node for node in topology.nodes} + directed = _directed_road_lanes(topology, lanes) + results: list[GameMapTrafficVehicle] = [] + seen_ids: set[str] = set() + allowed = { + "id", + "nodes", + "end_behavior", + "vehicle_type", + "dimensions_lwh_m", + "speed_mps", + "start_distance_m", + } + for index, raw_value in enumerate(raw_values): + if not isinstance(raw_value, dict): + raise GameMapError(f"traffic[{index}] must be a mapping") + unknown = set(raw_value) - allowed + missing = {"id", "nodes", "end_behavior"} - set(raw_value) + if unknown or missing: + detail = ( + f"unknown fields {sorted(unknown)}" + if unknown + else f"missing fields {sorted(missing)}" + ) + raise GameMapError(f"traffic[{index}] has {detail}") + vehicle_id = str(raw_value["id"]).strip() + if not vehicle_id or vehicle_id in seen_ids: + raise GameMapError(f"Traffic id {vehicle_id!r} is empty or duplicated") + seen_ids.add(vehicle_id) + raw_nodes = raw_value["nodes"] + if not isinstance(raw_nodes, list) or len(raw_nodes) < 2: + raise GameMapError( + f"Traffic {vehicle_id!r}.nodes requires at least two nodes" + ) + node_ids = tuple(str(item).strip() for item in raw_nodes) + for node_id in node_ids: + if node_id not in nodes: + raise GameMapError( + f"Traffic {vehicle_id!r} references unknown node {node_id!r}" + ) + if nodes[node_id].node_type == "parking_lot": + raise GameMapError( + f"Traffic {vehicle_id!r} cannot visit parking-lot node {node_id!r}" + ) + end_behavior = str(raw_value["end_behavior"]).strip() + if end_behavior not in {"reverse", "wrap"}: + raise GameMapError( + f"Traffic {vehicle_id!r}.end_behavior must be reverse or wrap" + ) + vehicle_type = str(raw_value.get("vehicle_type", "car")).strip().lower() + if vehicle_type not in _VEHICLE_DIMENSIONS_LWH_M: + raise GameMapError( + f"Traffic {vehicle_id!r}.vehicle_type must be car, truck, or bus" + ) + dimensions_raw = raw_value.get( + "dimensions_lwh_m", _VEHICLE_DIMENSIONS_LWH_M[vehicle_type] + ) + if not isinstance(dimensions_raw, (list, tuple)) or len(dimensions_raw) != 3: + raise GameMapError( + f"Traffic {vehicle_id!r}.dimensions_lwh_m requires three values" + ) + try: + dimensions = tuple(float(item) for item in dimensions_raw) + except (TypeError, ValueError) as exc: + raise GameMapError( + f"Traffic {vehicle_id!r}.dimensions_lwh_m must be numeric" + ) from exc + if any(not math.isfinite(item) or item <= 0.0 for item in dimensions): + raise GameMapError( + f"Traffic {vehicle_id!r}.dimensions_lwh_m must be positive and finite" + ) + speed_value = raw_value.get("speed_mps") + try: + speed_mps = None if speed_value is None else float(speed_value) + start_distance_m = float(raw_value.get("start_distance_m", 0.0)) + except (TypeError, ValueError) as exc: + raise GameMapError( + f"Traffic {vehicle_id!r} speed_mps and start_distance_m must be numeric" + ) from exc + if speed_mps is not None and (not math.isfinite(speed_mps) or speed_mps <= 0.0): + raise GameMapError( + f"Traffic {vehicle_id!r}.speed_mps must be positive and finite" + ) + if not math.isfinite(start_distance_m) or start_distance_m < 0.0: + raise GameMapError( + f"Traffic {vehicle_id!r}.start_distance_m must be nonnegative and finite" + ) + + centerline, speed_limits, route_element_ids = _compile_waypoint_route( + node_ids, + end_behavior, + topology, + lanes, + directed, + speed_mps, + ) + route_length = _polyline_length(centerline) + if start_distance_m >= route_length: + raise GameMapError( + f"Traffic {vehicle_id!r}.start_distance_m must be less than route length {route_length:.2f} m" + ) + results.append( + GameMapTrafficVehicle( + vehicle_id=vehicle_id, + node_ids=node_ids, + end_behavior=end_behavior, + vehicle_type=vehicle_type, + dimensions_lwh_m=dimensions, + speed_mps=speed_mps, + start_distance_m=start_distance_m, + centerline_world=centerline, + speed_limits_mps=speed_limits, + route_element_ids=route_element_ids, + ) + ) + if traffic_count is None: + return tuple(results) + if traffic_count < len(results): + raise GameMapError( + f"traffic_count is {traffic_count}, but traffic defines " + f"{len(results)} vehicles; remove entries or increase traffic_count" + ) + generated_count = traffic_count - len(results) + if generated_count: + results.extend( + _generate_traffic( + generated_count, + results, + topology, + lanes, + directed, + map_id, + spawns, + ) + ) + return tuple(results) + + +__all__ = ["compile_traffic"] diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/types.py b/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/types.py new file mode 100644 index 000000000..98ac547c3 --- /dev/null +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/types.py @@ -0,0 +1,850 @@ +# SPDX-License-Identifier: Apache-2.0 +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. + +"""Runtime types for resolved semantic game maps.""" + +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path +from typing import Any + +import numpy as np +import numpy.typing as npt + +FloatArray = npt.NDArray[np.float32] + + +@dataclass(frozen=True) +class GameMapBoundaryAttributes: + """Resolved attributes for a structural map element.""" + + curb: bool + """Whether the element emits physical curb boundaries.""" + + +@dataclass(frozen=True) +class GameMapLinearAttributes(GameMapBoundaryAttributes): + """Resolved lane, surface, and marking attributes.""" + + lane_width_m: float + """Width of one directed lane in metres.""" + + curb_offset_m: float + """Paved offset between the outer lane edge and curb.""" + + directions: tuple[str, ...] + """Ordered lane directions across the element.""" + + speed_limit_mps: float + """Lane speed limit in metres per second.""" + + marking_style: str + """ClipGT-compatible outer lane-marking style.""" + + marking_color: str + """ClipGT-compatible outer lane-marking color.""" + + divider_markings: tuple[tuple[str, str], ...] + """Style and color for each adjacent lane pair.""" + + @property + def lane_width_total_m(self) -> float: + """Return the total width occupied by lanes.""" + return self.lane_width_m * len(self.directions) + + @property + def surface_width_m(self) -> float: + """Return the curb-to-curb paved width.""" + return self.lane_width_total_m + 2.0 * self.curb_offset_m + + +@dataclass(frozen=True) +class GameMapNode: + """One explicitly posed node in the authored road network.""" + + node_id: str + """Stable author-defined node identifier.""" + + node_type: str + """Node discriminator such as intersection, road joint, or parking lot.""" + + x_m: float + """Map-space x coordinate of the node origin.""" + + y_m: float + """Map-space y coordinate of the node origin.""" + + profile_id: str | None + """Optional source profile used to resolve node attributes.""" + + attributes: GameMapBoundaryAttributes | GameMapLinearAttributes + """Effective node attributes after applying profile defaults.""" + + geometry: dict[str, float] + """Validated node-type-specific geometry parameters.""" + + polygon_vertices_xy: tuple[tuple[float, float], ...] = () + """Authored map-space polygon vertices for a parking-lot node.""" + + +@dataclass(frozen=True, eq=False) +class GameMapRoad: + """One topological road edge between two structural nodes.""" + + road_id: str + """Stable author-defined road identifier.""" + + from_node_id: str + """Node at the beginning of the authored road geometry.""" + + to_node_id: str + """Node at the end of the authored road geometry.""" + + profile_id: str | None + """Optional source profile used to resolve road attributes.""" + + attributes: GameMapLinearAttributes + """Effective road attributes after applying profile defaults.""" + + bezier_spans_world: tuple[FloatArray, ...] + """Compiler-generated map-space cubic spans shaped ``[4, 3]``; empty is straight.""" + + def __eq__(self, other: object) -> bool: + """Compare road metadata and cubic span values.""" + if not isinstance(other, GameMapRoad): + return NotImplemented + return ( + self.road_id == other.road_id + and self.from_node_id == other.from_node_id + and self.to_node_id == other.to_node_id + and self.profile_id == other.profile_id + and self.attributes == other.attributes + and len(self.bezier_spans_world) == len(other.bezier_spans_world) + and all( + np.array_equal(first, second) + for first, second in zip( + self.bezier_spans_world, + other.bezier_spans_world, + strict=True, + ) + ) + ) + + +@dataclass(frozen=True) +class GameMapParkingAccess: + """A parking-lot node's inferred access corridor.""" + + access_id: str + """Stable identifier derived from the parking-lot node identifier.""" + + source_node_id: str + """Intersection or driveway node at the road end of the access.""" + + parking_lot_node_id: str + """Parking-lot node reached by the access.""" + + opening_vertex_index: int + """Zero-based runtime index of the first vertex in the opening edge.""" + + +@dataclass(frozen=True) +class GameMapTopology: + """Persisted node graph and its derived adjacency.""" + + nodes: tuple[GameMapNode, ...] + """Typed, explicitly posed graph nodes.""" + + roads: tuple[GameMapRoad, ...] + """Authored topological road edges.""" + + parking_accesses: tuple[GameMapParkingAccess, ...] + """Access corridors derived from parking-lot node connections.""" + + adjacency: tuple[tuple[str, tuple[str, ...]], ...] + """Node identifiers paired with stable incident edge/link references.""" + + +@dataclass(frozen=True) +class GameMapVisualVariant: + """Optional seed image and prompt for one visual variant.""" + + name: str + """Variant slug used to select this visual conditioning.""" + + image: str | None + """Optional map-relative or ``package://`` seed-image reference.""" + + prompt: str + """World-model text prompt paired with the seed image.""" + + +@dataclass(frozen=True) +class GameMapSpawn: + """Vehicle spawn resolved onto a directed lane.""" + + spawn_id: str + """Stable author-defined spawn identifier.""" + + lane_id: str + """Directed lane containing the spawn.""" + + distance_m: float + """Distance from the directed lane start.""" + + position_world: FloatArray + """World position with shape ``[3]``.""" + + yaw_rad: float + """World heading following the directed lane.""" + + variants: tuple[GameMapVisualVariant, ...] + """Available visual seed variants; ``default`` is always present.""" + + +@dataclass(frozen=True) +class GameMapTrafficVehicle: + """One map-authored vehicle and its compiled cyclic route.""" + + vehicle_id: str + """Stable author-defined traffic identifier.""" + + node_ids: tuple[str, ...] + """Ordered author-defined node waypoints.""" + + end_behavior: str + """Whether the waypoint list wraps or is traversed in reverse.""" + + vehicle_type: str + """Motor-vehicle category used by conditioning and physics.""" + + dimensions_lwh_m: tuple[float, float, float] + """Full vehicle length, width, and height in metres.""" + + speed_mps: float | None + """Optional maximum speed; ``None`` follows lane speed limits.""" + + start_distance_m: float + """Initial arc distance along the resolved cyclic route.""" + + centerline_world: FloatArray + """Closed, directed route centerline with shape ``[N, 3]``.""" + + speed_limits_mps: FloatArray + """Per-route-sample target speeds with shape ``[N]``.""" + + route_element_ids: tuple[str, ...] + """Owning road or node identifier for each route segment.""" + + def __post_init__(self) -> None: + if len(self.route_element_ids) != len(self.centerline_world) - 1: + raise ValueError( + "route_element_ids must contain one identifier per route segment" + ) + + +@dataclass(frozen=True) +class GameMapLane: + """Explicit directed lane and its legal successors.""" + + lane_id: str + """Stable compiler-generated lane identifier.""" + + element_id: str + """Owning routable map-element identifier.""" + + centerline_world: FloatArray + """Directed centerline with shape ``[N, 3]``.""" + + left_edge_world: FloatArray + """Left rail in travel direction with shape ``[N, 3]``.""" + + right_edge_world: FloatArray + """Right rail in travel direction with shape ``[N, 3]``.""" + + roadside_edge_world: FloatArray + """Physical roadside edge to the right of travel with shape ``[N, 3]``.""" + + speed_limit_mps: float + """Authored speed limit for this lane.""" + + marking_style: str + """ClipGT-compatible lane-marking style.""" + + marking_color: str + """ClipGT-compatible lane-marking color.""" + + left_marking_style: str + """ClipGT-compatible marking style for the directed left rail.""" + + left_marking_color: str + """ClipGT-compatible marking color for the directed left rail.""" + + right_marking_style: str + """ClipGT-compatible marking style for the directed right rail.""" + + right_marking_color: str + """ClipGT-compatible marking color for the directed right rail.""" + + successor_ids: tuple[str, ...] + """Legal successor lane identifiers.""" + + allows_taxi_stops: bool = True + """Whether taxi targets may be sampled from this lane.""" + + conditioning_visible: bool = True + """Whether the lane is emitted into world-model map conditioning.""" + + +@dataclass(frozen=True) +class GameMapElement: + """Resolved map-element geometry used by previews and diagnostics.""" + + element_id: str + """Stable author-defined identifier.""" + + element_type: str + """Schema discriminator such as ``road`` or ``intersection``.""" + + profile_id: str | None + """Optional source profile used to resolve element attributes.""" + + attributes: GameMapBoundaryAttributes | GameMapLinearAttributes + """Effective attributes controlling this element.""" + + surface_world: FloatArray + """Closed surface polygon with shape ``[N, 3]``.""" + + road_boundaries: tuple[GameMapRoadBoundary, ...] + """Element-owned semantic boundary polylines excluding declared openings.""" + + curbs: tuple[GameMapCurb, ...] + """Physical curb polylines used as collision barriers.""" + + +@dataclass(frozen=True) +class GameMapRoadBoundary: + """One semantic road-boundary polyline owned by a resolved map element.""" + + boundary_id: str + """Stable compiler-generated identifier scoped to the owning element.""" + + polyline_world: FloatArray + """World-space boundary points with shape ``[N, 3]``.""" + + +@dataclass(frozen=True) +class GameMapCurb: + """One stable curb polyline owned by a resolved map element.""" + + curb_id: str + """Stable compiler-generated identifier scoped to the owning element.""" + + polyline_world: FloatArray + """World-space curb points with shape ``[N, 3]``.""" + + +@dataclass(frozen=True) +class GameMapLineMarking: + """Resolved line marking emitted into model conditioning.""" + + marking_id: str + """Stable compiler-generated marking identifier.""" + + polyline_world: FloatArray + """World-space marking centerline with shape ``[N, 3]``.""" + + style: str + """ClipGT-compatible lane-line style.""" + + color: str + """ClipGT-compatible lane-line color.""" + + +@dataclass(frozen=True) +class GameMapLaneDivider: + """One resolved divider shared by two adjacent authored lanes.""" + + divider_id: str + """Stable compiler-generated divider identifier.""" + + lane_edges: tuple[tuple[str, str], tuple[str, str]] + """Adjacent ``(lane_id, side)`` pairs represented by the divider.""" + + polyline_world: FloatArray + """World-space divider centerline with shape ``[N, 3]``.""" + + style: str + """ClipGT-compatible lane-line style.""" + + color: str + """ClipGT-compatible lane-line color.""" + + +@dataclass(frozen=True) +class ResolvedGameMap: + """Validated semantic map with generated runtime geometry.""" + + schema_version: int + """Authoring schema version.""" + + map_id: str + """Stable map identifier.""" + + name: str + """Human-readable map name.""" + + source_path: Path + """Canonical YAML source path.""" + + compiler_settings: dict[str, object] + """Resolved authoring settings that affect generated map geometry.""" + + topology: GameMapTopology + """First-class authored topology retained alongside derived lane geometry.""" + + lanes: tuple[GameMapLane, ...] + """Directed road and intersection lanes.""" + + elements: tuple[GameMapElement, ...] + """Resolved element surfaces used by conditioning and previews.""" + + road_marking_polygons_world: tuple[FloatArray, ...] + """Closed road-marking polygons used by conditioning and previews.""" + + lane_dividers: tuple[GameMapLaneDivider, ...] + """Resolved non-virtual dividers between adjacent authored lanes.""" + + line_markings: tuple[GameMapLineMarking, ...] + """Standalone painted lines used by conditioning and previews.""" + + ground_vertices: FloatArray + """Flat ground-mesh vertices.""" + + ground_faces: npt.NDArray[np.int32] + """Ground-mesh triangle indices.""" + + spawns: tuple[GameMapSpawn, ...] + """Playable vehicle spawns.""" + + traffic: tuple[GameMapTrafficVehicle, ...] = () + """Map-authored vehicles with compiled cyclic public-road routes.""" + + @property + def default_spawn(self) -> GameMapSpawn: + """Return the first declared spawn.""" + return self.spawns[0] + + @property + def variants(self) -> tuple[str, ...]: + """Return variants available at the default spawn.""" + names = [variant.name for variant in self.default_spawn.variants] + return tuple(names) + + +def game_map_to_dict(game_map: ResolvedGameMap) -> dict[str, Any]: + """Serialize a resolved map into JSON-compatible values.""" + return { + "schema_version": game_map.schema_version, + "map_id": game_map.map_id, + "name": game_map.name, + "source_path": str(game_map.source_path), + "compiler_settings": game_map.compiler_settings, + "topology": { + "nodes": [ + { + "node_id": node.node_id, + "node_type": node.node_type, + "x_m": node.x_m, + "y_m": node.y_m, + "profile_id": node.profile_id, + "attributes": _attributes_to_dict(node.attributes), + "geometry": node.geometry, + "polygon_vertices_xy": [ + list(point) for point in node.polygon_vertices_xy + ], + } + for node in game_map.topology.nodes + ], + "roads": [ + { + "road_id": road.road_id, + "from_node_id": road.from_node_id, + "to_node_id": road.to_node_id, + "profile_id": road.profile_id, + "attributes": _attributes_to_dict(road.attributes), + "bezier_spans_world": [ + span.tolist() for span in road.bezier_spans_world + ], + } + for road in game_map.topology.roads + ], + "parking_accesses": [ + { + "access_id": access.access_id, + "source_node_id": access.source_node_id, + "parking_lot_node_id": access.parking_lot_node_id, + "opening_vertex_index": access.opening_vertex_index, + } + for access in game_map.topology.parking_accesses + ], + "adjacency": [ + [node_id, list(references)] + for node_id, references in game_map.topology.adjacency + ], + }, + "lanes": [ + { + "lane_id": lane.lane_id, + "element_id": lane.element_id, + "centerline_world": lane.centerline_world.tolist(), + "left_edge_world": lane.left_edge_world.tolist(), + "right_edge_world": lane.right_edge_world.tolist(), + "roadside_edge_world": lane.roadside_edge_world.tolist(), + "speed_limit_mps": lane.speed_limit_mps, + "marking_style": lane.marking_style, + "marking_color": lane.marking_color, + "left_marking_style": lane.left_marking_style, + "left_marking_color": lane.left_marking_color, + "right_marking_style": lane.right_marking_style, + "right_marking_color": lane.right_marking_color, + "successor_ids": list(lane.successor_ids), + "allows_taxi_stops": lane.allows_taxi_stops, + "conditioning_visible": lane.conditioning_visible, + } + for lane in game_map.lanes + ], + "elements": [ + { + "element_id": element.element_id, + "element_type": element.element_type, + "profile_id": element.profile_id, + "attributes": _attributes_to_dict(element.attributes), + "surface_world": element.surface_world.tolist(), + "road_boundaries": [ + { + "boundary_id": boundary.boundary_id, + "polyline_world": boundary.polyline_world.tolist(), + } + for boundary in element.road_boundaries + ], + "curbs": [ + { + "curb_id": curb.curb_id, + "polyline_world": curb.polyline_world.tolist(), + } + for curb in element.curbs + ], + } + for element in game_map.elements + ], + "road_marking_polygons_world": [ + polygon.tolist() for polygon in game_map.road_marking_polygons_world + ], + "lane_dividers": [ + { + "divider_id": divider.divider_id, + "lane_edges": [list(edge) for edge in divider.lane_edges], + "polyline_world": divider.polyline_world.tolist(), + "style": divider.style, + "color": divider.color, + } + for divider in game_map.lane_dividers + ], + "line_markings": [ + { + "marking_id": marking.marking_id, + "polyline_world": marking.polyline_world.tolist(), + "style": marking.style, + "color": marking.color, + } + for marking in game_map.line_markings + ], + "ground_vertices": game_map.ground_vertices.tolist(), + "ground_faces": game_map.ground_faces.tolist(), + "spawns": [ + { + "spawn_id": spawn.spawn_id, + "lane_id": spawn.lane_id, + "distance_m": spawn.distance_m, + "position_world": spawn.position_world.tolist(), + "yaw_rad": spawn.yaw_rad, + "variants": [ + { + "name": variant.name, + "image": variant.image, + "prompt": variant.prompt, + } + for variant in spawn.variants + ], + } + for spawn in game_map.spawns + ], + "traffic": [ + { + "vehicle_id": vehicle.vehicle_id, + "node_ids": list(vehicle.node_ids), + "end_behavior": vehicle.end_behavior, + "vehicle_type": vehicle.vehicle_type, + "dimensions_lwh_m": list(vehicle.dimensions_lwh_m), + "speed_mps": vehicle.speed_mps, + "start_distance_m": vehicle.start_distance_m, + "centerline_world": vehicle.centerline_world.tolist(), + "speed_limits_mps": vehicle.speed_limits_mps.tolist(), + "route_element_ids": list(vehicle.route_element_ids), + } + for vehicle in game_map.traffic + ], + } + + +def _attributes_to_dict( + attributes: GameMapBoundaryAttributes | GameMapLinearAttributes, +) -> dict[str, Any]: + """Serialize resolved element attributes.""" + result: dict[str, Any] = {"curb": attributes.curb} + if isinstance(attributes, GameMapLinearAttributes): + result.update( + { + "lane_width_m": attributes.lane_width_m, + "curb_offset_m": attributes.curb_offset_m, + "directions": list(attributes.directions), + "speed_limit_mps": attributes.speed_limit_mps, + "marking_style": attributes.marking_style, + "marking_color": attributes.marking_color, + "divider_markings": [ + list(marking) for marking in attributes.divider_markings + ], + } + ) + return result + + +def _attributes_from_dict( + raw: dict[str, Any], *, linear: bool +) -> GameMapBoundaryAttributes | GameMapLinearAttributes: + """Deserialize resolved attributes for one map element.""" + if not linear: + return GameMapBoundaryAttributes(curb=bool(raw["curb"])) + return GameMapLinearAttributes( + curb=bool(raw["curb"]), + lane_width_m=float(raw["lane_width_m"]), + curb_offset_m=float(raw["curb_offset_m"]), + directions=tuple(str(value) for value in raw["directions"]), + speed_limit_mps=float(raw["speed_limit_mps"]), + marking_style=str(raw["marking_style"]), + marking_color=str(raw["marking_color"]), + divider_markings=tuple( + (str(value[0]), str(value[1])) for value in raw["divider_markings"] + ), + ) + + +def _lane_divider_from_dict(raw: dict[str, Any]) -> GameMapLaneDivider: + edges = list(raw["lane_edges"]) + if len(edges) != 2 or any(len(edge) != 2 for edge in edges): + raise ValueError("lane_dividers[].lane_edges must contain exactly two pairs") + return GameMapLaneDivider( + divider_id=str(raw["divider_id"]), + lane_edges=( + (str(edges[0][0]), str(edges[0][1])), + (str(edges[1][0]), str(edges[1][1])), + ), + polyline_world=np.asarray(raw["polyline_world"], dtype=np.float32), + style=str(raw["style"]), + color=str(raw["color"]), + ) + + +def game_map_from_dict(value: dict[str, Any]) -> ResolvedGameMap: + """Deserialize embedded semantic-map metadata.""" + raw_topology = dict(value["topology"]) + topology = GameMapTopology( + nodes=tuple( + GameMapNode( + node_id=str(raw["node_id"]), + node_type=str(raw["node_type"]), + x_m=float(raw["x_m"]), + y_m=float(raw["y_m"]), + profile_id=( + None if raw.get("profile_id") is None else str(raw["profile_id"]) + ), + attributes=_attributes_from_dict( + dict(raw["attributes"]), + linear=str(raw["node_type"]) in {"driveway", "road_joint"}, + ), + geometry={ + str(key): float(item) for key, item in raw["geometry"].items() + }, + polygon_vertices_xy=tuple( + (float(point[0]), float(point[1])) + for point in raw.get("polygon_vertices_xy", ()) + ), + ) + for raw in raw_topology["nodes"] + ), + roads=tuple( + GameMapRoad( + road_id=str(raw["road_id"]), + from_node_id=str(raw["from_node_id"]), + to_node_id=str(raw["to_node_id"]), + profile_id=( + None if raw.get("profile_id") is None else str(raw["profile_id"]) + ), + attributes=_attributes_from_dict(dict(raw["attributes"]), linear=True), + bezier_spans_world=tuple( + np.asarray(span, dtype=np.float32) + for span in raw["bezier_spans_world"] + ), + ) + for raw in raw_topology["roads"] + ), + parking_accesses=tuple( + GameMapParkingAccess( + access_id=str(raw["access_id"]), + source_node_id=str(raw["source_node_id"]), + parking_lot_node_id=str(raw["parking_lot_node_id"]), + opening_vertex_index=int(raw["opening_vertex_index"]), + ) + for raw in raw_topology["parking_accesses"] + ), + adjacency=tuple( + (str(raw[0]), tuple(str(reference) for reference in raw[1])) + for raw in raw_topology["adjacency"] + ), + ) + lanes = tuple( + GameMapLane( + lane_id=str(raw["lane_id"]), + element_id=str(raw["element_id"]), + centerline_world=np.asarray(raw["centerline_world"], dtype=np.float32), + left_edge_world=np.asarray(raw["left_edge_world"], dtype=np.float32), + right_edge_world=np.asarray(raw["right_edge_world"], dtype=np.float32), + roadside_edge_world=np.asarray( + raw.get("roadside_edge_world", raw["right_edge_world"]), + dtype=np.float32, + ), + speed_limit_mps=float(raw["speed_limit_mps"]), + marking_style=str(raw["marking_style"]), + marking_color=str(raw["marking_color"]), + left_marking_style=str(raw.get("left_marking_style", raw["marking_style"])), + left_marking_color=str(raw.get("left_marking_color", raw["marking_color"])), + right_marking_style=str( + raw.get("right_marking_style", raw["marking_style"]) + ), + right_marking_color=str( + raw.get("right_marking_color", raw["marking_color"]) + ), + successor_ids=tuple(str(item) for item in raw["successor_ids"]), + allows_taxi_stops=bool(raw["allows_taxi_stops"]), + conditioning_visible=bool(raw.get("conditioning_visible", True)), + ) + for raw in value["lanes"] + ) + elements = tuple( + GameMapElement( + element_id=str(raw["element_id"]), + element_type=str(raw["element_type"]), + profile_id=( + None if raw.get("profile_id") is None else str(raw["profile_id"]) + ), + attributes=_attributes_from_dict( + dict(raw["attributes"]), + linear=str(raw["element_type"]) + in { + "road", + "road_joint", + "driveway", + "parking_access", + }, + ), + surface_world=np.asarray(raw["surface_world"], dtype=np.float32), + road_boundaries=tuple( + GameMapRoadBoundary( + boundary_id=str( + boundary.get("boundary_id", boundary.get("curb_id")) + ), + polyline_world=np.asarray( + boundary["polyline_world"], dtype=np.float32 + ), + ) + for boundary in raw.get("road_boundaries", raw.get("curbs", [])) + ), + curbs=tuple( + GameMapCurb( + curb_id=str(curb["curb_id"]), + polyline_world=np.asarray(curb["polyline_world"], dtype=np.float32), + ) + for curb in raw.get("curbs", []) + ), + ) + for raw in value["elements"] + ) + spawns = tuple( + GameMapSpawn( + spawn_id=str(raw["spawn_id"]), + lane_id=str(raw["lane_id"]), + distance_m=float(raw["distance_m"]), + position_world=np.asarray(raw["position_world"], dtype=np.float32), + yaw_rad=float(raw["yaw_rad"]), + variants=tuple( + GameMapVisualVariant( + name=str(variant["name"]), + image=( + None if variant.get("image") is None else str(variant["image"]) + ), + prompt=str(variant["prompt"]), + ) + for variant in raw["variants"] + ), + ) + for raw in value["spawns"] + ) + traffic = tuple( + GameMapTrafficVehicle( + vehicle_id=str(raw["vehicle_id"]), + node_ids=tuple(str(item) for item in raw["node_ids"]), + end_behavior=str(raw["end_behavior"]), + vehicle_type=str(raw["vehicle_type"]), + dimensions_lwh_m=tuple(float(item) for item in raw["dimensions_lwh_m"]), + speed_mps=( + None if raw.get("speed_mps") is None else float(raw["speed_mps"]) + ), + start_distance_m=float(raw["start_distance_m"]), + centerline_world=np.asarray(raw["centerline_world"], dtype=np.float32), + speed_limits_mps=np.asarray(raw["speed_limits_mps"], dtype=np.float32), + route_element_ids=tuple(str(item) for item in raw["route_element_ids"]), + ) + for raw in value.get("traffic", []) + ) + return ResolvedGameMap( + schema_version=int(value["schema_version"]), + map_id=str(value["map_id"]), + name=str(value["name"]), + source_path=Path(str(value["source_path"])), + compiler_settings=dict(value.get("compiler_settings", {})), + topology=topology, + lanes=lanes, + elements=elements, + road_marking_polygons_world=tuple( + np.asarray(polygon, dtype=np.float32) + for polygon in value.get("road_marking_polygons_world", []) + ), + lane_dividers=tuple( + _lane_divider_from_dict(raw) for raw in value.get("lane_dividers", []) + ), + line_markings=tuple( + GameMapLineMarking( + marking_id=str(raw["marking_id"]), + polyline_world=np.asarray(raw["polyline_world"], dtype=np.float32), + style=str(raw["style"]), + color=str(raw["color"]), + ) + for raw in value.get("line_markings", []) + ), + ground_vertices=np.asarray(value["ground_vertices"], dtype=np.float32), + ground_faces=np.asarray(value["ground_faces"], dtype=np.int32), + spawns=spawns, + traffic=traffic, + ) diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/vicinity.py b/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/vicinity.py new file mode 100644 index 000000000..d278e0af6 --- /dev/null +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/game_map/vicinity.py @@ -0,0 +1,196 @@ +# SPDX-License-Identifier: Apache-2.0 +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. + +"""Resolve graph-local actor visibility around a map-space vehicle pose.""" + +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np + +from omnidreams_game_engine.game_map.types import ResolvedGameMap + +_BOUNDARY_EPSILON_M = 1.0e-4 + + +@dataclass(frozen=True) +class GameMapVicinity: + """Semantic location and actor-visible element sets for one ego pose.""" + + location_element_id: str + traffic_element_ids: frozenset[str] + pedestrian_element_ids: frozenset[str] + + +def _polygon_contains(point: np.ndarray, polygon: np.ndarray) -> bool: + """Return whether a point is inside or on a pre-normalized polygon.""" + starts = polygon + ends = np.roll(polygon, -1, axis=0) + vectors = ends - starts + lengths_sq = np.einsum("ij,ij->i", vectors, vectors) + relative = point[None, :] - starts + alpha = np.divide( + np.einsum("ij,ij->i", relative, vectors), + lengths_sq, + out=np.zeros_like(lengths_sq), + where=lengths_sq > 1.0e-12, + ) + closest = starts + np.clip(alpha, 0.0, 1.0)[:, None] * vectors + offsets = point[None, :] - closest + if np.any(np.einsum("ij,ij->i", offsets, offsets) <= _BOUNDARY_EPSILON_M**2): + return True + + crosses_y = (starts[:, 1] > point[1]) != (ends[:, 1] > point[1]) + if not np.any(crosses_y): + return False + crossing_starts = starts[crosses_y] + crossing_vectors = vectors[crosses_y] + crossing_x = crossing_starts[:, 0] + ( + (point[1] - crossing_starts[:, 1]) + * crossing_vectors[:, 0] + / crossing_vectors[:, 1] + ) + return bool(np.count_nonzero(point[0] < crossing_x) % 2) + + +@dataclass(frozen=True) +class _PolygonLookup: + """A small vectorized bounding-box index over semantic polygons.""" + + element_ids: tuple[str, ...] + polygons: tuple[np.ndarray, ...] + minimums_xy: np.ndarray + maximums_xy: np.ndarray + + @classmethod + def build(cls, entries: tuple[tuple[str, np.ndarray], ...]) -> _PolygonLookup: + element_ids: list[str] = [] + polygons: list[np.ndarray] = [] + for element_id, polygon in entries: + vertices = np.asarray(polygon[:, :2], dtype=np.float64) + if len(vertices) > 1 and np.allclose(vertices[0], vertices[-1]): + vertices = vertices[:-1] + element_ids.append(element_id) + polygons.append(vertices) + if not polygons: + empty = np.empty((0, 2), dtype=np.float64) + return cls((), (), empty, empty.copy()) + return cls( + tuple(element_ids), + tuple(polygons), + np.asarray([polygon.min(axis=0) for polygon in polygons]), + np.asarray([polygon.max(axis=0) for polygon in polygons]), + ) + + def containing_element(self, point_xy: np.ndarray) -> str | None: + """Return the first indexed polygon containing ``point_xy``.""" + within_bounds = np.all( + (point_xy >= self.minimums_xy - _BOUNDARY_EPSILON_M) + & (point_xy <= self.maximums_xy + _BOUNDARY_EPSILON_M), + axis=1, + ) + for index in np.flatnonzero(within_bounds): + if _polygon_contains(point_xy, self.polygons[int(index)]): + return self.element_ids[int(index)] + return None + + +class GameMapVicinityResolver: + """Resolve the current road/node neighborhood from compiled map geometry.""" + + def __init__(self, game_map: ResolvedGameMap) -> None: + self._nodes = {node.node_id: node for node in game_map.topology.nodes} + self._roads = {road.road_id: road for road in game_map.topology.roads} + self._incident_roads: dict[str, set[str]] = { + node_id: set() for node_id in self._nodes + } + for road in self._roads.values(): + self._incident_roads[road.from_node_id].add(road.road_id) + self._incident_roads[road.to_node_id].add(road.road_id) + self._parking_lots_by_access_node: dict[str, set[str]] = {} + self._access_source_by_id: dict[str, str] = {} + for access in game_map.topology.parking_accesses: + self._parking_lots_by_access_node.setdefault( + access.source_node_id, set() + ).add(access.parking_lot_node_id) + self._access_source_by_id[access.access_id] = access.source_node_id + elements = {element.element_id: element for element in game_map.elements} + self._node_polygons = _PolygonLookup.build( + tuple( + (node_id, elements[node_id].surface_world) + for node_id in sorted(self._nodes) + if node_id in elements + ) + ) + self._road_polygons = _PolygonLookup.build( + tuple( + (road_id, elements[road_id].surface_world) + for road_id in sorted(self._roads) + if road_id in elements + ) + ) + self._access_polygons = _PolygonLookup.build( + tuple( + ( + self._access_source_by_id[access_id], + elements[access_id].surface_world, + ) + for access_id in sorted(self._access_source_by_id) + if access_id in elements + ) + ) + + def _location_element(self, point_xy: np.ndarray) -> str | None: + node_id = self._node_polygons.containing_element(point_xy) + if node_id is not None: + return node_id + road_id = self._road_polygons.containing_element(point_xy) + if road_id is not None: + return road_id + return self._access_polygons.containing_element(point_xy) + + def _expanded_elements(self, location: str) -> set[str]: + """Return nodes within one public-road hop and all their incident roads.""" + if location in self._roads: + road = self._roads[location] + seed_nodes = {road.from_node_id, road.to_node_id} + else: + seed_nodes = {location} + + first_roads = { + road_id + for node_id in seed_nodes + for road_id in self._incident_roads.get(node_id, ()) + } + expanded_nodes = set(seed_nodes) + for road_id in first_roads: + road = self._roads[road_id] + expanded_nodes.update((road.from_node_id, road.to_node_id)) + expanded_roads = { + road_id + for node_id in expanded_nodes + for road_id in self._incident_roads.get(node_id, ()) + } + return expanded_nodes | expanded_roads + + def resolve( + self, + x_m: float, + y_m: float, + *, + previous: GameMapVicinity | None = None, + ) -> GameMapVicinity | None: + """Return the graph neighborhood, preserving ``previous`` while off-road.""" + point_xy = np.asarray([x_m, y_m], dtype=np.float64) + location = self._location_element(point_xy) + if location is None: + return previous + traffic = self._expanded_elements(location) + pedestrians = set(traffic) + for node_id in traffic: + pedestrians.update(self._parking_lots_by_access_node.get(node_id, ())) + return GameMapVicinity(location, frozenset(traffic), frozenset(pedestrians)) + + +__all__ = ["GameMapVicinity", "GameMapVicinityResolver"] diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/input/keyboard.py b/apps/omnidreams_game_engine/omnidreams_game_engine/input/keyboard.py index 3360342d8..3f4bf9074 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/input/keyboard.py +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/input/keyboard.py @@ -65,7 +65,7 @@ def request_reset(self) -> None: self._reset_pending = True def request_exit_scene(self) -> None: - """Request a return to the scene selector from a bound device button.""" + """Request that the current scene end from a bound device button.""" with self._lock: self._exit_scene_pending = True diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/input_config/app.py b/apps/omnidreams_game_engine/omnidreams_game_engine/input_config/app.py index 66fccc11e..2a69512eb 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/input_config/app.py +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/input_config/app.py @@ -822,7 +822,7 @@ def _build_buttons(self) -> None: justify="left", text=( "Optionally bind one button to toggle reverse, one to reset / " - "respawn, and one to exit the scene (back to the scene selector). " + "respawn, and one to exit the scene. " "Click Bind, then press the button on your device. Leave unbound " "to skip -- you can always reset with the R key and exit with the " "X key." diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/motion_conformance.py b/apps/omnidreams_game_engine/omnidreams_game_engine/motion_conformance.py new file mode 100644 index 000000000..07a9da2fd --- /dev/null +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/motion_conformance.py @@ -0,0 +1,226 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Optical-flow conformance checks for game-engine driving video.""" + +from __future__ import annotations + +import math +import time +from collections.abc import Sequence +from dataclasses import dataclass +from typing import Any + +import cv2 +import numpy as np +import torch +import torch.nn.functional as torch_functional + +_FLOW_WIDTH = 160 +_FLOW_HEIGHT = 88 +_MIN_COMPONENT_PX = 0.15 +_STRONG_COMPONENT_PX = 0.40 +_MIN_GENERATED_COMPONENT_PX = 0.10 +_WEAK_RATIO = 0.20 +_MIN_USABLE_PAIRS = 3 + + +@dataclass(frozen=True) +class MotionConformanceResult: + """Chunk-level agreement between conditioning and generated camera motion.""" + + mismatched: bool + """Whether the generated chunk confidently disagrees with conditioning.""" + + axis: str + """Dominant tested motion axis, or ``"none"`` when evidence is insufficient.""" + + condition_component_px: float + """Median signed conditioning motion on the tested axis.""" + + generated_component_px: float + """Median signed generated motion on the tested axis.""" + + usable_pairs: int + """Number of frame pairs carrying sufficient conditioning motion.""" + + elapsed_ms: float + """Wall time spent measuring the chunk.""" + + def as_metrics(self) -> dict[str, float | str | bool]: + """Return JSON-compatible diagnostic values.""" + return { + "mismatched": self.mismatched, + "axis": self.axis, + "condition_component_px": self.condition_component_px, + "generated_component_px": self.generated_component_px, + "usable_pairs": float(self.usable_pairs), + "elapsed_ms": self.elapsed_ms, + } + + +def compare_motion( + condition_frames: Sequence[object], + generated_frames: Sequence[object], + *, + yaw_delta_rad: float, + longitudinal_delta_m: float, +) -> MotionConformanceResult: + """Compare dominant generated motion with the aligned condition video.""" + started_at = time.perf_counter() + condition = _flow_signatures(condition_frames) + generated = _flow_signatures(generated_frames) + axis = _dominant_axis(yaw_delta_rad, longitudinal_delta_m) + if axis == "none" or len(condition) != len(generated): + return _result(False, "none", 0.0, 0.0, 0, started_at) + + component_index = 0 if axis == "turn" else 1 + usable = [ + index + for index, signature in enumerate(condition) + if abs(signature[component_index]) >= _MIN_COMPONENT_PX + ] + if len(usable) < _MIN_USABLE_PAIRS: + return _result(False, "none", 0.0, 0.0, len(usable), started_at) + + condition_component = float( + np.median([condition[index][component_index] for index in usable]) + ) + generated_component = float( + np.median([generated[index][component_index] for index in usable]) + ) + opposite = ( + condition_component * generated_component < 0.0 + and abs(generated_component) >= _MIN_GENERATED_COMPONENT_PX + ) + too_weak = abs(condition_component) >= _STRONG_COMPONENT_PX and abs( + generated_component + ) < _WEAK_RATIO * abs(condition_component) + return _result( + opposite or too_weak, + axis, + condition_component, + generated_component, + len(usable), + started_at, + ) + + +def _dominant_axis(yaw_delta_rad: float, longitudinal_delta_m: float) -> str: + turn_strength = abs(float(yaw_delta_rad)) / math.radians(1.0) + longitudinal_strength = abs(float(longitudinal_delta_m)) / 0.15 + if max(turn_strength, longitudinal_strength) < 1.0: + return "none" + return "turn" if turn_strength >= longitudinal_strength else "longitudinal" + + +def _flow_signatures(frames: Sequence[object]) -> list[tuple[float, float]]: + gray_frames = [_gray_frame(frame) for frame in frames] + signatures: list[tuple[float, float]] = [] + for previous, current in zip(gray_frames[:-1], gray_frames[1:], strict=True): + flow = cv2.calcOpticalFlowFarneback( + previous, + current, + None, + pyr_scale=0.5, + levels=2, + winsize=15, + iterations=2, + poly_n=5, + poly_sigma=1.1, + flags=0, + ) + flow = flow[_FLOW_HEIGHT // 3 :] + height, width = flow.shape[:2] + yy, xx = np.mgrid[:height, :width].astype(np.float32) + xx -= (width - 1) * 0.5 + yy += _FLOW_HEIGHT // 3 + yy -= (_FLOW_HEIGHT - 1) * 0.5 + radius = np.sqrt(xx * xx + yy * yy) + valid = radius > 0.15 * min(_FLOW_WIDTH, _FLOW_HEIGHT) + horizontal = float(np.median(flow[..., 0])) + radial = float( + np.median( + (flow[..., 0][valid] * xx[valid] + flow[..., 1][valid] * yy[valid]) + / radius[valid] + ) + ) + signatures.append((horizontal, radial)) + return signatures + + +def _gray_frame(frame: object) -> np.ndarray: + to_tensor = getattr(frame, "to_cuda_tensor", None) + if callable(to_tensor): + tensor = to_tensor() + if torch.is_tensor(tensor): + source_event = getattr(frame, "to_cuda_event", lambda: None)() + if tensor.is_cuda and source_event is not None: + torch.cuda.current_stream(tensor.device).wait_event(source_event) + if tensor.ndim != 3 or tensor.shape[-1] not in (3, 4): + raise ValueError( + f"motion frame must be HWC RGB(A), got {tuple(tensor.shape)}" + ) + rgb = tensor[..., :3].permute(2, 0, 1).unsqueeze(0).float() + resized = torch_functional.interpolate( + rgb, + size=(_FLOW_HEIGHT, _FLOW_WIDTH), + mode="area", + )[0] + gray = resized[0] * 0.299 + resized[1] * 0.587 + resized[2] * 0.114 + return gray.round().clamp(0, 255).to(torch.uint8).cpu().numpy() + + value: Any = frame + if hasattr(value, "to_numpy"): + value = value.to_numpy() + elif hasattr(value, "detach"): + value = value.detach().cpu().numpy() + array = np.asarray(value) + if array.ndim == 4 and array.shape[0] == 1: + array = array[0] + if array.ndim == 3 and array.shape[0] in (3, 4) and array.shape[-1] not in (3, 4): + array = np.moveaxis(array, 0, -1) + if array.ndim != 3 or array.shape[-1] not in (3, 4): + raise ValueError(f"motion frame must be HWC RGB(A), got {array.shape}") + if array.dtype != np.uint8: + scale = 255.0 if np.issubdtype(array.dtype, np.floating) else 1.0 + array = np.clip(array * scale, 0.0, 255.0).astype(np.uint8) + resized = cv2.resize( + np.ascontiguousarray(array[..., :3]), + (_FLOW_WIDTH, _FLOW_HEIGHT), + interpolation=cv2.INTER_AREA, + ) + return cv2.cvtColor(resized, cv2.COLOR_RGB2GRAY) + + +def _result( + mismatched: bool, + axis: str, + condition_component: float, + generated_component: float, + usable_pairs: int, + started_at: float, +) -> MotionConformanceResult: + return MotionConformanceResult( + mismatched=mismatched, + axis=axis, + condition_component_px=condition_component, + generated_component_px=generated_component, + usable_pairs=usable_pairs, + elapsed_ms=(time.perf_counter() - started_at) * 1000.0, + ) + + +__all__ = ["MotionConformanceResult", "compare_motion"] diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/presenter.py b/apps/omnidreams_game_engine/omnidreams_game_engine/presenter.py index 74b475d84..4aad8b607 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/presenter.py +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/presenter.py @@ -106,7 +106,15 @@ def prepare_frame(self, frame: PresentedFrame, view_mode: str) -> None: and frame.model_rgb_host_uint8 is not None and self._cuda_rgb_interop is None ): - _prefetch_to_numpy(frame.model_rgb_host_uint8) + raster = getattr(self, "_raster", None) + _prefetch_to_numpy( + select_presented_rgb( + frame, + view_mode, + width=1 if raster is None else raster.width, + height=1 if raster is None else raster.height, + ) + ) return if view_mode != "model_rgb": _prefetch_to_numpy(frame.rgb_host_uint8) @@ -144,21 +152,28 @@ def present_frame(self, frame: PresentedFrame, view_mode: str) -> None: self._present_array(host_rgb) return if view_mode == "model_rgb" and frame.model_rgb_host_uint8 is not None: + raster = getattr(self, "_raster", None) + model_view = select_presented_rgb( + frame, + view_mode, + width=1 if raster is None else raster.width, + height=1 if raster is None else raster.height, + ) cuda_presented = ( self._present_cuda_rgb( - frame.model_rgb_host_uint8, + model_view, status_message=frame.status_message, flare_opacity=flare_opacity, ) if flare_opacity > 0.0 else self._present_cuda_rgb( - frame.model_rgb_host_uint8, + model_view, status_message=frame.status_message, ) ) if cuda_presented: return - rgb = _with_status_overlay(frame.model_rgb_host_uint8, frame.status_message) + rgb = _with_status_overlay(model_view, frame.status_message) if flare_opacity > 0.0: self._present_array(rgb, flare_opacity=flare_opacity) else: diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/rasterizer.py b/apps/omnidreams_game_engine/omnidreams_game_engine/rasterizer.py index bb4250f0e..d37d30528 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/rasterizer.py +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/rasterizer.py @@ -124,8 +124,8 @@ def __init__(self, raster: RasterConfig, bev: BevConfig | None = None) -> None: self.ctx.set_max_tessellation_levels(cube=3) # Use thinner BEV linework so the small map panel doesn't get # swallowed by the heavier polylines designed for the main view. - bev_line_width = max(2.0, float(raster.line_width_px) * 0.4) - bev_pole_width = max(2.0, float(raster.pole_width_px) * 0.6) + bev_line_width = max(1.5, float(raster.line_width_px) * 0.2) + bev_pole_width = max(1.5, float(raster.pole_width_px) * 0.4) self.ctx.set_line_widths( polyline_regular=float(raster.line_width_px), polyline_bev=bev_line_width, diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/renderer_settings.py b/apps/omnidreams_game_engine/omnidreams_game_engine/renderer_settings.py new file mode 100644 index 000000000..9f334b98e --- /dev/null +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/renderer_settings.py @@ -0,0 +1,115 @@ +# SPDX-License-Identifier: Apache-2.0 +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. + +"""File-backed renderer and BEV settings.""" + +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path + +from omnidreams_game_engine.config import BevConfig, RasterConfig +from omnidreams_game_engine.yaml_config import ( + StrictConfigError, + load_yaml_mapping, + require_bool, + require_exact_keys, + require_float, + require_int, + require_mapping, + require_version, +) + +_RASTER_FLOAT_FIELDS = { + "near_plane_m", + "far_plane_m", + "fog_start_m", + "fog_end_m", + "fog_power", + "triangle_raytrace_distance_m", + "lane_segment_interval_m", + "polyline_segment_interval_m", + "line_width_px", + "pole_width_px", + "dual_line_offset_m", +} +_RASTER_INT_FIELDS = {"width", "height", "triangle_raytrace_edge_samples"} +_BEV_FLOAT_FIELDS = {"height_m", "fov_deg", "tilt_deg"} +_BEV_INT_FIELDS = {"width", "height"} + + +@dataclass(frozen=True) +class RendererSettings: + """Portable visual settings loaded from a renderer YAML file.""" + + raster: RasterConfig + """Primary-camera rasterization settings.""" + + bev: BevConfig + """Top-down HUD rasterization settings.""" + + visual_flare_enabled: bool + """Whether collision visual flares are rendered.""" + + +def load_renderer_settings(path: Path) -> RendererSettings: + """Load a complete renderer YAML document. + + Args: + path: Renderer YAML path. + + Returns: + Validated visual settings. + """ + doc = load_yaml_mapping(path) + require_exact_keys( + doc, {"schema_version", "raster", "bev", "visual_flare_enabled"}, "renderer" + ) + require_version(doc, "renderer") + raw_raster = require_mapping(doc["raster"], "renderer.raster") + require_exact_keys( + raw_raster, _RASTER_FLOAT_FIELDS | _RASTER_INT_FIELDS, "renderer.raster" + ) + raster_values = { + name: require_float(raw_raster[name], f"renderer.raster.{name}", minimum=0.0) + for name in _RASTER_FLOAT_FIELDS + } + raster_values.update( + { + name: require_int(raw_raster[name], f"renderer.raster.{name}") + for name in _RASTER_INT_FIELDS + } + ) + if raster_values["near_plane_m"] >= raster_values["far_plane_m"]: + raise StrictConfigError( + "renderer.raster.near_plane_m must be less than far_plane_m" + ) + if raster_values["fog_start_m"] >= raster_values["fog_end_m"]: + raise StrictConfigError( + "renderer.raster.fog_start_m must be less than fog_end_m" + ) + + raw_bev = require_mapping(doc["bev"], "renderer.bev") + require_exact_keys( + raw_bev, {"enabled"} | _BEV_FLOAT_FIELDS | _BEV_INT_FIELDS, "renderer.bev" + ) + bev_values = { + name: require_float(raw_bev[name], f"renderer.bev.{name}", minimum=0.0) + for name in _BEV_FLOAT_FIELDS + } + bev_values.update( + { + name: require_int(raw_bev[name], f"renderer.bev.{name}") + for name in _BEV_INT_FIELDS + } + ) + bev_values["enabled"] = require_bool(raw_bev["enabled"], "renderer.bev.enabled") + if not 0.0 < bev_values["fov_deg"] < 180.0: + raise StrictConfigError("renderer.bev.fov_deg must be between 0 and 180") + return RendererSettings( + raster=RasterConfig(**raster_values), + bev=BevConfig(**bev_values), + visual_flare_enabled=require_bool( + doc["visual_flare_enabled"], "renderer.visual_flare_enabled" + ), + ) diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/runtime/loop.py b/apps/omnidreams_game_engine/omnidreams_game_engine/runtime/loop.py index 39e87292b..bedd02fdc 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/runtime/loop.py +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/runtime/loop.py @@ -7,7 +7,7 @@ import queue import time from collections import deque -from collections.abc import Callable +from collections.abc import Callable, Sequence from dataclasses import dataclass, replace from typing import Protocol @@ -121,12 +121,6 @@ class MainLoopState: frame_count: int chunks_outstanding: int last_consumed_chunk_index: int | None - # Out-of-bounds overlay text, refreshed each tick from - # ``simulation.last_proximity``. ``None`` when solidly in-bounds. - oob_message: str | None - # Consecutive chunks at/above ``LoopConfig.oob_respawn_proximity``; the - # auto-respawn fires once it reaches ``oob_respawn_debounce_chunks``. - oob_respawn_streak: int def __init__(self) -> None: self.next_present_time = time.perf_counter() @@ -134,8 +128,69 @@ def __init__(self) -> None: self.frame_count = 0 self.chunks_outstanding = 0 self.last_consumed_chunk_index = None - self.oob_message = None - self.oob_respawn_streak = 0 + + +class CommandTimeline: + """Preserve control transitions observed between model chunk requests.""" + + def __init__(self) -> None: + self._latest = DriverCommand() + self._observed = False + self._pending: list[tuple[float, DriverCommand]] = [] + self.overflow_count = 0 + + def observe(self, command: DriverCommand, sample_time: float) -> None: + if not self._observed or command != self._latest: + self._pending.append((float(sample_time), command)) + self._latest = command + self._observed = True + + def commands_for_chunk( + self, *, chunk_size: int, frame_interval_s: float + ) -> tuple[DriverCommand, ...]: + if chunk_size <= 0: + raise ValueError("chunk_size must be positive") + transitions = self._pending + self._pending = [] + if not transitions: + return tuple(self._latest for _ in range(chunk_size)) + if len(transitions) > chunk_size: + dropped = len(transitions) - chunk_size + self.overflow_count += dropped + logger.warning( + "Dropped {} oldest control transitions that could not fit in a " + "{}-frame model chunk ({} dropped total)", + dropped, + chunk_size, + self.overflow_count, + ) + transitions = transitions[-chunk_size:] + + scheduled: list[DriverCommand] = [] + safe_frame_interval_s = max(float(frame_interval_s), 1e-9) + for index, (sample_time, command) in enumerate(transitions): + if index + 1 >= len(transitions): + break + duration_s = max(0.0, transitions[index + 1][0] - sample_time) + frame_count = max(1, round(duration_s / safe_frame_interval_s)) + scheduled.extend(command for _ in range(frame_count)) + scheduled.extend( + transitions[-1][1] for _ in range(max(1, chunk_size - len(scheduled))) + ) + if len(scheduled) > chunk_size: + # Preserve the newest transitions when a long or noisy history cannot + # fit in one fixed-size model block. + dropped = len(scheduled) - chunk_size + self.overflow_count += dropped + logger.warning( + "Dropped {} oldest scheduled controls that could not fit in a " + "{}-frame model chunk ({} dropped total)", + dropped, + chunk_size, + self.overflow_count, + ) + scheduled = scheduled[-chunk_size:] + return tuple(scheduled[:chunk_size]) @dataclass(frozen=True) @@ -145,17 +200,6 @@ class LoopConfig: frame_interval_s: float poll_timeout_s: float = 0.001 history_capacity: int = 16 - # OOB thresholds applied to ``simulation.last_proximity`` (see - # :meth:`MapBounds.proximity` for the 0.0 / (0,1] / 2.0 semantics). - # Defaults match the alpasim driver: warn at the "approaching" 0.6, - # respawn at the 2.0 off-map sentinel. Both no-op when the scene has no - # geometry (proximity reads 0.0). - oob_warn_proximity: float = 0.6 - oob_respawn_proximity: float = 2.0 - # Consecutive chunks at/above ``oob_respawn_proximity`` before the - # auto-respawn fires. Default 1 matches alpasim (immediate respawn); - # raise it to debounce measurement noise. - oob_respawn_debounce_chunks: int = 1 # When set, the loop exits cleanly once chunk index N has been consumed # off the present queue. Chunk 0 is warmup and excluded from the trace, # so consuming chunks 0..N yields N traced chunks (1..N). @@ -165,11 +209,6 @@ class LoopConfig: """Whether to capture PhysX geometry independently of the selected view.""" -# OOB overlay strings, module-level so the HUD can match on them for styling. -OOB_WARN_MESSAGE = "Approaching map edge, turn back to avoid respawn" -OOB_RESPAWN_MESSAGE = "Respawning..." - - def should_request_chunk(state: MainLoopState) -> bool: return state.chunks_outstanding < 1 @@ -193,7 +232,7 @@ def _advance_present_deadline( def make_chunk_request( state: MainLoopState, simulation: SimulationBackend, - command: DriverCommand, + commands: Sequence[DriverCommand], input_sample_time: float, chunk_history: ChunkHistory, config: LoopConfig, @@ -214,8 +253,12 @@ def make_chunk_request( set_physx_debug_enabled = getattr(simulation, "set_physx_debug_enabled", None) if callable(set_physx_debug_enabled): set_physx_debug_enabled(view_mode == "physx" or config.capture_physics_debug) + if len(commands) != chunk_size: + raise ValueError( + f"commands must match requested chunk size; got {len(commands)} for {chunk_size}" + ) trajectory = simulation.pose_chunk( - command=command, + commands=commands, chunk_size=chunk_size, frame_interval_s=config.frame_interval_s, extrapolation_offset_s=0.0, @@ -264,24 +307,14 @@ def present_queued_frame( queued_frame: QueuedFrame, presenter: PresenterBackend, view_mode: str, - oob_message: str | None = None, trace_context: TraceContext | None = None, trace_dependencies: list[int] | None = None, ) -> float: - """Hand a freshly-dequeued frame to the presenter. - - ``oob_message`` is merged into the frame's ``status_message`` only for - the duration of this present call (via :func:`dataclasses.replace`), - so the original ``QueuedFrame`` keeps whatever message the backend - attached -- e.g. the world-model's "Optimizing world model..." - transition text on the first chunk's last frame stays intact across - re-presents that intersperse the warmup window with an OOB warning. - """ + """Hand a freshly-dequeued frame to the presenter.""" frame_times = queued_frame.chunk_times.frames[queued_frame.frame_index] frame_times.sample_display_pose_time = time.perf_counter() - display_frame = _frame_with_overlay(queued_frame.frame, oob_message) present_call_begin_time = time.perf_counter() - presenter.present_frame(display_frame, view_mode=view_mode) + presenter.present_frame(queued_frame.frame, view_mode=view_mode) present_time = time.perf_counter() frame_times.present_time = present_time if trace_context is not None: @@ -311,108 +344,6 @@ def present_queued_frame( return present_time -def _frame_with_overlay( - frame: PresentedFrame, oob_message: str | None -) -> PresentedFrame: - """Return ``frame`` with ``oob_message`` merged into ``status_message``. - - The OOB message wins over an existing ``status_message`` because it's - a more time-sensitive affordance (the user is about to be teleported); - returns the frame unchanged when there's no OOB message to merge in. - """ - if oob_message is None: - return frame - return replace(frame, status_message=oob_message) - - -def update_oob_state( - state: MainLoopState, simulation: SimulationBackend, config: LoopConfig -) -> bool: - """Refresh ``state.oob_message`` from the simulation's OOB proximity. - - Reads ``simulation.last_proximity`` defensively (defaults to ``0.0`` for - backends that don't track OOB). Returns ``True`` only on the chunk that - fires the auto-respawn, which requires proximity to stay at/above - :attr:`LoopConfig.oob_respawn_proximity` for - :attr:`LoopConfig.oob_respawn_debounce_chunks` consecutive chunks - (debouncing single-chunk spikes from a corner ray missing the mesh). - """ - proximity = float(getattr(simulation, "last_proximity", 0.0)) - previous_message = state.oob_message - - if proximity >= config.oob_respawn_proximity: - state.oob_respawn_streak += 1 - state.oob_message = OOB_RESPAWN_MESSAGE - if state.oob_respawn_streak >= max(1, config.oob_respawn_debounce_chunks): - _log_oob_transition( - previous_message, - OOB_RESPAWN_MESSAGE, - proximity, - streak=state.oob_respawn_streak, - action="firing respawn", - ) - return True - if previous_message != OOB_RESPAWN_MESSAGE: - _log_oob_transition( - previous_message, - OOB_RESPAWN_MESSAGE, - proximity, - streak=state.oob_respawn_streak, - action="respawn pending", - ) - return False - - # Below respawn threshold; reset the debounce streak so a brief dip - # back into the warning band can't accumulate toward a respawn. - state.oob_respawn_streak = 0 - - if proximity >= config.oob_warn_proximity: - state.oob_message = OOB_WARN_MESSAGE - if previous_message != OOB_WARN_MESSAGE: - _log_oob_transition( - previous_message, - OOB_WARN_MESSAGE, - proximity, - streak=0, - action="warning", - ) - return False - - state.oob_message = None - if previous_message is not None: - _log_oob_transition( - previous_message, - None, - proximity, - streak=0, - action="cleared", - ) - return False - - -def _log_oob_transition( - previous: str | None, - current: str | None, - proximity: float, - *, - streak: int, - action: str, -) -> None: - """Log OOB state transitions to stderr, once per state edge.""" - prev_label = "in-bounds" if previous is None else _truncate(previous, 32) - curr_label = "in-bounds" if current is None else _truncate(current, 32) - logger.info( - f"[loop] oob {prev_label!r} -> {curr_label!r}" - f" proximity={proximity:.3f} streak={streak} action={action}", - ) - - -def _truncate(text: str, limit: int) -> str: - if len(text) <= limit: - return text - return text[: limit - 1] + "\u2026" - - def push_telemetry( runtime_controls: RuntimeControls, simulation: SimulationBackend, @@ -476,18 +407,17 @@ def run_main_loop( request, so sim cadence is gated by display-driven requests, not the poll rate. ``initial_presented_frame`` seeds the re-present path used while the pipeline warms up; ``loading_status`` (if given) supplies the loading-phase - overlay text until the first real frame, with the OOB warning taking - precedence. + overlay text until the first real frame. - Returns ``True`` when the user requested a reset or the OOB auto-respawn - fired (caller rebuilds the simulation and re-runs), ``False`` when the - presenter requested close. + Returns ``True`` when the user requested a reset (the caller rebuilds the + simulation and re-runs), or ``False`` when the presenter requested close. """ state = MainLoopState() last_presented_frame: PresentedFrame = initial_presented_frame ready_frames: deque[QueuedFrame] = deque() chunk_history = ChunkHistory(config.history_capacity) visual_flare_events = VisualFlareEventQueue() + command_timeline = CommandTimeline() trigger_visual_flare_callback = getattr( presenter, "trigger_visual_flare", _noop_visual_flare ) @@ -512,6 +442,7 @@ def run_main_loop( ) input_sample_begin = time.perf_counter() sampled = input_backend.sample() + command_timeline.observe(sampled.command, sampled.sample_time) input_sample_end = time.perf_counter() last_input_sample_event = _trace_main_range( active_trace, @@ -528,10 +459,18 @@ def run_main_loop( if should_request_chunk(state) and ( runtime_application is None or runtime_application.is_running ): + request_chunk_size = ( + config.initial_chunk_size + if state.next_chunk_index == 0 + else config.chunk_size + ) chunk_request = make_chunk_request( state=state, simulation=simulation, - command=sampled.command, + commands=command_timeline.commands_for_chunk( + chunk_size=request_chunk_size, + frame_interval_s=config.frame_interval_s, + ), input_sample_time=sampled.sample_time, chunk_history=chunk_history, config=config, @@ -539,12 +478,20 @@ def run_main_loop( trace_context=active_trace, view_mode=view_mode, ) - if ( - config.visual_flare_enabled - and chunk_request.trajectory.actor_collision_detected + if config.visual_flare_enabled and ( + chunk_request.trajectory.actor_collision_detected + or chunk_request.trajectory.static_collision_detected ): + collision_indices = [ + index + for index in ( + chunk_request.trajectory.actor_collision_frame_index, + chunk_request.trajectory.static_collision_frame_index, + ) + if index is not None + ] collision_frame_index = ( - chunk_request.trajectory.actor_collision_frame_index + min(collision_indices) if collision_indices else 0 ) visual_flare_events.schedule( chunk_index=chunk_request.chunk_times.chunk_index, @@ -565,13 +512,6 @@ def run_main_loop( ), ) pipeline.request_pose_chunk(chunk_request) - # The pose chunk just advanced authoritative state, so refresh the - # OOB overlay from the new boundary frame and auto-respawn (same - # ``return True`` as a manual reset) when far enough off-map. - if ( - runtime_application is None or runtime_application.is_running - ) and update_oob_state(state, simulation, config): - return True # Republish telemetry per chunk so read-side observers (e.g. the # presenter's ``/state`` endpoint) see the latest state. if runtime_application is None: @@ -626,7 +566,6 @@ def run_main_loop( queued_frame, presenter, view_mode=view_mode, - oob_message=state.oob_message, trace_context=present_trace, trace_dependencies=event_dependencies( queued_frame.worker_ready_event_id, @@ -648,18 +587,15 @@ def run_main_loop( # present, so drop it instead of carrying it forward as a # dependency of some later, unrelated present. last_present_wait_event = None - # Re-present the last frame with the current overlay: OOB warning - # wins, else the loading-phase status until the first real frame. - # The merged frame is local so ``last_presented_frame`` is unchanged. - overlay = state.oob_message - if ( - overlay is None - and loading_status is not None - and state.frame_count == 0 - ): - overlay = loading_status() + display_frame = last_presented_frame + if loading_status is not None and state.frame_count == 0: + status_message = loading_status() + if status_message is not None: + display_frame = replace( + last_presented_frame, status_message=status_message + ) presenter.present_frame( - _frame_with_overlay(last_presented_frame, overlay), + display_frame, view_mode=view_mode, ) diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/scene_fixture.py b/apps/omnidreams_game_engine/omnidreams_game_engine/scene_fixture.py index 853691cc4..684e8ec0e 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/scene_fixture.py +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/scene_fixture.py @@ -15,12 +15,14 @@ import yaml from PIL import Image +from omnidreams_game_engine.camera_defaults import ( + DEFAULT_FRONT_CAMERA_LOGICAL_NAME, + default_front_camera_rig, +) from omnidreams_game_engine.math3d import rig_pose_from_state from omnidreams_game_engine.ply_io import save_mesh_vf _SCENE_ID = "synthetic-test-scene" -_CAMERA_CLIPGT_NAME = "camera:front:wide:120fov" -_CAMERA_LOGICAL_NAME = "camera_front_wide_120fov" _FPS = 30 # Default trajectory length: 180 frames (6 s @ 30 fps, ~60 m) is enough for the # scene-loader tests; the runtime helper passes a larger ``length_frames``. @@ -38,11 +40,8 @@ _LANE_LINE_OFFSET_M = 1.8 _ROAD_BOUNDARY_OFFSET_M = 9.0 _POLE_OFFSET_M = 9.5 -# Periodic roadside furniture spacing (poles ~50 m like streetlamps, plus parked -# cars / signs on alternating shoulders), anchored to the wavy centerline. +# Periodic roadside furniture spacing, anchored to the wavy centerline. _POLE_PERIOD_M = 50.0 -_PARKED_CAR_PERIOD_M = 150.0 -_PARKED_CAR_LATERAL_M = 5.0 _TRAFFIC_SIGN_PERIOD_M = 200.0 _TRAFFIC_SIGN_LATERAL_M = 7.0 _TRAFFIC_SIGN_HEIGHT_M = 2.5 @@ -296,40 +295,6 @@ def _traffic_light_rows() -> list[dict[str, object]]: ] -_OBSTACLE_TRACKS: tuple[dict[str, object], ...] = ( - { - "trackline_id": "car-001", - "category": "Automobile", # -> Car bbox color - "center": (25.0, -1.8, 0.8), - "size": (4.7, 2.0, 1.6), - }, - { - "trackline_id": "truck-001", - "category": "Heavy_Truck", # -> Truck bbox color - "center": (48.0, 2.0, 1.75), - "size": (9.0, 2.6, 3.5), - }, - { - "trackline_id": "pedestrian-001", - "category": "Pedestrian", - "center": (18.0, 4.0, 0.9), - "size": (0.6, 0.6, 1.8), - }, - { - "trackline_id": "cyclist-001", - "category": "Cyclist", - "center": (23.0, -3.5, 0.9), - "size": (1.8, 0.5, 1.5), - }, - { - "trackline_id": "debris-001", - "category": "Debris", # -> Others bbox color - "center": (43.0, -5.0, 0.35), - "size": (0.8, 0.8, 0.7), - }, -) - - def _periodic_poles( x_m: np.ndarray, y_m: np.ndarray, @@ -469,144 +434,7 @@ def _periodic_traffic_signs( return rows -def _periodic_parked_cars( - x_m: np.ndarray, - y_m: np.ndarray, - yaw_rad: np.ndarray, - *, - period_m: float = _PARKED_CAR_PERIOD_M, - lateral_offset_m: float = _PARKED_CAR_LATERAL_M, -) -> list[dict[str, object]]: - """Emit parked-car obstacle definitions on alternating shoulders along - the trajectory. - - Each entry has the same shape as ``_OBSTACLE_TRACKS`` so the existing - ``_obstacle_rows`` plumbing can append them. The car's yaw is set to - the centerline yaw so cars look parked parallel to the road, not - randomly oriented. - """ - if len(x_m) == 0: - return [] - tracks: list[dict[str, object]] = [] - next_x = period_m - side_idx = 0 - while next_x < float(x_m[-1]): - # Find the trajectory frame closest to ``next_x``. - i = int(np.searchsorted(x_m, next_x)) - if i >= len(x_m): - break - side = 1.0 if side_idx % 2 == 0 else -1.0 - cx = float(x_m[i]) - cy = float(y_m[i]) - cyaw = float(yaw_rad[i]) - dx = -math.sin(cyaw) * lateral_offset_m * side - dy = math.cos(cyaw) * lateral_offset_m * side - tracks.append( - { - "trackline_id": f"parked-car-{side_idx:04d}", - "category": "Automobile", - "center": (cx + dx, cy + dy, 0.8), - "size": (4.7, 2.0, 1.6), - "yaw_rad": cyaw, - } - ) - next_x += period_m - side_idx += 1 - return tracks - - -def _obstacle_rows( - timestamps_us: np.ndarray, extra_tracks: tuple[dict[str, object], ...] = () -) -> list[dict[str, object]]: - """Emit two stationary samples per track at the first and last trajectory - timestamps so ``WorldVehicleBBoxTrack.interpolate_at_timestamp`` resolves at - every render frame without needing extrapolation.""" - sample_frames = (0, int(len(timestamps_us)) - 1) - rows: list[dict[str, object]] = [] - for track in (*_OBSTACLE_TRACKS, *extra_tracks): - center_xyz = track["center"] - size_xyz = track["size"] - assert isinstance(center_xyz, tuple) and isinstance(size_xyz, tuple) - track_yaw = float(track.get("yaw_rad", 0.0)) # type: ignore[arg-type] - for sample_idx, frame_idx in enumerate(sample_frames): - rows.append( - { - "key": { - "clip_id": _SCENE_ID, - "timestamp_micros": int(timestamps_us[frame_idx]), - "label_class_id": f"{track['trackline_id']}_s{sample_idx}", - }, - "obstacle": { - "trackline_id": track["trackline_id"], - "center": _point_xyz(*center_xyz), - "size": _point_xyz(*size_xyz), - "orientation": _orientation_from_yaw(track_yaw), - "category": track["category"], - }, - "version": 1, - } - ) - return rows - - def _calibration_row() -> list[dict[str, object]]: - # Pinned to the production `camera:front:wide:120fov` calibration extracted - # from the clipgt sample scene so synthetic-scene renders exercise the same - # ftheta polynomial and mounting pose as real CI data. - rig = { - "rig": { - "properties": {}, - "vehicle": {}, - "vehicleio": {}, - "sensors": [ - { - "name": _CAMERA_CLIPGT_NAME, - "protocol": "camera.virtual", - "parameter": "video=synthetic/camera_front_wide_120fov.mp4", - "nominalSensor2Rig_FLU": { - "roll-pitch-yaw": [ - 0.292217969894409, - 0.464194804430008, - -0.191304489970207, - ], - "t": [ - 1.69035196304321, - 0.00553808081895113, - 1.45306670665741, - ], - }, - "correction_sensor_R_FLU": { - "roll-pitch-yaw": [ - -0.1592078059911728, - 0.11539523303508759, - 0.5026581287384033, - ], - }, - "correction_rig_T": [ - -0.057110343128442764, - -0.0032010308932513, - 0.008508340455591679, - ], - "properties": { - "width": "3848", - "height": "2168", - "cx": "1921.318705874846", - "cy": "1076.978854184438", - "Model": "ftheta", - "polynomial-type": "pixeldistance-to-angle", - "polynomial": ( - "0 0.0005385247479413695 -1.598462177407655e-09 " - "6.250864794463573e-12 -2.194585699335322e-15 " - "4.525222700710391e-19" - ), - "linear-c": "1.000000", - "linear-d": "0.000000", - "linear-e": "0.000000", - }, - } - ], - } - } return [ { "key": { @@ -614,7 +442,10 @@ def _calibration_row() -> list[dict[str, object]]: "timestamp_micros": int(_START_TIMESTAMP_US), "label_class_id": "calibration", }, - "calibration_estimate": {"name": "default", "rig_json": json.dumps(rig)}, + "calibration_estimate": { + "name": "default", + "rig_json": json.dumps(default_front_camera_rig()), + }, "version": 1, } ] @@ -680,7 +511,10 @@ def _metadata_doc(num_frames: int = _DEFAULT_TRAJECTORY_FRAMES) -> dict[str, obj "scene_id": _SCENE_ID, "dataset_hash": "synthetic-dataset-hash", "is_resumable": False, - "sensors": {"camera_ids": [_CAMERA_LOGICAL_NAME], "lidar_ids": []}, + "sensors": { + "camera_ids": [DEFAULT_FRONT_CAMERA_LOGICAL_NAME], + "lidar_ids": [], + }, "time_range": { "start": int(_START_TIMESTAMP_US), "end": int( @@ -732,10 +566,7 @@ def build_synthetic_scene_usdz( """Build a procedural USDZ that the scene loader can ingest unchanged. The geometry (trajectory, lane lines, road boundary, intersection, - crosswalk, poles, signs, lights, obstacles) is fixed and deterministic. - Three optional overrides exist for the runtime "synthetic-scene" mode - where we want a real-looking demo without shipping any HD-map data: - + crosswalk, poles, signs, and lights) is fixed and deterministic. Args: path: Destination USDZ file. initial_rgb: ``(H, W, 3)`` ``uint8`` RGB image to embed as @@ -746,11 +577,10 @@ def build_synthetic_scene_usdz( prompt: Default text prompt embedded as ``prompt.txt``. Defaults to a generic forward-driving description. length_frames: How many trajectory frames the synthetic road carries. - Lane lines, road boundaries, and obstacle tracks are all spec'd - along this trajectory, so larger values produce more drivable - road. Default 180 (~6 s, 60 m at the default 10 m/s) keeps the - test fixture small; runtime callers typically pass 18 000 - (~10 minutes, ~6 km) so a demo never runs out of road. The + Lane lines and road boundaries are spec'd along this trajectory, + so larger values produce more drivable road. Default 180 + (~6 s, 60 m at the default 10 m/s) keeps the + test fixture small. The single intersection / crosswalk / road-island stay anchored at their original near-start coordinates regardless of length. """ @@ -803,10 +633,6 @@ def build_synthetic_scene_usdz( # past the road boundary. The world model needs *some* visible # structure to condition on; black HDMap frames produce drift. pole_polylines.extend(_off_road_poles(x_m, y_m, yaw_rad)) - # Periodic parked cars on alternating shoulders so the sides of the - # road don't read as empty over multi-km drives. - extra_obstacles = tuple(_periodic_parked_cars(x_m, y_m, yaw_rad)) - base_rgb = _initial_rgb() if initial_rgb is None else _normalise_rgb(initial_rgb) variant1_rgb = _first_image_variant(base_rgb, shift_px=16) variant2_rgb = _first_image_variant(base_rgb, shift_px=48) @@ -937,10 +763,4 @@ def build_synthetic_scene_usdz( ], ), ) - _write_parquet_entry( - zf, - "clipgt/obstacle.parquet", - _obstacle_rows(timestamps_us, extra_tracks=extra_obstacles), - ) - return path diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/scene_loader.py b/apps/omnidreams_game_engine/omnidreams_game_engine/scene_loader.py index af9c525bd..fc2866285 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/scene_loader.py +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/scene_loader.py @@ -25,11 +25,11 @@ from PIL import Image from omnidreams_game_engine.colors import ( - BBOX_V3_COLORS, HDMAP_V3_COLORS, LANE_LINE_STYLE_CONFIG, ) from omnidreams_game_engine.config import RasterConfig +from omnidreams_game_engine.game_map.types import game_map_from_dict from omnidreams_game_engine.math3d import ( euler_xyz_degrees_to_matrix, extract_yaw_from_transform, @@ -53,7 +53,6 @@ WorldLineSegments, WorldPolygonList, WorldTriangleList, - WorldVehicleBBoxTrack, ) _GROUND_MESH_NAME = "mesh_ground.ply" @@ -649,95 +648,6 @@ def _build_polygon_loop_layer( ) -def _map_obstacle_category_to_bbox_type(category: str) -> str: - normalized = category.replace("_", " ").replace("-", " ").title().replace(" ", "_") - if normalized in { - "Bus", - "Heavy_Truck", - "Train_Or_Tram_Car", - "Trolley_Bus", - "Trailer", - "Truck", - }: - return "Truck" - if normalized in {"Vehicle", "Automobile", "Other_Vehicle", "Car"}: - return "Car" - if normalized in {"Person", "Pedestrian"}: - return "Pedestrian" - if normalized in {"Rider", "Cyclist", "Motorcycle", "Bicycle"}: - return "Cyclist" - return "Others" - - -def _load_vehicle_bbox_tracks(zf: zipfile.ZipFile) -> tuple[WorldVehicleBBoxTrack, ...]: - if "clipgt/obstacle.parquet" not in zf.namelist(): - return tuple() - - obstacle_rows = _read_parquet_records(zf, "clipgt/obstacle.parquet") - grouped_rows: dict[str, list[dict[str, Any]]] = defaultdict(list) - for row in obstacle_rows: - obstacle_payload = row["obstacle"] - track_id = str(obstacle_payload.get("trackline_id", "")) - if track_id == "": - continue - grouped_rows[track_id].append(row) - - tracks: list[WorldVehicleBBoxTrack] = [] - for track_id in sorted(grouped_rows.keys()): - observations = sorted( - grouped_rows[track_id], key=lambda obs: int(obs["key"]["timestamp_micros"]) - ) - timestamps_us: list[int] = [] - centers_world: list[list[float]] = [] - dimensions_lwh: list[list[float]] = [] - orientations_xyzw: list[list[float]] = [] - for observation in observations: - obstacle_payload = observation["obstacle"] - center = obstacle_payload["center"] - size = obstacle_payload["size"] - orientation = obstacle_payload["orientation"] - quaternion = np.array( - [ - float(orientation["x"]), - float(orientation["y"]), - float(orientation["z"]), - float(orientation["w"]), - ], - dtype=np.float32, - ) - if float(np.linalg.norm(quaternion)) <= 1e-8: - continue - timestamps_us.append(int(observation["key"]["timestamp_micros"])) - centers_world.append( - [float(center["x"]), float(center["y"]), float(center["z"])] - ) - dimensions_lwh.append( - [float(size["x"]), float(size["y"]), float(size["z"])] - ) - orientations_xyzw.append(quaternion.tolist()) - - if len(timestamps_us) < 2: - continue - object_type = _map_obstacle_category_to_bbox_type( - str(observations[0]["obstacle"].get("category", "Others")) - ) - if object_type not in BBOX_V3_COLORS: - object_type = "Others" - tracks.append( - WorldVehicleBBoxTrack( - track_id=track_id, - object_type=object_type, - timestamps_us=np.asarray(timestamps_us, dtype=np.int64), - centers_world=np.asarray(centers_world, dtype=np.float32), - dimensions_lwh=np.asarray(dimensions_lwh, dtype=np.float32), - orientations_xyzw=np.asarray(orientations_xyzw, dtype=np.float32), - # TODO: Excluding ego obstacle requires metadata not currently parsed here. - max_extrapolation_us=500_000.0, - ) - ) - return tuple(tracks) - - def _load_map_layers( zf: zipfile.ZipFile, raster: RasterConfig, @@ -896,8 +806,12 @@ def load_scene_bundle( initial_rgb = _load_initial_image(zf, camera_name, variant, raster) prompt = _load_prompt(zf, variant, prompt_override) line_layers, triangle_layers, polygon_layers = _load_map_layers(zf, raster) - vehicle_bbox_tracks = _load_vehicle_bbox_tracks(zf) ground_mesh_vertices, ground_mesh_faces = _load_ground_mesh(zf) + game_map = ( + game_map_from_dict(json.loads(zf.read("game_map.json"))) + if "game_map.json" in zf.namelist() + else None + ) return SceneBundle( scene_path=scene_path, @@ -913,9 +827,9 @@ def load_scene_bundle( line_layers=line_layers, triangle_layers=triangle_layers, polygon_layers=polygon_layers, - vehicle_bbox_tracks=vehicle_bbox_tracks, ground_mesh_vertices=ground_mesh_vertices, ground_mesh_faces=ground_mesh_faces, + game_map=game_map, ) diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/screenshot.jpg b/apps/omnidreams_game_engine/omnidreams_game_engine/screenshot.jpg index 25e08f86f..0cda39e86 100644 Binary files a/apps/omnidreams_game_engine/omnidreams_game_engine/screenshot.jpg and b/apps/omnidreams_game_engine/omnidreams_game_engine/screenshot.jpg differ diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/simulation/backend.py b/apps/omnidreams_game_engine/omnidreams_game_engine/simulation/backend.py index 290ebd0d0..a0233f31e 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/simulation/backend.py +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/simulation/backend.py @@ -1,6 +1,7 @@ # SPDX-License-Identifier: Apache-2.0 # SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +from collections.abc import Sequence from typing import Protocol from omnidreams_game_engine.types import ( @@ -20,12 +21,12 @@ def set_physx_debug_enabled(self, enabled: bool) -> None: def pose_chunk( self, - command: DriverCommand, + commands: Sequence[DriverCommand], chunk_size: int, frame_interval_s: float, extrapolation_offset_s: float, ) -> TrajectoryChunk: - """Advance authoritative state by ``chunk_size`` frames and return the trajectory. + """Advance with one command per frame and return the trajectory. Mutates state to ``trajectory.boundary_state_after_chunk``. Sim wall-clock time advances by ``chunk_size * frame_interval_s`` per call, regardless of diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/simulation/ego_vehicle_kinematics.py b/apps/omnidreams_game_engine/omnidreams_game_engine/simulation/ego_vehicle_kinematics.py index 29c794a9b..348bc0c9b 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/simulation/ego_vehicle_kinematics.py +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/simulation/ego_vehicle_kinematics.py @@ -3,7 +3,7 @@ import math import time -from collections.abc import Callable +from collections.abc import Callable, Sequence import numpy as np from loguru import logger @@ -17,7 +17,6 @@ ) from omnidreams_game_engine.simulation.game_physics import GamePhysicsWorld from omnidreams_game_engine.simulation.ground_snap import GroundSnapper -from omnidreams_game_engine.simulation.map_bounds import MapBounds from omnidreams_game_engine.types import ( DriverCommand, PhysXChunkTimings, @@ -307,7 +306,7 @@ def integrate_vehicle( def sample_chunk_trajectory( start_state: VehicleState, start_timestamp_us: int, - command: DriverCommand, + commands: Sequence[DriverCommand], chunk_size: int, chunk_config: ChunkConfig, vehicle_config: VehicleConfig, @@ -320,6 +319,10 @@ def sample_chunk_trajectory( physics_step_fn: PhysicsStepFn = step_physics_world, include_start_state: bool = False, ) -> TrajectoryChunk: + if len(commands) != chunk_size: + raise ValueError( + f"commands must match chunk_size; got {len(commands)} for {chunk_size}" + ) timestamps = np.array( [ start_timestamp_us + frame_idx * chunk_config.frame_interval_us @@ -343,6 +346,8 @@ def sample_chunk_trajectory( max_detached_actors = 0 actor_collision_detected = False actor_collision_frame_index: int | None = None + static_collision_detected = False + static_collision_frame_index: int | None = None if physics_world is not None: physx_started_at = time.perf_counter() physics_world.synchronize_window( @@ -353,6 +358,7 @@ def sample_chunk_trajectory( physx_sync_s += sync_elapsed_s physx_elapsed_s += sync_elapsed_s for frame_idx in range(chunk_size): + command = commands[frame_idx] use_start_state = include_start_state and frame_idx == 0 if not use_start_state: state = integrate_fn( @@ -386,6 +392,12 @@ def sample_chunk_trajectory( actor_collision_detected |= actor_collision_this_frame if actor_collision_this_frame and actor_collision_frame_index is None: actor_collision_frame_index = frame_idx + static_collision_this_frame = bool( + getattr(physics_world, "last_step_static_barrier_impact", False) + ) + static_collision_detected |= static_collision_this_frame + if static_collision_this_frame and static_collision_frame_index is None: + static_collision_frame_index = frame_idx physx_elapsed_s += time.perf_counter() - physx_started_at step_timings = getattr(physics_world, "last_step_timings", None) if step_timings is not None: @@ -417,10 +429,13 @@ def sample_chunk_trajectory( rig_poses_world=poses, vehicle_states=tuple(vehicle_states), boundary_state_after_chunk=state, + applied_commands=tuple(commands), dynamic_actors=dynamic_actors, physics_debug_frames=tuple(physics_debug_frames), actor_collision_detected=actor_collision_detected, actor_collision_frame_index=actor_collision_frame_index, + static_collision_detected=static_collision_detected, + static_collision_frame_index=static_collision_frame_index, physx_elapsed_s=physx_elapsed_s if physics_world is not None else None, physx_timings=( PhysXChunkTimings( @@ -466,33 +481,6 @@ def build_ground_snapper(scene: SceneBundle) -> GroundSnapper | None: return GroundSnapper(scene.ground_mesh_vertices, scene.ground_mesh_faces) -def build_map_bounds(scene: SceneBundle) -> MapBounds | None: - """Compute OOB bounds from every spatial layer in ``scene``. - - Decoupled from :func:`build_ground_snapper` because the OOB check - cares about the union of all geometry (lane markers, vehicle - tracks, polygons, ground), not just the ground mesh -- many scenes - ship a ground mesh that's a small strip representing only the road - surface, which would respawn the user the moment they drove onto a - sidewalk. Logs the resulting AABB so it's easy to confirm the - bounds match the scene's playable area. - """ - bounds = MapBounds.from_scene(scene) - if bounds is None: - logger.info( - "[ego_vehicle_kinematics] scene has no spatial geometry; " - "OOB respawn will not fire.", - ) - return None - logger.info( - f"[ego_vehicle_kinematics] map bounds: " - f"x=[{bounds.x_min:.1f}, {bounds.x_max:.1f}] ({bounds.width_m:.1f} m), " - f"y=[{bounds.y_min:.1f}, {bounds.y_max:.1f}] ({bounds.height_m:.1f} m). " - "Adds 50 m margin + 100 m warning zone for OOB.", - ) - return bounds - - class EgoVehicleKinematics: def __init__( self, @@ -500,9 +488,6 @@ def __init__( vehicle_config: VehicleConfig, ground_snapper: GroundSnapper | None, initial_timestamp_us: int, - map_bounds: MapBounds | None = None, - oob_margin_m: float = 50.0, - oob_warning_zone_m: float = 100.0, scene: SceneBundle | None = None, integrate_fn: Callable[ [VehicleState, DriverCommand, float, VehicleConfig], VehicleState @@ -517,9 +502,6 @@ def __init__( self._vehicle_config = vehicle_config self._ground_snapper = ground_snapper self._next_timestamp_us = initial_timestamp_us - self._map_bounds = map_bounds - self._oob_margin_m = float(oob_margin_m) - self._oob_warning_zone_m = float(oob_warning_zone_m) self._integrate_fn = integrate_fn self._physics_step_fn = physics_step_fn self._include_initial_state_in_next_chunk = bool( @@ -547,24 +529,6 @@ def game_entities(self) -> tuple[GameEntity, ...]: *actors, ) - @property - def last_proximity(self) -> float: - """Out-of-bounds proximity of the latest simulated frame. - - Delegates to :meth:`MapBounds.proximity` (see it for the - 0.0 / (0,1] / 2.0 semantics) against the union AABB of every spatial - layer, not just ``mesh_ground.ply`` -- a road-only ground mesh would - respawn the ego the moment it touched a sidewalk. Returns ``0.0`` when - the scene has no geometry (the OOB respawn path no-ops). - """ - if self._map_bounds is None: - return 0.0 - return self._map_bounds.proximity( - (self._state.x_m, self._state.y_m), - margin_m=self._oob_margin_m, - warning_zone_m=self._oob_warning_zone_m, - ) - def close(self) -> None: """Release the Ludus PhysX scene owned by this rollout.""" if self._physics_world is not None: @@ -573,7 +537,7 @@ def close(self) -> None: def pose_chunk( self, - command: DriverCommand, + commands: Sequence[DriverCommand], chunk_size: int, frame_interval_s: float, extrapolation_offset_s: float, @@ -590,7 +554,7 @@ def pose_chunk( trajectory = sample_chunk_trajectory( start_state=self._state, start_timestamp_us=self._next_timestamp_us, - command=command, + commands=commands, chunk_size=chunk_size, chunk_config=chunk_config, vehicle_config=self._vehicle_config, diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/simulation/game_physics.py b/apps/omnidreams_game_engine/omnidreams_game_engine/simulation/game_physics.py index 2e339f86d..241e6c6e5 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/simulation/game_physics.py +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/simulation/game_physics.py @@ -34,6 +34,10 @@ ) from omnidreams_game_engine.config import VehicleConfig +from omnidreams_game_engine.game_map.vicinity import ( + GameMapVicinity, + GameMapVicinityResolver, +) from omnidreams_game_engine.simulation.components import ( BoxColliderComponent, GameEntity, @@ -44,7 +48,7 @@ suspension_for_object, vehicle_dynamics_for_object, ) -from omnidreams_game_engine.simulation.traffic_ai import TrafficDriverAI +from omnidreams_game_engine.simulation.map_traffic import MapTrafficController from omnidreams_game_engine.types import ( DynamicActorTrajectory, PhysicsDebugFrame, @@ -58,13 +62,15 @@ """Maximum time moving tracks can remain outside a stationary PhysX window.""" _PHYSX_BARRIER_SPACING_M = 2.0 +_BARRIER_CONTACT_SLOP_M = 0.05 +"""Extra proximity accepted when reinforcing a resolved barrier contact.""" + _PHYSX_DEBUG_FORWARD_M = 125.0 _PHYSX_DEBUG_REAR_M = 15.0 _PHYSX_DEBUG_LATERAL_M = 100.0 _VISUAL_FLARE_MIN_SPEED_DELTA_MPS = 5.0 * 0.44704 _VISUAL_FLARE_COLLISION_WINDOW_US = 500_000 _NON_EGO_MAX_DRIVE_SPEED_MPS = 15.0 * 0.44704 -_PERSISTENT_TRACK_TIMESTAMP_US = np.iinfo(np.int64).max // 4 _PHYSX_SIMULATION_RADIUS_M = 96.0 """Collision horizon around the last recenter point. @@ -150,28 +156,6 @@ def _ego_model(vehicle: VehicleConfig) -> RigidBodyModel: return rigid_body_model_from_vehicle_config(vehicle) -def _recorded_actor_trajectory(scene_object: SceneObject) -> DynamicActorTrajectory: - """Build one reusable renderer track that holds its final pose indefinitely.""" - timestamps = scene_object.timestamps_us - positions = scene_object.positions_m - orientations = scene_object.orientations_xyzw - if int(timestamps[-1]) < _PERSISTENT_TRACK_TIMESTAMP_US: - timestamps = np.concatenate( - (timestamps, np.asarray([_PERSISTENT_TRACK_TIMESTAMP_US], dtype=np.int64)) - ) - positions = np.concatenate((positions, positions[-1:]), axis=0) - orientations = np.concatenate((orientations, orientations[-1:]), axis=0) - return DynamicActorTrajectory( - entity_id=scene_object.object_id, - object_type=scene_object.object_type, - timestamps_us=timestamps, - translations_world=positions, - orientations_xyzw=orientations, - dimensions_lwh=np.asarray(scene_object.model.half_extents_m, dtype=np.float32) - * 2.0, - ) - - def _simplify_barrier_segments(segments_world: np.ndarray) -> tuple[np.ndarray, ...]: """Coalesce dense ordered map strokes into game-scale wall segments.""" segments = np.asarray(segments_world, dtype=np.float32) @@ -196,6 +180,62 @@ def _simplify_barrier_segments(segments_world: np.ndarray) -> tuple[np.ndarray, return tuple(simplified) +def _reinforce_static_barrier_rebound( + barriers: tuple[InvisibleBarrier, ...], + requested_ego: BodyState, + resolved_velocity_mps: np.ndarray, + ego_model: RigidBodyModel, + restitution: float, +) -> tuple[np.ndarray, bool]: + """Raise outward barrier velocity and report inward barrier contact.""" + position = np.asarray(requested_ego.position_m[:2], dtype=np.float32) + incoming_velocity = np.asarray(requested_ego.linear_velocity_mps, dtype=np.float32) + reinforced = np.asarray(resolved_velocity_mps, dtype=np.float32).copy() + yaw = _yaw_from_quaternion_xyzw(requested_ego.orientation_xyzw) + forward = np.asarray([math.cos(yaw), math.sin(yaw)], dtype=np.float32) + left = np.asarray([-forward[1], forward[0]], dtype=np.float32) + half_extents = ego_model.half_extents_m + contact_detected = False + + for barrier in barriers: + start = np.asarray(barrier.start_xy_m, dtype=np.float32) + end = np.asarray(barrier.end_xy_m, dtype=np.float32) + segment = end - start + length_squared = float(np.dot(segment, segment)) + if length_squared <= 1.0e-8: + continue + alpha = float( + np.clip(np.dot(position - start, segment) / length_squared, 0.0, 1.0) + ) + offset = position - (start + segment * alpha) + distance = float(np.linalg.norm(offset)) + if distance > 1.0e-6: + normal = offset / distance + else: + speed = float(np.linalg.norm(incoming_velocity[:2])) + normal = ( + -incoming_velocity[:2] / speed + if speed > 1.0e-6 + else np.asarray([1.0, 0.0], dtype=np.float32) + ) + support = ( + abs(float(np.dot(normal, forward))) * half_extents[0] + + abs(float(np.dot(normal, left))) * half_extents[1] + ) + contact_distance = support + barrier.thickness_m * 0.5 + if distance > contact_distance + _BARRIER_CONTACT_SLOP_M: + continue + incoming_normal_speed = float(np.dot(incoming_velocity[:2], normal)) + if incoming_normal_speed >= 0.0: + continue + contact_detected = True + target_outward_speed = -restitution * incoming_normal_speed + resolved_normal_speed = float(np.dot(reinforced[:2], normal)) + if resolved_normal_speed < target_outward_speed: + reinforced[:2] += normal * (target_outward_speed - resolved_normal_speed) + return reinforced, contact_detected + + class GamePhysicsWorld: """Adapt a scene bundle to Ludus and delegate all simulation to PhysX.""" @@ -205,23 +245,46 @@ def __init__( vehicle: VehicleConfig, *, model_adapter: Callable[[RigidBodyModel], RigidBodyModel] | None = None, + static_barrier_segments_world: np.ndarray | None = None, + static_barrier_restitution: float | None = None, ) -> None: started_at = time.perf_counter() self._vehicle = vehicle + if static_barrier_restitution is not None and ( + not math.isfinite(static_barrier_restitution) + or not 0.0 <= static_barrier_restitution <= 1.0 + ): + raise ValueError("static_barrier_restitution must be within [0, 1]") + self._static_barrier_restitution = static_barrier_restitution adapt_model = model_adapter or (lambda model: model) - def adapted_object(track: object) -> SceneObject: - scene_object = self._object_from_track(track, vehicle) - return replace(scene_object, model=adapt_model(scene_object.model)) - - objects = tuple(adapted_object(track) for track in scene.vehicle_bbox_tracks) - barriers = ( - self._build_barriers(scene) if vehicle.static_collision_enabled else () + game_map = getattr(scene, "game_map", None) + self._map_traffic = MapTrafficController( + () if game_map is None else game_map.traffic, + vehicle, ) - self.graph = PhysicsObjectGraph(objects=objects, barriers=barriers) - self._recorded_trajectories_by_id = { - obj.object_id: _recorded_actor_trajectory(obj) for obj in objects - } + if ( + vehicle.static_collision_enabled + and static_barrier_segments_world is not None + ): + segments = np.asarray(static_barrier_segments_world, dtype=np.float32) + if segments.ndim != 3 or segments.shape[1:] != (2, 3): + raise ValueError( + "static_barrier_segments_world must have shape (N, 2, 3)" + ) + barriers = tuple( + InvisibleBarrier( + tuple(float(value) for value in segment[0]), + tuple(float(value) for value in segment[1]), + barrier_id=f"semantic-{index}", + ) + for index, segment in enumerate(_simplify_barrier_segments(segments)) + ) + else: + barriers = ( + self._build_barriers(scene) if vehicle.static_collision_enabled else () + ) + self.graph = PhysicsObjectGraph(objects=(), barriers=barriers) initial_transform = getattr(scene, "initial_rig_to_world", None) initial_xy = ( np.asarray(initial_transform[:2, 3], dtype=np.float32) @@ -229,11 +292,23 @@ def adapted_object(track: object) -> SceneObject: else np.zeros(2, dtype=np.float32) ) initial_timestamp_us = int(getattr(scene, "initial_timestamp_us", 0)) - self._physics_graph = self.graph.copy_for_physx( + self._vicinity_resolver = ( + None if game_map is None else GameMapVicinityResolver(game_map) + ) + self._map_vicinity: GameMapVicinity | None = ( + None + if self._vicinity_resolver is None + else self._vicinity_resolver.resolve( + float(initial_xy[0]), float(initial_xy[1]) + ) + ) + self._map_traffic.set_vicinity(self._map_vicinity) + base_physics_graph = self.graph.copy_for_physx( initial_xy, _PHYSX_SIMULATION_RADIUS_M, timestamp_us=initial_timestamp_us, ) + self._physics_graph = self._with_active_map_traffic(base_physics_graph) self._physics_center_xy = initial_xy.copy() self._physics_timestamp_us = initial_timestamp_us self._entities = [ @@ -243,6 +318,9 @@ def adapted_object(track: object) -> SceneObject: self._detached_entity_ids: set[str] = set() self.last_step_timings = None self.last_step_actor_collision = False + self.last_step_static_barrier_collision = False + self.last_step_static_barrier_impact = False + self._static_barrier_contact_active = False self._visual_flare_collision_velocity_mps: np.ndarray | None = None self._visual_flare_driving_direction_xy: np.ndarray | None = None self._visual_flare_impact_normal_xy: np.ndarray | None = None @@ -250,40 +328,45 @@ def adapted_object(track: object) -> SceneObject: self._pending_struck_vehicle_ids: set[str] = set() self._ego_model = adapt_model(_ego_model(vehicle)) self._world = PhysXWorld( - self._physics_graph, + base_physics_graph, self._ego_model, actor_collision_enabled=vehicle.actor_collision_enabled, max_actor_drive_speed_mps=_NON_EGO_MAX_DRIVE_SPEED_MPS, + max_actor_drive_speeds_mps=self._map_traffic.max_drive_speeds_mps, + ) + self._world.synchronize( + self._physics_graph, + timestamp_us=initial_timestamp_us, + initial_object_timestamps_us=self._map_traffic.active_timestamps_us, ) - self._traffic_ai = TrafficDriverAI() - self._traffic_ai.synchronize(self._physics_graph.objects) self._refresh_debug_barriers() logger.info( "[physics] PhysX graph ready in {:.1f} ms; objects={}/{} barriers={}/{} simulation_radius_m={:.0f}", (time.perf_counter() - started_at) * 1000.0, len(self._physics_graph.objects), - len(self.graph.objects), + len(self.graph.objects) + len(self._map_traffic.objects), len(self._physics_graph.barriers), len(self.graph.barriers), _PHYSX_SIMULATION_RADIUS_M, ) - @staticmethod - def _object_from_track(track: object, vehicle: VehicleConfig) -> SceneObject: - dimensions = np.asarray(track.dimensions_lwh[0], dtype=np.float32) - return SceneObject( - object_id=track.track_id, - object_type=track.object_type, - model=rigid_body_model_for_object( - track.object_type, - dimensions, - restitution=vehicle.collision_restitution, - friction=vehicle.collision_friction, - ), - timestamps_us=np.asarray(track.timestamps_us, dtype=np.int64), - positions_m=np.asarray(track.centers_world, dtype=np.float32), - orientations_xyzw=np.asarray(track.orientations_xyzw, dtype=np.float32), - max_extrapolation_us=track.max_extrapolation_us, + def _with_active_map_traffic( + self, physics_graph: PhysicsObjectGraph + ) -> PhysicsObjectGraph: + """Add only graph-nearby procedural traffic to a PhysX window.""" + incoming_ids = { + scene_object.object_id for scene_object in physics_graph.objects + } + additions = tuple( + scene_object + for scene_object in self._map_traffic.active_objects + if scene_object.object_id not in incoming_ids + ) + if not additions: + return physics_graph + return PhysicsObjectGraph( + objects=physics_graph.objects + additions, + barriers=physics_graph.barriers, ) @staticmethod @@ -358,12 +441,24 @@ def _active_collider_ids(self) -> set[str]: return set(self._world.active_collider_ids) def synchronize_window( - self, center_xy_m: np.ndarray, timestamp_us: int | None = None + self, + center_xy_m: np.ndarray, + timestamp_us: int | None = None, ) -> bool: """Incrementally recenter active PhysX topology when the ego moves.""" center = np.asarray(center_xy_m, dtype=np.float32) if center.shape != (2,): raise ValueError("center_xy_m must have shape (2,)") + traffic_topology_changed = False + if self._vicinity_resolver is not None: + self._map_vicinity = self._vicinity_resolver.resolve( + float(center[0]), + float(center[1]), + previous=self._map_vicinity, + ) + traffic_topology_changed = self._map_traffic.set_vicinity( + self._map_vicinity + ) center_is_current = ( float(np.linalg.norm(center - self._physics_center_xy)) < _PHYSX_RECENTER_DISTANCE_M @@ -373,18 +468,23 @@ def synchronize_window( <= timestamp_us - self._physics_timestamp_us < _PHYSX_TOPOLOGY_REFRESH_INTERVAL_US ) - if center_is_current and timestamp_is_current: + if center_is_current and timestamp_is_current and not traffic_topology_changed: return False physics_graph = self.graph.copy_for_physx( center, _PHYSX_SIMULATION_RADIUS_M, timestamp_us=timestamp_us, ) + physics_graph = self._with_active_map_traffic(physics_graph) incoming_ids = {obj.object_id for obj in physics_graph.objects} retained_detached = tuple( obj for obj in self._physics_graph.objects if obj.object_id in self._detached_entity_ids + and ( + obj.object_id not in self._map_traffic.object_ids + or obj.object_id in self._map_traffic.active_object_ids + ) and obj.object_id not in incoming_ids ) if retained_detached: @@ -392,8 +492,11 @@ def synchronize_window( objects=physics_graph.objects + retained_detached, barriers=physics_graph.barriers, ) - self._world.synchronize(physics_graph, timestamp_us=timestamp_us) - self._traffic_ai.synchronize(physics_graph.objects) + self._world.synchronize( + physics_graph, + timestamp_us=timestamp_us, + initial_object_timestamps_us=self._map_traffic.active_timestamps_us, + ) existing_entities = {entity.entity_id: entity for entity in self._entities} self._entities = [ existing_entities.get(scene_object.object_id) @@ -581,11 +684,36 @@ def step( ego_before_step = _body_state_from_vehicle( state, self._ego_model.half_extents_m[2] ) + self._map_traffic.prepare_step(self._world, ego_before_step, dt_s) physics_step = self._world.step_compact( ego_before_step, timestamp_us, dt_s, ) + self.last_step_static_barrier_collision = False + if self._static_barrier_restitution is not None: + ( + reinforced_velocity, + self.last_step_static_barrier_collision, + ) = _reinforce_static_barrier_rebound( + self._physics_graph.barriers, + ego_before_step, + physics_step.ego.linear_velocity_mps, + self._ego_model, + self._static_barrier_restitution, + ) + physics_step = replace( + physics_step, + ego=replace( + physics_step.ego, + linear_velocity_mps=reinforced_velocity, + ), + ) + self.last_step_static_barrier_impact = ( + self.last_step_static_barrier_collision + and not self._static_barrier_contact_active + ) + self._static_barrier_contact_active = self.last_step_static_barrier_collision active_objects = { scene_object.object_id: scene_object for scene_object in self._physics_graph.objects @@ -613,7 +741,12 @@ def step( if separation_m <= 1e-6: continue impact_normal_xy = separation_xy / separation_m - _, _, track_velocity_mps = scene_object.sample(timestamp_us) + traffic_state = self._map_traffic.state(object_id) + if traffic_state is None: + continue + _, _, track_velocity_mps = scene_object.sample( + int(traffic_state.timestamp_us) + ) relative_velocity_xy = ( track_velocity_mps[:2] - ego_before_step.linear_velocity_mps[:2] ) @@ -645,11 +778,6 @@ def step( flare_driving_direction, self._visual_flare_impact_normal_xy, ) - significant_struck_vehicle_ids = ( - self._pending_struck_vehicle_ids.copy() - if self.last_step_actor_collision - else set() - ) collision_window_expired = ( self._visual_flare_collision_deadline_us is not None and timestamp_us > self._visual_flare_collision_deadline_us @@ -662,46 +790,17 @@ def step( self._pending_struck_vehicle_ids.clear() actor_samples = [] pending_controls = [] - native_track_samples = getattr(physics_step, "track_samples", ()) - for actor_index, (object_id, body, native_detached) in enumerate( - physics_step.actor_samples - ): - scene_object = active_objects[object_id] - if actor_index < len(native_track_samples): - ( - track_object_id, - track_position, - track_orientation, - track_velocity, - ) = native_track_samples[actor_index] - if track_object_id != object_id: - raise RuntimeError("native actor and track samples are misaligned") - else: - # Compatibility path for light-weight test doubles and older - # native modules while their timestamp sampling remains Python-side. - track_position, track_orientation, track_velocity = scene_object.sample( - timestamp_us - ) - decision = self._traffic_ai.update( + for object_id, body, _native_detached in physics_step.actor_samples: + decision = self._map_traffic.observe_physics( object_id, - struck=object_id in significant_struck_vehicle_ids, + struck=object_id in physics_step.struck_object_ids, body=body, - track_position=track_position, - track_orientation_xyzw=track_orientation, - track_velocity_mps=track_velocity, dt_s=dt_s, ) if decision is None: - detached = native_detached - else: - detached = decision.detached_from_track - # Do not let the track motor erase momentum while the visual - # effect's short impact-measurement window is still open. - drive_enabled = ( - decision.drive_enabled - and object_id not in self._pending_struck_vehicle_ids - ) - pending_controls.append((object_id, drive_enabled, detached)) + raise RuntimeError(f"PhysX returned unmanaged NPC {object_id!r}") + detached = decision.detached_from_track + pending_controls.append((object_id, decision.drive_enabled, detached)) actor_samples.append( (object_id, body.position_m, body.orientation_xyzw, detached) ) @@ -714,6 +813,15 @@ def step( entity = self._entities_by_id[object_id] entity.transform.position_m = position.copy() entity.transform.orientation_xyzw = orientation.copy() + traffic_state = self._map_traffic.state(object_id) + if traffic_state is None: + raise RuntimeError(f"Missing state for managed NPC {object_id!r}") + entity.rigid_body.linear_velocity_mps = ( + traffic_state.linear_velocity_mps.copy() + ) + entity.rigid_body.angular_velocity_radps = ( + traffic_state.angular_velocity_radps.copy() + ) entity.detached_from_track = detached self._detached_entity_ids = detached_ids @@ -807,7 +915,9 @@ def build_trajectories( samples_by_frame: list[tuple[tuple[str, np.ndarray, np.ndarray, bool], ...]], ) -> tuple[DynamicActorTrajectory, ...]: """Pack PhysX object samples for Ludus RGB and BEV HD-map rendering.""" - if not self.graph.objects or not samples_by_frame: + map_traffic = getattr(self, "_map_traffic", None) + active_map_objects = () if map_traffic is None else map_traffic.active_objects + if (not self.graph.objects and not active_map_objects) or not samples_by_frame: return () result: list[DynamicActorTrajectory] = [] simulated_timestamps = np.asarray(timestamps_us, dtype=np.int64) @@ -817,7 +927,8 @@ def build_trajectories( simulated_ids = { object_id for frame in samples_by_id for object_id in frame.keys() } - for scene_object in self.graph.objects: + render_objects = (*self.graph.objects, *active_map_objects) + for scene_object in render_objects: physically_simulated = scene_object.object_id in simulated_ids if physically_simulated: detached = any( @@ -843,7 +954,6 @@ def build_trajectories( ) trajectory_timestamps = simulated_timestamps else: - result.append(self._recorded_trajectories_by_id[scene_object.object_id]) continue result.append( DynamicActorTrajectory( diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/simulation/map_bounds.py b/apps/omnidreams_game_engine/omnidreams_game_engine/simulation/map_bounds.py deleted file mode 100644 index ebf0991ab..000000000 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/simulation/map_bounds.py +++ /dev/null @@ -1,149 +0,0 @@ -# SPDX-License-Identifier: Apache-2.0 -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. - -"""Out-of-bounds detection bounds for interactive_drive. - -Stands in for alpasim's GT-trajectory bounds in the interactive case -where the user drives freely instead of replaying a recorded path. The -AABB is the union of *every* spatial layer in the -:class:`SceneBundle` -- ground mesh, lane markers, drivable triangles, -vehicle bbox tracks, polygons -- so it covers the full extent of the -scene's content rather than only the road surface. - -The proximity calculation itself is a verbatim port of -:meth:`alpasim_runtime.events.state.is_ego_off_map`: distance from the -AABB+margin edge, ramped over a 100 m warning zone, with a hard ``2.0`` -sentinel when the ego has actually crossed. -""" - -from __future__ import annotations - -from dataclasses import dataclass - -import numpy as np - -from omnidreams_game_engine.types import SceneBundle - - -@dataclass(frozen=True) -class MapBounds: - """XY axis-aligned bounding box of the scene's navigable area. - - Built once at scene load time; the runtime loop reads :meth:`proximity` - once per chunk to drive the OOB warning overlay and auto-respawn. - Values are plain ``float`` so the per-chunk read path doesn't pay any - array indexing cost. - """ - - x_min: float - y_min: float - x_max: float - y_max: float - - @property - def width_m(self) -> float: - return self.x_max - self.x_min - - @property - def height_m(self) -> float: - return self.y_max - self.y_min - - @classmethod - def from_scene(cls, scene: SceneBundle) -> "MapBounds | None": - """Compute the union AABB of every spatial layer in ``scene``. - - Returns ``None`` when the scene has no usable spatial content - (empty fixtures, etc.) -- callers then default to "always - in-bounds" so the OOB respawn path is a no-op for that scene. - """ - xs: list[np.ndarray] = [] - ys: list[np.ndarray] = [] - - # Ground mesh vertices. Authoritative for the drivable surface - # but typically the smallest piece of geometry in the scene. - if scene.ground_mesh_vertices is not None: - verts = np.asarray(scene.ground_mesh_vertices, dtype=np.float32) - if verts.size: - xs.append(verts[:, 0]) - ys.append(verts[:, 1]) - - # Line segments (lane lines, polylines). ``segments_world`` is - # shaped ``(N, 2, 3)`` (pair of endpoints) per the scene loader, - # so flatten the first two axes to harvest every endpoint. - for layer in scene.line_layers: - seg = np.asarray(layer.segments_world, dtype=np.float32) - if seg.size: - flat = seg.reshape(-1, seg.shape[-1]) - xs.append(flat[:, 0]) - ys.append(flat[:, 1]) - - # Triangles (drivable surface, intersection plates). ``(N, 3, 3)``; - # flatten to vertices. - for layer in scene.triangle_layers: - tri = np.asarray(layer.triangles_world, dtype=np.float32) - if tri.size: - flat = tri.reshape(-1, tri.shape[-1]) - xs.append(flat[:, 0]) - ys.append(flat[:, 1]) - - # Polygons -- each entry is its own ``(N, 3)`` ring. - for layer in scene.polygon_layers: - for ring in layer.polygons_world: - pts = np.asarray(ring, dtype=np.float32) - if pts.size: - xs.append(pts[:, 0]) - ys.append(pts[:, 1]) - - # Vehicle track centers (other actors driving through the scene). - # The scene loader stores them as ``(N, 3)`` per track. - for track in scene.vehicle_bbox_tracks: - centers = np.asarray(track.centers_world, dtype=np.float32) - if centers.size: - xs.append(centers[:, 0]) - ys.append(centers[:, 1]) - - if not xs or not ys: - return None - - all_x = np.concatenate(xs) - all_y = np.concatenate(ys) - return cls( - x_min=float(all_x.min()), - y_min=float(all_y.min()), - x_max=float(all_x.max()), - y_max=float(all_y.max()), - ) - - def proximity( - self, - ego_xy: tuple[float, float], - *, - margin_m: float = 50.0, - warning_zone_m: float = 100.0, - ) -> float: - """Distance-from-AABB proximity, matching alpasim's ``is_ego_off_map``. - - Returns: - - ``0.0`` when the ego is more than ``warning_zone_m`` inside - the AABB expanded by ``margin_m`` -- solidly in-bounds. - - ``(0.0, 1.0]`` linearly ramping over the ``warning_zone_m`` - band as the ego approaches the edge. - - ``2.0`` (alpasim's sentinel) when the ego has actually - crossed the AABB+margin boundary. - """ - bx_min = self.x_min - margin_m - by_min = self.y_min - margin_m - bx_max = self.x_max + margin_m - by_max = self.y_max + margin_m - - dist_to_edge = min( - ego_xy[0] - bx_min, - bx_max - ego_xy[0], - ego_xy[1] - by_min, - by_max - ego_xy[1], - ) - if dist_to_edge < 0.0: - return 2.0 - if warning_zone_m > 0.0 and dist_to_edge < warning_zone_m: - return float(np.clip(1.0 - dist_to_edge / warning_zone_m, 0.0, 1.0)) - return 0.0 diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/simulation/map_traffic.py b/apps/omnidreams_game_engine/omnidreams_game_engine/simulation/map_traffic.py new file mode 100644 index 000000000..96a8c53fa --- /dev/null +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/simulation/map_traffic.py @@ -0,0 +1,611 @@ +# SPDX-License-Identifier: Apache-2.0 +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. + +"""Runtime tracks and simple car-following controls for authored map traffic.""" + +from __future__ import annotations + +import math +from dataclasses import dataclass +from enum import Enum + +import numpy as np +from ludus_renderer import BodyState, PhysXWorld, SceneObject + +from omnidreams_game_engine.config import VehicleConfig +from omnidreams_game_engine.game_map.types import GameMapTrafficVehicle +from omnidreams_game_engine.game_map.vicinity import GameMapVicinity +from omnidreams_game_engine.simulation.components import rigid_body_model_for_object + +_OBJECT_ID_PREFIX = "map-traffic:" +_MIN_CLEARANCE_M = 2.0 +_TIME_HEADWAY_S = 1.25 +_BRAKING_MARGIN_M = 8.0 +_LANE_CORRIDOR_M = 2.25 +_MAX_HEADING_DELTA_RAD = math.radians(40.0) +_HEADWAY_GRID_CELL_M = 64.0 +_RESTART_AFTER_STOPPED_S = 1.0 +_MAX_COLLISION_SETTLING_S = 3.0 +_STOPPED_LINEAR_SPEED_MPS = 0.10 +_STOPPED_ANGULAR_SPEED_RADPS = 0.10 +_RECOVERED_POSITION_ERROR_M = 0.60 +_RECOVERED_HEADING_ERROR_RAD = math.radians(8.0) +_RECOVERED_VELOCITY_ERROR_MPS = 0.75 +_TRACK_LOOKAHEAD_S = 0.35 + + +def _yaw_from_quaternion_xyzw(quaternion: np.ndarray) -> float: + x, y, z, w = (float(value) for value in quaternion) + return math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)) + + +class MapTrafficPhase(str, Enum): + """Authoritative gameplay phase for a map traffic vehicle.""" + + TRAVERSING = "traversing" + COLLISION = "collision" + RECOVERING = "recovering" + + +@dataclass(frozen=True) +class MapTrafficDecision: + """Native actuator and renderer state derived from an NPC's phase.""" + + drive_enabled: bool + detached_from_track: bool + + +@dataclass +class MapTrafficVehicleState: + """Single gameplay-owned state for one map traffic vehicle.""" + + object_id: str + scene_object: SceneObject + timestamp_us: float + duration_us: int + route_segment_index: int + max_speed_mps: float + route_element_ids: tuple[str, ...] + position_m: np.ndarray + orientation_xyzw: np.ndarray + linear_velocity_mps: np.ndarray + angular_velocity_radps: np.ndarray + velocity_scale: float = 1.0 + phase: MapTrafficPhase = MapTrafficPhase.TRAVERSING + stopped_duration_s: float = 0.0 + collision_duration_s: float = 0.0 + + @property + def decision(self) -> MapTrafficDecision: + """Return control outputs derived solely from the gameplay phase.""" + return MapTrafficDecision( + drive_enabled=self.phase is not MapTrafficPhase.COLLISION, + detached_from_track=self.phase is MapTrafficPhase.COLLISION, + ) + + @property + def element_id(self) -> str: + """Return the semantic element occupied by the logical route pose.""" + segment = int( + np.searchsorted( + self.scene_object.timestamps_us, int(self.timestamp_us), side="right" + ) + - 1 + ) + return self.route_element_ids[ + min(max(segment, 0), len(self.route_element_ids) - 1) + ] + + +@dataclass(frozen=True) +class _TrafficObservation: + position_xy: np.ndarray + velocity_xy: np.ndarray + half_length_m: float + + +@dataclass(frozen=True) +class _RouteProjection: + timestamp_us: float + segment_index: int + distance_sq: float + progress_distance: float + + +def _route_track( + traffic: GameMapTrafficVehicle, vehicle: VehicleConfig +) -> tuple[SceneObject, int, int]: + positions = np.asarray(traffic.centerline_world, dtype=np.float32).copy() + dimensions = np.asarray(traffic.dimensions_lwh_m, dtype=np.float32) + positions[:, 2] += dimensions[2] * 0.5 + segment_lengths = np.linalg.norm(np.diff(positions, axis=0), axis=1) + segment_speeds = np.maximum( + np.minimum(traffic.speed_limits_mps[:-1], traffic.speed_limits_mps[1:]), + np.float32(0.1), + ) + durations_us = np.maximum( + np.rint(segment_lengths / segment_speeds * 1_000_000.0).astype(np.int64), + np.int64(1), + ) + timestamps_us = np.concatenate( + (np.zeros(1, dtype=np.int64), np.cumsum(durations_us, dtype=np.int64)) + ) + + tangents = np.diff(positions[:, :2], axis=0) + yaw = np.arctan2(tangents[:, 1], tangents[:, 0]) + yaw = np.concatenate((yaw, yaw[:1])) + orientations = np.zeros((len(positions), 4), dtype=np.float32) + orientations[:, 2] = np.sin(yaw * 0.5) + orientations[:, 3] = np.cos(yaw * 0.5) + + cumulative_distance = np.concatenate( + (np.zeros(1, dtype=np.float64), np.cumsum(segment_lengths, dtype=np.float64)) + ) + start_timestamp_us = int( + np.interp( + traffic.start_distance_m, + cumulative_distance, + timestamps_us.astype(np.float64), + ) + ) + object_id = f"{_OBJECT_ID_PREFIX}{traffic.vehicle_id}" + scene_object = SceneObject( + object_id=object_id, + object_type=traffic.vehicle_type, + model=rigid_body_model_for_object( + traffic.vehicle_type, + dimensions, + restitution=vehicle.collision_restitution, + friction=vehicle.collision_friction, + ), + timestamps_us=timestamps_us, + positions_m=positions, + orientations_xyzw=orientations, + ) + return scene_object, start_timestamp_us, int(timestamps_us[-1]) + + +class MapTrafficController: + """Own route, collision, recovery, and physical snapshots for map traffic.""" + + def __init__( + self, + traffic: tuple[GameMapTrafficVehicle, ...], + vehicle: VehicleConfig, + ) -> None: + states: list[MapTrafficVehicleState] = [] + for definition in traffic: + scene_object, start_timestamp_us, duration_us = _route_track( + definition, vehicle + ) + position, orientation, velocity = scene_object.sample(start_timestamp_us) + states.append( + MapTrafficVehicleState( + object_id=scene_object.object_id, + scene_object=scene_object, + timestamp_us=float(start_timestamp_us), + duration_us=duration_us, + route_segment_index=self._route_segment_index( + scene_object, start_timestamp_us + ), + max_speed_mps=float(np.max(definition.speed_limits_mps)), + route_element_ids=definition.route_element_ids, + position_m=position.copy(), + orientation_xyzw=orientation.copy(), + linear_velocity_mps=velocity.copy(), + angular_velocity_radps=np.zeros(3, dtype=np.float32), + ) + ) + self._states = tuple(states) + self._states_by_id = {state.object_id: state for state in states} + self._active_ids: frozenset[str] = frozenset() + + @property + def objects(self) -> tuple[SceneObject, ...]: + """Return every procedural traffic object owned by this controller.""" + return tuple(state.scene_object for state in self._states) + + @property + def active_objects(self) -> tuple[SceneObject, ...]: + """Return only traffic objects selected for the current map vicinity.""" + return tuple( + state.scene_object + for state in self._states + if state.object_id in self._active_ids + ) + + @property + def active_object_ids(self) -> frozenset[str]: + """Return traffic IDs selected for PhysX and renderer conditioning.""" + return self._active_ids + + @property + def active_timestamps_us(self) -> dict[str, int]: + """Return logical track timestamps used to initialize newly active bodies.""" + return { + object_id: int(self._states_by_id[object_id].timestamp_us) + for object_id in self._active_ids + } + + @property + def object_ids(self) -> frozenset[str]: + """Return stable IDs used to retain procedural actors across windows.""" + return frozenset(self._states_by_id) + + @property + def max_drive_speeds_mps(self) -> dict[str, float]: + """Return per-object actuator caps derived from compiled route speeds.""" + return {state.object_id: state.max_speed_mps for state in self._states} + + def state(self, object_id: str) -> MapTrafficVehicleState | None: + """Return the authoritative state for a map NPC, if owned here.""" + return self._states_by_id.get(object_id) + + @staticmethod + def _route_segment_index(scene_object: SceneObject, timestamp_us: float) -> int: + segment_index = int( + np.searchsorted(scene_object.timestamps_us, int(timestamp_us), side="right") + - 1 + ) + return min(max(segment_index, 0), len(scene_object.timestamps_us) - 2) + + @staticmethod + def _set_route_snapshot(state: MapTrafficVehicleState) -> None: + state.route_segment_index = MapTrafficController._route_segment_index( + state.scene_object, state.timestamp_us + ) + position, orientation, velocity = state.scene_object.sample( + int(state.timestamp_us) + ) + state.position_m = position.copy() + state.orientation_xyzw = orientation.copy() + state.linear_velocity_mps = velocity.copy() + state.angular_velocity_radps = np.zeros(3, dtype=np.float32) + + @classmethod + def _reset_offscreen(cls, state: MapTrafficVehicleState) -> None: + state.phase = MapTrafficPhase.TRAVERSING + state.stopped_duration_s = 0.0 + state.collision_duration_s = 0.0 + state.velocity_scale = 1.0 + cls._set_route_snapshot(state) + + def set_vicinity(self, vicinity: GameMapVicinity | None) -> bool: + """Select nearby cars and reset displaced cars once the player leaves.""" + visible_elements = ( + frozenset() if vicinity is None else vicinity.traffic_element_ids + ) + for state in self._states: + if ( + state.phase is not MapTrafficPhase.TRAVERSING + and state.element_id not in visible_elements + ): + self._reset_offscreen(state) + active_ids = frozenset( + state.object_id + for state in self._states + if state.element_id in visible_elements + ) + for object_id in active_ids - self._active_ids: + self._set_route_snapshot(self._states_by_id[object_id]) + changed = active_ids != self._active_ids + self._active_ids = active_ids + return changed + + @staticmethod + def _drive_target_timestamp_us(state: MapTrafficVehicleState) -> int: + """Return a bounded actuator target derived from physical route progress.""" + if state.phase is not MapTrafficPhase.TRAVERSING: + return int(state.timestamp_us) + lookahead_us = _TRACK_LOOKAHEAD_S * 1_000_000.0 * state.velocity_scale + return int((state.timestamp_us + lookahead_us) % state.duration_us) + + def _observation(self, state: MapTrafficVehicleState) -> _TrafficObservation: + if state.object_id in self._active_ids: + position = state.position_m[:2] + _, _, velocity = state.scene_object.sample(int(state.timestamp_us)) + else: + position, _, velocity = state.scene_object.sample(int(state.timestamp_us)) + position = position[:2] + return _TrafficObservation( + position_xy=np.asarray(position, dtype=np.float32), + velocity_xy=np.asarray(velocity[:2], dtype=np.float32), + half_length_m=float(state.scene_object.model.half_extents_m[0]), + ) + + @staticmethod + def _cyclic_timestamp_distance( + timestamp_us: float, reference_us: float, duration_us: int + ) -> float: + delta = abs(timestamp_us - reference_us) % duration_us + return min(delta, duration_us - delta) + + @classmethod + def _route_projection( + cls, + state: MapTrafficVehicleState, + position_xy: np.ndarray, + segment_index: int, + ) -> _RouteProjection: + positions = state.scene_object.positions_m[:, :2] + timestamps = state.scene_object.timestamps_us + start = positions[segment_index] + segment = positions[segment_index + 1] - start + length_sq = float(np.dot(segment, segment)) + alpha = 0.0 + if length_sq > 1.0e-12: + alpha = float(np.dot(position_xy - start, segment) / length_sq) + alpha = min(max(alpha, 0.0), 1.0) + projection = start + alpha * segment + offset = position_xy - projection + timestamp_us = ( + float( + timestamps[segment_index] + + alpha * (timestamps[segment_index + 1] - timestamps[segment_index]) + ) + % state.duration_us + ) + return _RouteProjection( + timestamp_us=timestamp_us, + segment_index=segment_index, + distance_sq=float(np.dot(offset, offset)), + progress_distance=cls._cyclic_timestamp_distance( + timestamp_us, state.timestamp_us, state.duration_us + ), + ) + + @staticmethod + def _projection_is_better( + candidate: _RouteProjection, current: _RouteProjection + ) -> bool: + return candidate.distance_sq < current.distance_sq - 1.0e-8 or ( + abs(candidate.distance_sq - current.distance_sq) <= 1.0e-8 + and candidate.progress_distance < current.progress_distance + ) + + @classmethod + def _nearest_local_route_projection( + cls, state: MapTrafficVehicleState, position_xy: np.ndarray + ) -> _RouteProjection: + """Walk from the route cursor to the nearest adjacent segment.""" + segment_count = len(state.scene_object.positions_m) - 1 + best = cls._route_projection( + state, position_xy, state.route_segment_index % segment_count + ) + visited = {best.segment_index} + while len(visited) < segment_count: + neighbor_indices = ( + (best.segment_index - 1) % segment_count, + (best.segment_index + 1) % segment_count, + ) + neighbors = tuple( + cls._route_projection(state, position_xy, segment_index) + for segment_index in neighbor_indices + if segment_index not in visited + ) + visited.update(projection.segment_index for projection in neighbors) + better = tuple( + projection + for projection in neighbors + if cls._projection_is_better(projection, best) + ) + if not better: + break + best = min( + better, + key=lambda projection: ( + projection.distance_sq, + projection.progress_distance, + ), + ) + return best + + @classmethod + def _nearest_route_projection( + cls, state: MapTrafficVehicleState, position_xy: np.ndarray + ) -> _RouteProjection: + """Search the full route when collision recovery needs reacquisition.""" + best = cls._route_projection(state, position_xy, 0) + for segment_index in range(1, len(state.scene_object.positions_m) - 1): + candidate = cls._route_projection(state, position_xy, segment_index) + if cls._projection_is_better(candidate, best): + best = candidate + return best + + @staticmethod + def _apply_route_projection( + state: MapTrafficVehicleState, projection: _RouteProjection + ) -> None: + state.timestamp_us = projection.timestamp_us + state.route_segment_index = projection.segment_index + + @staticmethod + def _is_recovered(state: MapTrafficVehicleState, body: BodyState) -> bool: + track_position, track_orientation, track_velocity = state.scene_object.sample( + int(state.timestamp_us) + ) + track_velocity = track_velocity * state.velocity_scale + heading_error = math.atan2( + math.sin( + _yaw_from_quaternion_xyzw(track_orientation) + - _yaw_from_quaternion_xyzw(body.orientation_xyzw) + ), + math.cos( + _yaw_from_quaternion_xyzw(track_orientation) + - _yaw_from_quaternion_xyzw(body.orientation_xyzw) + ), + ) + return ( + float(np.linalg.norm(track_position[:2] - body.position_m[:2])) + <= _RECOVERED_POSITION_ERROR_M + and abs(heading_error) <= _RECOVERED_HEADING_ERROR_RAD + and float(np.linalg.norm(track_velocity[:2] - body.linear_velocity_mps[:2])) + <= _RECOVERED_VELOCITY_ERROR_MPS + ) + + def observe_physics( + self, + object_id: str, + *, + struck: bool, + body: BodyState, + dt_s: float, + ) -> MapTrafficDecision | None: + """Update one NPC from PhysX and advance its collision state machine.""" + state = self._states_by_id.get(object_id) + if state is None: + return None + state.position_m = body.position_m.copy() + state.orientation_xyzw = body.orientation_xyzw.copy() + state.linear_velocity_mps = body.linear_velocity_mps.copy() + state.angular_velocity_radps = body.angular_velocity_radps.copy() + + if struck: + state.phase = MapTrafficPhase.COLLISION + state.stopped_duration_s = 0.0 + state.collision_duration_s = 0.0 + return state.decision + + if state.phase is MapTrafficPhase.COLLISION: + state.collision_duration_s += dt_s + linear_speed = float(np.linalg.norm(body.linear_velocity_mps[:2])) + angular_speed = float(np.linalg.norm(body.angular_velocity_radps)) + stopped = ( + linear_speed <= _STOPPED_LINEAR_SPEED_MPS + and angular_speed <= _STOPPED_ANGULAR_SPEED_RADPS + ) + state.stopped_duration_s = ( + state.stopped_duration_s + dt_s if stopped else 0.0 + ) + if ( + state.stopped_duration_s >= _RESTART_AFTER_STOPPED_S + or state.collision_duration_s >= _MAX_COLLISION_SETTLING_S + ): + self._apply_route_projection( + state, + self._nearest_route_projection(state, body.position_m[:2]), + ) + state.phase = MapTrafficPhase.RECOVERING + state.stopped_duration_s = 0.0 + state.collision_duration_s = 0.0 + state.velocity_scale = 1.0 + elif state.phase is MapTrafficPhase.RECOVERING and self._is_recovered( + state, body + ): + state.phase = MapTrafficPhase.TRAVERSING + + return state.decision + + @staticmethod + def _grid_cell(position_xy: np.ndarray) -> tuple[int, int]: + return ( + math.floor(float(position_xy[0]) / _HEADWAY_GRID_CELL_M), + math.floor(float(position_xy[1]) / _HEADWAY_GRID_CELL_M), + ) + + def _headway_scale( + self, + observation: _TrafficObservation, + candidates: tuple[_TrafficObservation, ...], + ) -> float: + velocity = observation.velocity_xy + speed_mps = float(np.linalg.norm(velocity[:2])) + if speed_mps <= 1.0e-4: + return 0.0 + forward = velocity[:2] / speed_mps + best_clearance = math.inf + for other in candidates: + if other is observation: + continue + delta = other.position_xy - observation.position_xy + longitudinal = float(np.dot(delta, forward)) + if longitudinal <= 0.0: + continue + lateral = abs(float(forward[0] * delta[1] - forward[1] * delta[0])) + if lateral > _LANE_CORRIDOR_M: + continue + other_speed = float(np.linalg.norm(other.velocity_xy)) + if other_speed > 1.0e-4: + other_heading = other.velocity_xy / other_speed + angle = math.acos( + float(np.clip(np.dot(forward, other_heading), -1.0, 1.0)) + ) + if angle > _MAX_HEADING_DELTA_RAD: + continue + clearance = longitudinal - observation.half_length_m - other.half_length_m + best_clearance = min(best_clearance, clearance) + desired_clearance = _MIN_CLEARANCE_M + _TIME_HEADWAY_S * speed_mps + if best_clearance <= desired_clearance: + return 0.0 + return float( + np.clip( + (best_clearance - desired_clearance) / _BRAKING_MARGIN_M, + 0.0, + 1.0, + ) + ) + + def prepare_step(self, world: PhysXWorld, ego: BodyState, dt_s: float) -> None: + """Advance all logical cars and publish active tracks in one native batch.""" + for state in self._states: + if state.phase is MapTrafficPhase.TRAVERSING: + if state.object_id in self._active_ids: + self._apply_route_projection( + state, + self._nearest_local_route_projection( + state, state.position_m[:2] + ), + ) + else: + state.timestamp_us = ( + state.timestamp_us + dt_s * 1_000_000.0 * state.velocity_scale + ) % state.duration_us + state.route_segment_index = self._route_segment_index( + state.scene_object, state.timestamp_us + ) + observations = { + state.object_id: self._observation(state) for state in self._states + } + ego_observation = _TrafficObservation( + position_xy=np.asarray(ego.position_m[:2], dtype=np.float32), + velocity_xy=np.asarray(ego.linear_velocity_mps[:2], dtype=np.float32), + half_length_m=float(world.ego_model.half_extents_m[0]), + ) + buckets: dict[tuple[int, int], list[_TrafficObservation]] = {} + for observation in (*observations.values(), ego_observation): + buckets.setdefault(self._grid_cell(observation.position_xy), []).append( + observation + ) + for state in self._states: + observation = observations[state.object_id] + cell_x, cell_y = self._grid_cell(observation.position_xy) + candidates = tuple( + candidate + for offset_x in (-1, 0, 1) + for offset_y in (-1, 0, 1) + for candidate in buckets.get((cell_x + offset_x, cell_y + offset_y), ()) + ) + state.velocity_scale = ( + 0.0 + if state.phase is MapTrafficPhase.COLLISION + else self._headway_scale(observation, candidates) + ) + world.apply_track_progress( + tuple( + ( + state.object_id, + self._drive_target_timestamp_us(state), + state.velocity_scale, + ) + for state in self._states + if state.object_id in self._active_ids + ) + ) + + +__all__ = [ + "MapTrafficController", + "MapTrafficDecision", + "MapTrafficPhase", + "MapTrafficVehicleState", +] diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/simulation/traffic_ai.py b/apps/omnidreams_game_engine/omnidreams_game_engine/simulation/traffic_ai.py deleted file mode 100644 index 24b50827f..000000000 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/simulation/traffic_ai.py +++ /dev/null @@ -1,157 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Traffic-agent recovery policy for physically simulated scene vehicles.""" - -from __future__ import annotations - -import math -from dataclasses import dataclass - -import numpy as np -from ludus_renderer import BodyState, SceneObject - -_RESTART_AFTER_STOPPED_S = 1.0 -"""Continuous stopped time required before a struck vehicle drives again.""" - -_STOPPED_LINEAR_SPEED_MPS = 0.10 -"""Maximum horizontal speed considered stationary by traffic AI.""" - -_STOPPED_ANGULAR_SPEED_RADPS = 0.10 -"""Maximum angular speed considered stationary by traffic AI.""" - - -def _yaw_from_quaternion_xyzw(quaternion: np.ndarray) -> float: - x, y, z, w = (float(value) for value in quaternion) - return math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)) - - -@dataclass(frozen=True) -class TrafficDriverDecision: - """Traffic-AI outputs consumed by the native vehicle actuator.""" - - drive_enabled: bool - """Whether the actor may apply its track-following driving command.""" - - detached_from_track: bool - """Whether rendering should use the simulated recovery trajectory.""" - - -@dataclass -class _TrafficDriverState: - drive_enabled: bool = True - """Whether track-following control is active.""" - - detached_from_track: bool = False - """Whether a collision has displaced the actor from its track.""" - - stopped_duration_s: float = 0.0 - """Continuous stationary time accumulated after the latest strike.""" - - -class TrafficDriverAI: - """Own collision recovery decisions for tracked road vehicles.""" - - def __init__(self) -> None: - self._states: dict[str, _TrafficDriverState] = {} - - def synchronize(self, objects: tuple[SceneObject, ...]) -> None: - """Synchronize AI state with the active vehicle object set.""" - vehicle_ids = { - scene_object.object_id - for scene_object in objects - if scene_object.model.vehicle is not None - } - self._states = { - object_id: self._states.get(object_id, _TrafficDriverState()) - for object_id in vehicle_ids - } - - def update( - self, - object_id: str, - *, - struck: bool, - body: BodyState, - track_position: np.ndarray, - track_orientation_xyzw: np.ndarray, - track_velocity_mps: np.ndarray, - dt_s: float, - ) -> TrafficDriverDecision | None: - """Advance one vehicle's recovery policy from observed physics state.""" - state = self._states.get(object_id) - if state is None: - return None - - if struck: - state.drive_enabled = False - state.detached_from_track = True - state.stopped_duration_s = 0.0 - - if not state.drive_enabled: - linear_speed = float(np.linalg.norm(body.linear_velocity_mps[:2])) - angular_speed = float(np.linalg.norm(body.angular_velocity_radps)) - stopped = ( - linear_speed <= _STOPPED_LINEAR_SPEED_MPS - and angular_speed <= _STOPPED_ANGULAR_SPEED_RADPS - ) - state.stopped_duration_s = ( - state.stopped_duration_s + dt_s if stopped else 0.0 - ) - if state.stopped_duration_s >= _RESTART_AFTER_STOPPED_S: - state.drive_enabled = True - state.stopped_duration_s = 0.0 - elif state.detached_from_track and self._is_recovered( - body, - track_position, - track_orientation_xyzw, - track_velocity_mps, - ): - state.detached_from_track = False - - return TrafficDriverDecision( - drive_enabled=state.drive_enabled, - detached_from_track=state.detached_from_track, - ) - - @staticmethod - def _is_recovered( - body: BodyState, - track_position: np.ndarray, - track_orientation_xyzw: np.ndarray, - track_velocity_mps: np.ndarray, - ) -> bool: - displacement = np.asarray(track_position[:2] - body.position_m[:2]) - heading_error = math.atan2( - math.sin( - _yaw_from_quaternion_xyzw(track_orientation_xyzw) - - _yaw_from_quaternion_xyzw(body.orientation_xyzw) - ), - math.cos( - _yaw_from_quaternion_xyzw(track_orientation_xyzw) - - _yaw_from_quaternion_xyzw(body.orientation_xyzw) - ), - ) - velocity_error = np.asarray( - track_velocity_mps[:2] - body.linear_velocity_mps[:2] - ) - return ( - float(np.linalg.norm(displacement)) <= 0.60 - and abs(heading_error) <= math.radians(8.0) - and float(np.linalg.norm(velocity_error)) <= 0.75 - ) - - -__all__ = ["TrafficDriverAI", "TrafficDriverDecision"] diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/slangpy_hud_presenter.py b/apps/omnidreams_game_engine/omnidreams_game_engine/slangpy_hud_presenter.py index b156c7137..68d2eaf04 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/slangpy_hud_presenter.py +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/slangpy_hud_presenter.py @@ -4,10 +4,10 @@ """Single-process slangpy-window HUD presenter for ``interactive-drive``. Plugs into the same engine seam as ``SlangPyPresenter`` (``--no-hud``), but -draws PIL chrome (panel, dropdowns, BEV minimap, speed/wheel/pedals) over the +draws PIL chrome (panel, controls, BEV minimap, speed/wheel/pedals) over the camera frame -- composited on CUDA when interop is available, else on the CPU. -Input goes straight to ``KeyboardState``; dropdown scene/variant changes are -handled by the demo's outer loop over this same long-lived window. +Input goes straight to ``KeyboardState``; scene and variant changes are handled +by the demo's outer loop over this same long-lived window. """ from __future__ import annotations @@ -377,15 +377,11 @@ def __init__( # Late-imports of helpers we need at runtime; ``demo`` imports # this module via the presenter factory, so direct top-level # imports would be circular. - from omnidreams_game_engine.demo import ( - KeyboardDriveState, - _scene_label, - ) + from omnidreams_game_engine.demo import KeyboardDriveState self._keyboard_drive = KeyboardDriveState( KeyboardStateDriveSink(keyboard, source="keyboard") ) - self._scene_label_fn = _scene_label # Window + device + surface setup mirrors SlangPyPresenter's # but with a resizable HUD-sized window and a display texture @@ -446,7 +442,6 @@ def __init__( self._wheel_base_size: int | None = None self._wheel_rotation_cache: _LRUCache = _LRUCache(maxsize=480) self._pedal_cache: _LRUCache = _LRUCache(maxsize=16) - self._scene_thumb_cache: dict[Any, Image.Image | None] = {} self._variant_thumb_cache: dict[tuple[Any, str], Image.Image | None] = {} self._bev_panel_cache_key: _BevPanelKey | None = None self._bev_panel_cache: Image.Image | None = None @@ -500,14 +495,10 @@ def __init__( # slangpy uploads to per frame. See :func:`_allocate_canvas`. self._canvas_buffer, self._canvas = _allocate_canvas(*self._configured_size) - self._scene_dropdown_open = False self._variant_dropdown_open = False - self._scene_header_rect: tuple[int, int, int, int] | None = None self._variant_header_rect: tuple[int, int, int, int] | None = None self._postprocess_rect: tuple[int, int, int, int] | None = None - self._scene_item_rects: list[tuple[tuple[int, int, int, int], Any]] = [] self._variant_item_rects: list[tuple[tuple[int, int, int, int], str]] = [] - self._hovered_scene_label: str | None = None self._hovered_variant: str | None = None self._mouse_pos: tuple[int, int] = (0, 0) self._speed_mph: float = 0.0 @@ -517,33 +508,26 @@ def __init__( self._current_scene = args.scene self._selected_variant = args.variant self._has_camera_frame = False - # ``_engine_active`` is False during the initial scene-selection - # wait (when the user hasn't picked a scene yet AND - # ``--auto-start`` was off) and during the brief gap between - # scene changes. Drives the camera-area placeholder text together - # with the model-warmup state below. Toggled by the demo wrapper - # via :meth:`set_engine_active` around each scene's run. + # ``_engine_active`` is False during the brief gap between scene + # changes. It drives the camera-area placeholder text together with + # the model-warmup state below. self._engine_active = False # Model-warmup status, wired by the demo via :meth:`set_model_status`. - # ``_model_can_prewarm`` is True when the model loads at startup - # (so the selection wait shows "Loading world model..." instead of - # "Load Scene"); ``_model_ready_probe`` returns True once warmup - # has finished. Defaults are inert so a presenter used without the - # wiring (or before it) behaves like the old "Load Scene" prompt. + # ``_model_can_prewarm`` is True when the model loads at startup; + # ``_model_ready_probe`` returns True once warmup has finished. self._model_can_prewarm = False self._model_ready_probe: Callable[[], bool] = lambda: True - # Scene-selection lock, wired by the demo via + # Scene-change lock, wired by the demo via # :meth:`set_scene_selection_locked` when --preload-scenes is on. - # While the probe returns True the scene/variant dropdowns ignore - # clicks and the placeholder shows a "Preloading scenes..." hint, so - # the user can't pick a scene until every scene is cached. + # While the probe returns True, changes wait until every scene is + # cached. self._scene_selection_locked_probe: Callable[[], bool] = lambda: False self._postprocess_preset = "" self._postprocess_enabled = False self._postprocess_callback: Callable[[bool], None] = lambda enabled: None - # Scene-change request set by the dropdown click handlers. The - # outer demo loop checks this after each ``app.run_scene`` returns: + # The outer demo loop checks scene-change requests after each + # ``app.run_scene`` returns: # if non-None, it calls ``app.load_scene`` for the requested scene # and re-enters the engine over the SAME presenter so the slangpy # window (and the warmed model) stay alive. @@ -551,9 +535,8 @@ def __init__( # Exit-to-selection request set by the ``x`` key or a wheel's bound # exit button. The outer demo loop checks this (ahead of # ``pending_scene_change``) after each ``app.run_scene`` returns: when - # set it tears down the rollout and re-enters the scene selector over - # the SAME presenter, so a long-running demo can stop the video model - # generating without closing the window or reloading the model. + # set it tears down the rollout. A future main menu can handle the + # request without rebuilding the presenter or reloading the model. self._pending_exit_scene = False self._key_codes = self._build_key_codes() @@ -1383,8 +1366,6 @@ def _render_canvas( if panel_w > 0: self._draw_panel(canvas, draw, panel_rect, wheel_state) - if self._scene_dropdown_open: - self._draw_scene_dropdown(canvas, draw) if self._variant_dropdown_open: self._draw_variant_dropdown(canvas, draw) @@ -1542,15 +1523,9 @@ def _draw_panel( header_x = px + margin header_w = panel_size[0] - margin * 2 header_y = py + 8 - variant_y = header_y + bar_h + 4 + variant_y = header_y postprocess_available = bool(self._postprocess_preset) postprocess_y = variant_y + bar_h + 4 - self._scene_header_rect = ( - header_x, - header_y, - header_x + header_w, - header_y + bar_h, - ) self._variant_header_rect = ( header_x, variant_y, @@ -1620,21 +1595,13 @@ def _get_panel_chrome(self, panel_size: tuple[int, int]) -> Image.Image: has_multiple_variants = ( current_scene_option is not None and len(current_scene_option.variants) > 1 ) - # ``_engine_active`` is part of the cache key because the scene - # header label changes shape ("Select Scene" when the engine - # isn't running, "Running clipgt-...\u2026" when it is). The - # demo wrapper also explicitly invalidates the cache around - # ``set_engine_active``; the key entry here is belt-and-braces. key = ( panel_size, str(self._current_scene), self._selected_variant, - self._scene_dropdown_open, self._variant_dropdown_open, has_multiple_variants, self._engine_active, - # Scene header reads "Preloading scenes..." while locked, so the - # lock state has to invalidate the cached chrome too. self._scene_selection_locked(), self._postprocess_preset, self._postprocess_enabled, @@ -1654,45 +1621,9 @@ def _get_panel_chrome(self, panel_size: tuple[int, int]) -> Image.Image: header_w = panel_w - margin * 2 header_y = 8 - # Scene header bar. Reserve room on the left for the green - # status dot and on the right for the dropdown arrow; the - # remaining width is what the scene label gets to use, and we - # truncate-with-ellipsis to fit. - scene_rect = (margin, header_y, margin + header_w, header_y + bar_h) - d.rounded_rectangle(scene_rect, radius=6, fill=HEADER_BG + (255,)) - d.ellipse( - (margin + 8, header_y + 11, margin + 18, header_y + 21), - fill=NVIDIA_GREEN + (255,), - ) - if self._engine_active: - scene_label_full = ( - f"Running {self._scene_label_fn(self._current_scene)}\u2026" - ) - elif self._scene_selection_locked(): - scene_label_full = "Preloading scenes\u2026" - else: - scene_label_full = "Select Scene" - scene_label_max_w = header_w - 26 - 30 # 26 left for dot, 30 right for arrow - scene_label = _truncate_text_to_width( - self._font_small, scene_label_full, scene_label_max_w - ) - d.text( - (margin + 26, header_y + 6), - scene_label, - fill=TEXT_COLOR, - font=self._font_small, - ) - scene_arrow = "\u25b2" if self._scene_dropdown_open else "\u25bc" - d.text( - (margin + header_w - 24, header_y + 6), - scene_arrow, - fill=LABEL_COLOR, - font=self._font_small, - ) - # Variant header bar. Same truncation pattern in case the # variant string is unusually long. - variant_y = header_y + bar_h + 4 + variant_y = header_y variant_rect = (margin, variant_y, margin + header_w, variant_y + bar_h) d.rounded_rectangle(variant_rect, radius=6, fill=HEADER_BG + (255,)) variant_full = f"Variant: {self._selected_variant}" @@ -1727,9 +1658,7 @@ def _get_panel_chrome(self, panel_size: tuple[int, int]) -> Image.Image: margin + header_w, postprocess_y + bar_h, ) - postprocess_clickable = not ( - self._scene_dropdown_open or self._variant_dropdown_open - ) + postprocess_clickable = not self._variant_dropdown_open d.rounded_rectangle(postprocess_rect, radius=6, fill=HEADER_BG + (255,)) d.text( (margin + 10, postprocess_y + 6), @@ -2149,56 +2078,7 @@ def _draw_bev_ego_footprint( # is unambiguous even when the footprint is only a few pixels wide. draw.line((footprint[0], footprint[1]), fill=(220, 255, 170, 255), width=2) - # -- Dropdowns --------------------------------------------------- - - def _draw_scene_dropdown( - self, canvas: Image.Image, draw: ImageDraw.ImageDraw - ) -> None: - if self._scene_header_rect is None: - return - sx, _sy, sr, sb = self._scene_header_rect - if not self._scene_options: - empty = (sx, sb + 2, sr, sb + 36) - draw.rounded_rectangle(empty, radius=6, fill=(70, 35, 35, 255)) - draw.text( - (sx + 12, sb + 9), - f"No scenes found in {self._args.scene_dir}", - fill=(255, 220, 220), - font=self._font_tiny, - ) - return - - item_h = 80 - items_top = sb + 2 - bg = (sx, items_top - 1, sr, items_top + len(self._scene_options) * item_h + 1) - draw.rounded_rectangle(bg, radius=6, fill=(35, 35, 50, 255)) - draw.rounded_rectangle(bg, radius=6, outline=(60, 60, 80, 255), width=1) - - self._scene_item_rects = [] - for idx, scene in enumerate(self._scene_options): - top = items_top + idx * item_h - rect = (sx, top, sr, top + item_h) - self._scene_item_rects.append((rect, scene)) - if self._scene_option_matches_current(scene): - draw.rectangle(rect, fill=ACTIVE_BG + (255,)) - elif scene.label == self._hovered_scene_label: - draw.rectangle(rect, fill=HOVER_BG + (255,)) - text_x = rect[0] + 12 - text_y = top + item_h // 2 - 8 - thumb = self._get_scene_thumbnail(scene) - if thumb is not None: - tw, th = thumb.size - tx = rect[0] + 6 - ty = top + max(0, (item_h - th) // 2) - canvas.paste(thumb, (tx, ty)) - draw.rectangle( - (tx, ty, tx + tw, ty + th), outline=(60, 60, 80, 255), width=1 - ) - text_x = tx + tw + 10 - label = _truncate_text_to_width( - self._font_tiny, scene.label, max(0, rect[2] - text_x - 8) - ) - draw.text((text_x, text_y), label, fill=TEXT_COLOR, font=self._font_tiny) + # -- Variant dropdown -------------------------------------------- def _draw_variant_dropdown( self, canvas: Image.Image, draw: ImageDraw.ImageDraw @@ -2210,7 +2090,7 @@ def _draw_variant_dropdown( return vx, vy, vr, vb = self._variant_header_rect # Taller rows when the scene ships per-variant previews, matching the - # scene dropdown; fall back to compact text-only rows otherwise. + # picker; fall back to compact text-only rows otherwise. has_thumbs = bool(scene_option.variant_thumbnails) item_h = 80 if has_thumbs else 34 items_top = vb + 2 @@ -2249,18 +2129,6 @@ def _draw_variant_dropdown( ) draw.text((text_x, text_y), label, fill=TEXT_COLOR, font=self._font_tiny) - def _get_scene_thumbnail(self, scene: Any) -> Image.Image | None: - if scene.path in self._scene_thumb_cache: - return self._scene_thumb_cache[scene.path] - if scene.thumbnail is None: - self._scene_thumb_cache[scene.path] = None - return None - thumb = scene.thumbnail - if thumb.mode != "RGBA": - thumb = thumb.convert("RGBA") - self._scene_thumb_cache[scene.path] = thumb - return thumb - def _get_variant_thumbnail(self, scene: Any, variant: str) -> Image.Image | None: key = (scene.path, variant) if key in self._variant_thumb_cache: @@ -2453,13 +2321,7 @@ def _on_mouse_event(self, event: Any) -> None: self._handle_click(self._mouse_pos) def _update_hover(self, pos: tuple[int, int]) -> None: - self._hovered_scene_label = None self._hovered_variant = None - if self._scene_dropdown_open: - for rect, scene in self._scene_item_rects: - if _rect_contains(rect, pos): - self._hovered_scene_label = scene.label - break if self._variant_dropdown_open: for rect, variant in self._variant_item_rects: if _rect_contains(rect, pos): @@ -2467,7 +2329,7 @@ def _update_hover(self, pos: tuple[int, int]) -> None: break def _handle_click(self, pos: tuple[int, int]) -> None: - dropdown_open = self._scene_dropdown_open or self._variant_dropdown_open + dropdown_open = self._variant_dropdown_open if ( not dropdown_open and self._postprocess_rect @@ -2484,13 +2346,10 @@ def _handle_click(self, pos: tuple[int, int]) -> None: self._postprocess_preset, ) return - # While scenes are still preloading, the scene/variant dropdowns are - # locked (the only mouse-clickable HUD elements), so ignore clicks - # until every scene is cached and selection is instant. + # Ignore variant changes until preloading completes. if self._scene_selection_locked(): return - # Variant dropdown sits on top of the scene dropdown items, so - # check it first. + # Handle variant rows before the header or underlying controls. if self._variant_dropdown_open: for rect, variant in self._variant_item_rects: if _rect_contains(rect, pos): @@ -2504,27 +2363,9 @@ def _handle_click(self, pos: tuple[int, int]) -> None: self._variant_dropdown_open = False return - if self._scene_dropdown_open: - for rect, scene in self._scene_item_rects: - if _rect_contains(rect, pos): - self._restart_backend(scene) - return - if self._scene_header_rect and _rect_contains(self._scene_header_rect, pos): - self._scene_dropdown_open = False - return - self._scene_dropdown_open = False - return - - if self._scene_header_rect and _rect_contains(self._scene_header_rect, pos): - self._scene_dropdown_open = True - self._variant_dropdown_open = False - self._panel_chrome_cache_key = None - return - # The variant dropdown is only meaningful once a scene is actually - # loaded/running. Before that (the initial selection wait and the gap - # between scene switches) the engine is inactive, so ignore clicks on - # the variant header. + # loaded/running. Between scene switches the engine is inactive, so + # ignore clicks on the variant header. current_scene_option = self._current_scene_option() if ( self._engine_active @@ -2534,16 +2375,10 @@ def _handle_click(self, pos: tuple[int, int]) -> None: and len(current_scene_option.variants) > 1 ): self._variant_dropdown_open = True - self._scene_dropdown_open = False self._panel_chrome_cache_key = None # -- Scene / variant restart ------------------------------------- - def _restart_backend(self, scene: Any) -> None: - logger.info(f"[demo] switching scene -> {scene.label}") - new_variant = scene.variants[0] if scene.variants else "default" - self._signal_scene_change(scene.path, new_variant) - def _restart_variant(self, variant: str) -> None: if variant == self._selected_variant: self._variant_dropdown_open = False @@ -2562,7 +2397,7 @@ def _signal_scene_change(self, scene_path: Any, variant: str) -> None: self._args.scene = scene_path self._args.variant = variant self._pending_scene_change = (scene_path, variant) - # An explicit scene pick supersedes any pending exit-to-selection. + # An explicit scene change supersedes any pending exit request. self._pending_exit_scene = False self._should_close_flag = True # Drop the wheel-set DriverCommand so a stale steer/throttle doesn't @@ -2571,15 +2406,14 @@ def _signal_scene_change(self, scene_path: Any, variant: str) -> None: @property def pending_scene_change(self) -> tuple[Any, str] | None: - """``(scene_path, variant)`` if a dropdown click is pending, else None.""" + """Return the requested ``(scene_path, variant)``, if any.""" return self._pending_scene_change def exit_scene(self) -> None: - """Request a return to the scene selector, keeping the window alive. + """Request that the current scene end while keeping the window alive. - Like :meth:`_signal_scene_change` but sets ``_pending_exit_scene`` so - the outer loop re-enters :meth:`wait_for_scene_selection`. No-op unless - a scene is running. + A future main menu can consume this separately from a direct scene + change. No-op unless a scene is running. """ if not self._engine_active: return @@ -2593,16 +2427,11 @@ def exit_scene(self) -> None: @property def pending_exit_scene(self) -> bool: - """True when the user asked to exit back to the scene selector.""" + """Return whether the user asked to end the current scene.""" return self._pending_exit_scene def acknowledge_exit_scene(self) -> None: - """Clear the exit request and reset per-rollout view state for the selector. - - Called before the outer loop re-enters :meth:`wait_for_scene_selection`; - resets the close flag, the selected variant, and the last rollout's - camera/BEV/speed so the selector doesn't ghost them. - """ + """Clear the exit request and reset per-rollout view state.""" self._pending_exit_scene = False self._should_close_flag = False self._reset_selected_variant_to_default() @@ -2699,9 +2528,9 @@ def wait_for_scene_selection(self) -> tuple[Any, str] | None: def wait_while_preloading(self, in_progress: Callable[[], bool]) -> None: """Pump the "Preloading scenes..." chrome until ``in_progress()`` clears. - Used by ``--auto-start`` + ``--preload-scenes`` so the auto-loaded - scene waits for the background preloader to finish (and is served from - its cache) instead of racing it with a second parse of the same USDZ. + Used by ``--preload-scenes`` so the selected map waits for the + background preloader to finish instead of racing it with a second + compile. Returns early if the window closes. Keeps the engine inactive so the camera area shows the locked "Preloading scenes..." placeholder. """ @@ -2736,7 +2565,6 @@ def _reset_scene_view_state(self) -> None: :meth:`acknowledge_exit_scene` so the next state starts clean instead of ghosting the just-ended rollout. """ - self._scene_dropdown_open = False self._variant_dropdown_open = False # The next backend renders into a fresh ``rgb_host_uint8`` buffer # so the camera resize cache (keyed on ``id(buffer)``) is now diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/streaming_presenter.py b/apps/omnidreams_game_engine/omnidreams_game_engine/streaming_presenter.py index 0303307f6..b9d3b6c81 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/streaming_presenter.py +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/streaming_presenter.py @@ -207,135 +207,11 @@ def _print_port_conflict_help(host: str, port: int, exc: OSError) -> None: border-color: white; color: #111; } - .scene-picker { - position: fixed; bottom: 16px; right: 16px; - background: rgba(0, 0, 0, 0.7); - border: 1px solid rgba(255, 255, 255, 0.18); - border-radius: 10px; - color: white; - font-size: 12px; - display: flex; flex-direction: column; - max-height: 60vh; - backdrop-filter: blur(6px); - overflow: hidden; - /* Animate the collapse so the toggle feels physical rather than a - hard show/hide. ``max-height`` is the lever rather than ``display`` - because ``display: none`` short-circuits transitions. */ - transition: max-height 0.18s ease-out; - } - .scene-picker.hidden { display: none; } - .scene-picker.collapsed { max-height: 38px; } - .scene-picker-toggle { - background: none; border: none; color: white; - padding: 9px 12px; - display: flex; align-items: center; gap: 8px; - cursor: pointer; - font-size: 11px; font-weight: 600; - text-transform: uppercase; - letter-spacing: 0.06em; - width: 100%; - text-align: left; - flex-shrink: 0; - pointer-events: auto; - user-select: none; - } - .scene-picker-toggle:hover { background: rgba(255, 255, 255, 0.06); } - .scene-picker-count { - opacity: 0.55; - font-weight: 400; - text-transform: none; - letter-spacing: 0; - font-size: 11px; - } - .scene-picker-chevron { - margin-left: auto; - font-size: 10px; - transition: transform 0.18s ease-out; - } - .scene-picker.collapsed .scene-picker-chevron { - transform: rotate(-90deg); - } - .scene-picker-list { - display: flex; flex-direction: column; gap: 6px; - padding: 0 10px 10px 10px; - overflow-y: auto; - } - /* Hide the list's scroll viewport entirely while the panel is - collapsed so no scrollbar artifacts leak through the parent's - ``overflow: hidden`` clipping. */ - .scene-picker.collapsed .scene-picker-list { overflow: hidden; } - /* Replace Chromium's default scrollbar (which carries the up/down - arrow buttons that were poking out the bottom-right of the - collapsed panel) with a slim button-less rail. Firefox's - standards-track ``scrollbar-width`` covers the same ground. */ - .scene-picker-list { scrollbar-width: thin; scrollbar-color: rgba(255,255,255,0.22) transparent; } - .scene-picker-list::-webkit-scrollbar { width: 6px; } - .scene-picker-list::-webkit-scrollbar-track { background: transparent; } - .scene-picker-list::-webkit-scrollbar-thumb { - background: rgba(255, 255, 255, 0.22); - border-radius: 3px; - } - .scene-picker-list::-webkit-scrollbar-button { display: none; } - .scene-picker-list::-webkit-scrollbar-corner { background: transparent; } - .scene-card { - width: 160px; - border-radius: 6px; - overflow: hidden; - cursor: pointer; - border: 2px solid transparent; - transition: border-color 0.1s, transform 0.05s; - background: rgba(255, 255, 255, 0.05); - pointer-events: auto; - user-select: none; - } - .scene-card:hover { border-color: rgba(120, 200, 255, 0.7); } - .scene-card.loading { - border-color: rgba(120, 200, 255, 1.0); - pointer-events: none; - opacity: 0.7; - } - .scene-card img { - width: 100%; height: 72px; - object-fit: cover; - display: block; - background: #222; - } - .scene-card .scene-label { - padding: 6px 8px; - font-size: 11px; line-height: 1.3; - } - /* Weather-variant pills, shown only for multi-variant scenes. */ - .scene-variants { - display: flex; flex-wrap: wrap; gap: 4px; - padding: 0 8px 8px 8px; - } - .variant-pill { - background: rgba(255, 255, 255, 0.1); - border: 1px solid rgba(255, 255, 255, 0.25); - border-radius: 999px; - color: white; - font-size: 10px; - padding: 2px 8px; - cursor: pointer; - pointer-events: auto; - user-select: none; - transition: background-color 0.1s, border-color 0.1s; - } - .variant-pill:hover { background: rgba(120, 200, 255, 0.3); border-color: rgba(120, 200, 255, 0.7); } - .variant-pill.loading { border-color: rgba(120, 200, 255, 1.0); opacity: 0.7; pointer-events: none; }
WASD / Arrows = Drive · 1 = World-Model RGB · 2 = HDMap · 3 = PhysX · R = Reset Rollout
-
-- @@ -381,8 +257,7 @@ def _print_port_conflict_help(host: str, port: int, exc: OSError) -> None: .catch(() => {}); // ignore network hiccups, next event will resync } // Skip key handling when focus is on a form input (e.g. a future -// settings panel). The scene picker is now click-driven so the -// keyboard never lands on a button there. +// settings panel). function shouldIgnoreKey(e) { const t = e.target; if (!t) return false; @@ -422,110 +297,6 @@ def _print_port_conflict_help(host: str, port: int, exc: OSError) -> None: setInterval(pollState, 100); pollState(); -// Scene picker. Hidden until /scenes returns at least one entry, -// then renders as a panel in the bottom-right. Auto-expanded on first -// load because nothing happens until the user picks a scene -- the -// server is blocked on ``wait_for_scene_selection`` and the MJPEG -// stream shows the "Select a scene to begin driving" overlay frame. -// After the first pick it auto-collapses (and click-outside collapses -// thereafter), so the panel stays out of the way during driving. -const scenePicker = document.getElementById('scene-picker'); -const scenePickerList = document.getElementById('scene-picker-list'); -const scenePickerToggle = document.getElementById('scene-picker-toggle'); -const scenePickerCount = document.getElementById('scene-picker-count'); -let SCENES = []; -let firstSceneLoaded = false; -function setScenePickerCollapsed(collapsed) { - scenePicker.classList.toggle('collapsed', collapsed); -} -scenePickerToggle.addEventListener('click', () => { - setScenePickerCollapsed(!scenePicker.classList.contains('collapsed')); -}); -// Click outside the panel collapses it -- but only after the user has -// actually picked their first scene. Pre-selection clicks (e.g. the -// user clicking on the camera area to dismiss something) don't tuck -// the picker away, since the panel is the only way to start driving. -document.addEventListener('mousedown', e => { - if (!firstSceneLoaded) return; - if (scenePicker.classList.contains('hidden')) return; - if (scenePicker.contains(e.target)) return; - setScenePickerCollapsed(true); -}); -async function fetchScenes() { - try { - const r = await fetch('/scenes', { cache: 'no-store' }); - if (!r.ok) return; - const data = await r.json(); - SCENES = Array.isArray(data.scenes) ? data.scenes : []; - scenePickerCount.textContent = SCENES.length ? `(${SCENES.length})` : ''; - if (!SCENES.length) { - scenePicker.classList.add('hidden'); - return; - } - scenePickerList.innerHTML = ''; - SCENES.forEach((s, i) => { - const card = document.createElement('div'); - card.className = 'scene-card'; - card.dataset.idx = String(i); - if (s.has_thumbnail) { - const img = document.createElement('img'); - img.src = '/thumbnail?scene=' + encodeURIComponent(s.path); - img.alt = ''; - img.onerror = () => { img.style.display = 'none'; }; - card.appendChild(img); - } - const label = document.createElement('div'); - label.className = 'scene-label'; - label.textContent = s.label || ('Scene ' + (i + 1)); - card.appendChild(label); - // Clicking the card (outside a pill) loads the default variant. - card.addEventListener('click', () => loadScene(i, card)); - const variants = Array.isArray(s.variants) ? s.variants : []; - if (variants.length > 1) { - const row = document.createElement('div'); - row.className = 'scene-variants'; - variants.forEach(v => { - const pill = document.createElement('button'); - pill.className = 'variant-pill'; - pill.type = 'button'; - pill.textContent = variantLabel(v); - pill.addEventListener('click', e => { - e.stopPropagation(); // don't also trigger the card's default-variant load - loadScene(i, card, v, pill); - }); - row.appendChild(pill); - }); - card.appendChild(row); - } - scenePickerList.appendChild(card); - }); - scenePicker.classList.remove('hidden'); - } catch {} -} -function variantLabel(v) { - const labels = { - default: 'Default', clear: 'Clear', snow: 'Snow', rain: 'Rain', - }; - return labels[v] || (v.charAt(0).toUpperCase() + v.slice(1)); -} -async function loadScene(idx, card, variant, pill) { - const scene = SCENES[idx]; - if (!scene) return; - (pill || card).classList.add('loading'); - try { - let url = '/scene/select?scene=' + encodeURIComponent(scene.path); - // No variant -> server uses the scene's default; a pill selects one. - if (variant) url += '&variant=' + encodeURIComponent(variant); - await fetch(url, { method: 'GET', cache: 'no-store' }); - } catch {} - // Tuck the panel away so the user gets the camera view back; the - // scene transition itself is driven by the server-side loop. From - // this point on, click-outside dismissal is enabled too. - firstSceneLoaded = true; - setScenePickerCollapsed(true); - setTimeout(() => { (pill || card).classList.remove('loading'); }, 1500); -} -fetchScenes(); @@ -752,7 +523,14 @@ def prepare_frame(self, frame: PresentedFrame, view_mode: str) -> None: ) ) elif view_mode == "model_rgb" and frame.model_rgb_host_uint8 is not None: - _prefetch_to_numpy(frame.model_rgb_host_uint8) + _prefetch_to_numpy( + select_presented_rgb( + frame, + view_mode, + width=self._raster.width, + height=self._raster.height, + ) + ) else: _prefetch_to_numpy(frame.rgb_host_uint8) if frame.bev_host_uint8 is not None: @@ -789,7 +567,13 @@ def with_flare(rgb: object) -> np.ndarray: self._publish( with_flare( _with_status_overlay( - frame.model_rgb_host_uint8, frame.status_message + select_presented_rgb( + frame, + view_mode, + width=self._raster.width, + height=self._raster.height, + ), + frame.status_message, ) ) ) @@ -986,10 +770,7 @@ def _serve_index(self) -> None: self.send_response(HTTPStatus.OK) self.send_header("Content-Type", "text/html; charset=utf-8") self.send_header("Content-Length", str(len(body))) - # Aggressive no-cache so a browser that still has a - # pre-scene-picker tab open doesn't keep rendering the old - # HTML after a server upgrade. The page is tiny (~10 KB) so - # bypassing the cache on every reload costs nothing. + # Always serve the current control page after a server restart. self.send_header( "Cache-Control", "no-store, no-cache, must-revalidate, max-age=0" ) diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/synthetic_scene.py b/apps/omnidreams_game_engine/omnidreams_game_engine/synthetic_scene.py index 00ae008e4..a7058f02a 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/synthetic_scene.py +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/synthetic_scene.py @@ -1,14 +1,13 @@ # SPDX-License-Identifier: Apache-2.0 # SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -"""Runtime helpers for booting interactive-drive without HD-map content. +"""Helpers for constructing procedural scene fixtures. Produces a fully procedural USDZ with the same on-disk layout the scene loader expects (synthetic trajectory + lane lines + intersection geometry + a caller-supplied initial RGB), so the world-model runtime runs unchanged and -unaware the scene is procedural. Thin wrapper around -``scene_fixture.build_synthetic_scene_usdz`` that adds an ``--initial-rgb`` -path so demos can seed a real driving photo instead of the test gradient. +unaware the scene is procedural. This wraps +``scene_fixture.build_synthetic_scene_usdz`` with file-based image input. """ from __future__ import annotations @@ -25,15 +24,12 @@ # Internal frame-rate of the underlying scene_fixture trajectory and the # nominal driving speed it bakes in. ``length_km`` is converted to -# ``length_frames`` via these so the runtime API stays in km (which is -# what users actually want to reason about) instead of seconds-of-clock- -# time-at-a-fixed-speed. +# ``length_frames`` using these constants. _SCENE_FIXTURE_FPS = 30 _SCENE_FIXTURE_SPEED_MPS = 10.0 # "Golden track" length handed to the scene loader: 20 km at 10 m/s = 60 000 -# frames (~12 MB temp USDZ, ~600 ms at startup), well past any demo session. -# Exposed as a kwarg only (not on the CLI) so tests can build smaller scenes. +# frames (~12 MB temp USDZ). Tests can request smaller scenes. _DEFAULT_LENGTH_KM = 20.0 diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/types.py b/apps/omnidreams_game_engine/omnidreams_game_engine/types.py index e4f189bfc..248376fd0 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/types.py +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/types.py @@ -3,7 +3,6 @@ from __future__ import annotations -import math from dataclasses import dataclass, field from pathlib import Path from typing import Any @@ -12,43 +11,13 @@ import numpy.typing as npt from flashdreams.serving.realtime.timing import VideoModelTimings +from omnidreams_game_engine.game_map.types import ResolvedGameMap FloatArray = npt.NDArray[np.float32] UInt8Array = npt.NDArray[np.uint8] Int32Array = npt.NDArray[np.int32] -def _normalized_quaternion_xyzw(quaternion_xyzw: FloatArray) -> FloatArray: - norm = float(np.linalg.norm(quaternion_xyzw)) - if norm <= 1e-8: - raise ValueError("Quaternion must have non-zero norm") - return (quaternion_xyzw / norm).astype(np.float32) - - -def _slerp_quaternion_xyzw( - q0_xyzw: FloatArray, q1_xyzw: FloatArray, alpha: float -) -> FloatArray: - q0 = _normalized_quaternion_xyzw(q0_xyzw) - q1 = _normalized_quaternion_xyzw(q1_xyzw) - dot = float(np.dot(q0, q1)) - if dot < 0.0: - q1 = -q1 - dot = -dot - dot = min(1.0, max(-1.0, dot)) - - if dot > 0.9995: - mixed = q0 + np.float32(alpha) * (q1 - q0) - return _normalized_quaternion_xyzw(mixed.astype(np.float32)) - - theta_0 = math.acos(dot) - sin_theta_0 = math.sin(theta_0) - theta = theta_0 * alpha - sin_theta = math.sin(theta) - s0 = math.cos(theta) - dot * sin_theta / max(sin_theta_0, 1e-8) - s1 = sin_theta / max(sin_theta_0, 1e-8) - return (np.float32(s0) * q0 + np.float32(s1) * q1).astype(np.float32) - - @dataclass(frozen=True) class CameraCalibration: clipgt_name: str @@ -85,61 +54,6 @@ class WorldPolygonList: layer_name: str -@dataclass(frozen=True) -class WorldVehicleBBoxTrack: - track_id: str - object_type: str - timestamps_us: npt.NDArray[np.int64] - centers_world: FloatArray - dimensions_lwh: FloatArray - orientations_xyzw: FloatArray - max_extrapolation_us: float - - def interpolate_at_timestamp( - self, timestamp_us: int - ) -> tuple[FloatArray, FloatArray, FloatArray] | None: - if len(self.timestamps_us) < 2: - return None - first_timestamp_us = int(self.timestamps_us[0]) - last_timestamp_us = int(self.timestamps_us[-1]) - if timestamp_us < first_timestamp_us: - if float(first_timestamp_us - timestamp_us) > self.max_extrapolation_us: - return None - left_index = 0 - right_index = 1 - elif timestamp_us > last_timestamp_us: - if float(timestamp_us - last_timestamp_us) > self.max_extrapolation_us: - return None - right_index = len(self.timestamps_us) - 1 - left_index = right_index - 1 - else: - right_index = int( - np.searchsorted(self.timestamps_us, np.int64(timestamp_us), side="left") - ) - if right_index == 0: - right_index = 1 - if right_index >= len(self.timestamps_us): - right_index = len(self.timestamps_us) - 1 - left_index = right_index - 1 - - t0 = int(self.timestamps_us[left_index]) - t1 = int(self.timestamps_us[right_index]) - alpha = 0.0 if t1 == t0 else float(timestamp_us - t0) / float(t1 - t0) - - center = (1.0 - alpha) * self.centers_world[ - left_index - ] + alpha * self.centers_world[right_index] - dims = (1.0 - alpha) * self.dimensions_lwh[ - left_index - ] + alpha * self.dimensions_lwh[right_index] - orientation = _slerp_quaternion_xyzw( - self.orientations_xyzw[left_index], - self.orientations_xyzw[right_index], - alpha, - ) - return center.astype(np.float32), dims.astype(np.float32), orientation - - @dataclass(frozen=True) class SceneBundle: scene_path: Path @@ -155,13 +69,14 @@ class SceneBundle: line_layers: tuple[WorldLineSegments, ...] triangle_layers: tuple[WorldTriangleList, ...] polygon_layers: tuple[WorldPolygonList, ...] = () - vehicle_bbox_tracks: tuple[WorldVehicleBBoxTrack, ...] = () # Ground-plane mesh from ``mesh_ground.ply``; used by # :class:`~omnidreams_game_engine.simulation.ground_snap.GroundSnapper` # to keep the ego on the ground. ``None`` when the USDZ ships no ground # mesh, in which case ground-snap no-ops. ground_mesh_vertices: FloatArray | None = None ground_mesh_faces: Int32Array | None = None + game_map: ResolvedGameMap | None = None + """Semantic gameplay map embedded by the game-map compiler.""" @dataclass(frozen=True) @@ -317,6 +232,9 @@ class TrajectoryChunk: """Per-frame authoritative ego states matching ``timestamps_us``.""" boundary_state_after_chunk: VehicleState + applied_commands: tuple[DriverCommand, ...] = () + """Per-frame controls used to produce ``vehicle_states``.""" + dynamic_actors: tuple[DynamicActorTrajectory, ...] = () physics_debug_frames: tuple[PhysicsDebugFrame, ...] = () """Per-frame active collider snapshots for the optional debug view.""" @@ -327,6 +245,12 @@ class TrajectoryChunk: actor_collision_frame_index: int | None = None """First frame in this chunk whose physics step reported actor contact.""" + static_collision_detected: bool = False + """Whether a new static-barrier impact began in this chunk.""" + + static_collision_frame_index: int | None = None + """First frame in this chunk whose physics step began static contact.""" + physx_elapsed_s: float | None = None """Wall time spent in PhysX synchronization and stepping for this chunk.""" @@ -336,6 +260,17 @@ class TrajectoryChunk: def __post_init__(self) -> None: """Reject trajectory fields that represent different simulation frames.""" frame_count = len(self.timestamps_us) + if not self.applied_commands: + object.__setattr__( + self, + "applied_commands", + tuple(DriverCommand() for _ in range(frame_count)), + ) + elif len(self.applied_commands) != frame_count: + raise ValueError( + "applied_commands must match timestamps_us; got " + f"{len(self.applied_commands)} commands for {frame_count} timestamps" + ) if self.rig_poses_world.shape != (frame_count, 4, 4): raise ValueError( "rig_poses_world must have shape " @@ -400,6 +335,15 @@ class PresentedFrame: application_state: object | None = None """Opaque application state synchronized to this frame.""" + driver_command: DriverCommand | None = None + """Control command used to produce this frame's authoritative pose.""" + + model_motion_metrics: dict[str, float | str | bool] | None = None + """Chunk-level generated/conditioning motion-conformance diagnostics.""" + + impact_kind: str | None = None + """Impact beginning on this frame: ``"actor"`` or ``"static"``.""" + @dataclass(frozen=True) class FrameChunk: diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/video_model/chunk_pipeline.py b/apps/omnidreams_game_engine/omnidreams_game_engine/video_model/chunk_pipeline.py index cb45e59d9..0e07b601a 100644 --- a/apps/omnidreams_game_engine/omnidreams_game_engine/video_model/chunk_pipeline.py +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/video_model/chunk_pipeline.py @@ -102,8 +102,7 @@ def __init__( self._worker_error_lock = threading.Lock() self._worker_error: BaseException | None = None # Set once ``warmup_model`` finishes on the worker thread (or fails). - # Lets callers overlap the scene-selection wait with the model load - # and show a "ready" affordance once the model is resident. + # Lets callers detect when the model is resident. self._model_ready = threading.Event() # Set once the worker queues its first generated chunk -- i.e. the # one-time first-chunk optimization is done. Never cleared; the model @@ -282,6 +281,16 @@ def render_command(backend: VideoModelBackend) -> bool: frame, rig_to_world=trajectory.rig_poses_world[frame_index].copy(), vehicle_state=replace(trajectory.vehicle_states[frame_index]), + driver_command=trajectory.applied_commands[frame_index], + impact_kind=( + "actor" + if trajectory.actor_collision_frame_index == frame_index + else ( + "static" + if trajectory.static_collision_frame_index == frame_index + else None + ) + ), application_state=application_state, ) frame_times = chunk_times.frames[frame_index] diff --git a/apps/omnidreams_game_engine/omnidreams_game_engine/yaml_config.py b/apps/omnidreams_game_engine/omnidreams_game_engine/yaml_config.py new file mode 100644 index 000000000..e6bd13481 --- /dev/null +++ b/apps/omnidreams_game_engine/omnidreams_game_engine/yaml_config.py @@ -0,0 +1,101 @@ +# SPDX-License-Identifier: Apache-2.0 +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. + +"""Strict YAML configuration validation helpers.""" + +from __future__ import annotations + +import math +from pathlib import Path +from typing import Any + +import yaml + + +class StrictConfigError(ValueError): + """Invalid strict YAML configuration.""" + + +def load_yaml_mapping(path: Path, *, suffix: str | None = None) -> dict[str, Any]: + """Load one YAML document as a mapping. + + Args: + path: YAML file to load. + suffix: Required filename suffix; ``None`` accepts any filename. + + Returns: + Parsed root mapping. + + Raises: + StrictConfigError: The path or YAML document is invalid. + """ + path = path.expanduser().resolve() + if not path.is_file(): + raise StrictConfigError(f"Configuration path does not exist: {path}") + if suffix is not None and not path.name.endswith(suffix): + raise StrictConfigError(f"Configuration must use the {suffix} suffix: {path}") + try: + value = yaml.safe_load(path.read_text(encoding="utf-8")) + except yaml.YAMLError as exc: + raise StrictConfigError(f"Could not parse {path}: {exc}") from exc + return require_mapping(value, str(path)) + + +def require_mapping(value: Any, context: str) -> dict[str, Any]: + """Return ``value`` after validating that it is a string-keyed mapping.""" + if not isinstance(value, dict) or any(not isinstance(key, str) for key in value): + raise StrictConfigError(f"{context} must be a mapping with string keys") + return value + + +def require_exact_keys(value: dict[str, Any], expected: set[str], context: str) -> None: + """Require a mapping to contain exactly ``expected`` keys.""" + missing = sorted(expected - value.keys()) + unknown = sorted(value.keys() - expected) + if missing: + raise StrictConfigError( + f"{context} is missing required keys: {', '.join(missing)}" + ) + if unknown: + raise StrictConfigError(f"{context} has unknown keys: {', '.join(unknown)}") + + +def require_version(value: dict[str, Any], context: str) -> None: + """Require schema version one.""" + version = value.get("schema_version") + if type(version) is not int or version != 1: + raise StrictConfigError(f"{context}.schema_version must be 1") + + +def require_bool(value: Any, context: str) -> bool: + """Return a strictly typed Boolean value.""" + if type(value) is not bool: + raise StrictConfigError(f"{context} must be a boolean") + return value + + +def require_int(value: Any, context: str, *, minimum: int = 1) -> int: + """Return an integer at or above ``minimum``.""" + if type(value) is not int or value < minimum: + raise StrictConfigError(f"{context} must be an integer >= {minimum}") + return value + + +def require_float( + value: Any, + context: str, + *, + minimum: float | None = None, + maximum: float | None = None, +) -> float: + """Return a finite numeric value within the requested range.""" + if type(value) not in (int, float): + raise StrictConfigError(f"{context} must be a number") + result = float(value) + if not math.isfinite(result): + raise StrictConfigError(f"{context} must be finite") + if minimum is not None and result < minimum: + raise StrictConfigError(f"{context} must be >= {minimum}") + if maximum is not None and result > maximum: + raise StrictConfigError(f"{context} must be <= {maximum}") + return result diff --git a/apps/omnidreams_game_engine/pyproject.toml b/apps/omnidreams_game_engine/pyproject.toml index 926da4965..f7b94f718 100644 --- a/apps/omnidreams_game_engine/pyproject.toml +++ b/apps/omnidreams_game_engine/pyproject.toml @@ -9,8 +9,12 @@ description = "Reusable legacy game runtime for OmniDreams applications" readme = "README.md" requires-python = ">=3.10,<3.13" dependencies = [ + "filelock>=3", "flashdreams[serving]", "flashdreams-omnidreams", + "opencv-python-headless>=4.5", + "pyyaml>=6", + "shapely>=2.0", ] [tool.uv.sources] diff --git a/integrations/omnidreams/ludus-renderer/ludus_renderer/_cpp/physx/bindings.cpp b/integrations/omnidreams/ludus-renderer/ludus_renderer/_cpp/physx/bindings.cpp index 082b63ce8..473d044cf 100644 --- a/integrations/omnidreams/ludus-renderer/ludus_renderer/_cpp/physx/bindings.cpp +++ b/integrations/omnidreams/ludus-renderer/ludus_renderer/_cpp/physx/bindings.cpp @@ -77,6 +77,9 @@ struct BodyRecord { bool detached = false; bool trackVisible = true; bool trackDriveEnabled = true; + bool externalTrackProgress = false; + std::int64_t trackTimestampUs = 0; + float trackVelocityScale = 1.0f; bool overlappingEgo = false; std::vector timestampsUs; std::vector positions; @@ -428,6 +431,31 @@ class NativeScene { } } + void setBodyTrackProgress( + const py::array_t& objectIds, + const py::array_t& timestampsUs, + const py::array_t& velocityScales) + { + if ( + objectIds.ndim() != 1 + || timestampsUs.ndim() != 1 + || velocityScales.ndim() != 1) + throw std::invalid_argument("track-progress arrays must be one-dimensional"); + const py::ssize_t count = objectIds.shape(0); + if (timestampsUs.shape(0) != count || velocityScales.shape(0) != count) + throw std::invalid_argument("track-progress arrays must have equal lengths"); + for (py::ssize_t index = 0; index < count; ++index) { + const float scale = velocityScales.data()[index]; + if (!std::isfinite(scale) || scale < 0.0f || scale > 1.0f) + throw std::invalid_argument( + "track velocity scales must be finite and within [0, 1]"); + BodyRecord& body = bodyAt(objectIds.data()[index]); + body.externalTrackProgress = true; + body.trackTimestampUs = timestampsUs.data()[index]; + body.trackVelocityScale = scale; + } + } + void setCollisionEnabled(BodyRecord& body, bool enabled) { if (body.collisionActive == enabled) @@ -571,7 +599,10 @@ class NativeScene { BodyRecord& body = entry.second; if (!body.hasTrack()) continue; - if (!isTrackVisible(body, timestampUs)) { + const std::int64_t trackTimestampUs = body.externalTrackProgress + ? body.trackTimestampUs + : timestampUs; + if (!isTrackVisible(body, trackTimestampUs)) { body.trackVisible = false; body.overlappingEgo = false; body.driveIntentActive = false; @@ -581,7 +612,9 @@ class NativeScene { } body.trackVisible = true; ++visibleCount; - const TrackSample track = sampleTrack(body, timestampUs); + TrackSample track = sampleTrack(body, trackTimestampUs); + track.velocity *= body.trackVelocityScale; + track.angularVelocity *= body.trackVelocityScale; writeTrackState(body, track); const PxTransform actorTransform = body.actor->getGlobalPose(); const PxVec3 actorVelocity = body.actor->getLinearVelocity(); @@ -1348,6 +1381,7 @@ PYBIND11_MODULE(ludus_physx_native, module) .def("set_body_track_drive_enabled", &NativeScene::setBodyTrackDriveEnabled) .def("set_body_detached", &NativeScene::setBodyDetached) .def("set_body_track_controls", &NativeScene::setBodyTrackControls) + .def("set_body_track_progress", &NativeScene::setBodyTrackProgress) .def("remove_body", &NativeScene::removeBody) .def("add_barrier", &NativeScene::addBarrier) .def("remove_barrier", &NativeScene::removeBarrier) diff --git a/integrations/omnidreams/ludus-renderer/ludus_renderer/physx.py b/integrations/omnidreams/ludus-renderer/ludus_renderer/physx.py index a046bf593..1f8cf9045 100644 --- a/integrations/omnidreams/ludus-renderer/ludus_renderer/physx.py +++ b/integrations/omnidreams/ludus-renderer/ludus_renderer/physx.py @@ -20,6 +20,7 @@ import hashlib import math import time +from collections.abc import Mapping from dataclasses import dataclass import numpy as np @@ -185,6 +186,7 @@ def __init__( *, actor_collision_enabled: bool = True, max_actor_drive_speed_mps: float | None = None, + max_actor_drive_speeds_mps: dict[str, float] | None = None, capacity: int | None = None, ) -> None: if max_actor_drive_speed_mps is not None and ( @@ -199,6 +201,12 @@ def __init__( self.ego_model = ego_model self.actor_collision_enabled = actor_collision_enabled self.max_actor_drive_speed_mps = max_actor_drive_speed_mps + self.max_actor_drive_speeds_mps = dict(max_actor_drive_speeds_mps or {}) + if any( + not math.isfinite(value) or value <= 0.0 + for value in self.max_actor_drive_speeds_mps.values() + ): + raise ValueError("per-actor drive speeds must be finite and positive") self._closed = False self._objects: dict[str, SceneObject] = {} self._object_slots: dict[str, int] = {} @@ -206,6 +214,7 @@ def __init__( self._object_native_ids: dict[str, int] = {} self._object_collision_active: dict[str, bool] = {} self._track_drive_enabled: dict[str, bool] = {} + self._track_progress_timestamp_us: dict[str, int] = {} self._detached_object_ids: set[str] = set() self._barriers: dict[str, InvisibleBarrier] = {} self._state_buffer = self._scene.state_buffer() @@ -310,6 +319,35 @@ def apply_track_controls( self._track_drive_enabled[object_id] = drive_enabled self._objects[object_id].detached = detached + def apply_track_progress( + self, progress: tuple[tuple[str, int, float], ...] + ) -> None: + """Override route time and target velocity for procedural tracks.""" + if not progress: + return + for object_id, _, velocity_scale in progress: + if object_id not in self._objects: + raise KeyError(object_id) + if not math.isfinite(velocity_scale) or not 0.0 <= velocity_scale <= 1.0: + raise ValueError("track velocity scale must be within [0, 1]") + self._scene.set_body_track_progress( + np.fromiter( + (self._object_native_ids[item[0]] for item in progress), + dtype=np.int64, + count=len(progress), + ), + np.fromiter( + (item[1] for item in progress), dtype=np.int64, count=len(progress) + ), + np.fromiter( + (item[2] for item in progress), + dtype=np.float32, + count=len(progress), + ), + ) + for object_id, timestamp_us, _ in progress: + self._track_progress_timestamp_us[object_id] = int(timestamp_us) + def _add_body( self, native_id: int, @@ -390,7 +428,9 @@ def add_object( state, False, self.actor_collision_enabled, - self.max_actor_drive_speed_mps, + self.max_actor_drive_speeds_mps.get( + scene_object.object_id, self.max_actor_drive_speed_mps + ), ) self._scene.set_body_track( native_id, @@ -427,6 +467,7 @@ def remove_object(self, object_id: str) -> None: del self._object_native_ids[object_id] del self._object_collision_active[object_id] del self._track_drive_enabled[object_id] + self._track_progress_timestamp_us.pop(object_id, None) self._detached_object_ids.discard(object_id) self._half_extents_buffer[slot] = 0.0 @@ -453,13 +494,19 @@ def remove_barrier(self, barrier_id: str) -> None: del self._barriers[barrier_id] def synchronize( - self, graph: PhysicsObjectGraph, *, timestamp_us: int | None = None + self, + graph: PhysicsObjectGraph, + *, + timestamp_us: int | None = None, + initial_object_timestamps_us: Mapping[str, int] | None = None, ) -> None: """Apply graph additions, replacements, and removals incrementally. Args: graph: Desired active topology. timestamp_us: Initial pose time for newly added objects. + initial_object_timestamps_us: Per-object initial track times that + override ``timestamp_us`` for newly added procedural actors. """ incoming_objects = {value.object_id: value for value in graph.objects} for object_id in tuple(self._objects): @@ -471,7 +518,17 @@ def synchronize( continue if current is not None: self.remove_object(object_id) - self.add_object(scene_object, timestamp_us=timestamp_us) + initial_timestamp = ( + None + if initial_object_timestamps_us is None + else initial_object_timestamps_us.get(object_id) + ) + self.add_object( + scene_object, + timestamp_us=( + timestamp_us if initial_timestamp is None else initial_timestamp + ), + ) incoming_barriers = { barrier.barrier_id or f"barrier-{index}": barrier @@ -518,7 +575,9 @@ def step_compact( visible_slots = tuple( (object_id, slot) for object_id, slot in self._object_slots.items() - if self._objects[object_id].is_visible_at(timestamp_us) + if self._objects[object_id].is_visible_at( + self._track_progress_timestamp_us.get(object_id, timestamp_us) + ) ) actor_samples = tuple( ( diff --git a/uv.lock b/uv.lock index da5a08d19..18b582274 100644 --- a/uv.lock +++ b/uv.lock @@ -3262,8 +3262,12 @@ name = "omnidreams-game-engine" version = "0.1.0" source = { editable = "apps/omnidreams_game_engine" } dependencies = [ + { name = "filelock" }, { name = "flashdreams", extra = ["serving"] }, { name = "flashdreams-omnidreams" }, + { name = "opencv-python-headless" }, + { name = "pyyaml" }, + { name = "shapely" }, ] [package.optional-dependencies] @@ -3277,10 +3281,14 @@ interactive = [ [package.metadata] requires-dist = [ + { name = "filelock", specifier = ">=3" }, { name = "flashdreams", extras = ["serving"], editable = "flashdreams" }, { name = "flashdreams-omnidreams", editable = "integrations/omnidreams" }, + { name = "opencv-python-headless", specifier = ">=4.5" }, { name = "pytest", marker = "extra == 'dev'", specifier = ">=8.0" }, { name = "pytest-manual-marker", marker = "extra == 'dev'", specifier = ">=2.0" }, + { name = "pyyaml", specifier = ">=6" }, + { name = "shapely", specifier = ">=2.0" }, { name = "slangpy", marker = "extra == 'interactive'", specifier = "==0.42.0" }, ] provides-extras = ["interactive", "dev"]