diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index f6dc97c..83e6823 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -9,28 +9,54 @@ on: jobs: test: runs-on: ubuntu-latest - strategy: - fail-fast: false - matrix: - python-version: ["3.10", "3.11", "3.12"] + # Single interpreter (python 3.11): the graphics/ML closure grl_snam_lab + # needs — pycvc-gl (the scriptable-scene bindings), numpy-cp311 and + # torch-cp311 — are cp311 columns in the cvcpkg catalog. Multi-interpreter + # coverage follows the cp312/cp313 columns of that closure. steps: - uses: actions/checkout@v5 - - name: Set up Python ${{ matrix.python-version }} - uses: actions/setup-python@v6 + + # Install grl-snam's whole dependency closure (build + runtime) from the + # cvcpkg catalog into a prefix: python311, pycvc-gl (which pulls pycvc + + # libcvc + cvcgl + vtk transitively), numpy + torch (CPU), and the + # pytest/ruff/black test/lint tools (grl-snam's depends.build). This + # replaces the old `pip install`: pip cannot install pycvc-gl (a cvcpkg + # package, not on PyPI), so `from pycvc_gl... import *` used to + # ModuleNotFoundError in CI. The recipe is the single source of truth for + # the closure (`install-deps `), so there is no requirements file. + - name: Install dependency closure via cvcpkg + id: deps + uses: transfix/libcvc-deps/.github/actions/cvcpkg-install@master with: - python-version: ${{ matrix.python-version }} - cache: pip - - name: Install package and dev tools - # Install via pip with the CPU-only PyTorch index to keep CI fast and - # avoid pulling multi-GB CUDA wheels. - run: | - python -m pip install --upgrade pip - pip install --index-url https://download.pytorch.org/whl/cpu torch - pip install -e . - pip install pytest pytest-cov ruff black - - name: Lint + recipe: cvcpkg/recipes/grl-snam + build_type: Release + + - name: Lint + test + env: + PREFIX: ${{ steps.deps.outputs.path }} run: | - black --check grl_snam tests - ruff check . - - name: Test - run: pytest -q + set -euo pipefail + if [ -z "${PREFIX}" ]; then + echo "::error::cvcpkg install-deps failed to resolve the grl-snam closure." + echo "::error::Is it published? Needs pycvc-gl +cvc.6 (package layout)," + echo "::error::torch-cp311, and pytest/ruff/black in the cvcpkg catalog." + exit 1 + fi + PY="${PREFIX}/bin/python3.11" + + # Test THIS checkout's grl_snam / grl_snam_lab (the PR's code); the + # compiled pycvc_gl + numpy + torch come from the cvcpkg prefix. + export PYTHONPATH="${PWD}:${PYTHONPATH:-}" + # The compiled bindings (_pycvc_gl / _pycvc) link libcvc.so.3, cvcGL, + # VTK etc. from the prefix's lib/, and we run the prefix python without + # sourcing its activate script, so put the prefix lib dir on the loader + # path — otherwise `import pycvc_gl` fails with + # "libcvc.so.3: cannot open shared object file". + export LD_LIBRARY_PATH="${PREFIX}/lib${LD_LIBRARY_PATH:+:${LD_LIBRARY_PATH}}" + + echo "== black --check ==" + "${PY}" -m black --check grl_snam tests + echo "== ruff check ==" + "${PREFIX}/bin/ruff" check . + echo "== pytest ==" + "${PY}" -m pytest -q diff --git a/cvcpkg/recipes/grl-snam/recipe.yaml b/cvcpkg/recipes/grl-snam/recipe.yaml index cf3cca2..f077217 100644 --- a/cvcpkg/recipes/grl-snam/recipe.yaml +++ b/cvcpkg/recipes/grl-snam/recipe.yaml @@ -45,6 +45,13 @@ patches: [] depends: build: - name: python311 + # Test/lint tools for the `test:` block below. cvcpkg has no separate + # test-deps group, so they are depends.build edges: staged into the build + # prefix and stripped from the shipped bundle (never a runtime dependency + # of grl-snam). Recipes live in libcvc-deps/recipes/{pytest,ruff,black}. + - name: pytest + - name: ruff + - name: black runtime: - name: python311 # The lab imports pycvc + pycvc_gl (the scriptable scene). Those bindings @@ -66,6 +73,15 @@ build: - platform: any script: build.sh +# Bundle self-test — the cvcpkg-native counterpart to the GRL-SNAM CI test job. +# Runs `pytest -q` under the build prefix (where depends.build staged pytest/ +# ruff/black and depends.runtime staged the pycvc-gl + numpy + torch closure) +# against the just-built package. A non-zero exit means the bundle is broken and +# must not ship. The FULL lint+test suite over the working tree runs in CI +# (.github/workflows/ci.yml), which installs this same closure via cvcpkg. +test: + script: test.sh + package: files: - lib/python3.11*/site-packages/grl_snam/ diff --git a/cvcpkg/recipes/grl-snam/test.sh b/cvcpkg/recipes/grl-snam/test.sh new file mode 100755 index 0000000..b52fa1e --- /dev/null +++ b/cvcpkg/recipes/grl-snam/test.sh @@ -0,0 +1,55 @@ +#!/usr/bin/env bash +# cvcpkg/recipes/grl-snam/test.sh — bundle self-test for the grl-snam package. +# +# Invoked by the packager after build.sh installs grl_snam / grl_snam_lab into +# $CVC_INSTALL_DIR. It runs under the build prefix, where: +# * depends.build staged the test/lint tools (pytest, ruff, black), and +# * depends.runtime staged the runtime closure (python311, pycvc-gl, numpy, +# torch) into $CVC_DEPS_PREFIX's own interpreter. +# Non-zero exit => the bundle is broken and must not ship. +# +# This is the cvcpkg-native counterpart to the GRL-SNAM CI test job. The v0.1.0 +# sdist ships no tests/ dir, so we run `pytest -q` against a bundle smoke test +# that imports the just-built package (proving the runner works and the runtime +# closure resolves); when a future release carries its suite, we run that +# instead. The FULL lint+test over the working tree runs in CI, which installs +# this exact closure via `cvcpkg install-deps`. +set -euo pipefail + +: "${CVC_INSTALL_DIR:?CVC_INSTALL_DIR must be set}" +: "${CVC_SOURCE_DIR:?CVC_SOURCE_DIR must be set}" +: "${CVC_DEPS_PREFIX:?CVC_DEPS_PREFIX must be set}" + +PY="${CVC_DEPS_PREFIX}/bin/python3.11" +[ -x "${PY}" ] || { echo "FAIL: no python3.11 in deps prefix (${CVC_DEPS_PREFIX})"; exit 1; } + +# Make the just-built grl_snam / grl_snam_lab importable alongside the deps +# already on the prefix interpreter's path (pycvc-gl, numpy, torch). +SP="$(find "${CVC_INSTALL_DIR}" -maxdepth 3 -type d -name site-packages -print -quit || true)" +export PYTHONPATH="${SP:-}${PYTHONPATH:+:${PYTHONPATH}}" + +echo "-- grl-snam bundle self-test --" +echo "-- test/lint tools staged via depends.build --" +"${PY}" -m pytest --version +"${PY}" -m black --version +# ruff is a native binary in the deps prefix bin/ (no `python -m` needed). +"${CVC_DEPS_PREFIX}/bin/ruff" --version + +echo "-- pytest -q --" +if [ -d "${CVC_SOURCE_DIR}/tests" ]; then + # A release that ships its suite: run it against the built package + deps. + "${PY}" -m pytest -q "${CVC_SOURCE_DIR}/tests" +else + # v0.1.0 sdist ships no tests/: exercise the runner against a bundle smoke + # test that imports the built package (proves closure + runner). + TMP="$(mktemp -d)" + trap 'rm -rf "${TMP}"' EXIT + cat > "${TMP}/test_bundle_smoke.py" <<'EOF' +def test_grl_snam_imports(): + import grl_snam + assert isinstance(grl_snam.__version__, str) +EOF + "${PY}" -m pytest -q "${TMP}" +fi + +echo "-- grl-snam recipe test passed --" diff --git a/grl_snam_lab/camera.py b/grl_snam_lab/camera.py index 14ecc1c..719da01 100644 --- a/grl_snam_lab/camera.py +++ b/grl_snam_lab/camera.py @@ -1,117 +1,4 @@ -"""A position-driven third-person chase camera. - -The live GRL-SNAM planner feeds a STREAM of agent positions — the camera must not -assume the path's shape (a circle, a spline, anything). ``ChaseCamera`` estimates -the travel direction from a *smoothed velocity* of the incoming positions and -critically damps the camera pose toward a trailing target, so the view eases -smoothly even when the raw direction is noisy or snaps around. - -It is pure math — no rendering, no volrover3, no assumption about the path. Feed -it ``update(pos, dt)`` and read back an ``(eye, target, up)`` to drive any camera -(in volrover3, write those into the ``volrover3.camera`` state tree; see -``examples/volrover_lab_animated.py``). - -Robustness for real input: - * two-stage smoothing — a light EMA on position (kills jitter) feeds a heavier - EMA on the velocity estimate (a stable heading); - * frame-rate independent — every smoothing factor is ``1 - exp(-dt/tau)``, so - irregular tick spacing doesn't change the feel; - * stop-safe — below ``min_speed`` the last good heading is held, so the camera - doesn't spin when the agent pauses or pivots in place. -""" - -from __future__ import annotations - -import math - - -def _ema(prev, new, dt, tau): - """Exponential moving average toward ``new`` with time-constant ``tau`` (s).""" - if prev is None: - return list(new) - a = 1.0 - math.exp(-dt / tau) if tau > 0.0 else 1.0 - return [prev[i] + (new[i] - prev[i]) * a for i in range(len(new))] - - -class ChaseCamera: - """Third-person follow camera driven by a stream of world positions. - - Parameters (world units / seconds): - back trailing distance behind the agent - height camera height above the agent - look_ahead aim this far ahead of the agent along its heading (0 = at it) - look_up aim this far above the agent's base (so it isn't at the feet) - pos_tau position-smoothing time constant (small; de-jitter) - vel_tau velocity/heading-smoothing time constant (larger; stable heading) - cam_tau camera-pose damping time constant (the "ease" of the follow) - min_speed below this horizontal speed the heading is frozen (stop-safe) - up world up axis (Z-up scenes: (0, 0, 1)) - """ - - def __init__( - self, - back: float = 55.0, - height: float = 40.0, - look_ahead: float = 0.0, - look_up: float = 3.0, - pos_tau: float = 0.15, - vel_tau: float = 0.40, - cam_tau: float = 0.55, - min_speed: float = 0.05, - up=(0.0, 0.0, 1.0), - ): - self.back = float(back) - self.height = float(height) - self.look_ahead = float(look_ahead) - self.look_up = float(look_up) - self.pos_tau = float(pos_tau) - self.vel_tau = float(vel_tau) - self.cam_tau = float(cam_tau) - self.min_speed = float(min_speed) - self.up = tuple(float(c) for c in up) - self.reset() - - def reset(self): - """Forget all history (snaps to the next update's target pose).""" - self._p = None # smoothed position - self._pprev = None - self._v = None # smoothed velocity (heading source) - self._head = None # last good horizontal unit heading (held while stopped) - self._eye = None - self._tgt = None - - def update(self, pos, dt: float): - """Feed the agent's current world position ``(x, y, z)`` and the seconds - since the last update; returns ``(eye, target, up)`` for the camera.""" - pos = [float(pos[0]), float(pos[1]), float(pos[2])] - dt = max(float(dt), 1e-4) - - # 1) light position smoothing (de-jitter without noticeable lag) - self._p = _ema(self._p, pos, dt, self.pos_tau) - - # 2) velocity from the smoothed-position delta, then smooth the velocity - if self._pprev is not None: - raw_v = [(self._p[i] - self._pprev[i]) / dt for i in range(3)] - self._v = _ema(self._v, raw_v, dt, self.vel_tau) - self._pprev = list(self._p) - - # 3) heading = horizontal unit velocity; hold the last one if too slow - if self._v is not None: - speed = math.hypot(self._v[0], self._v[1]) - if speed >= self.min_speed: - self._head = (self._v[0] / speed, self._v[1] / speed) - hx, hy = self._head if self._head is not None else (1.0, 0.0) - - # 4) target pose: trail behind + above; look at (a touch ahead of) the agent - p = self._p - target_eye = [p[0] - hx * self.back, p[1] - hy * self.back, p[2] + self.height] - target_look = [ - p[0] + hx * self.look_ahead, - p[1] + hy * self.look_ahead, - p[2] + self.look_up, - ] - - # 5) critically damp the camera toward the target pose (smooth follow) - self._eye = _ema(self._eye, target_eye, dt, self.cam_tau) - self._tgt = _ema(self._tgt, target_look, dt, self.cam_tau) - return tuple(self._eye), tuple(self._tgt), self.up +"""Chase camera — moved to :mod:`pycvc_gl.camera` (a generic scene utility that +ships with the pycvc_gl bindings). Re-exported here for backward compatibility.""" +from pycvc_gl.camera import * # noqa: F401,F403 +from pycvc_gl.camera import ChaseCamera # noqa: F401 diff --git a/grl_snam_lab/lab.py b/grl_snam_lab/lab.py index 04354f8..6ae1fc1 100644 --- a/grl_snam_lab/lab.py +++ b/grl_snam_lab/lab.py @@ -1,271 +1,8 @@ -"""The Lab class and pure-Python geometry helpers. - -The geometry math (terrain triangulation, polyline indexing) is kept pure -Python so it is testable without the compiled ``pycvc`` bindings; the ``Lab`` -methods build ``pycvc`` objects and add them to a ``pycvc_gl`` scene. -""" - -from __future__ import annotations - -from typing import Iterable, Sequence - -# A flat, row-major XYZ vertex buffer and a flat triangle-index buffer. -FlatVerts = list -FlatIndices = list - -Color = tuple # (r, g, b) in 0..1 -Bounds2D = tuple # (min_x, min_y, max_x, max_y) -Bounds3D = tuple # (min_x, min_y, min_z, max_x, max_y, max_z) - - -# ── pure-Python geometry helpers (no pycvc) ───────────────────────────── - - -def terrain_mesh( - heights: Sequence[Sequence[float]], bounds: Bounds2D -) -> tuple[FlatVerts, FlatIndices]: - """Triangulate a ``rows x cols`` heightmap over an XY box into a surface. - - Returns ``(vertices_flat, triangles_flat)``: ``vertices_flat`` is - ``[x,y,z, ...]`` row-major over the grid (z is the height), and - ``triangles_flat`` is ``[i,j,k, ...]`` (two triangles per grid cell). - """ - rows = len(heights) - cols = len(heights[0]) if rows else 0 - if rows < 2 or cols < 2: - raise ValueError("terrain_mesh needs a grid of at least 2x2") - min_x, min_y, max_x, max_y = bounds - dx = (max_x - min_x) / (cols - 1) - dy = (max_y - min_y) / (rows - 1) - - verts: FlatVerts = [] - for r in range(rows): - y = min_y + r * dy - row = heights[r] - for c in range(cols): - verts += [min_x + c * dx, y, float(row[c])] - - tris: FlatIndices = [] - for r in range(rows - 1): - for c in range(cols - 1): - v = r * cols + c - tris += [v, v + 1, v + cols] - tris += [v + 1, v + cols + 1, v + cols] - return verts, tris - - -def _flatten_points(points: Iterable[Sequence[float]]) -> tuple[FlatVerts, int]: - """Flatten an iterable of (x, y, z) into a flat buffer; return (buf, n).""" - verts: FlatVerts = [] - n = 0 - for p in points: - verts += [float(p[0]), float(p[1]), float(p[2])] - n += 1 - return verts, n - - -def polyline_indices(n: int) -> FlatIndices: - """Consecutive-segment line indices for a path of ``n`` vertices: - ``[0,1, 1,2, ..., n-2,n-1]``.""" - idx: FlatIndices = [] - for i in range(n - 1): - idx += [i, i + 1] - return idx - - -# ── the Lab ───────────────────────────────────────────────────────────── - - -def _require_pycvc(): - try: - import pycvc - import pycvc_gl - except ImportError as exc: # pragma: no cover - environment guard - raise ImportError( - "grl_snam_lab needs the pycvc / pycvc_gl bindings (from libcvc). " - "Install libcvc with its Python bindings into your prefix " - "(e.g. via cvcpkg) and put them on PYTHONPATH." - ) from exc - return pycvc, pycvc_gl - - -class Lab: - """A live 3D GRL-SNAM scene: add terrain, obstacles, agent paths, agents, - and scalar fields, then show it. Domain-general — no dataset knowledge.""" - - def __init__(self, app=None, scene=None): - """Build a Lab. - - Standalone (default): ``Lab()`` creates its own pycvc app + a fresh - ``pycvc_gl.SceneGraph`` and renders through ``show()`` / ``render_png()``. - - Embedded: pass an existing ``app`` and/or ``scene`` to drive a HOST's - live scene instead — e.g. inside volrover3:: - - import vrhost - lab = Lab(app=vrhost.app(), scene=vrhost.scene()) - lab.add_terrain(...) # appears in the running volrover3 window - - ``scene`` is the host's live ``pycvc_gl.SceneGraph`` (adopted via - ``vrhost.scene()``); every ``add_*`` mutates the running scene. Don't call - ``show()`` in that mode — the host owns the render loop. If only ``app`` is - given, a new SceneGraph is built on it; if only ``scene`` is given, its app - is reused. - """ - self._pycvc, self._gl = _require_pycvc() - # One app handle owns this Lab's whole graphics context. Every pycvc - # object built below (geometry/volume) and the scene co-own it via - # shared_ptr, so it outlives them — there is no global singleton. - self._app = app if app is not None else self._pycvc.make_app() - # The REAL cvcGL SceneGraph (directly wrapped — not a facade): add_* - # return the live node; move()/recolor() mutate it in place. - self._scene = scene if scene is not None else self._gl.SceneGraph(self._app) - - # -- meshes -------------------------------------------------------------- - - def add_mesh( - self, - name: str, - vertices: Sequence[float], - triangles: Sequence[int], - color: Color | None = None, - ): - """Add a triangle mesh (obstacle / building / surface) from flat - row-major ``vertices`` (``[x,y,z,...]``) and ``triangles`` - (``[i,j,k,...]``).""" - g = self._pycvc.geometry(self._app) - g.add_vertices(list(vertices)) - g.add_triangles(list(triangles)) - self._scene.addGraphics(name, g) - if color is not None: - self.recolor(name, color) # single-color material (faithful; see recolor) - return self - - def add_terrain( - self, - heights: Sequence[Sequence[float]], - bounds: Bounds2D, - color: Color | None = None, - ): - """Add a terrain surface from a heightmap over an XY box.""" - verts, tris = terrain_mesh(heights, bounds) - return self.add_mesh("terrain", verts, tris, color=color) - - # -- agents & paths ------------------------------------------------------ - - def add_path(self, name: str, points: Iterable[Sequence[float]], color: Color | None = None): - """Add an agent trajectory as a polyline through ``points``.""" - verts, n = _flatten_points(points) - if n < 2: - raise ValueError("add_path needs at least 2 points") - g = self._pycvc.geometry(self._app) - g.add_vertices(verts) - g.add_lines(polyline_indices(n)) - self._scene.addGraphics(name, g) - if color is not None: - self.recolor(name, color) # single-color material (faithful; see recolor) - return self - - def add_markers( - self, name: str, positions: Iterable[Sequence[float]], color: Color | None = None - ): - """Add agents as points (markers) at ``positions``.""" - verts, n = _flatten_points(positions) - if n < 1: - raise ValueError("add_markers needs at least 1 position") - g = self._pycvc.geometry(self._app) - g.add_vertices(verts) - self._scene.addGraphics(name, g) - if color is not None: - self.recolor(name, color) # single-color material (faithful; see recolor) - return self - - # -- scalar fields ------------------------------------------------------- - - def add_field( - self, - name: str, - values: Sequence[float], - dims: tuple[int, int, int], - bounds: Bounds3D, - ): - """Add a scalar field (risk / SDF / path-loss) as a volume node. - - ``values`` is a flat, row-major (x fastest) grid of ``nx*ny*nz`` - scalars; ``dims`` = (nx, ny, nz); ``bounds`` = the object-space box. - """ - nx, ny, nz = dims - v = self._pycvc.volume(self._app) - v.set_float_grid(list(values), nx, ny, nz, *bounds) - self._scene.addGraphics(name, v) - return self - - # -- external / VTK props ------------------------------------------------ - - def add_prop(self, name: str, prop, bounds: Bounds3D, parent: str = ""): - """Add a pre-built VTK prop (e.g. a ``vtkActor`` loaded from glTF/OBJ) as a - named scene node. ``bounds`` = ``(min_x, min_y, min_z, max_x, max_y, - max_z)`` (used for framing/clipping). ``parent`` (default the graphics - root) makes it a CHILD of that node so it inherits the parent's transform - (e.g. a building mesh under the terrain node). Lets a whole city mesh join - the scene without going through cvc::geometry. See ``grl_snam_lab.scenes``.""" - self._gl.add_prop(self._scene, name, prop, *[float(c) for c in bounds], parent) - return self - - # -- in-place UPDATE (the animation path) -------------------------------- - - def node(self, name: str): - """The live scene node named ``name`` (a ``pycvc_gl.GraphicsNode``), or - ``None`` if absent. Call ``.setPosition/.setScale/.setRotation/ - .setTransform/.resetTransform`` on it to move it without a rebuild.""" - return self._scene.getGraphics(name) - - def move(self, name: str, x: float, y: float, z: float): - """Move node ``name`` to ``(x, y, z)`` by mutating its transform — the - geometry + actor are reused, nothing is destroyed or recreated. This is - the per-frame animation primitive (build once, ``move`` each frame).""" - n = self._scene.getGraphics(name) - if n is None: - raise KeyError(f"grl_snam_lab.Lab.move: no node named {name!r}") - n.setPosition(float(x), float(y), float(z)) - return self - - def recolor(self, name: str, color: Color): - """Recolor node ``name`` in place with a single flat material color. - - Uses the node's material color (setUseSingleColor + setColor), NOT - per-vertex colors: cvcGL renders per-vertex float colors through VTK's - lookup table (a channel-mangling bug — a red mesh comes out blue), so a - uniform color must go through the actor property to be faithful. - """ - gn = self._scene.geometry_node(name) - if gn is None: - raise KeyError(f"grl_snam_lab.Lab.recolor: no mesh node named {name!r}") - gn.setUseSingleColor(True) - gn.setColor(*[float(c) for c in color]) - return self - - # -- lifecycle ----------------------------------------------------------- - - def set_axis_visible(self, visible: bool): - """Show/hide the scene's XYZ axis gnomon (a distraction for clean demos).""" - self._scene.setAxisVisible(bool(visible)) - return self - - def pump(self): - self._scene.processEvents() - return self - - def num_nodes(self) -> int: - return self._scene.num_graphics() - - def has(self, name: str) -> bool: - return self._scene.hasGraphics(name) - - def show(self, title: str = "GRL-SNAM lab", width: int = 1024, height: int = 768): - """Open an interactive window (blocks until closed; needs a display).""" - self._gl.show(self._scene, title, width, height) - - def render_png(self, path: str, width: int = 1024, height: int = 768): - """Render one frame to a PNG (offscreen; needs a GL context).""" - self._gl.render_png(self._scene, path, width, height) +"""Scene Lab + pure-Python geometry helpers — moved to :mod:`pycvc_gl.lab` +(a generic scene utility that ships with the pycvc_gl bindings). Re-exported here +for backward compatibility.""" +from pycvc_gl.lab import * # noqa: F401,F403 + +# `import *` skips underscore-prefixed names, but tests/test_lab.py imports +# `_flatten_points` from here (its old home), so surface the helpers by name too. +from pycvc_gl.lab import Lab, terrain_mesh, polyline_indices, _flatten_points # noqa: F401 diff --git a/grl_snam_lab/scenes.py b/grl_snam_lab/scenes.py index d03e377..5005bb2 100644 --- a/grl_snam_lab/scenes.py +++ b/grl_snam_lab/scenes.py @@ -1,407 +1,7 @@ -"""Load real-world scene geometry (terrain heightfields + glTF city meshes) into -a :class:`grl_snam_lab.lab.Lab`. - -These are generic loaders for a ``geometry_bundle`` export — a ``terrain.json`` -heightfield plus a ``buildings.glb`` (glTF 2.0) city mesh, as produced by the -CVC-DBG ``geometry-scene-gen`` tool (e.g. the Austin bundle). The glTF is read -with VTK's ``vtkGLTFReader`` (no trimesh/pygltflib needed) and added as a single -VTK prop; the terrain becomes a draped surface mesh; a bilinear ``sampler`` lets -you drape an agent onto the terrain. - -ATTRIBUTION: bundles generated from OpenStreetMap are © OpenStreetMap -contributors and licensed under the Open Database License (ODbL, -https://openstreetmap.org/copyright); SRTM terrain is US public domain. If you -publish renders, credit OpenStreetMap. Do NOT ship the Esri ``satellite.png`` -overlay some bundles carry — it is proprietary; these loaders never touch it. -""" - -from __future__ import annotations - -import json -import os - - -def terrain_grid(path: str): - """Read a ``terrain.json`` heightfield: returns ``(grid, bounds2d, rows, - cols)`` where ``grid[row][col]`` is height (row -> y, col -> x) and - ``bounds2d`` = ``(min_x, min_y, max_x, max_y)``. - - The stored grid is TOP-DOWN (row 0 = north = ``max_y``, the raster/SRTM/GeoTIFF - convention the geometry-scene-gen tool inherits). Our mesh + sampler use the - opposite, bottom-up convention (row 0 -> ``min_y``, so a rising world ``y`` - walks up the row index). We normalize here by reversing the rows so BOTH the - terrain mesh and the drape sampler agree with the glTF's world frame — without - this flip the terrain is mirrored along Y and the ``buildings.glb`` (which is - baked in true world coordinates) sinks into / floats above it. - """ - d = json.load(open(path)) - b = d["bounds"] - grid = list(reversed(d["grid"])) # top-down raster -> bottom-up (row 0 = min_y) - return grid, (b["min_x"], b["min_y"], b["max_x"], b["max_y"]), d["rows"], d["cols"] - - -def terrain_sampler(path: str): - """A bilinear height function ``h(x, y)`` over a ``terrain.json`` grid — use - it to DRAPE an agent/path onto the real terrain (clamped at the edges).""" - grid, (min_x, min_y, max_x, max_y), rows, cols = terrain_grid(path) - sx = (cols - 1) / (max_x - min_x) if max_x > min_x else 0.0 - sy = (rows - 1) / (max_y - min_y) if max_y > min_y else 0.0 - - def h(x: float, y: float) -> float: - fx = (x - min_x) * sx - fy = (y - min_y) * sy - fx = 0.0 if fx < 0 else (cols - 1 if fx > cols - 1 else fx) - fy = 0.0 if fy < 0 else (rows - 1 if fy > rows - 1 else fy) - c0, r0 = int(fx), int(fy) - c1 = c0 + 1 if c0 < cols - 1 else c0 - r1 = r0 + 1 if r0 < rows - 1 else r0 - tx, ty = fx - c0, fy - r0 - top = grid[r0][c0] * (1 - tx) + grid[r0][c1] * tx - bot = grid[r1][c0] * (1 - tx) + grid[r1][c1] * tx - return top * (1 - ty) + bot * ty - - return h - - -def add_terrain_json(lab, path: str, name: str = "terrain", color=(0.34, 0.40, 0.28)): - """Add a ``terrain.json`` heightfield to ``lab`` as a draped surface; returns - a ``sampler`` ``h(x, y)`` for the SAME field (so agents can drape onto it). - - NOTE: uses the Lab's terrain mesh, which names the node ``"terrain"``. - """ - grid, bounds2d, _rows, _cols = terrain_grid(path) - lab.add_terrain(grid, bounds=bounds2d, color=color) - return terrain_sampler(path) - - -def add_gltf( - lab, path: str, name: str, color=(0.74, 0.74, 0.78), opacity: float = 1.0, parent: str = "" -): - """Load a glTF/GLB mesh with VTK and add it to ``lab`` as one named prop node. - ``parent`` (default the root) makes it a CHILD of that node so it stays aligned - to and moves with it (e.g. buildings under the terrain). Returns the - ``vtkActor``. Needs the vtk-python wrappers (vtkmodules).""" - from vtkmodules.vtkIOGeometry import vtkGLTFReader - from vtkmodules.vtkFiltersGeometry import vtkCompositeDataGeometryFilter - from vtkmodules.vtkRenderingCore import vtkActor, vtkPolyDataMapper - - reader = vtkGLTFReader() - reader.SetFileName(path) - reader.Update() - # glTF comes back as a multiblock; flatten to one polydata. - geom = vtkCompositeDataGeometryFilter() - geom.SetInputConnection(reader.GetOutputPort()) - geom.Update() - pd = geom.GetOutput() - - mapper = vtkPolyDataMapper() - mapper.SetInputData(pd) - mapper.SetStatic(1) # geometry never changes -> VTK caches the VBO, no per-frame rebuild - mapper.ScalarVisibilityOff() # use the single material color, not any glTF scalars - actor = vtkActor() - actor.SetMapper(mapper) - prop = actor.GetProperty() - prop.SetColor(*color) - prop.SetOpacity(opacity) - # Ground-level views leave many building faces facing away from the light; a - # strong ambient term keeps them from going black so the city reads clearly. - prop.SetAmbient(0.45) - prop.SetDiffuse(0.7) - prop.SetSpecular(0.05) - - b = pd.GetBounds() # (xmin,xmax, ymin,ymax, zmin,zmax) - lab.add_prop(name, actor, (b[0], b[2], b[4], b[1], b[3], b[5]), parent=parent) - return actor - - -# ── grounded routing: keep a vehicle ON THE STREETS, out of the buildings ──── -# -# The scene is real: a vehicle should DRIVE the streets, not fly over rooftops or -# clip through walls. We rasterize the city footprint into a boolean occupancy -# grid, then A*-route a grounded path through the free space (streets) between -# waypoints and drape it on the terrain. All pure-numpy/heapq — no extra deps. - - -def building_occupancy( - glb_path: str, bounds2d, nx: int = 512, ny: int = 512, inflate_m: float = 10.0, cache: bool = True -): - """Rasterize ``buildings.glb`` into a SOLID boolean occupancy grid (``True`` = - inside a building footprint) over ``bounds2d`` = ``(min_x, min_y, max_x, max_y)``. - - ``occ[r][c]``: ``r`` -> y (row 0 = ``min_y``), ``c`` -> x — the same bottom-up - world frame the terrain mesh/sampler use. Footprints are grown by ``inflate_m`` - (a clearance halo) so a routed path keeps a vehicle's width off the walls. - - The mask is produced by rendering the city mesh **top-down orthographically** - and thresholding: VTK's polygon rasterizer fills every triangle, so building - INTERIORS are solid — unlike marking only vertex cells (which leaves interiors - and sparse-wall gaps a path could thread straight through). Needs an offscreen - GL context (present wherever the scene renders). - - The mask is cached next to ``glb_path`` (``.occ_x_i.npy``) - keyed by the grid params, so a demo loads it instantly on later runs instead of - re-rasterizing (and can precompute it out-of-process to avoid opening a second - GL context inside a host app). Pass ``cache=False`` to always recompute. - """ - import os - - import numpy as np - - cache_path = "%s.occ_%dx%d_i%d.npy" % (glb_path, nx, ny, int(round(inflate_m))) - if cache and os.path.exists(cache_path) and os.path.getmtime(cache_path) >= os.path.getmtime(glb_path): - return np.load(cache_path) - - # Register VTK's OpenGL2 render factory — in a raw Python interpreter (unlike the - # C++ VTK_MODULE_INIT path) vtkRenderWindow() has no GL override until this import - # runs, and offscreen Render() otherwise crashes. - import vtkmodules.vtkRenderingOpenGL2 # noqa: F401 - from vtkmodules.vtkIOGeometry import vtkGLTFReader - from vtkmodules.vtkFiltersGeometry import vtkCompositeDataGeometryFilter - from vtkmodules.vtkRenderingCore import ( - vtkActor, - vtkPolyDataMapper, - vtkRenderer, - vtkRenderWindow, - vtkWindowToImageFilter, - ) - from vtkmodules.util.numpy_support import vtk_to_numpy - - min_x, min_y, max_x, max_y = bounds2d - reader = vtkGLTFReader() - reader.SetFileName(glb_path) - reader.Update() - gf = vtkCompositeDataGeometryFilter() - gf.SetInputConnection(reader.GetOutputPort()) - gf.Update() - - mapper = vtkPolyDataMapper() - mapper.SetInputData(gf.GetOutput()) - mapper.ScalarVisibilityOff() - actor = vtkActor() - actor.SetMapper(mapper) - actor.GetProperty().SetColor(1.0, 1.0, 1.0) - actor.GetProperty().LightingOff() # flat white footprint on black — a clean mask - - ren = vtkRenderer() - ren.SetBackground(0.0, 0.0, 0.0) - ren.AddActor(actor) - win = vtkRenderWindow() - win.SetOffScreenRendering(1) - win.AddRenderer(ren) - win.SetSize(nx, ny) - - cam = ren.GetActiveCamera() - cam.ParallelProjectionOn() # orthographic: pixel<->world is a pure linear map - cx, cy = 0.5 * (min_x + max_x), 0.5 * (min_y + max_y) - cam.SetFocalPoint(cx, cy, 0.0) - cam.SetPosition(cx, cy, 10000.0) # straight above, looking down -Z - cam.SetViewUp(0.0, 1.0, 0.0) # world +Y = image up (north-up map) - cam.SetParallelScale(0.5 * (max_y - min_y)) # half the world height fills the viewport - cam.SetClippingRange(1.0, 20000.0) - win.Render() - - w2i = vtkWindowToImageFilter() - w2i.SetInput(win) - w2i.Update() - img = w2i.GetOutput() - arr = vtk_to_numpy(img.GetPointData().GetScalars()).reshape(ny, nx, -1) - # VTK image origin is bottom-left: row 0 = bottom = min_y, col 0 = left = min_x — - # exactly our occ[r->y][c->x] convention, no flip needed. - occ = arr[:, :, 0] > 127 - win.Finalize() - - cell = min((max_x - min_x) / nx, (max_y - min_y) / ny) - k = int(round(inflate_m / cell)) if cell > 0 else 0 - for _ in range(k): # 4-connected binary dilation, k steps ~= inflate_m of clearance - d = occ.copy() - d[1:, :] |= occ[:-1, :] - d[:-1, :] |= occ[1:, :] - d[:, 1:] |= occ[:, :-1] - d[:, :-1] |= occ[:, 1:] - occ = d - - if cache: - try: - np.save(cache_path, occ) - except OSError: # read-only bundle dir — just skip caching - pass - return occ - - -def _nearest_free(occ, rc): - """Nearest ``(r, c)`` with ``occ`` False, spiralling out from ``rc``.""" - ny, nx = occ.shape - r0, c0 = rc - if not occ[r0, c0]: - return rc - for rad in range(1, max(nx, ny)): - for dr in range(-rad, rad + 1): - edge = range(-rad, rad + 1) if abs(dr) == rad else (-rad, rad) - for dc in edge: - r, c = r0 + dr, c0 + dc - if 0 <= r < ny and 0 <= c < nx and not occ[r, c]: - return (r, c) - return rc - - -def _line_of_sight(occ, a, b): - """True if the straight cell line ``a``->``b`` crosses no blocked cell (Bresenham).""" - r0, c0 = a - r1, c1 = b - dr, dc = abs(r1 - r0), abs(c1 - c0) - sr = 1 if r0 < r1 else -1 - sc = 1 if c0 < c1 else -1 - err = dc - dr - r, c = r0, c0 - while True: - if occ[r, c]: - return False - if (r, c) == (r1, c1): - return True - e2 = 2 * err - if e2 > -dr: - err -= dr - c += sc - if e2 < dc: - err += dc - r += sr - - -def _astar(occ, start, goal): - """8-connected grid A* over free cells; returns a list of ``(r, c)`` or ``None``.""" - import heapq - - ny, nx = occ.shape - - def h(a): - return ((a[0] - goal[0]) ** 2 + (a[1] - goal[1]) ** 2) ** 0.5 - - nbrs = [(-1, 0), (1, 0), (0, -1), (0, 1), (-1, -1), (-1, 1), (1, -1), (1, 1)] - openq = [(h(start), 0.0, start)] - came = {} - gcost = {start: 0.0} - while openq: - _, gc, cur = heapq.heappop(openq) - if cur == goal: - break - if gc > gcost.get(cur, float("inf")): - continue - for dr, dc in nbrs: - nr, ncl = cur[0] + dr, cur[1] + dc - if not (0 <= nr < ny and 0 <= ncl < nx) or occ[nr, ncl]: - continue - if dr and dc and (occ[cur[0] + dr, cur[1]] or occ[cur[0], cur[1] + dc]): - continue # don't cut a diagonal across a blocked building corner - step = 1.41421356 if dr and dc else 1.0 - ng = gc + step - if ng < gcost.get((nr, ncl), float("inf")): - gcost[(nr, ncl)] = ng - came[(nr, ncl)] = cur - heapq.heappush(openq, (ng + h((nr, ncl)), ng, (nr, ncl))) - if goal not in came and goal != start: - return None - path = [goal] - c = goal - while c != start: - c = came[c] - path.append(c) - path.reverse() - return path - - -def _simplify(occ, path): - """String-pull a grid path: keep only corners needed to preserve line-of-sight, - so the route is straight street runs instead of a staircase.""" - if len(path) < 3: - return path - out = [path[0]] - i = 0 - while i < len(path) - 1: - j = len(path) - 1 - while j > i + 1 and not _line_of_sight(occ, path[i], path[j]): - j -= 1 - out.append(path[j]) - i = j - return out - - -def plan_ground_route(occ, bounds2d, waypoints_xy, close_loop: bool = True): - """Plan a GROUNDED route (world ``(x, y)`` list) that visits ``waypoints_xy`` in - order through the free space of ``occ`` (streets), optionally closing the loop. - - Each waypoint is snapped to the nearest free cell; legs are A*-routed and - line-of-sight simplified. Drape the result on the terrain to drive it. Returns - ``[]`` if no leg is routable. - """ - ny, nx = occ.shape - min_x, min_y, max_x, max_y = bounds2d - - def to_cell(x, y): - c = int(round((x - min_x) / (max_x - min_x) * (nx - 1))) - r = int(round((y - min_y) / (max_y - min_y) * (ny - 1))) - return (max(0, min(ny - 1, r)), max(0, min(nx - 1, c))) - - def to_world(rc): - r, c = rc - return (min_x + c / (nx - 1) * (max_x - min_x), min_y + r / (ny - 1) * (max_y - min_y)) - - wps = list(waypoints_xy) - if close_loop and wps: - wps = wps + [wps[0]] - cells = [_nearest_free(occ, to_cell(x, y)) for x, y in wps] - full = [] - for a, b in zip(cells, cells[1:]): - seg = _astar(occ, a, b) - if not seg: - continue - seg = _simplify(occ, seg) - if full: - seg = seg[1:] # drop the duplicated junction cell - full += seg - return [to_world(rc) for rc in full] - - -def resample_polyline(points_xy, spacing: float): - """Resample a 2-D polyline to roughly uniform ``spacing`` (world units) so a - draped route animates at constant ground speed and the chase-cam heading is - smooth. Returns a list of ``(x, y)``.""" - if len(points_xy) < 2: - return list(points_xy) - out = [tuple(points_xy[0])] - carry = 0.0 - for (x0, y0), (x1, y1) in zip(points_xy, points_xy[1:]): - seg = ((x1 - x0) ** 2 + (y1 - y0) ** 2) ** 0.5 - if seg <= 1e-9: - continue - d = carry - while d < seg: - t = d / seg - out.append((x0 + (x1 - x0) * t, y0 + (y1 - y0) * t)) - d += spacing - carry = d - seg - out.append(tuple(points_xy[-1])) - return out - - -def load_geometry_bundle( - lab, - bundle_dir: str, - terrain_color=(0.34, 0.40, 0.28), - building_color=(0.74, 0.74, 0.78), - buildings: bool = True, -): - """Load a ``geometry_bundle`` directory (``terrain.json`` + ``buildings.glb``) - into ``lab``. Returns the terrain ``sampler`` ``h(x, y)`` for draping agents. - - ``buildings=False`` loads only the terrain (much lighter). The Esri - ``satellite.png`` some bundles carry is intentionally NOT loaded (proprietary). - """ - terrain_path = os.path.join(bundle_dir, "terrain.json") - sampler = add_terrain_json(lab, terrain_path, color=terrain_color) - if buildings: - glb = os.path.join(bundle_dir, "buildings.glb") - if os.path.exists(glb): - # buildings are CHILDREN of the terrain node so they stay aligned to it - # (same world frame) and move with any terrain transform. - add_gltf(lab, glb, "buildings", color=building_color, parent="terrain") - return sampler +"""Scene loaders + grounded routing — moved to :mod:`pycvc_gl.scenes`. Re-exported +here for backward compatibility.""" +from pycvc_gl.scenes import * # noqa: F401,F403 +from pycvc_gl.scenes import ( # noqa: F401 + terrain_grid, terrain_sampler, add_terrain_json, add_gltf, load_geometry_bundle, + building_occupancy, plan_ground_route, resample_polyline, +) diff --git a/grl_snam_lab/vehicle.py b/grl_snam_lab/vehicle.py index a62eec2..246b44c 100644 --- a/grl_snam_lab/vehicle.py +++ b/grl_snam_lab/vehicle.py @@ -1,135 +1,4 @@ -"""Smoothly orient a vehicle to the terrain it drives on. - -:class:`VehiclePose` turns a stream of ``(x, y)`` ground positions plus a terrain -height sampler into a smoothed 4x4 model transform (row-major, for -``pycvc_gl`` ``GraphicsNode.setTransform``) that - -* **yaws** the vehicle to its heading (estimated from the position stream), and -* **pitches / rolls** it to the terrain **surface normal** — so it banks over a - crest and leans on a side-slope like a real vehicle — - -with frame-rate-independent exponential damping so the pose never snaps (the same -smoothing idea as :class:`grl_snam_lab.camera.ChaseCamera`). - -The vehicle mesh is assumed modeled with **+X = forward, +Z = up, base at z = 0**. -""" - -from __future__ import annotations - -import math - - -def _norm3(v): - m = math.sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]) or 1.0 - return (v[0] / m, v[1] / m, v[2] / m) - - -def _cross(a, b): - return (a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]) - - -def _ema(prev, target, tau, dt): - """Frame-rate-independent exponential smoothing of a scalar toward ``target``.""" - if prev is None: - return target - a = 1.0 - math.exp(-dt / max(tau, 1e-6)) - return prev + a * (target - prev) - - -class VehiclePose: - """Stateful smoother: feed it ``update(x, y, dt)`` each frame, get back a 4x4 - row-major transform that seats a +X-forward / +Z-up / base-at-0 mesh on the - terrain, oriented to heading + slope. - - Tunables (all seconds of smoothing time-constant, larger = smoother/laggier): - ``heading_tau`` (yaw), ``normal_tau`` (pitch/roll). ``normal_step`` is the - finite-difference distance (world units) used to read the terrain slope; - ``lift`` raises the body slightly along the normal to avoid z-fighting with the - road; ``min_speed`` (world units/sec) is the speed below which heading is held. - """ - - def __init__( - self, - sampler, - *, - lift: float = 0.2, - heading_tau: float = 0.30, - normal_tau: float = 0.45, - normal_step: float = 6.0, - min_speed: float = 0.5, - ): - self._h = sampler - self._lift = lift - self._heading_tau = heading_tau - self._normal_tau = normal_tau - self._e = normal_step - self._min_speed = min_speed - self._prev = None # previous raw (x, y) for velocity - self._fwd = None # smoothed forward XY unit vector - self._n = None # smoothed terrain normal (3,) - - def _terrain_normal(self, x, y): - """Unit surface normal of the height field at ``(x, y)`` (central diff).""" - e = self._e - dzdx = (self._h(x + e, y) - self._h(x - e, y)) / (2.0 * e) - dzdy = (self._h(x, y + e) - self._h(x, y - e)) / (2.0 * e) - return _norm3((-dzdx, -dzdy, 1.0)) - - def update(self, x, y, dt): - """Advance by ``dt`` seconds to position ``(x, y)``; return a 16-float - row-major model matrix (list) for ``GraphicsNode.setTransform``.""" - dt = max(dt, 1e-4) - - # -- heading from the (damped) velocity of the position stream -- - if self._prev is None: - self._prev = (x, y) - vx = (x - self._prev[0]) / dt - vy = (y - self._prev[1]) / dt - self._prev = (x, y) - speed = math.hypot(vx, vy) - if speed > self._min_speed: - hx, hy = vx / speed, vy / speed - if self._fwd is None: - self._fwd = (hx, hy) - else: - a = 1.0 - math.exp(-dt / max(self._heading_tau, 1e-6)) - fx = self._fwd[0] + a * (hx - self._fwd[0]) - fy = self._fwd[1] + a * (hy - self._fwd[1]) - mag = math.hypot(fx, fy) or 1.0 - self._fwd = (fx / mag, fy / mag) - elif self._fwd is None: - self._fwd = (1.0, 0.0) - - # -- damped terrain normal (pitch + roll) -- - tn = self._terrain_normal(x, y) - if self._n is None: - self._n = tn - else: - self._n = _norm3( - ( - _ema(self._n[0], tn[0], self._normal_tau, dt), - _ema(self._n[1], tn[1], self._normal_tau, dt), - _ema(self._n[2], tn[2], self._normal_tau, dt), - ) - ) - n = self._n - - # -- orthonormal body frame: X = forward (in the tangent plane), Z = up = n -- - f0 = (self._fwd[0], self._fwd[1], 0.0) - d = f0[0] * n[0] + f0[1] * n[1] + f0[2] * n[2] - f = _norm3((f0[0] - d * n[0], f0[1] - d * n[1], f0[2] - d * n[2])) - left = _norm3(_cross(n, f)) # local +Y = left; (f, left, n) is right-handed - - # -- translation: draped ground point, lifted a touch along the normal -- - z = self._h(x, y) - tx = x + self._lift * n[0] - ty = y + self._lift * n[1] - tz = z + self._lift * n[2] - - # row-major 4x4; matrix columns are the world directions of local X, Y, Z. - return [ - f[0], left[0], n[0], tx, - f[1], left[1], n[1], ty, - f[2], left[2], n[2], tz, - 0.0, 0.0, 0.0, 1.0, - ] +"""Vehicle pose smoother — moved to :mod:`pycvc_gl.vehicle`. Re-exported here for +backward compatibility.""" +from pycvc_gl.vehicle import * # noqa: F401,F403 +from pycvc_gl.vehicle import VehiclePose # noqa: F401