diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 592c0f3..c9e448a 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -21,43 +21,39 @@ jobs: - name: Checkout uses: actions/checkout@v6 - - name: Set up Python ${{ matrix.python-version }} - uses: actions/setup-python@v6 + - name: Install uv + uses: astral-sh/setup-uv@v5 with: python-version: ${{ matrix.python-version }} - name: Install package + dev extras - run: | - python -m pip install --upgrade pip - # [dependency-groups] is uv syntax; pip doesn't resolve it, so install tools explicitly - python -m pip install -e . pytest ruff + run: uv sync --group dev - name: Lint (ruff) - run: ruff check . + run: uv run ruff check . # Optional but recommended: verifies console scripts + dispatch without launching GUI. # NOTE: calling plain `cosmo` (no args) attempts to start the GUI by default. - name: CLI sanity check (no GUI) run: | - cosmo --version - cosmo convert --help - cosmo calibrate --help + uv run cosmo --version + uv run cosmo convert --help + uv run cosmo calibrate --help # Run smoke tests first for fast feedback, then run the rest (skip smoke). - name: Run smoke tests (CLI / entrypoints) env: MPLBACKEND: Agg - run: pytest -q -m smoke + run: uv run pytest -q -m smoke - name: Run full test suite (skip smoke) env: MPLBACKEND: Agg - run: pytest -q -m "not smoke" + run: uv run pytest -q -m "not smoke" - name: Build wheel/sdist run: | - python -m pip install build - python -m build + uv build ls -la dist integration: @@ -66,16 +62,13 @@ jobs: if: github.event_name == 'schedule' || github.event_name == 'workflow_dispatch' steps: - uses: actions/checkout@v6 - - uses: actions/setup-python@v6 + - name: Install uv + uses: astral-sh/setup-uv@v5 with: python-version: "3.12" - name: Install package + dev extras - run: | - python -m pip install --upgrade pip - python -m pip install -e . pytest ruff - # ensure optional deps are available - python -m pip install betterosi mcap + run: uv sync --group dev --group mcap - name: Run integration tests env: MPLBACKEND: Agg - run: pytest -q -m integration + run: uv run pytest -q -m integration diff --git a/pyproject.toml b/pyproject.toml index fa5705c..2e032cd 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -4,11 +4,11 @@ build-backend = "hatchling.build" [project] name = "cosmo" -version = "0.4.1" +version = "0.5.0" description = "COSMO conversion tools (OpenLABEL -> OSI/MCAP + Omega-Prime CSV) with GUI" readme = "README.md" requires-python = ">=3.10" -dependencies = ["numpy", "pandas", "pillow", "matplotlib", "dataprov", "reverse_geocoder", "pycountry"] +dependencies = ["numpy", "pandas", "pillow", "matplotlib", "dataprov", "reverse_geocoder", "pycountry", "orbit-georef", "opendrive-map"] [dependency-groups] @@ -26,6 +26,12 @@ cosmo-cli = [{include-group = "mcap"}, {include-group = "plot"}] cosmo-gui = [{include-group = "gui"}, {include-group = "mcap"}, {include-group = "plot"}] trajectory-explorer = [{include-group = "gui"}, {include-group = "mcap"}, {include-group = "ontology"}] +# orbit-georef and opendrive-map are standalone packages living as subdirectories +# of the ORBIT repo, pinned here to per-lib git tags. Bump the tag to upgrade. +[tool.uv.sources] +orbit-georef = { git = "https://github.com/RI-SE/ORBIT.git", subdirectory = "orbit-georef", tag = "orbit-georef-v0.1.2" } +opendrive-map = { git = "https://github.com/RI-SE/ORBIT.git", subdirectory = "opendrive-map", tag = "opendrive-map-v0.1.0" } + [project.scripts] cosmo = "cosmo.cli.main:main" cosmo-gui = "cosmo.cli.gui:main" diff --git a/src/cosmo/app/convert_app.py b/src/cosmo/app/convert_app.py index 96d867a..e1b3bbd 100644 --- a/src/cosmo/app/convert_app.py +++ b/src/cosmo/app/convert_app.py @@ -59,6 +59,10 @@ class ConvertConfig: # ISO 3166-1 numeric country code; auto-derived from georef lat/lon if None country_code: Optional[int] = None + # object output frame: "global" (absolute proj_string CRS) or "local" + # (map-local; offset recorded in OSI proj_frame_offset) + object_frame: str = "global" + # output control out_dir: Optional[str] = None # If None => /runs/_convert_/ run_name: Optional[str] = None # Optional override for the run folder name @@ -277,6 +281,7 @@ def run_convert(cfg: ConvertConfig, log_fn: Optional[LogFn] = None) -> ConvertRe corrector=corrector, stabilize_size=cfg.stabilize_size, country_code=cfg.country_code, + object_frame=cfg.object_frame, ) # determine produced outputs diff --git a/src/cosmo/cli/convert.py b/src/cosmo/cli/convert.py index 6515020..6755081 100644 --- a/src/cosmo/cli/convert.py +++ b/src/cosmo/cli/convert.py @@ -141,6 +141,11 @@ def build_parser() -> argparse.ArgumentParser: help="ISO 3166-1 numeric country code (e.g. 752=Sweden); " "auto-derived from georef lat/lon if omitted.") + ap.add_argument("--object-frame", choices=["global", "local"], default="global", + help="Object coordinate frame: 'global' = absolute proj_string CRS " + "(default); 'local' = map-local, shifted by the OpenDRIVE header " + " and recorded in OSI proj_frame_offset (requires --odr).") + # Run folder naming ap.add_argument("--run-name", required=False, help="Optional override for run folder name") @@ -279,6 +284,7 @@ def main(argv=None) -> int: output_prefix=getattr(args, "output_prefix", None), stabilize_size=args.stabilize_size, country_code=args.country_code, + object_frame=args.object_frame, ) def _log(line: str) -> None: diff --git a/src/cosmo/converters/openlabel_to_omega.py b/src/cosmo/converters/openlabel_to_omega.py index baf713b..98f3132 100644 --- a/src/cosmo/converters/openlabel_to_omega.py +++ b/src/cosmo/converters/openlabel_to_omega.py @@ -39,6 +39,18 @@ except ImportError: # pragma: no cover betterosi = None +# orbit-georef owns the ORBIT georef JSON format + pixel→ground transform. +try: + from orbit_georef import GeoTransformer +except ImportError: # pragma: no cover + GeoTransformer = None + +# opendrive-map owns the authoritative OpenDRIVE header (map frame origin). +try: + from opendrive_map import read_offset as _read_map_offset +except ImportError: # pragma: no cover + _read_map_offset = None + from cosmo.converters.ontology_mapper import OntologyMapper # ----------------------------------------------------------------------------- @@ -308,7 +320,14 @@ def load_alignment( f"Unsupported ORBIT georef transform_method='{georef.get('transform_method')}'. " "Expected homography-style pixel->ground transform." ) - if "transformation_matrix" in georef: + # orbit-georef owns the ORBIT georef format; use it for full exports and + # fall back to inline parsing for partial/legacy calibration-style files. + is_full_orbit_georef = all( + k in georef for k in ("transformation_matrix", "inverse_matrix", "reference_point") + ) + if GeoTransformer is not None and georef_data_path and is_full_orbit_georef: + H = GeoTransformer.from_file(georef_data_path).transform_matrix + elif "transformation_matrix" in georef: H = np.array(georef["transformation_matrix"], dtype=np.float64) elif "homography" in georef: H = np.array(georef["homography"], dtype=np.float64) @@ -556,6 +575,7 @@ def convert_openlabel_to_omega( corrector=None, stabilize_size: bool = False, country_code: Optional[int] = None, + object_frame: str = "global", ): """ Convert OpenLABEL -> Omega-Prime CSV and optionally OSI GroundTruth MCAP. @@ -598,6 +618,20 @@ def _log(msg: str) -> None: if write_mcap and betterosi is None: _log('[COSMO] MCAP requested but betterosi is not installed; will write CSV only.') + # Object output frame. "global": absolute coords in the proj_string CRS (default). + # "local": coords shifted by the map's header so objects and the embedded + # OpenDRIVE share one frame; the offset is recorded in OSI proj_frame_offset. + frame_off = (0.0, 0.0, 0.0) + if object_frame == "local": + if not (odr_path and _read_map_offset and os.path.isfile(odr_path)): + raise ValueError( + "object_frame='local' requires --odr with a header " + "(and opendrive-map installed)." + ) + frame_off = _read_map_offset(odr_path) + _log(f"[COSMO] object_frame=local; subtracting map offset {frame_off}") + elif object_frame != "global": + raise ValueError(f"object_frame must be 'global' or 'local', got {object_frame!r}") vt_name_to_code, vr_name_to_code, vt_default, vr_default = build_enum_code_maps() @@ -752,6 +786,13 @@ def _write_ground_truth(writer_mcap, t_sec: float, total_nanos: int, moving_obje gt.proj_string = _gt_proj_string if _gt_country_code: gt.country_code = _gt_country_code + if object_frame == "local" and any(frame_off): + # OSI: world = proj_frame_offset + osi, then proj_string. Records the + # local→projected-CRS shift so local coords stay fully georeferenced. + gt.proj_frame_offset = betterosi.GroundTruthProjFrameOffset( + position=betterosi.Vector3D(x=frame_off[0], y=frame_off[1], z=frame_off[2]), + yaw=0.0, + ) # Provide log_time explicitly; readers often build indices from it. [4](https://ika-rwth-aachen.github.io/omega-prime/notebooks/tutorial/) writer_mcap.add(gt, topic="ground_truth", log_time=total_nanos) @@ -809,6 +850,8 @@ def _write_ground_truth(writer_mcap, t_sec: float, total_nanos: int, moving_obje else: length, width, height = estimate_dims(label_type, w_px, h_px, oid) + X, Y, Z = X - frame_off[0], Y - frame_off[1], Z - frame_off[2] + if stabilize_size: length, width, height = size_avg.get(oid, (length, width, height)) @@ -911,6 +954,8 @@ def _write_ground_truth(writer_mcap, t_sec: float, total_nanos: int, moving_obje else: length, width, height = estimate_dims(label_type, w_px, h_px, oid) + X, Y, Z = X - frame_off[0], Y - frame_off[1], Z - frame_off[2] + if stabilize_size: length, width, height = size_avg.get(oid, (length, width, height)) diff --git a/src/cosmo/trajectory_explorer.py b/src/cosmo/trajectory_explorer.py index 4bb00bb..cb58f68 100644 --- a/src/cosmo/trajectory_explorer.py +++ b/src/cosmo/trajectory_explorer.py @@ -4,12 +4,14 @@ import argparse import json import math -import re -import xml.etree.ElementTree as ET -from dataclasses import dataclass, field from pathlib import Path import numpy as np + +# opendrive-map is the shared source of lane geometry (all primitives, full width +# polynomials, laneOffset). The viewer draws in absolute UTM, so it adds the map +# back to opendrive-map's local coordinates. +import opendrive_map as odm import pandas as pd try: @@ -141,280 +143,38 @@ def _fmt_dims(dims: tuple[float, float, float] | None) -> str: # --------------------------------------------------------------------------- -# Data structures -# --------------------------------------------------------------------------- - -@dataclass -class GeomSegment: - s: float - x: float - y: float - hdg: float - length: float - kind: str # "line" or "paramPoly3" - poly3_params: dict = field(default_factory=dict) - - -@dataclass -class LaneWidth: - sOffset: float - a: float - b: float - c: float - d: float - - -@dataclass -class Lane: - id: int - widths: list[LaneWidth] - - -@dataclass -class LaneSection: - s: float - lanes: list[Lane] - - -@dataclass -class Road: - id: str - length: float - geom_segments: list[GeomSegment] - lane_sections: list[LaneSection] - - -@dataclass -class ParkingObject: - road_id: str - s: float - t: float - hdg: float - width: float - length: float - outline_corners: list[tuple[float, float]] = field(default_factory=list) # (u, v) local coords - - -# --------------------------------------------------------------------------- -# XODR parsing -# --------------------------------------------------------------------------- - -def parse_xodr(path: str) -> tuple[list[Road], list[ParkingObject], tuple[float, float]]: - return parse_xodr_text(open(path, encoding="utf-8").read()) - - -def parse_xodr_text(text: str) -> tuple[list[Road], list[ParkingObject], tuple[float, float]]: - text = re.sub(r'\s+xmlns="[^"]+"', "", text) - root = ET.fromstring(text) - - offset_x, offset_y = 0.0, 0.0 - header = root.find("header") - if header is not None: - off = header.find("offset") - if off is not None: - offset_x = float(off.get("x", 0.0)) - offset_y = float(off.get("y", 0.0)) - - roads: list[Road] = [] - parking: list[ParkingObject] = [] - - for road_el in root.findall("road"): - road_id = road_el.get("id", "") - length = float(road_el.get("length", 0)) - - geom_segments: list[GeomSegment] = [] - for g in road_el.findall("./planView/geometry"): - s = float(g.get("s", 0)) - x = float(g.get("x", 0)) + offset_x - y = float(g.get("y", 0)) + offset_y - hdg = float(g.get("hdg", 0)) - seg_len = float(g.get("length", 0)) - children = list(g) - kind = children[0].tag if children else "line" - params = children[0].attrib if children else {} - geom_segments.append(GeomSegment(s, x, y, hdg, seg_len, kind, params)) - - lane_sections: list[LaneSection] = [] - for ls_el in road_el.findall("./lanes/laneSection"): - ls_s = float(ls_el.get("s", 0)) - lanes: list[Lane] = [] - for side in ("left", "right"): - side_el = ls_el.find(side) - if side_el is None: - continue - for lane_el in side_el.findall("lane"): - lid = int(lane_el.get("id", 0)) - widths = [] - for w_el in lane_el.findall("width"): - widths.append(LaneWidth( - sOffset=float(w_el.get("sOffset", 0)), - a=float(w_el.get("a", 0)), - b=float(w_el.get("b", 0)), - c=float(w_el.get("c", 0)), - d=float(w_el.get("d", 0)), - )) - lanes.append(Lane(id=lid, widths=widths)) - lane_sections.append(LaneSection(s=ls_s, lanes=lanes)) - - for obj in road_el.findall("./objects/object"): - if obj.get("type") == "parking": - corners = [ - (float(c.get("u", 0)), float(c.get("v", 0))) - for c in obj.findall("./outline/cornerLocal") - ] - parking.append(ParkingObject( - road_id=road_id, - s=float(obj.get("s", 0)), - t=float(obj.get("t", 0)), - hdg=float(obj.get("hdg", 0)), - width=float(obj.get("width", 2)), - length=float(obj.get("length", 5)), - outline_corners=corners, - )) - - roads.append(Road(id=road_id, length=length, - geom_segments=geom_segments, - lane_sections=lane_sections)) - - return roads, parking, (offset_x, offset_y) - - -# --------------------------------------------------------------------------- -# Geometry sampling -# --------------------------------------------------------------------------- - -def _sample_road(road: Road) -> list[tuple[float, float, float, float]]: - """Return [(s, x, y, hdg), ...] sampled along the road centerline.""" - n_total = max(20, int(road.length / 0.5)) - result: list[tuple[float, float, float, float]] = [] - - for seg in road.geom_segments: - if seg.length <= 0: - continue - n_seg = max(2, int(n_total * seg.length / road.length)) - x0, y0, hdg0 = seg.x, seg.y, seg.hdg - - for i in range(n_seg): - t = seg.length * i / (n_seg - 1) - - if seg.kind == "paramPoly3": - p = seg.poly3_params - p_range = p.get("pRange", "normalized") - p_max = 1.0 if p_range == "normalized" else seg.length - p_val = p_max * t / seg.length - aU, bU = float(p.get("aU", 0)), float(p.get("bU", 0)) - cU, dU = float(p.get("cU", 0)), float(p.get("dU", 0)) - aV, bV = float(p.get("aV", 0)), float(p.get("bV", 0)) - cV, dV = float(p.get("cV", 0)), float(p.get("dV", 0)) - u = aU + bU * p_val + cU * p_val**2 + dU * p_val**3 - v = aV + bV * p_val + cV * p_val**2 + dV * p_val**3 - x = x0 + u * math.cos(hdg0) - v * math.sin(hdg0) - y = y0 + u * math.sin(hdg0) + v * math.cos(hdg0) - du = bU + 2 * cU * p_val + 3 * dU * p_val**2 - dv = bV + 2 * cV * p_val + 3 * dV * p_val**2 - hdg = hdg0 + math.atan2(dv, du) if (du**2 + dv**2) > 1e-12 else hdg0 - else: # line - x = x0 + t * math.cos(hdg0) - y = y0 + t * math.sin(hdg0) - hdg = hdg0 - - result.append((seg.s + t, x, y, hdg)) - - return result - - -# --------------------------------------------------------------------------- -# Lane polygon building +# Lane polygons + centerlines (from the shared opendrive-map library) # --------------------------------------------------------------------------- -def _lane_width_at(lane: Lane, s_rel: float) -> float: - """Evaluate lane width polynomial at s_rel from lane section start.""" - best_w = LaneWidth(0, 0, 0, 0, 0) - for w in lane.widths: - if w.sOffset <= s_rel: - best_w = w - ds = s_rel - best_w.sOffset - return best_w.a + best_w.b * ds + best_w.c * ds**2 + best_w.d * ds**3 - - -def build_lane_polygons(road: Road) -> list[tuple[int, list[tuple[float, float]]]]: - """Return list of (lane_id, [(x,y), ...]) closed polygons.""" - samples = _sample_road(road) - if not samples: +def _poly_exterior_xy(poly, ox: float, oy: float) -> list[tuple[float, float]]: + """Absolute (x+offset) exterior ring of a shapely polygon (largest part if multi).""" + if poly.is_empty: return [] + geom = poly if poly.geom_type == "Polygon" else max(poly.geoms, key=lambda g: g.area) + return [(x + ox, y + oy) for x, y in geom.exterior.coords] - polygons: list[tuple[int, list[tuple[float, float]]]] = [] - - for ls_idx, ls in enumerate(road.lane_sections): - s_start = ls.s - s_end = road.lane_sections[ls_idx + 1].s if ls_idx + 1 < len(road.lane_sections) else road.length - - sec_samples = [(s, x, y, h) for s, x, y, h in samples if s_start <= s <= s_end + 0.01] - if not sec_samples: - continue - - for lane in ls.lanes: - if lane.id == 0: - continue - is_right = lane.id < 0 - perp_sign = -1 if is_right else 1 - - same_side = [ln for ln in ls.lanes if (ln.id < 0) == is_right and ln.id != 0] - same_side.sort(key=lambda ln: abs(ln.id)) - this_rank = abs(lane.id) - inner_lanes = [ln for ln in same_side if abs(ln.id) < this_rank] - inner_pts: list[tuple[float, float]] = [] - outer_pts: list[tuple[float, float]] = [] +def map_geometry_from_xodr(path: str | None = None, text: str | None = None): + """Return (lane_polys, centerlines, parking_polys) in absolute UTM via opendrive-map. - for s, x, y, hdg in sec_samples: - s_rel = s - s_start - perp_dir = hdg + perp_sign * math.pi / 2 - d_inner = sum(_lane_width_at(ln, s_rel) for ln in inner_lanes) - d_outer = d_inner + _lane_width_at(lane, s_rel) - xi = x + d_inner * math.cos(perp_dir) - yi = y + d_inner * math.sin(perp_dir) - xo = x + d_outer * math.cos(perp_dir) - yo = y + d_outer * math.sin(perp_dir) - inner_pts.append((xi, yi)) - outer_pts.append((xo, yo)) - - poly = inner_pts + list(reversed(outer_pts)) - polygons.append((lane.id, poly)) - - return polygons - - -# --------------------------------------------------------------------------- -# Parking rectangle -# --------------------------------------------------------------------------- - -def build_parking_rect(obj: ParkingObject, road: Road) -> list[tuple[float, float]]: - samples = _sample_road(road) - if not samples: - return [] - - s_arr = np.array([s for s, *_ in samples]) - idx = int(np.searchsorted(s_arr, obj.s)) - idx = min(max(idx, 0), len(samples) - 1) - _, rx, ry, rhdg = samples[idx] - - perp = rhdg + math.pi / 2 - cx = rx + obj.t * math.cos(perp) - cy = ry + obj.t * math.sin(perp) - - obj_hdg = rhdg + obj.hdg - if obj.outline_corners: - corners_local = obj.outline_corners + lane_polys: list of (lane_id, [(x, y), ...]); centerlines/parking_polys: list of [(x, y), ...]. + """ + if path: + net = odm.RoadNetwork.from_file(path) + elif text: + net = odm.RoadNetwork.from_text(text) else: - hl, hw = obj.length / 2, obj.width / 2 - corners_local = [(-hl, -hw), (hl, -hw), (hl, hw), (-hl, hw)] - result = [] - for u, v in corners_local: - wx = cx + u * math.cos(obj_hdg) - v * math.sin(obj_hdg) - wy = cy + u * math.sin(obj_hdg) + v * math.cos(obj_hdg) - result.append((wx, wy)) - return result + return [], [], [] + ox, oy, _ = net.offset + lane_polys = [(ln.lane_id, _poly_exterior_xy(ln.polygon, ox, oy)) for ln in net.lanes] + lane_polys = [(lid, pts) for lid, pts in lane_polys if pts] + centerlines = [ + [(x + ox, y + oy) for _s, x, y, _h in odm.sample_reference_line(road, 0.5)] + for road in net.roads + ] + parking_polys = [_poly_exterior_xy(p, ox, oy) for p in net.parking_polygons] + parking_polys = [pts for pts in parking_polys if pts] + return lane_polys, centerlines, parking_polys # --------------------------------------------------------------------------- @@ -479,6 +239,14 @@ def load_from_mcap(path: str) -> tuple[str | None, dict[int, dict]]: for gt in betterosi.read(path, return_ground_truth=True, mcap_topics=["ground_truth"]): ts = gt.timestamp ts_nanos = int(ts.seconds) * 1_000_000_000 + int(ts.nanos) + # Normalise to absolute (proj_string CRS) so local-frame mcaps overlay the + # absolute map: world = R(yaw)·osi + proj_frame_offset (offset absent => no-op). + pfo = getattr(gt, "proj_frame_offset", None) + ofx = ofy = 0.0 + ccos, csin = 1.0, 0.0 + if pfo is not None and getattr(pfo, "position", None) is not None: + ofx, ofy = pfo.position.x, pfo.position.y + ccos, csin = math.cos(pfo.yaw), math.sin(pfo.yaw) for mo in gt.moving_object: oid = int(mo.id.value) if oid not in obj_meta: @@ -496,8 +264,8 @@ def load_from_mcap(path: str) -> tuple[str | None, dict[int, dict]]: b = mo.base obj_rows.setdefault(oid, []).append({ "total_nanos": ts_nanos, - "x": float(b.position.x), - "y": float(b.position.y), + "x": float(b.position.x) * ccos - float(b.position.y) * csin + ofx, + "y": float(b.position.x) * csin + float(b.position.y) * ccos + ofy, "vel_x": float(b.velocity.x), "vel_y": float(b.velocity.y), "yaw": float(b.orientation.yaw), @@ -678,7 +446,8 @@ def cancel(self) -> None: self._cancelled = True def run(self) -> None: - result: dict = {"trajs_a": {}, "trajs_b": {}, "trajs_c": {}, "roads": [], "parking": []} + result: dict = {"trajs_a": {}, "trajs_b": {}, "trajs_c": {}, + "lane_polys": [], "centerlines": [], "parking_polys": []} xodr_xmls: list = [] try: for slot in ("a", "b", "c"): @@ -709,25 +478,15 @@ def run(self) -> None: if self._cancelled: return - if self._xodr_path: - self.progress.emit(f"Parsing map: {Path(self._xodr_path).name}…") - try: - roads, parking, _ = parse_xodr(self._xodr_path) - except Exception as exc: - self.failed.emit(f"Could not load XODR: {exc}") - return - elif xodr_xmls: - self.progress.emit("Parsing embedded map…") + # Lane polygons + centerlines + parking all come from opendrive-map. + if self._xodr_path or xodr_xmls: + self.progress.emit("Building map geometry…") try: - roads, parking, _ = parse_xodr_text(xodr_xmls[0]) + geom = (map_geometry_from_xodr(path=self._xodr_path) if self._xodr_path + else map_geometry_from_xodr(text=xodr_xmls[0])) + result["lane_polys"], result["centerlines"], result["parking_polys"] = geom except Exception as exc: - print(f"Warning: could not parse embedded XODR: {exc}") - roads, parking = [], [] - else: - roads, parking = [], [] - - result["roads"] = roads - result["parking"] = parking + print(f"Warning: opendrive-map geometry failed: {exc}") if not self._cancelled: self.done.emit(result) except Exception as exc: @@ -785,46 +544,40 @@ def _add_traj_paths( def build_scene( scene: QtWidgets.QGraphicsScene, - roads: list[Road], - parking: list[ParkingObject], - road_map: dict[str, Road], + lane_polys: list[tuple[int, list[tuple[float, float]]]], + centerlines: list[list[tuple[float, float]]], + parking_polys: list[list[tuple[float, float]]], trajectories: dict[int, dict], obj_colors: dict[int, QtGui.QColor], path_alpha: int = 255, ) -> dict[int, QtWidgets.QGraphicsPathItem]: no_pen = QtGui.QPen(QtCore.Qt.NoPen if hasattr(QtCore.Qt, "NoPen") else QtCore.Qt.PenStyle.NoPen) - # 1. Lane polygons - for road in roads: - for lane_id, poly in build_lane_polygons(road): - if lane_id < 0: - brush = QtGui.QBrush(QtGui.QColor(0, 180, 0, 80)) - else: - brush = QtGui.QBrush(QtGui.QColor(0, 0, 200, 80)) - scene.addPolygon(_qpoly(poly), no_pen, brush) + # 1. Lane polygons (absolute UTM, from opendrive-map) + for lane_id, poly in lane_polys: + if lane_id < 0: + brush = QtGui.QBrush(QtGui.QColor(0, 180, 0, 80)) + else: + brush = QtGui.QBrush(QtGui.QColor(0, 0, 200, 80)) + scene.addPolygon(_qpoly(poly), no_pen, brush) - # 2. Centerlines + # 2. Centerlines (road reference lines, absolute UTM) center_pen = QtGui.QPen(QtGui.QColor(0, 0, 0)) center_pen.setWidthF(0.15) - for road in roads: - samples = _sample_road(road) + for samples in centerlines: if len(samples) < 2: continue path = QtGui.QPainterPath() - path.moveTo(samples[0][1], -samples[0][2]) - for _, x, y, _ in samples[1:]: + path.moveTo(samples[0][0], -samples[0][1]) + for x, y in samples[1:]: path.lineTo(x, -y) scene.addPath(path, center_pen) - # 3. Parking + # 3. Parking (absolute UTM, from opendrive-map) park_brush = QtGui.QBrush(QtGui.QColor(128, 128, 128, 100)) park_pen = QtGui.QPen(QtGui.QColor(64, 64, 64)) park_pen.setWidthF(0.1) - for obj in parking: - road = road_map.get(obj.road_id) - if road is None: - continue - rect_pts = build_parking_rect(obj, road) + for rect_pts in parking_polys: if rect_pts: scene.addPolygon(_qpoly(rect_pts), park_pen, park_brush) @@ -1106,7 +859,7 @@ def _load_slot(self, path: str) -> tuple[str | None, dict[int, dict]]: raise ValueError(f"Unsupported file type: {path}") def _fit(self): - self.view.fitInView(self.scene.sceneRect(), _keep_aspect_ratio()) + self.view.fitInView(self.scene.itemsBoundingRect(), _keep_aspect_ratio()) def _wheel_zoom(self, event): try: @@ -1134,6 +887,7 @@ def _reload(self): self._play_timer.stop() self._btn_play.setText("▶") self.scene.clear() + self.scene.setSceneRect(QtCore.QRectF()) # let the rect recompute from new items self._traj_items_a = {} self._traj_items_b = {} self._traj_items_c = {} @@ -1218,7 +972,13 @@ def _on_load_failed(self, msg: str) -> None: except Exception: pass self._progress_dlg.close() + # Show the error on a clean scene and frame it, so it is visible regardless of + # the previous view transform and does not pollute the next load's scene rect. + self.scene.clear() + self.scene.setSceneRect(QtCore.QRectF()) self.scene.addText(f"Error loading: {msg}") + self.scene.setSceneRect(self.scene.itemsBoundingRect()) + self.view.fitInView(self.scene.itemsBoundingRect(), _keep_aspect_ratio()) def _on_load_done(self, result: dict) -> None: self._trajs_a = result["trajs_a"] @@ -1226,7 +986,8 @@ def _on_load_done(self, result: dict) -> None: self._trajs_c = result["trajs_c"] if hasattr(self, "_load_label"): self._load_label.setText("Building scene…") - self._rebuild_ui_and_scene(result["roads"], result["parking"]) + self._rebuild_ui_and_scene(result["lane_polys"], result["centerlines"], + result["parking_polys"]) if hasattr(self, "_progress_dlg"): try: self._progress_dlg.finished.disconnect(self._on_load_cancel) @@ -1234,7 +995,7 @@ def _on_load_done(self, result: dict) -> None: pass self._progress_dlg.close() - def _rebuild_ui_and_scene(self, roads, parking) -> None: + def _rebuild_ui_and_scene(self, lane_polys, centerlines, parking_polys) -> None: all_ids = set(self._trajs_a) | set(self._trajs_b) | set(self._trajs_c) self._obj_colors = _build_obj_colors(all_ids) self._build_type_subtype_objs() @@ -1253,10 +1014,9 @@ def _rebuild_ui_and_scene(self, roads, parking) -> None: self._all_nanos = _build_full_timeline(nanos_set) path_alpha = 80 if self._all_nanos else 255 - road_map = {r.id: r for r in roads} self._traj_items_a = build_scene( - self.scene, roads, parking, road_map, + self.scene, lane_polys, centerlines, parking_polys, self._trajs_a, self._obj_colors, path_alpha=path_alpha, ) if self._trajs_b: @@ -1282,6 +1042,10 @@ def _rebuild_ui_and_scene(self, roads, parking) -> None: self._slider.setValue(0) self._update_frame(0) + # Reset the scene rect to the current items: QGraphicsScene.sceneRect() only + # grows (never shrinks), so after a clear+reload a stale rect would make + # fitInView zoom past the new content and show nothing. + self.scene.setSceneRect(self.scene.itemsBoundingRect()) self.view.fitInView(self.scene.sceneRect(), _keep_aspect_ratio()) def _build_type_subtype_objs(self) -> None: diff --git a/uv.lock b/uv.lock index 5242f18..82a8078 100644 --- a/uv.lock +++ b/uv.lock @@ -463,13 +463,15 @@ wheels = [ [[package]] name = "cosmo" -version = "0.4.1" +version = "0.5.0" source = { editable = "." } dependencies = [ { name = "dataprov" }, { name = "matplotlib" }, { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, { name = "numpy", version = "2.4.4", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, + { name = "opendrive-map" }, + { name = "orbit-georef" }, { name = "pandas", version = "2.3.3", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, { name = "pandas", version = "3.0.2", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, { name = "pillow" }, @@ -530,6 +532,8 @@ requires-dist = [ { name = "dataprov" }, { name = "matplotlib" }, { name = "numpy" }, + { name = "opendrive-map", git = "https://github.com/RI-SE/ORBIT.git?subdirectory=opendrive-map&tag=opendrive-map-v0.1.0" }, + { name = "orbit-georef", git = "https://github.com/RI-SE/ORBIT.git?subdirectory=orbit-georef&tag=orbit-georef-v0.1.2" }, { name = "pandas" }, { name = "pillow" }, { name = "pycountry" }, @@ -1954,6 +1958,29 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/8f/43/8e03427e0b8ca8db3af3cb2df5cdf079c1fa43b47b3765de7d27a4fde087/omega_prime-0.3.0-py3-none-any.whl", hash = "sha256:f003f452c734cd9c1cd7573d48772ad7ed546f88570e14a517fcac4ffa897f9d", size = 60612, upload-time = "2026-03-20T10:03:14.63Z" }, ] +[[package]] +name = "opendrive-map" +version = "0.1.0" +source = { git = "https://github.com/RI-SE/ORBIT.git?subdirectory=opendrive-map&tag=opendrive-map-v0.1.0#5e5b0d1ba63d6553bc787fe3b0ecfbcd6c4a70cd" } +dependencies = [ + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "numpy", version = "2.4.4", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, + { name = "pyproj", version = "3.7.1", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "pyproj", version = "3.7.2", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, + { name = "shapely" }, +] + +[[package]] +name = "orbit-georef" +version = "0.1.2" +source = { git = "https://github.com/RI-SE/ORBIT.git?subdirectory=orbit-georef&tag=orbit-georef-v0.1.2#13007cc51bd002c55fd60352d1ad54130fb2fcba" } +dependencies = [ + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "numpy", version = "2.4.4", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, + { name = "pyproj", version = "3.7.1", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "pyproj", version = "3.7.2", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, +] + [[package]] name = "packaging" version = "26.0"