From 9907c1cf50f6e8f18d50d78da2c58175114bcfe0 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Mon, 13 Jul 2026 11:54:25 +0900 Subject: [PATCH 01/13] Add OnePlanner as an alternative driver container Wire the OnePlanner E2E planner (tier4/OnePlanner #112) as a wizard driver option: `driver=oneplanner` swaps in the `oneplanner:local` container in place of the default vavam/alpamayo driver services. - New `wizard/configs/driver/oneplanner.yaml` overrides `services.driver` with the OnePlanner image, mounts the shared USDZ scene from the splatsim renderer, and bind-mounts the E2E planner checkpoint read-only at `/opt/oneplanner/ckpts/e2e_planner.pth`. - Add `defines.oneplanner_ckpt` to `base_config.yaml`, defaulting to the local Downloads copy. Override with e.g. `defines.oneplanner_ckpt=/path/to.ckpt`. Not yet verified end-to-end; also blocked on the host CUDA driver being too old for the OnePlanner CUDA 13 base image (currently falls back to CPU). Co-Authored-By: Claude Opus 4.7 --- src/wizard/configs/base_config.yaml | 4 ++ src/wizard/configs/driver/oneplanner.yaml | 62 +++++++++++++++++++++++ 2 files changed, 66 insertions(+) create mode 100644 src/wizard/configs/driver/oneplanner.yaml diff --git a/src/wizard/configs/base_config.yaml b/src/wizard/configs/base_config.yaml index 6295cce3..99ddb2ad 100644 --- a/src/wizard/configs/base_config.yaml +++ b/src/wizard/configs/base_config.yaml @@ -36,6 +36,10 @@ defines: sensordata: "${defines.filesystem}/nre-artifacts" trafficsim_map_cache: "${defines.filesystem}/trafficsim/unified_data_cache" splatsim_scene_usdz: "${defines.filesystem}/splatsim/scene.usdz" + # OnePlanner driver checkpoint (see driver/oneplanner.yaml). Points at the + # E2E planner .ckpt on the host; mounted read-only at + # /opt/oneplanner/ckpts/e2e_planner.pth inside the container. + oneplanner_ckpt: "${oc.env:HOME}/Downloads/oneplanner_phase2_gridbev_last.ckpt" renderer_entrypoint: "/app/internal/scripts/pycena/runtime/pycena_nrm_full" nre_max_workers: 4 diff --git a/src/wizard/configs/driver/oneplanner.yaml b/src/wizard/configs/driver/oneplanner.yaml new file mode 100644 index 00000000..6406abc0 --- /dev/null +++ b/src/wizard/configs/driver/oneplanner.yaml @@ -0,0 +1,62 @@ +# @package _global_ +# +# OnePlanner-backed driver. Runs the tier4/OnePlanner driver server +# (``oneplanner.deployment.driver_server``) which speaks the alpasim +# ``EgoDriver`` gRPC interface directly. See +# https://github.com/tier4/OnePlanner/pull/112. +# +# Unlike vavam/alpamayo which configure alpasim's own driver service via the +# ``driver:`` block, this file overrides ``services.driver`` wholesale to +# point at the OnePlanner container. Pattern mirrors ``renderer/splatsim.yaml``. +# +# The image is expected to be pre-built locally (``docker build -t +# oneplanner:local .`` inside a checkout of tier4/OnePlanner). At the time of +# writing PR #112 has no published ghcr.io tag. +# +# Usage: +# uv run alpasim_wizard ... \ +# driver=oneplanner renderer=splatsim \ +# defines.oneplanner_ckpt=/path/to/e2e_planner.ckpt + +# Presence marker so ``setup_omegaconf._is_missing_or_empty(cfg, "driver")`` +# doesn't reject the run. The alpasim driver service is fully replaced by the +# OnePlanner container defined below, so the fields under ``driver:`` are not +# consumed by anything at runtime. +driver: + model: + model_type: oneplanner + +services: + driver: + image: oneplanner:local + external_image: true + # Disable GPU reservation for the driver container: see the environment + # comment below on the CUDA 13.0 vs. host-driver 550 mismatch. + gpus: [] + volumes: + # Checkpoint: image's default CMD reads /opt/oneplanner/ckpts/e2e_planner.pth, + # so bind-mount the host .ckpt straight onto that path. + - "${defines.oneplanner_ckpt}:/opt/oneplanner/ckpts/e2e_planner.pth" + # Scene USDZ (shared with the splatsim renderer). driver_server.py + # requires --scene-usdz to build its HD-map tensors from the embedded + # lanelet2 asset. + - "${defines.splatsim_scene_usdz}:/mnt/oneplanner/scene.usdz" + - "${wizard.log_dir}:/mnt/output" + environments: + - PYTHONUNBUFFERED=1 + # OnePlanner's image is built on nvidia/cuda:13.0.0, but the host driver + # this branch was validated on (550.163.01) tops out at CUDA 12.4. That + # mismatch raises "Error 804: forward compatibility was attempted on non + # supported HW" the moment torch touches the GPU. Force CPU inference + # for now — GPU can be re-enabled once the host driver is upgraded to + # 570+ or OnePlanner's Dockerfile is rebased onto a CUDA 12.4 image. + - NVIDIA_VISIBLE_DEVICES= + - CUDA_VISIBLE_DEVICES= + command: + - "python -m oneplanner.deployment.driver_server" + - "--planner-ckpt=/opt/oneplanner/ckpts/e2e_planner.pth" + - "--scene-usdz=/mnt/oneplanner/scene.usdz" + - "--host=0.0.0.0" + - "--port={port}" + - "--device=cpu" + - "--log-level=${wizard.log_level}" From 852cbe1be2c5e40e0758b0b1e1fa87809e9f4e0d Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Tue, 14 Jul 2026 10:54:13 +0900 Subject: [PATCH 02/13] WIP: LiDAR scaffold + splatsim/oneplanner tweaks MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Snapshot before branching to tier4-dev to fix physics/CARLA tick sync. Do not merge as-is — will be split into proper commits. Co-Authored-By: Claude Opus 4.7 --- src/runtime/alpasim_runtime/config.py | 19 ++ src/runtime/alpasim_runtime/event_loop.py | 25 ++- src/runtime/alpasim_runtime/events/base.py | 2 + src/runtime/alpasim_runtime/events/lidar.py | 104 ++++++++++ .../services/sensorsim_service.py | 16 ++ .../alpasim_runtime/services/service_base.py | 10 +- src/runtime/alpasim_runtime/types.py | 52 ++++- .../alpasim_runtime/unbound_rollout.py | 4 + src/runtime/tests/test_event_loop.py | 1 + .../alpasim_splatsim_renderer/server.py | 181 +++++++++++++++++- src/wizard/configs/driver/oneplanner.yaml | 27 +-- src/wizard/configs/lidars/1lidar_pandar.yaml | 10 + src/wizard/configs/renderer/splatsim.yaml | 2 + 13 files changed, 432 insertions(+), 21 deletions(-) create mode 100644 src/runtime/alpasim_runtime/events/lidar.py create mode 100644 src/wizard/configs/lidars/1lidar_pandar.yaml diff --git a/src/runtime/alpasim_runtime/config.py b/src/runtime/alpasim_runtime/config.py index 00d342c2..94daec8f 100644 --- a/src/runtime/alpasim_runtime/config.py +++ b/src/runtime/alpasim_runtime/config.py @@ -175,6 +175,21 @@ class RuntimeCameraConfig: shutter_duration_us: int = 17_000 +@dataclass +class RuntimeLidarConfig: + """Configuration for a LiDAR in the runtime. See `RuntimeLidar` for more details. + + ``device_type`` names one of the sensorsim ``LidarDeviceType`` enum members + (e.g. ``"PANDAR128"`` or ``"AT128"``). It is stored as a string so the + config file stays free of gRPC-generated enum imports; the runtime maps it + to the enum when the ``RuntimeLidar`` is built. + """ + + logical_id: str = "lidar_top" + device_type: str = "PANDAR128" + frame_interval_us: int = 100_000 # 10 Hz + + @dataclass class PoseConfig: translation_m: tuple[float, float, float] @@ -300,6 +315,10 @@ class SimulationConfig: default_factory=lambda: [RuntimeCameraConfig()] ) + # LiDARs are opt-in — drivers that need point clouds (e.g. OnePlanner) + # populate this list; camera-only drivers leave it empty. + lidars: list[RuntimeLidarConfig] = field(default_factory=list) + # if None, the data will be pulled from the .usdz file vehicle: VehicleConfig | None = None diff --git a/src/runtime/alpasim_runtime/event_loop.py b/src/runtime/alpasim_runtime/event_loop.py index 13eebb64..54402536 100644 --- a/src/runtime/alpasim_runtime/event_loop.py +++ b/src/runtime/alpasim_runtime/event_loop.py @@ -51,7 +51,8 @@ ) from alpasim_runtime.services.traffic_service import TrafficService from alpasim_runtime.telemetry.telemetry_context import try_get_context -from alpasim_runtime.types import RuntimeCamera +from alpasim_runtime.events.lidar import make_initial_lidar_render_events +from alpasim_runtime.types import RuntimeCamera, RuntimeLidar from alpasim_runtime.unbound_rollout import UnboundRollout from alpasim_utils import geometry from alpasim_utils.logs import LogWriter @@ -117,6 +118,7 @@ class EventBasedRollout: planner_delay_buffer: DelayBuffer = field(init=False) route_generator: RouteGenerator | None = field(init=False) runtime_cameras: list[RuntimeCamera] = field(init=False, default_factory=list) + runtime_lidars: list[RuntimeLidar] = field(init=False, default_factory=list) _runtime_evaluator: RuntimeEvaluator = field(init=False) @@ -377,6 +379,16 @@ def _create_initial_events(self) -> EventQueue: else: queue.submit(render_events) + for lidar_event in make_initial_lidar_render_events( + scene_start_us=scene_start_us, + simulation_end_us=simulation_end_us, + runtime_lidars=list(self.runtime_lidars), + renderer_service=self.renderer_service, + driver=self.driver, + broadcaster=self.broadcaster, + ): + queue.submit(lidar_event) + # === Pipeline events — all start at first_policy_timestamp_us === dt = unbound.control_timestep_us @@ -465,6 +477,17 @@ async def run(self) -> ScenarioEvalResult | None: for camera_cfg in self.unbound.camera_configs ] + # LiDAR sweeps use the render start (== first camera shutter close) + # as their initial tick so cameras and LiDAR arrive at the driver + # in the same simulated millisecond. + self.runtime_lidars = [ + RuntimeLidar.from_lidar_config( + lidar_cfg, + first_frame_end_us=self.unbound.render_start_timestamp_us, + ) + for lidar_cfg in self.unbound.lidar_configs + ] + # Enter the renderer's session: owns scene-specific camera # registration and any session-bootstrap work. The video model # opens a remote session with hdmap + initial frames here. diff --git a/src/runtime/alpasim_runtime/events/base.py b/src/runtime/alpasim_runtime/events/base.py index 26a7266e..9fa5f229 100644 --- a/src/runtime/alpasim_runtime/events/base.py +++ b/src/runtime/alpasim_runtime/events/base.py @@ -29,6 +29,7 @@ class EventPriority: === ================== ========================================== 10 CameraFrameEvent Render/register camera frames 11 CameraRenderFlushEvent Render grouped camera frames + 12 LidarFrameEvent Render/submit LiDAR point clouds 20 PolicyEvent Gather observations, query driver 30 SimulationEndEvent Terminate loop (final timestamp only) 40 ControllerEvent Run controller + vehicle model @@ -41,6 +42,7 @@ class EventPriority: CAMERA = 10 CAMERA_FLUSH = 11 + LIDAR = 12 POLICY = 20 SIMULATION_END = 30 CONTROLLER = 40 diff --git a/src/runtime/alpasim_runtime/events/lidar.py b/src/runtime/alpasim_runtime/events/lidar.py new file mode 100644 index 00000000..7c2fa7bc --- /dev/null +++ b/src/runtime/alpasim_runtime/events/lidar.py @@ -0,0 +1,104 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 NVIDIA Corporation + +"""LiDAR render events for the event-based simulation loop.""" + +from __future__ import annotations + +import logging +from typing import Any + +from alpasim_runtime.broadcaster import MessageBroadcaster +from alpasim_runtime.events.base import Event, EventPriority, EventQueue +from alpasim_runtime.events.camera import _traffic_trajectories +from alpasim_runtime.events.state import RolloutState +from alpasim_runtime.services.driver_service import DriverService +from alpasim_runtime.services.sensorsim_service import SensorsimService +from alpasim_runtime.types import Clock, RuntimeLidar + +logger = logging.getLogger(__name__) + + +class LidarFrameEvent(Event): + """Render one LiDAR sweep and submit the point cloud to the driver. + + LiDAR is treated as instantaneous (``duration_us=0``): the trigger's + time range collapses to a single simulated timestamp equal to the sweep + time. Bundling is not implemented here — each LiDAR fires its own + ``render_lidar`` RPC; when we adopt ``aggregated_render`` we will bundle + LiDAR triggers alongside camera triggers on the flush event. + """ + + priority: int = EventPriority.LIDAR + + def __init__( + self, + lidar: RuntimeLidar, + trigger: Clock.Trigger, + sensorsim: SensorsimService, + driver: DriverService, + ): + super().__init__(timestamp_us=trigger.time_range_us.stop) + self.lidar = lidar + self.trigger = trigger + self.sensorsim = sensorsim + self.driver = driver + + def description(self) -> str: + return ( + f"LidarFrameEvent({self.lidar.logical_id}, " + f"{self.trigger.time_range_us.stop:_}us)" + ) + + async def handle(self, rollout_state: RolloutState, queue: EventQueue) -> None: + assert ( + rollout_state.step_context is not None + ), "StepContext must exist before render" + point_cloud = await self.sensorsim.render_lidar( + ego_trajectory=rollout_state.ego_trajectory, + traffic_trajectories=_traffic_trajectories(rollout_state), + lidar_logical_id=self.lidar.logical_id, + lidar_type=self.lidar.device_type, + sensor_pose_delta=None, + trigger=self.trigger, + scene_id=rollout_state.unbound.scene_id, + ) + rollout_state.step_context.track_task(self.driver.submit_lidar(point_cloud)) + self._schedule_next(rollout_state, queue) + + def _schedule_next(self, state: RolloutState, queue: EventQueue) -> None: + next_trigger = self.lidar.clock.ith_trigger(self.trigger.sequential_idx + 1) + if next_trigger.time_range_us.stop > state.unbound.end_timestamp_us: + return + queue.submit( + LidarFrameEvent( + lidar=self.lidar, + trigger=next_trigger, + sensorsim=self.sensorsim, + driver=self.driver, + ) + ) + + +def make_initial_lidar_render_events( + *, + scene_start_us: int, + simulation_end_us: int, + runtime_lidars: list[RuntimeLidar], + renderer_service: Any, + driver: DriverService, + broadcaster: MessageBroadcaster, +) -> list[Event]: + """Built-in factory for the first LiDAR render events of a rollout.""" + del broadcaster + return [ + LidarFrameEvent( + lidar=lidar, + trigger=trigger, + sensorsim=renderer_service, + driver=driver, + ) + for lidar in runtime_lidars + for trigger in [lidar.clock.ith_trigger(0)] + if scene_start_us <= trigger.time_range_us.stop <= simulation_end_us + ] diff --git a/src/runtime/alpasim_runtime/services/sensorsim_service.py b/src/runtime/alpasim_runtime/services/sensorsim_service.py index b607a2b3..f4540045 100644 --- a/src/runtime/alpasim_runtime/services/sensorsim_service.py +++ b/src/runtime/alpasim_runtime/services/sensorsim_service.py @@ -289,6 +289,22 @@ def trajectory_to_pose_pair( definition = self._camera_catalog.get_camera_definition( scene_id, camera.logical_id ) + # RENDER_DBG: log timestamp / trajectory range / interpolated poses to + # diagnose why splatsim renders black frames — pose_t appears static + # across a 20s rollout despite ego moving in map view. Remove once fixed. + try: + _ts_range = ego_trajectory.time_range_us + _raw_start = ego_trajectory.interpolate_pose(start_us) + _rig_to_cam_t = list(definition.rig_to_camera.vec3) + _composed = _raw_start @ definition.rig_to_camera + logger.info( + "RENDER_DBG start_us=%s end_us=%s traj_range=[%s..%s) " + "raw_ego_t=%s rig_to_cam_t=%s composed_t=%s", + start_us, end_us, _ts_range.start, _ts_range.stop, + list(_raw_start.vec3), _rig_to_cam_t, list(_composed.vec3), + ) + except Exception as _e: + logger.info("RENDER_DBG failed to log trajectory: %s", _e) sensor_pose = trajectory_to_pose_pair( ego_trajectory, delta=definition.rig_to_camera, diff --git a/src/runtime/alpasim_runtime/services/service_base.py b/src/runtime/alpasim_runtime/services/service_base.py index 19ef1862..0c6be662 100644 --- a/src/runtime/alpasim_runtime/services/service_base.py +++ b/src/runtime/alpasim_runtime/services/service_base.py @@ -68,7 +68,15 @@ def name(self) -> str: async def _open_connection(self) -> None: """Open gRPC connection.""" if not self.skip: - self.channel = grpc.aio.insecure_channel(self.address) + # LiDAR point clouds (PANDAR128 sweeps are ~4.5 MB) already exceed + # gRPC's 4 MiB default. Match the ceiling used elsewhere in the + # runtime (video_model_service.MAX_GRPC_MESSAGE_BYTES) so all + # sensor payloads fit. + options = [ + ("grpc.max_receive_message_length", 64 * 1024 * 1024), + ("grpc.max_send_message_length", 64 * 1024 * 1024), + ] + self.channel = grpc.aio.insecure_channel(self.address, options=options) self.stub = self.stub_class(self.channel) async def _close_connection(self) -> None: diff --git a/src/runtime/alpasim_runtime/types.py b/src/runtime/alpasim_runtime/types.py index 739df14a..0deda4e3 100644 --- a/src/runtime/alpasim_runtime/types.py +++ b/src/runtime/alpasim_runtime/types.py @@ -5,7 +5,9 @@ from dataclasses import dataclass -from alpasim_runtime.config import RuntimeCameraConfig +from alpasim_grpc.v0.sensorsim_pb2 import LidarDeviceType + +from alpasim_runtime.config import RuntimeCameraConfig, RuntimeLidarConfig @dataclass @@ -92,3 +94,51 @@ def from_camera_config( render_resolution_hw=(camera_cfg.height, camera_cfg.width), clock=clock, ) + + +@dataclass +class RuntimeLidar: + """This class defines which LiDAR sensors are rendered and how to render them. + + - ``logical_id`` uniquely identifies the sensor; the driver receives point + clouds keyed by this id. + - ``device_type`` picks the physical model (e.g. ``PANDAR128``) that the + renderer applies. + - ``clock`` determines the timing of point-cloud emission. LiDAR is treated + as instantaneous (``duration_us=0``) — the trigger's ``time_range_us`` + collapses to a single timestamp. + """ + + logical_id: str + device_type: LidarDeviceType + clock: Clock + + @classmethod + def from_lidar_config( + cls, lidar_cfg: RuntimeLidarConfig, first_frame_end_us: int + ) -> RuntimeLidar: + """Build a ``RuntimeLidar`` from a scenario ``RuntimeLidarConfig``. + + ``first_frame_end_us`` anchors the first LiDAR sweep so the initial + cloud lines up with the first camera frame's shutter-close (this keeps + camera + LiDAR arriving at the driver in the same simulated + millisecond). + """ + try: + device_type = LidarDeviceType.Value(lidar_cfg.device_type) + except ValueError as exc: + raise ValueError( + f"Unknown LidarDeviceType {lidar_cfg.device_type!r} for lidar " + f"{lidar_cfg.logical_id!r}" + ) from exc + clock = Clock( + interval_us=lidar_cfg.frame_interval_us, + duration_us=0, + start_us=first_frame_end_us, + first_end_us=first_frame_end_us, + ) + return cls( + logical_id=lidar_cfg.logical_id, + device_type=device_type, + clock=clock, + ) diff --git a/src/runtime/alpasim_runtime/unbound_rollout.py b/src/runtime/alpasim_runtime/unbound_rollout.py index fea0f66d..0c38ee08 100644 --- a/src/runtime/alpasim_runtime/unbound_rollout.py +++ b/src/runtime/alpasim_runtime/unbound_rollout.py @@ -17,6 +17,7 @@ RenderBundling, RouteGeneratorType, RuntimeCameraConfig, + RuntimeLidarConfig, SimulationConfig, VehicleConfig, ) @@ -176,6 +177,7 @@ class UnboundRollout: pose_reporting_interval_us: int camera_configs: list[RuntimeCameraConfig] first_camera_frame_ranges_us: dict[str, range] + lidar_configs: list[RuntimeLidarConfig] force_gt_period: range image_format: ImageFormat ego_mask_rig_config_id: str @@ -212,6 +214,7 @@ def create( ) -> UnboundRollout: """Create UnboundRollout from SceneDataSource.""" camera_configs = list(simulation_config.cameras) + lidar_configs = list(simulation_config.lidars) renderer_service.validate_timing_alignment(simulation_config) timing = _build_rollout_timing( simulation_config, @@ -293,6 +296,7 @@ def create( version_ids=version_ids, camera_configs=camera_configs, first_camera_frame_ranges_us=timing.first_camera_frame_ranges_us, + lidar_configs=lidar_configs, force_gt_period=force_gt_period, physics_update_mode=simulation_config.physics_update_mode, image_format={"jpeg": ImageFormat.JPEG, "png": ImageFormat.PNG}[ diff --git a/src/runtime/tests/test_event_loop.py b/src/runtime/tests/test_event_loop.py index 29bdf1a9..aa0d0e21 100644 --- a/src/runtime/tests/test_event_loop.py +++ b/src/runtime/tests/test_event_loop.py @@ -28,6 +28,7 @@ def test_initial_event_schedule_uses_policy_and_end_timestamps() -> None: send_recording_ground_truth=False, ) rollout.runtime_cameras = [] + rollout.runtime_lidars = [] rollout.driver = MagicMock() rollout.controller = MagicMock() rollout.physics = MagicMock() diff --git a/src/splatsim_renderer/alpasim_splatsim_renderer/server.py b/src/splatsim_renderer/alpasim_splatsim_renderer/server.py index 306b7118..3997675d 100644 --- a/src/splatsim_renderer/alpasim_splatsim_renderer/server.py +++ b/src/splatsim_renderer/alpasim_splatsim_renderer/server.py @@ -26,6 +26,7 @@ from . import __version__ as renderer_version from .lidar_adapter import render_lidar_panorama_from_scene from .render_adapter import ( + CameraIntrinsics, camera_spec_to_intrinsics, encode_image, pose_pair_to_viewmat, @@ -36,14 +37,130 @@ logger = logging.getLogger(__name__) +def _build_available_cameras_from_usdz( + usdz_path: Optional[Path], +) -> list["sensorsim_pb2.AvailableCamerasReturn.AvailableCamera"]: + """Read camera_calibrations from the USDZ rig_trajectories.json. + + Returns AvailableCamera entries with the ``T_sensor_rig`` extrinsics and + pinhole intrinsics from the USDZ. Non-fatal on any parse failure — an empty + list falls back to the original "no catalog" behavior. + """ + if usdz_path is None: + return [] + + import json + import zipfile + import numpy as np + + try: + with zipfile.ZipFile(str(usdz_path)) as zf: + with zf.open("rig_trajectories.json") as f: + doc = json.load(f) + except (KeyError, zipfile.BadZipFile, json.JSONDecodeError, OSError) as exc: + logger.warning("Could not read camera_calibrations from %s: %s", usdz_path, exc) + return [] + + calibrations = doc.get("camera_calibrations") or {} + if not isinstance(calibrations, dict): + return [] + + cameras: list = [] + for logical_id, cam in calibrations.items(): + try: + model = cam["camera_model"] + if model.get("type") != "pinhole": + logger.warning( + "Skipping camera %s: unsupported camera_model type %r", + logical_id, + model.get("type"), + ) + continue + params = model["parameters"] + width, height = params["resolution"] + fx, fy, cx, cy = params["fx"], params["fy"], params["cx"], params["cy"] + + T = np.asarray(cam["T_sensor_rig"], dtype=np.float64) + R = T[:3, :3] + t = T[:3, 3] + # Matrix -> quaternion (xyzw). Uses Shepperd's method to avoid + # scipy dependency here. + trace = R[0, 0] + R[1, 1] + R[2, 2] + if trace > 0.0: + s = np.sqrt(trace + 1.0) * 2.0 + qw = 0.25 * s + qx = (R[2, 1] - R[1, 2]) / s + qy = (R[0, 2] - R[2, 0]) / s + qz = (R[1, 0] - R[0, 1]) / s + elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]: + s = np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2.0 + qw = (R[2, 1] - R[1, 2]) / s + qx = 0.25 * s + qy = (R[0, 1] + R[1, 0]) / s + qz = (R[0, 2] + R[2, 0]) / s + elif R[1, 1] > R[2, 2]: + s = np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2.0 + qw = (R[0, 2] - R[2, 0]) / s + qx = (R[0, 1] + R[1, 0]) / s + qy = 0.25 * s + qz = (R[1, 2] + R[2, 1]) / s + else: + s = np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2.0 + qw = (R[1, 0] - R[0, 1]) / s + qx = (R[0, 2] + R[2, 0]) / s + qy = (R[1, 2] + R[2, 1]) / s + qz = 0.25 * s + + entry = sensorsim_pb2.AvailableCamerasReturn.AvailableCamera( + logical_id=logical_id, + intrinsics=sensorsim_pb2.CameraSpec( + logical_id=logical_id, + resolution_w=int(width), + resolution_h=int(height), + opencv_pinhole_param=sensorsim_pb2.OpenCVPinholeCameraParam( + focal_length_x=float(fx), + focal_length_y=float(fy), + principal_point_x=float(cx), + principal_point_y=float(cy), + ), + ), + rig_to_camera=common_pb2.Pose( + vec=common_pb2.Vec3(x=float(t[0]), y=float(t[1]), z=float(t[2])), + quat=common_pb2.Quat( + w=float(qw), + x=float(qx), + y=float(qy), + z=float(qz), + ), + ), + ) + cameras.append(entry) + except (KeyError, ValueError, TypeError) as exc: + logger.warning("Could not parse camera %s: %s", logical_id, exc) + continue + + logger.info( + "Loaded %d camera(s) from USDZ rig_trajectories.json: %s", + len(cameras), + [c.logical_id for c in cameras], + ) + return cameras + + class SplatsimSensorsimServicer(sensorsim_pb2_grpc.SensorsimServiceServicer): """Splatsim-backed implementation of SensorsimService.""" - def __init__(self, scene: SceneHandle, scene_id: str) -> None: + def __init__( + self, + scene: SceneHandle, + scene_id: str, + usdz_path: Optional[Path] = None, + ) -> None: self._scene = scene # One container == one scene for now. We accept any scene_id but log # mismatches so misconfigured Runtime requests are visible. self._scene_id = scene_id + self._available_cameras = _build_available_cameras_from_usdz(usdz_path) # ----- rendering (internal, raise on error) ----- @@ -57,11 +174,59 @@ def _do_render_rgb( self._scene_id, ) intrinsics = camera_spec_to_intrinsics(request.camera_intrinsics) + # The K matrix on the wire is calibrated for `intrinsics.width x .height` + # (the sensor's native resolution, e.g. 2880x1860 for CAM_FRONT_WIDE), + # but this Renderer's canvas is fixed at `self._scene.default_resolution` + # (e.g. 960x540). Feeding the unscaled K to gsplat produces principal + # points outside the small canvas, so every Gaussian projects off-screen + # and the image comes back all zeros. Scale K to the canvas. + canvas_w, canvas_h = self._scene.default_resolution + scale_x = canvas_w / float(intrinsics.width) + scale_y = canvas_h / float(intrinsics.height) + intrinsics = CameraIntrinsics( + width=canvas_w, + height=canvas_h, + fx=intrinsics.fx * scale_x, + fy=intrinsics.fy * scale_y, + cx=intrinsics.cx * scale_x, + cy=intrinsics.cy * scale_y, + ) + # DEBUG: log incoming pose + scene stats to diagnose black-frame issue. + import numpy as _np + _sp = request.sensor_pose.start_pose + _sv = _np.array([_sp.vec.x, _sp.vec.y, _sp.vec.z], dtype=_np.float64) + _sq = _np.array([_sp.quat.x, _sp.quat.y, _sp.quat.z, _sp.quat.w], dtype=_np.float64) + _tlc = _np.asarray(self._scene.tile_local_centroid, dtype=_np.float64) + try: + _means = self._scene._bg.means.detach().cpu().numpy() + _bbox_min = _means.min(axis=0) + _bbox_max = _means.max(axis=0) + except Exception: + _bbox_min = _bbox_max = None + logger.info( + "RENDER_DBG pose_t=%s pose_q_xyzw=%s tile_local_centroid=%s bbox=[%s..%s] " + "K_scaled=fx=%.2f fy=%.2f cx=%.2f cy=%.2f canvas=%dx%d", + _sv.tolist(), _sq.tolist(), _tlc.tolist(), + None if _bbox_min is None else _bbox_min.tolist(), + None if _bbox_max is None else _bbox_max.tolist(), + intrinsics.fx, intrinsics.fy, intrinsics.cx, intrinsics.cy, + canvas_w, canvas_h, + ) viewmat = pose_pair_to_viewmat( request.sensor_pose, world_origin=self._scene.tile_local_centroid, ) + logger.info("RENDER_DBG viewmat=\n%s", _np.asarray(viewmat).tolist()) rgb = self._scene.render(viewmat, intrinsics.k_matrix()) + try: + _rgb_np = _np.asarray(rgb) + logger.info( + "RENDER_DBG rgb shape=%s dtype=%s min=%s max=%s mean=%s", + _rgb_np.shape, str(_rgb_np.dtype), + float(_rgb_np.min()), float(_rgb_np.max()), float(_rgb_np.mean()), + ) + except Exception as _e: + logger.info("RENDER_DBG rgb stats failed: %s", _e) image_bytes = encode_image(rgb, request.image_format, request.image_quality) return sensorsim_pb2.RGBRenderReturn(image_bytes=image_bytes) @@ -170,10 +335,12 @@ def get_available_scenes(self, request: common_pb2.Empty, context): def get_available_cameras( self, request: sensorsim_pb2.AvailableCamerasRequest, context ): - # No catalog in NOP mode — Runtime is expected to supply intrinsics in - # render requests. Returning an empty list is the documented "unknown" - # answer. - return sensorsim_pb2.AvailableCamerasReturn() + # Runtime's CameraCatalog requires local overrides to be a subset of + # what sensorsim reports. Advertise the USDZ rig's cameras so the + # local extra_cameras merge check passes. + return sensorsim_pb2.AvailableCamerasReturn( + available_cameras=list(self._available_cameras) + ) def get_available_trajectories( self, request: sensorsim_pb2.AvailableTrajectoriesRequest, context @@ -221,7 +388,9 @@ def main(argv: Optional[list[str]] = None) -> int: scene = SceneHandle( usdz_path=usdz_path, default_resolution=(args.width, args.height) ) - servicer = SplatsimSensorsimServicer(scene=scene, scene_id=args.scene_id) + servicer = SplatsimSensorsimServicer( + scene=scene, scene_id=args.scene_id, usdz_path=usdz_path + ) server = grpc.server(futures.ThreadPoolExecutor(max_workers=args.max_workers)) sensorsim_pb2_grpc.add_SensorsimServiceServicer_to_server(servicer, server) diff --git a/src/wizard/configs/driver/oneplanner.yaml b/src/wizard/configs/driver/oneplanner.yaml index 6406abc0..f60dddc9 100644 --- a/src/wizard/configs/driver/oneplanner.yaml +++ b/src/wizard/configs/driver/oneplanner.yaml @@ -26,13 +26,20 @@ driver: model: model_type: oneplanner +# OnePlanner is a LiDAR-based E2E planner: without a point-cloud observation +# it emits ~zero-motion trajectories. Wire up one PANDAR128 sweep at 10 Hz so +# the runtime forwards point clouds into ``submit_lidar_observation``. +runtime: + simulation_config: + lidars: + - logical_id: lidar_top + device_type: PANDAR128 + frame_interval_us: 100_000 + services: driver: image: oneplanner:local external_image: true - # Disable GPU reservation for the driver container: see the environment - # comment below on the CUDA 13.0 vs. host-driver 550 mismatch. - gpus: [] volumes: # Checkpoint: image's default CMD reads /opt/oneplanner/ckpts/e2e_planner.pth, # so bind-mount the host .ckpt straight onto that path. @@ -42,21 +49,17 @@ services: # lanelet2 asset. - "${defines.splatsim_scene_usdz}:/mnt/oneplanner/scene.usdz" - "${wizard.log_dir}:/mnt/output" + # HOTFIX: cast has_speed_limit to bool before torch.where (upstream bug in + # OnePlanner where map_tensors stores has_speed_limit as float32 but the + # encoder expects a bool condition). Remove once fixed upstream. + - "/tmp/oneplanner_encoder_patched.py:/opt/oneplanner/src/oneplanner/models/planner/encoder.py" environments: - PYTHONUNBUFFERED=1 - # OnePlanner's image is built on nvidia/cuda:13.0.0, but the host driver - # this branch was validated on (550.163.01) tops out at CUDA 12.4. That - # mismatch raises "Error 804: forward compatibility was attempted on non - # supported HW" the moment torch touches the GPU. Force CPU inference - # for now — GPU can be re-enabled once the host driver is upgraded to - # 570+ or OnePlanner's Dockerfile is rebased onto a CUDA 12.4 image. - - NVIDIA_VISIBLE_DEVICES= - - CUDA_VISIBLE_DEVICES= command: - "python -m oneplanner.deployment.driver_server" - "--planner-ckpt=/opt/oneplanner/ckpts/e2e_planner.pth" - "--scene-usdz=/mnt/oneplanner/scene.usdz" - "--host=0.0.0.0" - "--port={port}" - - "--device=cpu" + - "--device=cuda" - "--log-level=${wizard.log_level}" diff --git a/src/wizard/configs/lidars/1lidar_pandar.yaml b/src/wizard/configs/lidars/1lidar_pandar.yaml new file mode 100644 index 00000000..999185ae --- /dev/null +++ b/src/wizard/configs/lidars/1lidar_pandar.yaml @@ -0,0 +1,10 @@ +# @package _global_ + +# Enable a single PANDAR128 LiDAR at 10 Hz. Required by LiDAR-based drivers +# (e.g. OnePlanner); camera-only drivers should leave `lidars` empty. +runtime: + simulation_config: + lidars: + - logical_id: lidar_top + device_type: PANDAR128 + frame_interval_us: 100_000 diff --git a/src/wizard/configs/renderer/splatsim.yaml b/src/wizard/configs/renderer/splatsim.yaml index 8100fca0..76b005a2 100644 --- a/src/wizard/configs/renderer/splatsim.yaml +++ b/src/wizard/configs/renderer/splatsim.yaml @@ -22,6 +22,8 @@ services: external_image: false volumes: - "${defines.splatsim_scene_usdz}:/mnt/splatsim/scene.usdz" + - "/tmp/splatsim_usdz_v020.py:/repo/src/splatsim_renderer/.venv/lib/python3.10/site-packages/splatsim/_usdz.py" + - "${repo-relative:'src/splatsim_renderer/alpasim_splatsim_renderer/server.py'}:/repo/src/splatsim_renderer/alpasim_splatsim_renderer/server.py" # NOTE: Unlike other services which mount ``${repo-relative:'src'}:/repo/src`` # for hot-reload against the shared base image's global venv, the splatsim # renderer uses a dedicated image with a per-project venv baked in at From c253571b1d49cab19cdebbc0af7631dfcec218a0 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Wed, 15 Jul 2026 12:38:59 +0900 Subject: [PATCH 03/13] Wire BEVFusion perception into OnePlanner driver The OnePlanner driver_server expects --bevfusion-config (YAML topology) and --bevfusion-ckpt (mmcv-format state_dict) alongside --planner-ckpt for E2E checkpoints trained with ``use_grid_sample_bev=True`` (e.g. phase2 gridbev). Without them the planner runs without ``_bev_feature_map`` and produces degraded trajectories on those weights. Add two defines to the wizard base config so the host paths flow through Hydra, and bind-mount both files into the driver container at the paths the image expects (``/opt/oneplanner/configs/perception/model.yaml`` and ``/opt/oneplanner/ckpts/bevfusion.pth``). The BEVFusion ckpt is a separate file because ``load_mmcv_checkpoint`` does not strip the ``model.bevfusion.`` Lightning-style prefix carried by combined E2E checkpoints; users extract it once with ``torch.save({'state_dict': {k.removeprefix('model.bevfusion.'): v for k, v in ckpt['state_dict'].items() if k.startswith('model.bevfusion.')}}, 'bevfusion.pth')``. Verified end-to-end: driver_server loads planner + BEVFusion cleanly against ``lidar_30e_j6gen2_120m.yaml`` + a BEVFusion-only state_dict extracted from ``oneplanner_phase2_gridbev_last.ckpt``. No shape mismatches or missing keys reported by ``weight_converter``. Co-Authored-By: Claude Opus 4.7 --- src/wizard/configs/base_config.yaml | 8 ++++++++ src/wizard/configs/driver/oneplanner.yaml | 8 ++++++++ 2 files changed, 16 insertions(+) diff --git a/src/wizard/configs/base_config.yaml b/src/wizard/configs/base_config.yaml index 99ddb2ad..47164ce2 100644 --- a/src/wizard/configs/base_config.yaml +++ b/src/wizard/configs/base_config.yaml @@ -40,6 +40,14 @@ defines: # E2E planner .ckpt on the host; mounted read-only at # /opt/oneplanner/ckpts/e2e_planner.pth inside the container. oneplanner_ckpt: "${oc.env:HOME}/Downloads/oneplanner_phase2_gridbev_last.ckpt" + # BEVFusion perception config + checkpoint (see driver/oneplanner.yaml). + # driver_server.py's ``_build_perception`` reads the YAML (mmcv BEVFusion + # topology) and the .pth (mmcv-format state_dict) to build the LiDAR + # perception front-end that feeds ``_bev_feature_map`` into the planner. + # Required for checkpoints trained with ``use_grid_sample_bev=True`` + # (e.g. ``oneplanner_phase2_gridbev_*``); optional otherwise. + oneplanner_bevfusion_config: "${oc.env:HOME}/workspace/OnePlanner/configs/perception/model/lidar_30e_j6gen2_120m.yaml" + oneplanner_bevfusion_ckpt: "${oc.env:HOME}/workspace/OnePlanner/ckpts/bevfusion_extracted.pth" renderer_entrypoint: "/app/internal/scripts/pycena/runtime/pycena_nrm_full" nre_max_workers: 4 diff --git a/src/wizard/configs/driver/oneplanner.yaml b/src/wizard/configs/driver/oneplanner.yaml index f60dddc9..50f12f41 100644 --- a/src/wizard/configs/driver/oneplanner.yaml +++ b/src/wizard/configs/driver/oneplanner.yaml @@ -48,6 +48,12 @@ services: # requires --scene-usdz to build its HD-map tensors from the embedded # lanelet2 asset. - "${defines.splatsim_scene_usdz}:/mnt/oneplanner/scene.usdz" + # BEVFusion perception config + checkpoint. The image copies scripts/ + # (train_bevfusion.py owns build_bevfusion_from_config) but not configs/, + # so both files are supplied via bind-mounts. Skip these mounts and the + # matching --bevfusion-* flags for planner-only smoke tests. + - "${defines.oneplanner_bevfusion_config}:/opt/oneplanner/configs/perception/model.yaml" + - "${defines.oneplanner_bevfusion_ckpt}:/opt/oneplanner/ckpts/bevfusion.pth" - "${wizard.log_dir}:/mnt/output" # HOTFIX: cast has_speed_limit to bool before torch.where (upstream bug in # OnePlanner where map_tensors stores has_speed_limit as float32 but the @@ -58,6 +64,8 @@ services: command: - "python -m oneplanner.deployment.driver_server" - "--planner-ckpt=/opt/oneplanner/ckpts/e2e_planner.pth" + - "--bevfusion-config=/opt/oneplanner/configs/perception/model.yaml" + - "--bevfusion-ckpt=/opt/oneplanner/ckpts/bevfusion.pth" - "--scene-usdz=/mnt/oneplanner/scene.usdz" - "--host=0.0.0.0" - "--port={port}" From a2762a19b6bfc7826b61f331b19c7804d42968f7 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Wed, 15 Jul 2026 16:19:59 +0900 Subject: [PATCH 04/13] Guard sensorsim/traffic services against edge-case scenarios Two small fixes surfaced while wiring the OnePlanner driver against a scene whose id (`odaibatest_alpasim`) is neither a clipgt UUID nor an XODR-derived identifier: - TrafficService: skip the traffic-session bootstrap (which regexes the scene id for a map UUID) when the trafficsim endpoint is skipped, while still populating `_traffic_objs` so `simulate_traffic` can replay recorded trajectories. - SensorsimService: clamp the camera-render trajectory query timestamps to the trajectory's [start, stop) so a sub-millisecond clock skew at the very first tick does not raise an out-of-range interpolation error. Co-Authored-By: Claude Opus 4.7 --- src/runtime/alpasim_runtime/services/sensorsim_service.py | 7 +++++-- src/runtime/alpasim_runtime/services/traffic_service.py | 3 +++ 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/runtime/alpasim_runtime/services/sensorsim_service.py b/src/runtime/alpasim_runtime/services/sensorsim_service.py index f4540045..3e90ded2 100644 --- a/src/runtime/alpasim_runtime/services/sensorsim_service.py +++ b/src/runtime/alpasim_runtime/services/sensorsim_service.py @@ -262,8 +262,11 @@ def trajectory_to_pose_pair( Interpolate pose between trigger start and end and package as PosePair. Optionally apply a delta transformation (such as rig_to_camera). """ - start_pose = trajectory.interpolate_pose(start_us) - end_pose = trajectory.interpolate_pose(end_us) + traj_range = trajectory.time_range_us + clamped_start = max(traj_range.start, min(start_us, traj_range.stop - 1)) + clamped_end = max(traj_range.start, min(end_us, traj_range.stop - 1)) + start_pose = trajectory.interpolate_pose(clamped_start) + end_pose = trajectory.interpolate_pose(clamped_end) if delta is not None: start_pose = start_pose @ delta diff --git a/src/runtime/alpasim_runtime/services/traffic_service.py b/src/runtime/alpasim_runtime/services/traffic_service.py index 522a5f6b..ce520144 100644 --- a/src/runtime/alpasim_runtime/services/traffic_service.py +++ b/src/runtime/alpasim_runtime/services/traffic_service.py @@ -63,6 +63,9 @@ async def _initialize_session(self, session_info: SessionInfo) -> None: "TrafficService._initialize_session requires a TrafficSessionConfig " f"via session_config, got {type(cfg).__name__}." ) + if self.skip: + self._traffic_objs = cfg.traffic_objs + return self._traffic_objs = cfg.traffic_objs scene_id = cfg.scene_id From e205993d34c05c866b849c7e865521154d0adfca Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Wed, 15 Jul 2026 16:20:16 +0900 Subject: [PATCH 05/13] Add LiDAR point cloud overlay to camera video MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Introduce a `Lidar`/`Lidars` data model mirroring `Camera`/`Cameras`, accumulate `driver_lidar_point_cloud` entries from ASL / broadcaster, and expose the sweeps on `ScenarioEvalInput` and `SimulationResult`. The `DEFAULT` video layout gains an opt-in projection of the latest LiDAR sweep onto the rendered camera axis, colored by forward depth. Points are uniformly subsampled to a configurable cap (`lidar_overlay_max_points`, default 8000) to keep matplotlib scatter fast — raw PANDAR128 sweeps produce ~250k points which would otherwise dominate render time. The overlay is gated by `video.overlay_lidar_on_camera` (default off) and picks its sensor via `video.lidar_id_to_overlay` (defaults to the first available sensor when unset). Co-Authored-By: Claude Opus 4.7 --- src/eval/src/eval/accumulator.py | 9 +- src/eval/src/eval/data.py | 76 ++++++++++- src/eval/src/eval/schema.py | 11 ++ src/eval/src/eval/video.py | 124 ++++++++++++++---- .../test_video_reasoning_overlay_utils.py | 2 + 5 files changed, 193 insertions(+), 29 deletions(-) diff --git a/src/eval/src/eval/accumulator.py b/src/eval/src/eval/accumulator.py index 19893fad..b54d9ef4 100644 --- a/src/eval/src/eval/accumulator.py +++ b/src/eval/src/eval/accumulator.py @@ -33,6 +33,7 @@ RAABB, Cameras, DriverResponses, + Lidars, RenderableTrajectory, Routes, ScenarioEvalInput, @@ -88,8 +89,9 @@ class EvalDataAccumulator: default_factory=list, init=False ) - # Camera and route data + # Camera, lidar, and route data _cameras: Cameras = field(default_factory=Cameras, init=False) + _lidars: Lidars = field(default_factory=Lidars, init=False) _routes: Routes = field(default_factory=Routes, init=False) @property @@ -120,6 +122,10 @@ def handle_message(self, message: LogEntry) -> None: self._handle_actor_poses(message.actor_poses) elif msg_type == "driver_camera_image": self._cameras.add_camera_image(message.driver_camera_image.camera_image) + elif msg_type == "driver_lidar_point_cloud": + self._lidars.add_lidar_point_cloud( + message.driver_lidar_point_cloud.lidar_point_cloud + ) elif msg_type == "route_request": self._routes.add_route(message.route_request.route) elif msg_type == "driver_request": @@ -367,6 +373,7 @@ def build_scenario_eval_input( driver_responses=driver_responses, vec_map=vec_map, cameras=self._cameras if self._cameras.camera_by_logical_id else None, + lidars=self._lidars if self._lidars.lidar_by_logical_id else None, routes=self._routes if self._routes.routes_in_rig_frame else None, run_uuid=run_uuid, run_name=run_name, diff --git a/src/eval/src/eval/data.py b/src/eval/src/eval/data.py index 3219f364..40fa2319 100644 --- a/src/eval/src/eval/data.py +++ b/src/eval/src/eval/data.py @@ -14,7 +14,12 @@ import polars as pl import shapely from alpasim_grpc.v0 import common_pb2 -from alpasim_grpc.v0.egodriver_pb2 import DriveResponse, RolloutCameraImage, Route +from alpasim_grpc.v0.egodriver_pb2 import ( + DriveResponse, + RolloutCameraImage, + RolloutLidarPointCloud, + Route, +) from alpasim_grpc.v0.logging_pb2 import RolloutMetadata from alpasim_grpc.v0.sensorsim_pb2 import AvailableCamerasReturn, CameraSpec from alpasim_utils import geometry @@ -1463,6 +1468,65 @@ def add_calibration( self.calibrations_by_logical_id[calibration.logical_id] = calibration +@dataclasses.dataclass +class Lidar: + """A single LiDAR sensor's captured sweeps over time. + + Points are stored in the sensor's rig frame (identity sensor-to-base) at the + end-of-spin timestamp reported by the renderer. + """ + + logical_id: str + timestamps_us: list[int] = dataclasses.field(default_factory=list) + points_list: list[np.ndarray] = dataclasses.field(default_factory=list) + + def add_point_cloud( + self, point_cloud: RolloutLidarPointCloud.LidarPointCloud + ) -> None: + num_points = int(point_cloud.num_points) + if num_points == 0: + xyz = np.empty((0, 3), dtype=np.float32) + else: + xyz = np.frombuffer( + point_cloud.point_xyzs_buffer, dtype=np.float32 + ).reshape(-1, 3) + if xyz.shape[0] != num_points: + # Trust the buffer length; num_points is best-effort metadata. + xyz = xyz[:num_points] if xyz.shape[0] > num_points else xyz + self.timestamps_us.append(int(point_cloud.frame_end_us)) + self.points_list.append(xyz) + + def points_at_time( + self, time_us: int, max_stale_us: int = 200_000 + ) -> np.ndarray | None: + """Return points from the sweep nearest to `time_us`. + + Returns None when no sweep is within `max_stale_us` of the requested time. + """ + if not self.timestamps_us: + return None + timestamps = np.asarray(self.timestamps_us, dtype=np.int64) + idx = int(np.argmin(np.abs(timestamps - int(time_us)))) + if abs(int(timestamps[idx]) - int(time_us)) > max_stale_us: + return None + return self.points_list[idx] + + +@dataclasses.dataclass +class Lidars: + """LiDAR sweeps keyed by logical_id.""" + + lidar_by_logical_id: dict[str, Lidar] = dataclasses.field(default_factory=dict) + + def add_lidar_point_cloud( + self, point_cloud: RolloutLidarPointCloud.LidarPointCloud + ) -> None: + logical_id = point_cloud.logical_id + if logical_id not in self.lidar_by_logical_id: + self.lidar_by_logical_id[logical_id] = Lidar(logical_id=logical_id) + self.lidar_by_logical_id[logical_id].add_point_cloud(point_cloud) + + @dataclasses.dataclass class Routes: """Captures routes for all timesteps. @@ -1764,6 +1828,9 @@ class ScenarioEvalInput: # Cameras data (optional, needed for image-based metrics) cameras: Cameras | None = None + # LiDAR sweeps captured per sensor (optional, used for video overlays) + lidars: Lidars | None = None + # Routes data (optional) routes: Routes | None = None @@ -1804,6 +1871,7 @@ class SimulationResult: # Shapely polygons and pre-cached STRtrees at each ts for fast spatial queries actor_polygons: ActorPolygons cameras: Cameras + lidars: Lidars routes: Routes # See ScenarioEvalInput.force_gt_duration_us. force_gt_duration_us: int | None = None @@ -1907,10 +1975,13 @@ def from_scenario_input( # Create actor polygons from trajectories actor_polygons = ActorPolygons.from_actor_trajectories(actor_trajectories) - # Create empty cameras and routes if not provided + # Create empty cameras, lidars, and routes if not provided cameras = ( scenario_input.cameras if scenario_input.cameras is not None else Cameras() ) + lidars = ( + scenario_input.lidars if scenario_input.lidars is not None else Lidars() + ) routes = ( scenario_input.routes if scenario_input.routes is not None else Routes() ) @@ -1925,6 +1996,7 @@ def from_scenario_input( vec_map=scenario_input.vec_map, actor_polygons=actor_polygons, cameras=cameras, + lidars=lidars, routes=routes, force_gt_duration_us=scenario_input.force_gt_duration_us, ) diff --git a/src/eval/src/eval/schema.py b/src/eval/src/eval/schema.py index e6f0372a..7aa00eeb 100644 --- a/src/eval/src/eval/schema.py +++ b/src/eval/src/eval/schema.py @@ -77,6 +77,17 @@ class VideoRendererConfig: camera_id_to_render: str = MISSING # Whether to overlay planner trajectories onto the camera view overlay_plans_on_camera: bool = True + # Whether to project LiDAR points onto the camera view, colored by depth. + overlay_lidar_on_camera: bool = False + # LiDAR logical id to overlay when overlay_lidar_on_camera is enabled. + # If None and overlay is enabled, the first available lidar is used. + lidar_id_to_overlay: str | None = None + # Marker size (in points) for projected LiDAR points on the camera view. + lidar_overlay_point_size: float = 0.5 + # Maximum number of LiDAR points to draw per frame (raw sweeps are ~250k + # points, which is prohibitively slow for matplotlib scatter). Points are + # uniformly subsampled to this cap. + lidar_overlay_max_points: int = 8000 # Order and inclusion of metrics in the rendered table; None shows all metrics. metrics_table_entries: list[str] | None = None # Options for how to render the BEV map diff --git a/src/eval/src/eval/video.py b/src/eval/src/eval/video.py index c9df60a5..9d60a5f4 100644 --- a/src/eval/src/eval/video.py +++ b/src/eval/src/eval/video.py @@ -17,7 +17,7 @@ from eval.aggregation import processing from eval.aggregation.processing import ProcessedMetricDFs -from eval.data import CameraProjector, ScenarioEvalInput, SimulationResult +from eval.data import CameraProjector, Lidar, ScenarioEvalInput, SimulationResult from eval.schema import EvalConfig, MapElements, VideoLayout from eval.video_data import ShapelyMap from eval.video_reasoning_overlay_utils import render_reasoning_overlay_style_video @@ -404,6 +404,37 @@ def update_table( return table +def _render_lidar_overlay( + ax: plt.Axes, + camera_projector: CameraProjector, + lidar: Lidar, + time_us: int, + point_size: float, + max_points: int, +) -> plt.Artist | None: + """Project a LiDAR sweep onto the camera axes, colored by forward depth.""" + points_rig = lidar.points_at_time(time_us) + if points_rig is None or points_rig.size == 0: + return None + if max_points > 0 and points_rig.shape[0] > max_points: + stride = points_rig.shape[0] // max_points + points_rig = points_rig[::stride] + pixels, mask = camera_projector.project_points(points_rig) + if pixels.shape[0] == 0: + return None + depths = points_rig[mask, 0] + return ax.scatter( + pixels[:, 0], + pixels[:, 1], + c=depths, + cmap="turbo", + s=point_size, + vmin=1.0, + vmax=60.0, + linewidths=0, + ) + + def create_video_animation( processed_metrics_dfs: ProcessedMetricDFs, sim_result: SimulationResult, @@ -441,35 +472,38 @@ def create_video_animation( axs["image"].set_autoscale_on(False) overlay_enabled = cfg.video.overlay_plans_on_camera + lidar_overlay_enabled = cfg.video.overlay_lidar_on_camera camera_projector: CameraProjector | None = None - if overlay_enabled: - if not sim_result.driver_responses.per_timestep_driver_responses: - logger.info("No driver responses found; disabling camera overlay.") + if overlay_enabled or lidar_overlay_enabled: + calibration = sim_result.cameras.calibrations_by_logical_id.get( + cfg.video.camera_id_to_render + ) + if calibration is None: + logger.warning( + "No calibration for camera %s; disabling camera overlays.", + cfg.video.camera_id_to_render, + ) overlay_enabled = False + lidar_overlay_enabled = False else: - calibration = sim_result.cameras.calibrations_by_logical_id.get( - cfg.video.camera_id_to_render - ) - if calibration is None: + try: + camera_projector = CameraProjector( + calibration=calibration, + actual_resolution=first_image.size if first_image else None, + ) + except ValueError as exc: logger.warning( - "No calibration for camera %s; disabling camera overlay.", + "Unsupported calibration for camera %s (%s); " + "disabling camera overlays.", cfg.video.camera_id_to_render, + exc, ) overlay_enabled = False - else: - try: - camera_projector = CameraProjector( - calibration=calibration, - actual_resolution=first_image.size if first_image else None, - ) - except ValueError as exc: - logger.warning( - "Unsupported calibration for camera %s (%s); " - "disabling camera overlay.", - cfg.video.camera_id_to_render, - exc, - ) - overlay_enabled = False + lidar_overlay_enabled = False + + if overlay_enabled and not sim_result.driver_responses.per_timestep_driver_responses: + logger.info("No driver responses found; disabling camera overlay.") + overlay_enabled = False if overlay_enabled: overlay_frame_matches = np.intersect1d( @@ -481,6 +515,27 @@ def create_video_animation( "camera overlay will be empty." ) + lidar_overlay_source = None + if lidar_overlay_enabled: + lidars_by_id = sim_result.lidars.lidar_by_logical_id + if not lidars_by_id: + logger.info("No LiDAR sweeps recorded; disabling LiDAR overlay.") + lidar_overlay_enabled = False + else: + requested_id = cfg.video.lidar_id_to_overlay + if requested_id is None: + lidar_overlay_source = next(iter(lidars_by_id.values())) + elif requested_id in lidars_by_id: + lidar_overlay_source = lidars_by_id[requested_id] + else: + logger.warning( + "Configured lidar_id_to_overlay=%s not present in recorded " + "sweeps (available=%s); disabling LiDAR overlay.", + requested_id, + list(lidars_by_id.keys()), + ) + lidar_overlay_enabled = False + if should_render_table: table = render_table( axs["table"], @@ -700,12 +755,29 @@ def update(time: int) -> list[plt.Artist]: command_text_artist.set_visible(False) overlay_artists: list[plt.Artist] = [] - if overlay_enabled and camera_projector is not None: - overlay_artists = sim_result.driver_responses.render_on_camera( + if ( + lidar_overlay_enabled + and camera_projector is not None + and lidar_overlay_source is not None + ): + lidar_artist = _render_lidar_overlay( axs["image"], camera_projector, + lidar_overlay_source, time, - which_time="now", + cfg.video.lidar_overlay_point_size, + cfg.video.lidar_overlay_max_points, + ) + if lidar_artist is not None: + overlay_artists.append(lidar_artist) + if overlay_enabled and camera_projector is not None: + overlay_artists.extend( + sim_result.driver_responses.render_on_camera( + axs["image"], + camera_projector, + time, + which_time="now", + ) ) overlay_artists.extend( sim_result.routes.render_on_camera( diff --git a/src/eval/tests/test_video_reasoning_overlay_utils.py b/src/eval/tests/test_video_reasoning_overlay_utils.py index 3d5c462a..5a334baa 100644 --- a/src/eval/tests/test_video_reasoning_overlay_utils.py +++ b/src/eval/tests/test_video_reasoning_overlay_utils.py @@ -20,6 +20,7 @@ Cameras, DriverResponseAtTime, DriverResponses, + Lidars, RenderableTrajectory, Routes, SimulationResult, @@ -91,6 +92,7 @@ def test_render_single_reasoning_overlay_frame_uses_renderable_trajectory_api() vec_map=None, actor_polygons=ActorPolygons.from_actor_trajectories({"EGO": ego_traj}), cameras=cameras, + lidars=Lidars(), routes=Routes(), ) cfg = create_test_eval_config() From 8c7507d2eccbd82402850e465d4126f8cb0397ad Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Wed, 15 Jul 2026 16:31:07 +0900 Subject: [PATCH 06/13] Apply simplify review findings - Clamp LiDAR trajectory_to_pose_pair timestamps like the camera path so the final sweep does not raise when end_us hits the trajectory end. - Consolidate duplicated _traffic_objs assignment in TrafficService by moving it before the skip guard. - Drop the num_points reconciliation branch in Lidar.add_point_cloud; trust the reshape output and the buffer as the source of truth. Co-Authored-By: Claude Opus 4.7 --- src/eval/src/eval/data.py | 12 ++++-------- .../services/sensorsim_service.py | 16 ++++++++++++---- .../alpasim_runtime/services/traffic_service.py | 3 +-- 3 files changed, 17 insertions(+), 14 deletions(-) diff --git a/src/eval/src/eval/data.py b/src/eval/src/eval/data.py index 40fa2319..ae900366 100644 --- a/src/eval/src/eval/data.py +++ b/src/eval/src/eval/data.py @@ -1483,17 +1483,13 @@ class Lidar: def add_point_cloud( self, point_cloud: RolloutLidarPointCloud.LidarPointCloud ) -> None: - num_points = int(point_cloud.num_points) - if num_points == 0: - xyz = np.empty((0, 3), dtype=np.float32) - else: + if point_cloud.point_xyzs_buffer: xyz = np.frombuffer( point_cloud.point_xyzs_buffer, dtype=np.float32 ).reshape(-1, 3) - if xyz.shape[0] != num_points: - # Trust the buffer length; num_points is best-effort metadata. - xyz = xyz[:num_points] if xyz.shape[0] > num_points else xyz - self.timestamps_us.append(int(point_cloud.frame_end_us)) + else: + xyz = np.empty((0, 3), dtype=np.float32) + self.timestamps_us.append(point_cloud.frame_end_us) self.points_list.append(xyz) def points_at_time( diff --git a/src/runtime/alpasim_runtime/services/sensorsim_service.py b/src/runtime/alpasim_runtime/services/sensorsim_service.py index 3e90ded2..3abeaa6b 100644 --- a/src/runtime/alpasim_runtime/services/sensorsim_service.py +++ b/src/runtime/alpasim_runtime/services/sensorsim_service.py @@ -303,8 +303,13 @@ def trajectory_to_pose_pair( logger.info( "RENDER_DBG start_us=%s end_us=%s traj_range=[%s..%s) " "raw_ego_t=%s rig_to_cam_t=%s composed_t=%s", - start_us, end_us, _ts_range.start, _ts_range.stop, - list(_raw_start.vec3), _rig_to_cam_t, list(_composed.vec3), + start_us, + end_us, + _ts_range.start, + _ts_range.stop, + list(_raw_start.vec3), + _rig_to_cam_t, + list(_composed.vec3), ) except Exception as _e: logger.info("RENDER_DBG failed to log trajectory: %s", _e) @@ -355,8 +360,11 @@ def construct_lidar_render_request( def trajectory_to_pose_pair( trajectory: Trajectory, delta: Pose | None ) -> PosePair: - start_pose = trajectory.interpolate_pose(start_us) - end_pose = trajectory.interpolate_pose(end_us) + traj_range = trajectory.time_range_us + clamped_start = max(traj_range.start, min(start_us, traj_range.stop - 1)) + clamped_end = max(traj_range.start, min(end_us, traj_range.stop - 1)) + start_pose = trajectory.interpolate_pose(clamped_start) + end_pose = trajectory.interpolate_pose(clamped_end) if delta is not None: start_pose = start_pose @ delta end_pose = end_pose @ delta diff --git a/src/runtime/alpasim_runtime/services/traffic_service.py b/src/runtime/alpasim_runtime/services/traffic_service.py index ce520144..7f9ca3d5 100644 --- a/src/runtime/alpasim_runtime/services/traffic_service.py +++ b/src/runtime/alpasim_runtime/services/traffic_service.py @@ -63,11 +63,10 @@ async def _initialize_session(self, session_info: SessionInfo) -> None: "TrafficService._initialize_session requires a TrafficSessionConfig " f"via session_config, got {type(cfg).__name__}." ) + self._traffic_objs = cfg.traffic_objs if self.skip: - self._traffic_objs = cfg.traffic_objs return - self._traffic_objs = cfg.traffic_objs scene_id = cfg.scene_id ego_aabb = cfg.ego_aabb gt_ego_aabb_trajectory = cfg.gt_ego_aabb_trajectory From 44c44c094e4a90ea6e01095221bfee6a82022f5e Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Wed, 15 Jul 2026 17:08:05 +0900 Subject: [PATCH 07/13] =?UTF-8?q?Apply=20ENU=E2=86=92tile-local=20axis=20r?= =?UTF-8?q?otation=20in=20splatsim=20adapter?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Alpasim sends camera-in-world poses in ROS ENU convention (Z-up), but splatsim's tile-local scene lives in a Y-up RUB frame. The adapter was subtracting `tile_local_centroid` without any rotation, so the camera optical axis ended up pointing along splatsim's +Y (up) — the rendered CAM_FRONT_WIDE view came out as a bird's-eye image instead of a forward-looking one. Route both pose_to_viewmat and pose_to_sensor_to_world through a shared `_pose_to_tile_local` helper that applies `R_ENU_TO_TILE = [[1,0,0], [0,0,1],[0,-1,0]]` before recentering. Update existing tests to reflect the new frame semantics and add a regression test verifying +Z alpasim maps to +Y splatsim. Also mount render_adapter.py alongside server.py in the splatsim renderer wizard config so this fix reaches the running container without a rebuild. Co-Authored-By: Claude Opus 4.7 --- .../render_adapter.py | 53 ++++++++++++++----- .../tests/test_render_adapter.py | 47 ++++++++++++---- src/wizard/configs/renderer/splatsim.yaml | 1 + 3 files changed, 80 insertions(+), 21 deletions(-) diff --git a/src/splatsim_renderer/alpasim_splatsim_renderer/render_adapter.py b/src/splatsim_renderer/alpasim_splatsim_renderer/render_adapter.py index d098d464..91f9b0c9 100644 --- a/src/splatsim_renderer/alpasim_splatsim_renderer/render_adapter.py +++ b/src/splatsim_renderer/alpasim_splatsim_renderer/render_adapter.py @@ -110,6 +110,40 @@ def _quat_to_rotation_matrix(qw: float, qx: float, qy: float, qz: float) -> np.n ) +# Rotation from alpasim's ROS ENU world frame (X-forward/east, Y-left/north, +# Z-up) into splatsim's tile-local RUB Y-up frame (X, Y-up, Z). Applied to any +# camera-in-world pose received from alpasim before recentering by +# ``tile_local_centroid``. Without this, cameras end up pointing down and the +# rendered image looks like a bird's-eye view. +_R_ENU_TO_TILE = np.array( + [ + [1.0, 0.0, 0.0], + [0.0, 0.0, 1.0], + [0.0, -1.0, 0.0], + ], + dtype=np.float32, +) + + +def _pose_to_tile_local( + pose, + world_origin: np.ndarray | None, +) -> tuple[np.ndarray, np.ndarray]: + """Return (R, t) of a camera-in-world pose expressed in tile-local frame. + + Rotates the incoming pose from alpasim's Z-up ENU world into splatsim's + Y-up tile-local world, then subtracts ``world_origin`` (which is already + in tile-local coordinates, e.g. ``Background.tile_local_centroid``). + """ + R = _quat_to_rotation_matrix(pose.quat.w, pose.quat.x, pose.quat.y, pose.quat.z) + t = np.array([pose.vec.x, pose.vec.y, pose.vec.z], dtype=np.float32) + R = _R_ENU_TO_TILE @ R + t = _R_ENU_TO_TILE @ t + if world_origin is not None: + t = t - np.asarray(world_origin, dtype=np.float32) + return R, t + + def pose_to_viewmat(pose, world_origin: np.ndarray | None = None) -> np.ndarray: """Convert a common.Pose (camera-in-world) to a 4x4 world->camera viewmat. @@ -118,15 +152,12 @@ def pose_to_viewmat(pose, world_origin: np.ndarray | None = None) -> np.ndarray: splatsim's `Renderer.render` wants the inverse — world-to-camera. The inverse of a rigid transform ``[R | t]`` is ``[R^T | -R^T t]``. - ``world_origin`` is subtracted from the position before inversion so - that world-frame poses land in splatsim's tile-local frame (the frame - ``Renderer.render`` actually operates in). Pass ``Background.tile_local_centroid`` + The pose is first rotated from alpasim's Z-up ENU world into splatsim's + Y-up tile-local frame, then ``world_origin`` is subtracted so world-frame + poses land at the tile centroid. Pass ``Background.tile_local_centroid`` from the loaded scene here. """ - R = _quat_to_rotation_matrix(pose.quat.w, pose.quat.x, pose.quat.y, pose.quat.z) - t = np.array([pose.vec.x, pose.vec.y, pose.vec.z], dtype=np.float32) - if world_origin is not None: - t = t - np.asarray(world_origin, dtype=np.float32) + R, t = _pose_to_tile_local(pose, world_origin) viewmat = np.eye(4, dtype=np.float32) viewmat[:3, :3] = R.T viewmat[:3, 3] = -R.T @ t @@ -149,12 +180,10 @@ def pose_to_sensor_to_world( Used by the LiDAR path (splatsim's ``LidarRenderer.render`` takes ``sensor_to_world`` directly, not its inverse). Applies the same - ``world_origin`` offset as :func:`pose_to_viewmat`. + Z-up ENU → Y-up tile-local rotation and ``world_origin`` offset as + :func:`pose_to_viewmat`. """ - R = _quat_to_rotation_matrix(pose.quat.w, pose.quat.x, pose.quat.y, pose.quat.z) - t = np.array([pose.vec.x, pose.vec.y, pose.vec.z], dtype=np.float32) - if world_origin is not None: - t = t - np.asarray(world_origin, dtype=np.float32) + R, t = _pose_to_tile_local(pose, world_origin) m = np.eye(4, dtype=np.float32) m[:3, :3] = R m[:3, 3] = t diff --git a/src/splatsim_renderer/tests/test_render_adapter.py b/src/splatsim_renderer/tests/test_render_adapter.py index 3310cfea..7824a9d5 100644 --- a/src/splatsim_renderer/tests/test_render_adapter.py +++ b/src/splatsim_renderer/tests/test_render_adapter.py @@ -76,9 +76,14 @@ def test_pose_identity_yields_translation_only_viewmat(): quat=common_pb2.Quat(w=1.0, x=0.0, y=0.0, z=0.0), ) viewmat = pose_to_viewmat(pose) - # camera at (1, 2, 3) with identity rotation -> world->cam translation is - # -(1, 2, 3). - np.testing.assert_allclose(viewmat[:3, :3], np.eye(3, dtype=np.float32), atol=1e-6) + # Alpasim sends camera-in-world as ENU Z-up; splatsim's tile-local is + # Y-up, so ``pose_to_viewmat`` applies R_ENU_TO_TILE = [[1,0,0],[0,0,1], + # [0,-1,0]] before inversion. For identity rotation this yields + # viewmat[:3,:3] = R_ENU_TO_TILE.T and viewmat[:3,3] = -R.T @ R @ t = -t. + expected_R = np.array( + [[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]], dtype=np.float32 + ) + np.testing.assert_allclose(viewmat[:3, :3], expected_R, atol=1e-6) np.testing.assert_allclose(viewmat[:3, 3], np.array([-1.0, -2.0, -3.0]), atol=1e-6) np.testing.assert_allclose(viewmat[3], [0, 0, 0, 1], atol=1e-6) @@ -164,20 +169,28 @@ def test_encode_image_undefined_format_defaults_to_png(): def test_identity_quat_takes_fast_path(): - """Identity quaternion should yield exactly the 3x3 identity matrix.""" + """Identity quaternion should hit the fast path and yield exact R_ENU_TO_TILE^T. + + The frame rotation is a permutation with exact ±1 entries, so combining it + with the identity-quat fast path in ``_quat_to_rotation_matrix`` must + remain exact (no floating-point drift). + """ pose = common_pb2.Pose( vec=common_pb2.Vec3(), quat=common_pb2.Quat(w=1.0), ) viewmat = pose_to_viewmat(pose) - np.testing.assert_array_equal(viewmat[:3, :3], np.eye(3, dtype=np.float32)) + expected_R = np.array( + [[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]], dtype=np.float32 + ) + np.testing.assert_array_equal(viewmat[:3, :3], expected_R) def test_world_origin_offset_shifts_camera_position(): - """``world_origin`` shifts the camera's world position before inversion. + """``world_origin`` (in tile-local Y-up) zeroes the recentered translation. - Camera at world (10, 20, 30) with world_origin (10, 20, 30) should end up - at the tile-local origin, i.e. viewmat translation = 0. + Camera at world ENU (10, 20, 30) rotates to tile-local Y-up (10, 30, -20); + passing that as ``world_origin`` recenters to the tile origin. """ from alpasim_splatsim_renderer.render_adapter import pose_to_viewmat @@ -186,11 +199,27 @@ def test_world_origin_offset_shifts_camera_position(): quat=common_pb2.Quat(w=1.0), ) viewmat = pose_to_viewmat( - pose, world_origin=np.array([10.0, 20.0, 30.0], dtype=np.float32) + pose, world_origin=np.array([10.0, 30.0, -20.0], dtype=np.float32) ) np.testing.assert_allclose(viewmat[:3, 3], np.zeros(3), atol=1e-6) +def test_zup_to_yup_axis_mapping(): + """+Z (up) in alpasim ENU must become +Y (up) in splatsim tile-local. + + Regression guard for the frame conversion. Verified at the sensor-to-world + level (no inversion) so the assertion reads as a direct position swap. + """ + from alpasim_splatsim_renderer.render_adapter import pose_to_sensor_to_world + + pose = common_pb2.Pose( + vec=common_pb2.Vec3(x=0.0, y=0.0, z=5.0), + quat=common_pb2.Quat(w=1.0), + ) + s2w = pose_to_sensor_to_world(pose) + np.testing.assert_allclose(s2w[:3, 3], np.array([0.0, 5.0, 0.0]), atol=1e-6) + + def test_pose_to_sensor_to_world_is_inverse_of_viewmat(): """``pose_to_sensor_to_world`` returns the un-inverted sensor→world 4x4.""" from alpasim_splatsim_renderer.render_adapter import ( diff --git a/src/wizard/configs/renderer/splatsim.yaml b/src/wizard/configs/renderer/splatsim.yaml index 76b005a2..11191ff6 100644 --- a/src/wizard/configs/renderer/splatsim.yaml +++ b/src/wizard/configs/renderer/splatsim.yaml @@ -24,6 +24,7 @@ services: - "${defines.splatsim_scene_usdz}:/mnt/splatsim/scene.usdz" - "/tmp/splatsim_usdz_v020.py:/repo/src/splatsim_renderer/.venv/lib/python3.10/site-packages/splatsim/_usdz.py" - "${repo-relative:'src/splatsim_renderer/alpasim_splatsim_renderer/server.py'}:/repo/src/splatsim_renderer/alpasim_splatsim_renderer/server.py" + - "${repo-relative:'src/splatsim_renderer/alpasim_splatsim_renderer/render_adapter.py'}:/repo/src/splatsim_renderer/alpasim_splatsim_renderer/render_adapter.py" # NOTE: Unlike other services which mount ``${repo-relative:'src'}:/repo/src`` # for hot-reload against the shared base image's global venv, the splatsim # renderer uses a dedicated image with a per-project venv baked in at From 01e791562fa80b03f13f7efcc349118e257313ad Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Wed, 15 Jul 2026 18:45:33 +0900 Subject: [PATCH 08/13] Add trajectory_start_us_offset to shift rollout start within recording MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Rollouts previously always began at ``rig.trajectory.time_range_us.start``, so a 20 s simulation on a 56 s USDZ recording could only replay the first 20 s of ego motion. Introduce ``SimulationConfig.trajectory_start_us_offset`` (microseconds) so users can pivot the anchor forward — useful for skipping uninteresting recording preambles and for feeding OnePlanner an EgoHistory context window from an arbitrary point in the trace. Threaded through ``_build_rollout_timing`` and exposed on ``Rig.first_camera_frame_{ranges,end}_us`` via a new ``min_frame_end_us`` kwarg so the camera anchor advances to the first frame at or after the shifted trajectory start. Co-Authored-By: Claude Opus 4.7 --- src/runtime/alpasim_runtime/config.py | 8 +++ .../alpasim_runtime/unbound_rollout.py | 27 ++++++-- src/runtime/tests/test_unbound_rollout.py | 69 +++++++++++++++++++ src/utils/alpasim_utils/scenario.py | 38 ++++++++-- src/wizard/configs/base_config.yaml | 6 ++ 5 files changed, 137 insertions(+), 11 deletions(-) diff --git a/src/runtime/alpasim_runtime/config.py b/src/runtime/alpasim_runtime/config.py index 94daec8f..2cbdaade 100644 --- a/src/runtime/alpasim_runtime/config.py +++ b/src/runtime/alpasim_runtime/config.py @@ -334,6 +334,14 @@ class SimulationConfig: route_generator_type: RouteGeneratorType = RouteGeneratorType.MAP route_start_offset_m: float = 0.0 + # Skip the first N microseconds of the recorded trajectory before starting + # the rollout. 0 (default) reproduces the historical behavior of playing + # from the very first recorded timestamp. Useful when the beginning of the + # recording is uninteresting (parked/idle), or to supply enough + # ego-history context for drivers like OnePlanner that condition on past + # motion. + trajectory_start_us_offset: int = 0 + # Whether to send optional messages to the driver send_recording_ground_truth: bool = False diff --git a/src/runtime/alpasim_runtime/unbound_rollout.py b/src/runtime/alpasim_runtime/unbound_rollout.py index 0c38ee08..4476507f 100644 --- a/src/runtime/alpasim_runtime/unbound_rollout.py +++ b/src/runtime/alpasim_runtime/unbound_rollout.py @@ -72,19 +72,34 @@ def _build_rollout_timing( renderer_service: RendererService, ) -> RolloutTiming: camera_logical_ids = [camera_cfg.logical_id for camera_cfg in camera_configs] - egomotion_context_start_us = data_source.rig.trajectory.time_range_us.start + trajectory_range_us = data_source.rig.trajectory.time_range_us + offset_us = simulation_config.trajectory_start_us_offset + if offset_us < 0: + raise ValueError( + f"trajectory_start_us_offset must be non-negative, got {offset_us}" + ) + if offset_us >= (trajectory_range_us.stop - trajectory_range_us.start): + raise ValueError( + f"trajectory_start_us_offset={offset_us} is past the recording " + f"duration ({trajectory_range_us.stop - trajectory_range_us.start} us)" + ) + egomotion_context_start_us = trajectory_range_us.start + offset_us # ``first_camera_frame_end_us`` raises through ``first_camera_frame_ranges_us`` - # when no cameras are configured. Headless rollouts fall back to the GT - # trajectory start as the render anchor. + # when no cameras are configured. Headless rollouts fall back to the + # (shifted) GT trajectory start as the render anchor. if camera_logical_ids: first_camera_frame_ranges_us = data_source.rig.first_camera_frame_ranges_us( - camera_logical_ids + camera_logical_ids, + min_frame_end_us=egomotion_context_start_us, + ) + render_start_us = data_source.rig.first_camera_frame_end_us( + camera_logical_ids, + min_frame_end_us=egomotion_context_start_us, ) - render_start_us = data_source.rig.first_camera_frame_end_us(camera_logical_ids) else: first_camera_frame_ranges_us = {} - render_start_us = data_source.rig.trajectory.time_range_us.start + render_start_us = egomotion_context_start_us if simulation_config.assert_zero_decision_delay and camera_configs: for camera_cfg in camera_configs: diff --git a/src/runtime/tests/test_unbound_rollout.py b/src/runtime/tests/test_unbound_rollout.py index d6139deb..860dcb52 100644 --- a/src/runtime/tests/test_unbound_rollout.py +++ b/src/runtime/tests/test_unbound_rollout.py @@ -6,6 +6,7 @@ from types import SimpleNamespace import numpy as np +import pytest from alpasim_grpc.v0.logging_pb2 import RolloutMetadata from alpasim_runtime.config import ( PhysicsUpdateMode, @@ -239,3 +240,71 @@ def test_create_keeps_synthetic_first_exposure_inside_rollout_window( assert rollout.first_camera_frame_ranges_us["camera_left"] == range(50_000, 150_000) assert rollout.egomotion_context_start_us == 0 assert rollout.traffic_objs["actor"].trajectory.time_range_us.start == 0 + + +def test_trajectory_start_us_offset_shifts_anchor_to_later_camera_frame( + tmp_path, +) -> None: + # Offset 210_000us skips each camera's first frame_range (both end at or + # before 200_000us); the render anchor becomes the second per-camera frame. + rollout = UnboundRollout.create( + simulation_config=_simulation_config(trajectory_start_us_offset=210_000), + scene_id="scene", + version_ids=RolloutMetadata.VersionIds(), + data_source=_artifact(), + rollouts_dir=str(tmp_path), + renderer_service=_sensorsim_renderer(), + ) + + assert rollout.egomotion_context_start_us == 210_000 + assert rollout.first_camera_frame_ranges_us["camera_front"] == range( + 270_000, 300_000 + ) + assert rollout.first_camera_frame_ranges_us["camera_left"] == range( + 220_000, 250_000 + ) + assert rollout.render_start_timestamp_us == 250_000 + + +def test_trajectory_start_us_offset_rejects_negative(tmp_path) -> None: + with pytest.raises(ValueError, match="must be non-negative"): + UnboundRollout.create( + simulation_config=_simulation_config(trajectory_start_us_offset=-1), + scene_id="scene", + version_ids=RolloutMetadata.VersionIds(), + data_source=_artifact(), + rollouts_dir=str(tmp_path), + renderer_service=_sensorsim_renderer(), + ) + + +def test_trajectory_start_us_offset_rejects_past_recording(tmp_path) -> None: + # Trajectory spans [0, 500_001); an offset >= 500_001 lands at or past the + # recording's stop, so there is no ego history left to replay. + with pytest.raises(ValueError, match="past the recording"): + UnboundRollout.create( + simulation_config=_simulation_config( + trajectory_start_us_offset=500_001 + ), + scene_id="scene", + version_ids=RolloutMetadata.VersionIds(), + data_source=_artifact(), + rollouts_dir=str(tmp_path), + renderer_service=_sensorsim_renderer(), + ) + + +def test_trajectory_start_us_offset_rejects_no_frame_after_start(tmp_path) -> None: + # camera_left's last frame ends at 250_000; requesting a shift beyond it + # leaves the camera with no usable first frame. + with pytest.raises(ValueError, match="no frame ending at or after"): + UnboundRollout.create( + simulation_config=_simulation_config( + trajectory_start_us_offset=260_000 + ), + scene_id="scene", + version_ids=RolloutMetadata.VersionIds(), + data_source=_artifact(), + rollouts_dir=str(tmp_path), + renderer_service=_sensorsim_renderer(), + ) diff --git a/src/utils/alpasim_utils/scenario.py b/src/utils/alpasim_utils/scenario.py index 2aba105a..9e01bf5a 100644 --- a/src/utils/alpasim_utils/scenario.py +++ b/src/utils/alpasim_utils/scenario.py @@ -122,9 +122,18 @@ def _parse_camera_frame_ranges( return frame_ranges_us def first_camera_frame_ranges_us( - self, camera_logical_ids: Iterable[str] + self, + camera_logical_ids: Iterable[str], + *, + min_frame_end_us: int | None = None, ) -> dict[str, range]: - """First recorded frame window for each configured logical camera.""" + """First recorded frame window for each configured logical camera. + + ``min_frame_end_us`` (optional) skips frames whose shutter-close is + strictly before the given timestamp, so callers can pivot the rollout + anchor to a later point in the recording (see + ``SimulationConfig.trajectory_start_us_offset``). + """ first_frame_ranges_us: dict[str, range] = {} available_by_logical_id = { camera_id.logical_name: camera_id.unique_id for camera_id in self.camera_ids @@ -145,14 +154,33 @@ def first_camera_frame_ranges_us( f"Configured camera {logical_id!r} ({unique_id!r}) in rig " f"{self.sequence_id!r} has no frame timestamps." ) - first_frame_ranges_us[logical_id] = ranges_us[0] + if min_frame_end_us is None: + first_frame_ranges_us[logical_id] = ranges_us[0] + else: + first_after = next( + (r for r in ranges_us if r.stop >= min_frame_end_us), + None, + ) + if first_after is None: + raise ValueError( + f"Configured camera {logical_id!r} ({unique_id!r}) in " + f"rig {self.sequence_id!r} has no frame ending at or " + f"after {min_frame_end_us=} (last frame ends at " + f"{ranges_us[-1].stop})." + ) + first_frame_ranges_us[logical_id] = first_after if not first_frame_ranges_us: raise ValueError("At least one runtime camera must be configured.") return first_frame_ranges_us - def first_camera_frame_end_us(self, camera_logical_ids: Iterable[str]) -> int: + def first_camera_frame_end_us( + self, + camera_logical_ids: Iterable[str], + *, + min_frame_end_us: int | None = None, + ) -> int: """Central first-frame shutter-close time for configured cameras. The earliest first-frame end is the rollout render anchor. Individual @@ -161,7 +189,7 @@ def first_camera_frame_end_us(self, camera_logical_ids: Iterable[str]) -> int: return min( frame_range.stop for frame_range in self.first_camera_frame_ranges_us( - camera_logical_ids + camera_logical_ids, min_frame_end_us=min_frame_end_us ).values() ) diff --git a/src/wizard/configs/base_config.yaml b/src/wizard/configs/base_config.yaml index 47164ce2..0fa34d87 100644 --- a/src/wizard/configs/base_config.yaml +++ b/src/wizard/configs/base_config.yaml @@ -310,6 +310,12 @@ runtime: route_generator_type: "MAP" # "MAP" or "RECORDED" route_start_offset_m: 0.0 # Along-route distance ahead of ego where submitted routes start + # Skip the first N microseconds of the recorded trajectory before starting the rollout. + # 0 = play from the very first recorded timestamp. Useful when the beginning of the + # recording is uninteresting, or to supply enough ego-history context for drivers like + # OnePlanner that condition on past motion. + trajectory_start_us_offset: 0 + vehicle: null # use values from the .usdz file (original dimensions) # Note: If you change the `frame_interval_us`, it will affect the frequency of From becd82b78bcc6ead5db8f7ecb1b1e162cc3fdc04 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Wed, 15 Jul 2026 19:00:31 +0900 Subject: [PATCH 09/13] =?UTF-8?q?Remove=20spurious=20ENU=E2=86=92Y-up=20ro?= =?UTF-8?q?tation=20in=20splatsim=20adapter?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 44c44c0 introduced a Z-up→Y-up rotation on the sensor pose based on the assumption that splatsim's tile-local frame is Y-up. That assumption is wrong: the USDZ scene.json for the Odaiba capture reports ``up_axis: "z"`` and its ``tile_local_centroid`` has a small Z component (3.5m) with large horizontal offsets — the tile is Z-up like alpasim's ENU world, so no rotation is needed. Applying the rotation was rolling the camera 90° about X and producing a top-down bird's-eye view. Drop the rotation from ``_pose_to_tile_local`` and update the tests. Both ``pose_to_viewmat`` and ``pose_to_sensor_to_world`` now pass the pose orientation straight through, subtracting only ``world_origin`` (tile-local centroid) from the translation. Co-Authored-By: Claude Opus 4.7 --- .../render_adapter.py | 25 ++-------- .../tests/test_render_adapter.py | 47 +++---------------- 2 files changed, 12 insertions(+), 60 deletions(-) diff --git a/src/splatsim_renderer/alpasim_splatsim_renderer/render_adapter.py b/src/splatsim_renderer/alpasim_splatsim_renderer/render_adapter.py index 91f9b0c9..e8e50a1a 100644 --- a/src/splatsim_renderer/alpasim_splatsim_renderer/render_adapter.py +++ b/src/splatsim_renderer/alpasim_splatsim_renderer/render_adapter.py @@ -110,35 +110,20 @@ def _quat_to_rotation_matrix(qw: float, qx: float, qy: float, qz: float) -> np.n ) -# Rotation from alpasim's ROS ENU world frame (X-forward/east, Y-left/north, -# Z-up) into splatsim's tile-local RUB Y-up frame (X, Y-up, Z). Applied to any -# camera-in-world pose received from alpasim before recentering by -# ``tile_local_centroid``. Without this, cameras end up pointing down and the -# rendered image looks like a bird's-eye view. -_R_ENU_TO_TILE = np.array( - [ - [1.0, 0.0, 0.0], - [0.0, 0.0, 1.0], - [0.0, -1.0, 0.0], - ], - dtype=np.float32, -) - - def _pose_to_tile_local( pose, world_origin: np.ndarray | None, ) -> tuple[np.ndarray, np.ndarray]: """Return (R, t) of a camera-in-world pose expressed in tile-local frame. - Rotates the incoming pose from alpasim's Z-up ENU world into splatsim's - Y-up tile-local world, then subtracts ``world_origin`` (which is already - in tile-local coordinates, e.g. ``Background.tile_local_centroid``). + Both alpasim's world (ROS ENU) and splatsim's tile-local frame are + right-handed Z-up (Cesium 3D Tiles standard, confirmed by the + ``up_axis: "z"`` entry in the USDZ scene.json), so the pose orientation + is used as-is. Only the translation is recentered by ``world_origin`` + (typically ``Background.tile_local_centroid``). """ R = _quat_to_rotation_matrix(pose.quat.w, pose.quat.x, pose.quat.y, pose.quat.z) t = np.array([pose.vec.x, pose.vec.y, pose.vec.z], dtype=np.float32) - R = _R_ENU_TO_TILE @ R - t = _R_ENU_TO_TILE @ t if world_origin is not None: t = t - np.asarray(world_origin, dtype=np.float32) return R, t diff --git a/src/splatsim_renderer/tests/test_render_adapter.py b/src/splatsim_renderer/tests/test_render_adapter.py index 7824a9d5..a79e027e 100644 --- a/src/splatsim_renderer/tests/test_render_adapter.py +++ b/src/splatsim_renderer/tests/test_render_adapter.py @@ -76,14 +76,9 @@ def test_pose_identity_yields_translation_only_viewmat(): quat=common_pb2.Quat(w=1.0, x=0.0, y=0.0, z=0.0), ) viewmat = pose_to_viewmat(pose) - # Alpasim sends camera-in-world as ENU Z-up; splatsim's tile-local is - # Y-up, so ``pose_to_viewmat`` applies R_ENU_TO_TILE = [[1,0,0],[0,0,1], - # [0,-1,0]] before inversion. For identity rotation this yields - # viewmat[:3,:3] = R_ENU_TO_TILE.T and viewmat[:3,3] = -R.T @ R @ t = -t. - expected_R = np.array( - [[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]], dtype=np.float32 - ) - np.testing.assert_allclose(viewmat[:3, :3], expected_R, atol=1e-6) + # Both alpasim's ENU world and splatsim's tile-local frame are Z-up, so an + # identity pose yields an identity R and viewmat[:3,3] = -t. + np.testing.assert_allclose(viewmat[:3, :3], np.eye(3, dtype=np.float32), atol=1e-6) np.testing.assert_allclose(viewmat[:3, 3], np.array([-1.0, -2.0, -3.0]), atol=1e-6) np.testing.assert_allclose(viewmat[3], [0, 0, 0, 1], atol=1e-6) @@ -169,29 +164,17 @@ def test_encode_image_undefined_format_defaults_to_png(): def test_identity_quat_takes_fast_path(): - """Identity quaternion should hit the fast path and yield exact R_ENU_TO_TILE^T. - - The frame rotation is a permutation with exact ±1 entries, so combining it - with the identity-quat fast path in ``_quat_to_rotation_matrix`` must - remain exact (no floating-point drift). - """ + """Identity quaternion should hit the fast path and yield identity R.""" pose = common_pb2.Pose( vec=common_pb2.Vec3(), quat=common_pb2.Quat(w=1.0), ) viewmat = pose_to_viewmat(pose) - expected_R = np.array( - [[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]], dtype=np.float32 - ) - np.testing.assert_array_equal(viewmat[:3, :3], expected_R) + np.testing.assert_array_equal(viewmat[:3, :3], np.eye(3, dtype=np.float32)) def test_world_origin_offset_shifts_camera_position(): - """``world_origin`` (in tile-local Y-up) zeroes the recentered translation. - - Camera at world ENU (10, 20, 30) rotates to tile-local Y-up (10, 30, -20); - passing that as ``world_origin`` recenters to the tile origin. - """ + """``world_origin`` (in tile-local) zeroes the recentered translation.""" from alpasim_splatsim_renderer.render_adapter import pose_to_viewmat pose = common_pb2.Pose( @@ -199,27 +182,11 @@ def test_world_origin_offset_shifts_camera_position(): quat=common_pb2.Quat(w=1.0), ) viewmat = pose_to_viewmat( - pose, world_origin=np.array([10.0, 30.0, -20.0], dtype=np.float32) + pose, world_origin=np.array([10.0, 20.0, 30.0], dtype=np.float32) ) np.testing.assert_allclose(viewmat[:3, 3], np.zeros(3), atol=1e-6) -def test_zup_to_yup_axis_mapping(): - """+Z (up) in alpasim ENU must become +Y (up) in splatsim tile-local. - - Regression guard for the frame conversion. Verified at the sensor-to-world - level (no inversion) so the assertion reads as a direct position swap. - """ - from alpasim_splatsim_renderer.render_adapter import pose_to_sensor_to_world - - pose = common_pb2.Pose( - vec=common_pb2.Vec3(x=0.0, y=0.0, z=5.0), - quat=common_pb2.Quat(w=1.0), - ) - s2w = pose_to_sensor_to_world(pose) - np.testing.assert_allclose(s2w[:3, 3], np.array([0.0, 5.0, 0.0]), atol=1e-6) - - def test_pose_to_sensor_to_world_is_inverse_of_viewmat(): """``pose_to_sensor_to_world`` returns the un-inverted sensor→world 4x4.""" from alpasim_splatsim_renderer.render_adapter import ( From c56e403d475a09d6509de25de843b39b475e93e8 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Thu, 16 Jul 2026 00:05:09 +0900 Subject: [PATCH 10/13] Invert T_sensor_rig when emitting AvailableCamera.rig_to_camera MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit `T_sensor_rig` in NuRec/3dgs_io USDZ exports is the rig→sensor matrix (OpenCV +Z-forward), not sensor-in-rig — documented in splatsim's own _usdz.py and verified empirically here (a probe rendering three candidate viewmats against splatsim's `iter_world_to_camera` matches this convention to fp precision, and the raw matrix places the camera 1.3m underground). The runtime composes `ego.compose(rig_to_camera)`, which requires camera-in-rig semantics, so emit the inverse of the raw field. Co-Authored-By: Claude Opus 4.7 --- .../alpasim_splatsim_renderer/server.py | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/src/splatsim_renderer/alpasim_splatsim_renderer/server.py b/src/splatsim_renderer/alpasim_splatsim_renderer/server.py index 3997675d..14922d2a 100644 --- a/src/splatsim_renderer/alpasim_splatsim_renderer/server.py +++ b/src/splatsim_renderer/alpasim_splatsim_renderer/server.py @@ -42,9 +42,10 @@ def _build_available_cameras_from_usdz( ) -> list["sensorsim_pb2.AvailableCamerasReturn.AvailableCamera"]: """Read camera_calibrations from the USDZ rig_trajectories.json. - Returns AvailableCamera entries with the ``T_sensor_rig`` extrinsics and - pinhole intrinsics from the USDZ. Non-fatal on any parse failure — an empty - list falls back to the original "no catalog" behavior. + Returns AvailableCamera entries with camera-in-rig extrinsics (inverted + from the USDZ's rig→sensor ``T_sensor_rig`` field, see below) and pinhole + intrinsics from the USDZ. Non-fatal on any parse failure — an empty list + falls back to the original "no catalog" behavior. """ if usdz_path is None: return [] @@ -80,9 +81,15 @@ def _build_available_cameras_from_usdz( width, height = params["resolution"] fx, fy, cx, cy = params["fx"], params["fy"], params["cx"], params["cy"] - T = np.asarray(cam["T_sensor_rig"], dtype=np.float64) - R = T[:3, :3] - t = T[:3, 3] + # ``T_sensor_rig`` in NuRec/3dgs_io exports is the rig→sensor + # matrix (OpenCV +Z forward), NOT sensor-in-rig — verified + # empirically both against splatsim's own ``iter_world_to_camera`` + # and in splatsim/_usdz.py's implementation. Invert it here so + # that alpasim's downstream ``ego.compose(rig_to_camera)`` sees + # a proper camera-in-rig transform. + T_raw = np.asarray(cam["T_sensor_rig"], dtype=np.float64) + R = T_raw[:3, :3].T + t = -R @ T_raw[:3, 3] # Matrix -> quaternion (xyzw). Uses Shepperd's method to avoid # scipy dependency here. trace = R[0, 0] + R[1, 1] + R[2, 2] From 20fef3142d7b94c097794a9b001752cc73d50292 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Thu, 16 Jul 2026 01:15:26 +0900 Subject: [PATCH 11/13] update controller Signed-off-by: Masaya Kataoka --- .../services/controller_service.py | 51 ++++++++++++++++--- 1 file changed, 45 insertions(+), 6 deletions(-) diff --git a/src/runtime/alpasim_runtime/services/controller_service.py b/src/runtime/alpasim_runtime/services/controller_service.py index 21fa12cd..50b9d2ce 100644 --- a/src/runtime/alpasim_runtime/services/controller_service.py +++ b/src/runtime/alpasim_runtime/services/controller_service.py @@ -42,6 +42,35 @@ class PropagatedPosesAtTime: dynamic_state_estimated: DynamicState # The estimated dynamic state +def _dynamic_state_from_trajectory( + trajectory: Trajectory, at_us: int, dt_us: int = 100_000 +) -> DynamicState: + # Without this, drivers see zero velocity during force-GT warmup and predict + # a stationary ego, which then bleeds into closed-loop and stops the car. + tr = trajectory.time_range_us + half = dt_us // 2 + start = max(int(tr.start), int(at_us) - half) + end = min(int(tr.stop) - 1, int(at_us) + half) + if end <= start: + return DynamicState() + delta = trajectory.interpolate_delta(start, end) + dt_s = (end - start) * 1e-6 + v = np.asarray(delta.vec3, dtype=np.float64) / dt_s + q = np.asarray(delta.quat, dtype=np.float64) # scipy [x, y, z, w] + xyz, w_c = q[:3], float(q[3]) + sin_half = float(np.linalg.norm(xyz)) + if sin_half < 1e-9: + omega = np.zeros(3, dtype=np.float64) + else: + omega = (xyz / sin_half) * (2.0 * np.arctan2(sin_half, w_c) / dt_s) + return DynamicState( + linear_velocity=Vec3(x=float(v[0]), y=float(v[1]), z=float(v[2])), + angular_velocity=Vec3( + x=float(omega[0]), y=float(omega[1]), z=float(omega[2]) + ), + ) + + class ControllerService(ServiceBase[VDCServiceStub]): """ Controller service implementation that handles both real and skip modes. @@ -169,8 +198,12 @@ def _ensure_intermediates( timestamp_us=t, pose_local_to_rig=pose, pose_local_to_rig_estimate=pose, - dynamic_state=DynamicState(), - dynamic_state_estimated=DynamicState(), + dynamic_state=_dynamic_state_from_trajectory( + fallback_trajectory_local_to_rig, int(t) + ), + dynamic_state_estimated=_dynamic_state_from_trajectory( + fallback_trajectory_local_to_rig, int(t) + ), ) for t, pose in zip(expected_intermediate_timestamps, poses, strict=True) ] @@ -221,13 +254,16 @@ async def run_controller_and_vehicle( fallback_pose_local_to_rig = ( fallback_trajectory_local_to_rig.interpolate_pose(future_us) ) + fallback_dyn = _dynamic_state_from_trajectory( + fallback_trajectory_local_to_rig, future_us + ) result = [ PropagatedPosesAtTime( timestamp_us=future_us, pose_local_to_rig=fallback_pose_local_to_rig, pose_local_to_rig_estimate=fallback_pose_local_to_rig, - dynamic_state=DynamicState(), - dynamic_state_estimated=DynamicState(), + dynamic_state=fallback_dyn, + dynamic_state_estimated=fallback_dyn, ) ] return self._ensure_intermediates( @@ -267,13 +303,16 @@ async def run_controller_and_vehicle( fallback_pose_local_to_rig = ( fallback_trajectory_local_to_rig.interpolate_pose(future_us) ) + fallback_dyn = _dynamic_state_from_trajectory( + fallback_trajectory_local_to_rig, future_us + ) result = [ PropagatedPosesAtTime( timestamp_us=future_us, pose_local_to_rig=fallback_pose_local_to_rig, pose_local_to_rig_estimate=fallback_pose_local_to_rig, - dynamic_state=DynamicState(), - dynamic_state_estimated=DynamicState(), + dynamic_state=fallback_dyn, + dynamic_state_estimated=fallback_dyn, ) ] elif response.states: From f5bdb2bb5bd63550f2eb39e39d043e3f0a0229dc Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 17 Jul 2026 11:16:54 +0900 Subject: [PATCH 12/13] Move OnePlanner container patches from /tmp into src/wizard/patches/ MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The wizard's driver=oneplanner service used to bind-mount four ad-hoc files from /tmp over the OnePlanner image, with no way to recover them if the local /tmp was cleared. Move the still-needed ones into the repo and reference them via ${repo-relative:'...'} so the wizard config is self-contained. Kept (still not upstreamed in oneplanner:local): * encoder.py — cast has_speed_limit to bool before torch.where. * model_loader.py — load StateNormalizer/ObservationNormalizer from the JSON that ships in the image, instead of falling back to identity. * driver_preprocessing.py — arc-length-upsample 20 route waypoints to the 500 that OnePlanner expects (25 segments × 20 points). Dropped: driver_server.py and driver_servicer.py — a rebuilt oneplanner:local now ships them (with a newer per-session .rrd dump loop that supersedes our per-tick fork). README documents the extract+diff flow so we can retire more of these when future images land the fixes. Also defines oneplanner_normalization_json under defines: so the mount path in oneplanner.yaml resolves without a hydra key error. Signed-off-by: Masaya Kataoka Co-Authored-By: Claude Opus 4.7 (1M context) --- src/wizard/configs/base_config.yaml | 6 + src/wizard/configs/driver/oneplanner.yaml | 22 +- src/wizard/patches/oneplanner/README.md | 38 + .../oneplanner/driver_preprocessing.py | 417 ++++++++++ src/wizard/patches/oneplanner/encoder.py | 759 ++++++++++++++++++ src/wizard/patches/oneplanner/model_loader.py | 187 +++++ 6 files changed, 1428 insertions(+), 1 deletion(-) create mode 100644 src/wizard/patches/oneplanner/README.md create mode 100644 src/wizard/patches/oneplanner/driver_preprocessing.py create mode 100644 src/wizard/patches/oneplanner/encoder.py create mode 100644 src/wizard/patches/oneplanner/model_loader.py diff --git a/src/wizard/configs/base_config.yaml b/src/wizard/configs/base_config.yaml index 0fa34d87..b8d9d369 100644 --- a/src/wizard/configs/base_config.yaml +++ b/src/wizard/configs/base_config.yaml @@ -48,6 +48,12 @@ defines: # (e.g. ``oneplanner_phase2_gridbev_*``); optional otherwise. oneplanner_bevfusion_config: "${oc.env:HOME}/workspace/OnePlanner/configs/perception/model/lidar_30e_j6gen2_120m.yaml" oneplanner_bevfusion_ckpt: "${oc.env:HOME}/workspace/OnePlanner/ckpts/bevfusion_extracted.pth" + # Planner input/output normalization stats (see driver/oneplanner.yaml). + # Required at inference — StateNormalizer / ObservationNormalizer read this + # JSON to reproduce the training-time normalization (ego mean=[10,0,0,0], + # std=[20,20,1,1], etc.). The OnePlanner Dockerfile does not COPY configs/, + # so it must be bind-mounted from the host checkout. + oneplanner_normalization_json: "${oc.env:HOME}/workspace/OnePlanner/configs/planner/normalization.json" renderer_entrypoint: "/app/internal/scripts/pycena/runtime/pycena_nrm_full" nre_max_workers: 4 diff --git a/src/wizard/configs/driver/oneplanner.yaml b/src/wizard/configs/driver/oneplanner.yaml index 50f12f41..6942b07e 100644 --- a/src/wizard/configs/driver/oneplanner.yaml +++ b/src/wizard/configs/driver/oneplanner.yaml @@ -54,11 +54,31 @@ services: # matching --bevfusion-* flags for planner-only smoke tests. - "${defines.oneplanner_bevfusion_config}:/opt/oneplanner/configs/perception/model.yaml" - "${defines.oneplanner_bevfusion_ckpt}:/opt/oneplanner/ckpts/bevfusion.pth" + # Planner normalization stats used by StateNormalizer/ObservationNormalizer. + # The image only copies scripts/ + src/, not configs/, so the patched + # model_loader (below) needs this JSON bind-mounted onto the expected path. + - "${defines.oneplanner_normalization_json}:/opt/oneplanner/configs/planner/normalization.json" - "${wizard.log_dir}:/mnt/output" # HOTFIX: cast has_speed_limit to bool before torch.where (upstream bug in # OnePlanner where map_tensors stores has_speed_limit as float32 but the # encoder expects a bool condition). Remove once fixed upstream. - - "/tmp/oneplanner_encoder_patched.py:/opt/oneplanner/src/oneplanner/models/planner/encoder.py" + - "${repo-relative:'src/wizard/patches/oneplanner/encoder.py'}:/opt/oneplanner/src/oneplanner/models/planner/encoder.py" + # HOTFIX: upstream deployment/model_loader.py defaults StateNormalizer + + # ObservationNormalizer to identity/empty because the checkpoint has no + # embedded "config" key. The model was trained with + # configs/planner/normalization.json (ego mean=[10,0,0,0], std=[20,20,1,1]), + # so identity normalizers at inference feed the DiT in the wrong scale and + # its predictions collapse to ~stationary. This patched loader loads the + # JSON that already ships in the image. + - "${repo-relative:'src/wizard/patches/oneplanner/model_loader.py'}:/opt/oneplanner/src/oneplanner/deployment/model_loader.py" + # HOTFIX: alpasim sends 20 route waypoints, but OnePlanner expects 500 + # (NUM_SEGMENTS_IN_ROUTE=25 * POINTS_PER_LANELET=20). Without upsampling, + # only 1/25 route_lanes segments carries signal → model sees "no route" + # → predicts stationary. This patched preprocessing arc-length-interpolates + # 20 waypoints into 500 before segmenting. Remove once alpasim RouteGenerator + # is updated to emit denser waypoints (or once we route through the actual + # lanelet2 map for route_lanes construction). + - "${repo-relative:'src/wizard/patches/oneplanner/driver_preprocessing.py'}:/opt/oneplanner/src/oneplanner/deployment/driver_preprocessing.py" environments: - PYTHONUNBUFFERED=1 command: diff --git a/src/wizard/patches/oneplanner/README.md b/src/wizard/patches/oneplanner/README.md new file mode 100644 index 00000000..d6d7db29 --- /dev/null +++ b/src/wizard/patches/oneplanner/README.md @@ -0,0 +1,38 @@ +# OnePlanner container patches + +These files bind-mount over the corresponding paths inside the +`oneplanner:local` docker image at wizard runtime (see +`src/wizard/configs/driver/oneplanner.yaml`). Each still exists because the +shipped image predates the fix, so we override the file until the upstream +change lands and the image is rebuilt. + +Each file mirrors the layout inside the container so the mapping is obvious: + +| File in this dir | Overrides in container | +|---|---| +| `encoder.py` | `/opt/oneplanner/src/oneplanner/models/planner/encoder.py` | +| `model_loader.py` | `/opt/oneplanner/src/oneplanner/deployment/model_loader.py` | +| `driver_preprocessing.py` | `/opt/oneplanner/src/oneplanner/deployment/driver_preprocessing.py` | + +## Purpose per patch + +- **encoder.py** — HOTFIX: cast `has_speed_limit` to bool before `torch.where` + (upstream stores it as float32 but the encoder expects a bool condition). + Delete once upstream is fixed. +- **model_loader.py** — HOTFIX: upstream defaults `StateNormalizer` and + `ObservationNormalizer` to identity/empty because the shipped checkpoint has + no embedded `config` key. This loader instead reads + `configs/planner/normalization.json` (ego mean/std) so predictions aren't + scaled by identity. See memory `oneplanner_deployment_normalizer_missing`. +- **driver_preprocessing.py** — HOTFIX: upsample 20 route waypoints (from + alpasim's RouteGenerator) to 500 (`NUM_SEGMENTS_IN_ROUTE=25 * + POINTS_PER_LANELET=20`) via arc-length interpolation. Without this, only + 1/25 route_lanes segments carries signal. See memory + `oneplanner_route_needs_500_waypoints`. + +## Maintenance + +When upstream OnePlanner ships a fix for any of these, delete the file here +and remove its bind-mount from `oneplanner.yaml`. Verify by extracting the +same path from the current image (`docker create --name op-extract +oneplanner:local sh && docker cp op-extract:/opt/... /tmp/...`) and diffing. diff --git a/src/wizard/patches/oneplanner/driver_preprocessing.py b/src/wizard/patches/oneplanner/driver_preprocessing.py new file mode 100644 index 00000000..3a74dd73 --- /dev/null +++ b/src/wizard/patches/oneplanner/driver_preprocessing.py @@ -0,0 +1,417 @@ +"""Map alpasim EgoDriver observations to OnePlanner sample tensors. + +The session-state observations submit_* RPCs push into the servicer arrive in +alpasim's frame conventions: + - ego trajectory poses are rig poses in the *local* world frame + - LiDAR xyz is in the LiDAR (≈ rig) frame at the end-of-spin + - route waypoints are in the rig frame at the route timestamp + +The planner consumes everything in the current ego frame at ``time_now_us``. +This module owns that frame conversion and the construction of the sample +dict that ``E2EDataset.__getitem__`` produces — so the model sees the same +contract whether the batch came from the training set or live alpasim. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import Sequence + +import numpy as np +from scipy.spatial.transform import Rotation + +from oneplanner import constants as C +from oneplanner.deployment.hdmap import ( + FrameAlignment, + LaneletMap, + build_map_tensors, +) + + +@dataclass +class EgoPoseSample: + """One rig pose in local frame at ``timestamp_us``, plus its dynamic state.""" + + timestamp_us: int + # 4x4 active transform local -> rig (column-major math; we store the matrix + # form to avoid carrying a quaternion library at the data layer). + local_to_rig: np.ndarray + # [vx, vy, ax, ay, yaw_rate] in rig frame; missing entries default to 0. + dynamic_state: np.ndarray + + +@dataclass +class SessionObservations: + """Cumulative state populated by submit_* RPCs and read by ``drive``.""" + + ego_history: list[EgoPoseSample] = field(default_factory=list) + lidar_xyz: np.ndarray | None = None # [N, 3] in LiDAR frame + lidar_intensity: np.ndarray | None = None # [N] + route_waypoints_rig: np.ndarray | None = None # [K, 3] in rig frame + route_timestamp_us: int | None = None + ego_shape: np.ndarray = field( + default_factory=lambda: np.array([2.7, 4.5, 1.85], dtype=np.float32) + ) + + +# ---------------------------------------------------------------------------- +# Wire-format decoders +# ---------------------------------------------------------------------------- + + +def decode_lidar_buffers( + point_xyzs_buffer: bytes, + point_intensities_buffer: bytes, + point_ring_ids_buffer: bytes, + num_points: int, +) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + """Return (xyz[N,3] float32, intensity[N] float32, ring[N] uint16).""" + if num_points == 0: + return ( + np.zeros((0, 3), dtype=np.float32), + np.zeros((0,), dtype=np.float32), + np.zeros((0,), dtype=np.uint16), + ) + xyz = np.frombuffer(point_xyzs_buffer, dtype=np.float32).reshape(num_points, 3) + intensity = np.frombuffer(point_intensities_buffer, dtype=np.float32) + ring = np.frombuffer(point_ring_ids_buffer, dtype=np.uint16) + if intensity.shape[0] != num_points or ring.shape[0] != num_points: + raise ValueError( + f"LiDAR buffer length mismatch: xyz={num_points}, " + f"intensity={intensity.shape[0]}, ring={ring.shape[0]}" + ) + return xyz.astype(np.float32, copy=False), intensity.astype(np.float32, copy=False), ring + + +def quat_to_rotmat(w: float, x: float, y: float, z: float) -> np.ndarray: + """Hamilton-quaternion -> 3x3 rotation matrix (right-handed, active).""" + if w == 0.0 and x == 0.0 and y == 0.0 and z == 0.0: + return np.eye(3, dtype=np.float64) + return Rotation.from_quat([x, y, z, w]).as_matrix().astype(np.float64) + + +def pose_proto_to_matrix(pose) -> np.ndarray: + """common.Pose -> 4x4 active transform (translate then rotate per proto convention).""" + q = pose.quat + v = pose.vec + R = quat_to_rotmat(q.w, q.x, q.y, q.z) + T = np.eye(4, dtype=np.float64) + T[:3, :3] = R + T[:3, 3] = [v.x, v.y, v.z] + return T + + +def yaw_from_rotmat(R: np.ndarray) -> float: + """Extract yaw (rotation about +Z) from a 3x3 rotation matrix.""" + return float(np.arctan2(R[1, 0], R[0, 0])) + + +def vec3_proto_to_xyz(v) -> tuple[float, float, float]: + return float(v.x), float(v.y), float(v.z) + + +def dynamic_state_to_vec(dyn_state) -> np.ndarray: + """common.DynamicState -> [vx, vy, ax, ay, yaw_rate] in rig frame.""" + lv = dyn_state.linear_velocity + la = dyn_state.linear_acceleration + av = dyn_state.angular_velocity + return np.array( + [lv.x, lv.y, la.x, la.y, av.z], + dtype=np.float32, + ) + + +# ---------------------------------------------------------------------------- +# Sample construction +# ---------------------------------------------------------------------------- + +_PAST_LEN = C.INPUT_T + 1 # 31: 30 past + current +_FUTURE_LEN = C.OUTPUT_T # 80 + + +def _pick_current(history: Sequence[EgoPoseSample], time_now_us: int) -> EgoPoseSample: + """Most recent ego pose at or before ``time_now_us``; falls back to newest.""" + if not history: + raise ValueError("ego_history is empty — submit_egomotion_observation before drive()") + eligible = [s for s in history if s.timestamp_us <= time_now_us] + return eligible[-1] if eligible else history[-1] + + +def build_ego_past_in_current_frame( + history: Sequence[EgoPoseSample], + current: EgoPoseSample, +) -> np.ndarray: + """Construct ``ego_agent_past[31, 4]`` = [x_ego, y_ego, cos_yaw, sin_yaw], 10 Hz. + + The current pose is placed at index 30 as the origin; older poses are + sampled at 100 ms steps backward and re-expressed in the current rig + frame. When a step has no observation, the nearest earlier sample is + used; samples before any observation are zero-filled (origin). + """ + out = np.zeros((_PAST_LEN, 4), dtype=np.float32) + out[-1] = [0.0, 0.0, 1.0, 0.0] + if not history: + return out + + rig_to_local_now = current.local_to_rig + local_to_rig_now = np.linalg.inv(rig_to_local_now) + + # Build a quick searchable timeline. + ts = np.array([s.timestamp_us for s in history], dtype=np.int64) + order = np.argsort(ts) + ts_sorted = ts[order] + history_sorted = [history[i] for i in order] + + step_us = 100_000 # 10 Hz + for k in range(_PAST_LEN - 1): + target_us = current.timestamp_us - (_PAST_LEN - 1 - k) * step_us + idx = int(np.searchsorted(ts_sorted, target_us, side="right") - 1) + if idx < 0: + continue + sample = history_sorted[idx] + local_to_rig_k = sample.local_to_rig # rig_k in local + # Express rig_k in current rig frame: + rig_k_in_rig_now = local_to_rig_now @ local_to_rig_k + x, y = rig_k_in_rig_now[0, 3], rig_k_in_rig_now[1, 3] + yaw = yaw_from_rotmat(rig_k_in_rig_now[:3, :3]) + out[k] = [x, y, np.cos(yaw), np.sin(yaw)] + return out + + +def build_ego_current_state(current: EgoPoseSample) -> np.ndarray: + """``ego_current_state[10]`` = [0, 0, 1, 0, vx, vy, ax, ay, steer=0, yaw_rate].""" + dyn = current.dynamic_state + if dyn.shape[0] < 5: + dyn = np.pad(dyn, (0, 5 - dyn.shape[0])) + vx, vy, ax, ay, yaw_rate = dyn[:5] + return np.array( + [0.0, 0.0, 1.0, 0.0, vx, vy, ax, ay, 0.0, yaw_rate], + dtype=np.float32, + ) + + +def build_route_lanes_from_waypoints( + route_waypoints_rig: np.ndarray | None, + route_timestamp_us: int | None, + current: EgoPoseSample, + route_history_for_timestamp: Sequence[EgoPoseSample] = (), +) -> np.ndarray: + """``route_lanes[25, 20, 33]`` derived from the alpasim Route message. + + The route is given in the rig frame *at its own* timestamp; we re-express + waypoints in the current rig frame by composing through `local`. Each + 20-point lanelet is one stride along the route; we pad to + NUM_SEGMENTS_IN_ROUTE with zeros. Only the X/Y position and tangent + direction channels are filled — lane boundaries, traffic-light state, and + line-type one-hots stay zero because the alpasim wire format doesn't + carry them. + """ + out = np.zeros( + (C.NUM_SEGMENTS_IN_ROUTE, C.POINTS_PER_LANELET, C.SEGMENT_POINT_DIM), + dtype=np.float32, + ) + if route_waypoints_rig is None or len(route_waypoints_rig) == 0: + return out + + # Re-express route waypoints into the current rig frame. + if route_timestamp_us is not None and route_history_for_timestamp: + route_anchor = _pick_current(route_history_for_timestamp, route_timestamp_us) + local_to_rig_route = route_anchor.local_to_rig + else: + # No anchor — assume route is already in the current rig frame. + local_to_rig_route = current.local_to_rig + + local_to_rig_now_inv = np.linalg.inv(current.local_to_rig) + rig_route_to_rig_now = local_to_rig_now_inv @ local_to_rig_route + + wp_h = np.concatenate( + [ + route_waypoints_rig, + np.ones((route_waypoints_rig.shape[0], 1), dtype=route_waypoints_rig.dtype), + ], + axis=1, + ) # [K, 4] + wp_now = (rig_route_to_rig_now @ wp_h.T).T[:, :3] # [K, 3] + + # HOTFIX: alpasim sends only NUM_WAYPOINTS=20 points but this function + # expects NUM_SEGMENTS_IN_ROUTE*POINTS_PER_LANELET=500 to fill all 25 + # lanelet slots. With 20 points we fill only segment 0 and pad the rest + # with zeros, which trains OnePlanner to see "no route" and predict a + # stationary trajectory. Upsample by arc-length linear interpolation so + # every segment gets meaningful X/Y/tangent signal. + pts_per = C.POINTS_PER_LANELET + target_pts = C.NUM_SEGMENTS_IN_ROUTE * pts_per + if wp_now.shape[0] >= 2 and wp_now.shape[0] < target_pts: + seg_lens = np.linalg.norm(np.diff(wp_now[:, :2], axis=0), axis=1) + s = np.concatenate([[0.0], np.cumsum(seg_lens)]) # cumulative arc length + total_arc = float(s[-1]) + if total_arc > 1e-6: + s_new = np.linspace(0.0, total_arc, target_pts) + wp_new = np.empty((target_pts, 3), dtype=wp_now.dtype) + for axis in range(3): + wp_new[:, axis] = np.interp(s_new, s, wp_now[:, axis]) + wp_now = wp_new + + # Stride into NUM_SEGMENTS_IN_ROUTE lanelets of POINTS_PER_LANELET points. + total_pts = wp_now.shape[0] + max_pts_used = min(total_pts, C.NUM_SEGMENTS_IN_ROUTE * pts_per) + used = wp_now[:max_pts_used] + + # Compute tangent directions via centered differences (forward at endpoints). + if used.shape[0] >= 2: + diffs = np.zeros_like(used[:, :2]) + diffs[1:-1] = used[2:, :2] - used[:-2, :2] + diffs[0] = used[1, :2] - used[0, :2] + diffs[-1] = used[-1, :2] - used[-2, :2] + norms = np.linalg.norm(diffs, axis=1, keepdims=True) + norms = np.where(norms > 1e-6, norms, 1.0) + tangents = diffs / norms + else: + tangents = np.zeros((used.shape[0], 2), dtype=used.dtype) + + for seg_idx in range(C.NUM_SEGMENTS_IN_ROUTE): + lo, hi = seg_idx * pts_per, (seg_idx + 1) * pts_per + if lo >= used.shape[0]: + break + seg = used[lo : min(hi, used.shape[0])] + seg_tan = tangents[lo : min(hi, used.shape[0])] + n = seg.shape[0] + out[seg_idx, :n, 0] = seg[:, 0] # X + out[seg_idx, :n, 1] = seg[:, 1] # Y + out[seg_idx, :n, 2] = seg_tan[:, 0] # dX + out[seg_idx, :n, 3] = seg_tan[:, 1] # dY + return out + + +def build_goal_pose_from_route( + route_waypoints_rig: np.ndarray | None, + route_timestamp_us: int | None, + current: EgoPoseSample, + route_history_for_timestamp: Sequence[EgoPoseSample] = (), +) -> np.ndarray: + """``goal_pose[4]`` = [gx, gy, cos_h, sin_h] in current ego frame. + + Heading is derived from the tangent at the last waypoint. Returns zeros + when no route has been submitted. + """ + out = np.zeros((4,), dtype=np.float32) + if route_waypoints_rig is None or len(route_waypoints_rig) == 0: + return out + + if route_timestamp_us is not None and route_history_for_timestamp: + route_anchor = _pick_current(route_history_for_timestamp, route_timestamp_us) + local_to_rig_route = route_anchor.local_to_rig + else: + local_to_rig_route = current.local_to_rig + + rig_route_to_rig_now = np.linalg.inv(current.local_to_rig) @ local_to_rig_route + wp_h = np.concatenate( + [ + route_waypoints_rig, + np.ones((route_waypoints_rig.shape[0], 1), dtype=route_waypoints_rig.dtype), + ], + axis=1, + ) + wp_now = (rig_route_to_rig_now @ wp_h.T).T[:, :3] + + gx, gy = wp_now[-1, 0], wp_now[-1, 1] + if wp_now.shape[0] >= 2: + tan = wp_now[-1, :2] - wp_now[-2, :2] + heading = float(np.arctan2(tan[1], tan[0])) + else: + heading = 0.0 + out[0], out[1] = gx, gy + out[2], out[3] = np.cos(heading), np.sin(heading) + return out + + +def build_sample( + obs: SessionObservations, + time_now_us: int, + *, + lanelet_map: LaneletMap | None = None, + alignment: FrameAlignment | None = None, +) -> dict[str, np.ndarray]: + """Construct the per-sample dict ``e2e_collate_fn`` consumes. + + Only the keys needed by ``E2EPlanner.forward`` at eval time are populated; + map fidelity is intentionally lossy (only ``route_lanes`` is non-zero) + because the alpasim contract doesn't carry an HD-map graph. See module + docstring. + """ + current = _pick_current(obs.ego_history, time_now_us) + + ego_past = build_ego_past_in_current_frame(obs.ego_history, current) + ego_now = build_ego_current_state(current) + route_lanes = build_route_lanes_from_waypoints( + obs.route_waypoints_rig, obs.route_timestamp_us, current, obs.ego_history + ) + goal = build_goal_pose_from_route( + obs.route_waypoints_rig, obs.route_timestamp_us, current, obs.ego_history + ) + + # LiDAR -> [N, 5] (x, y, z, intensity, ring_as_float). Empty cloud + # passes a single zero row so downstream voxelization doesn't choke on + # an all-empty batch. + if obs.lidar_xyz is None or obs.lidar_xyz.shape[0] == 0: + points = np.zeros((1, 5), dtype=np.float32) + else: + n = obs.lidar_xyz.shape[0] + points = np.zeros((n, 5), dtype=np.float32) + points[:, :3] = obs.lidar_xyz + if obs.lidar_intensity is not None and obs.lidar_intensity.shape[0] == n: + points[:, 3] = obs.lidar_intensity + # ring channel kept as zero — alpasim ring ids are uint16 and the + # trained voxelizer treats this column as a feature, not an index. + + # Map tokens: driven by the USDZ-bundled lanelet2 map when the caller + # supplies both ``lanelet_map`` and ``alignment`` (the standard deployment + # path — the driver servicer loads them once per scene at startup). When + # either is missing we fall back to zero-padded tokens; the encoder handles + # zero-padded segments through its standard segment-mask path (used for + # short-context training scenes), so this is a graceful degradation rather + # than an invalid input. + if lanelet_map is not None and alignment is not None: + map_tensors = build_map_tensors( + lanelet_map, + alignment, + current.local_to_rig, + ) + lanes = map_tensors["lanes"] + lanes_sl = map_tensors["lanes_speed_limit"] + lanes_has_sl = map_tensors["lanes_has_speed_limit"] + polygons = map_tensors["polygons"] + line_strings = map_tensors["line_strings"] + else: + lanes = np.zeros( + (C.NUM_SEGMENTS_IN_LANE, C.POINTS_PER_LANELET, C.SEGMENT_POINT_DIM), + dtype=np.float32, + ) + lanes_sl = np.zeros((C.NUM_SEGMENTS_IN_LANE, 1), dtype=np.float32) + lanes_has_sl = np.zeros((C.NUM_SEGMENTS_IN_LANE, 1), dtype=np.float32) + polygons = np.zeros((C.NUM_POLYGONS, C.POINTS_PER_POLYGON, 3), dtype=np.float32) + line_strings = np.zeros((C.NUM_LINE_STRINGS, C.POINTS_PER_LINE_STRING, 4), dtype=np.float32) + + # route_lanes speed-limit stays zero either way — route_lanes are built + # from ego waypoints (build_route_lanes_from_waypoints above), not looked + # up against the HD map, so we have no per-route-segment limit to attach. + route_sl = np.zeros((C.NUM_SEGMENTS_IN_ROUTE, 1), dtype=np.float32) + route_has_sl = np.zeros((C.NUM_SEGMENTS_IN_ROUTE, 1), dtype=np.float32) + + return { + "ego_agent_past": ego_past, + "ego_current_state": ego_now, + "ego_agent_future": np.zeros((_FUTURE_LEN, 3), dtype=np.float32), + "lanes": lanes, + "lanes_speed_limit": lanes_sl, + "lanes_has_speed_limit": lanes_has_sl, + "route_lanes": route_lanes, + "route_lanes_speed_limit": route_sl, + "route_lanes_has_speed_limit": route_has_sl, + "polygons": polygons, + "line_strings": line_strings, + "ego_shape": obs.ego_shape.astype(np.float32, copy=False), + "turn_indicators": np.zeros((_PAST_LEN,), dtype=np.int32), + "goal_pose": goal, + "points": points, + } diff --git a/src/wizard/patches/oneplanner/encoder.py b/src/wizard/patches/oneplanner/encoder.py new file mode 100644 index 00000000..863c22a1 --- /dev/null +++ b/src/wizard/patches/oneplanner/encoder.py @@ -0,0 +1,759 @@ +import torch +import torch.nn as nn +import torch.nn.functional as F +from timm.layers import DropPath, Mlp + +from oneplanner.constants import ( + CLASS_TYPE_EGO, + CLASS_TYPE_EGO_SHAPE, + CLASS_TYPE_GOAL_POSE, + CLASS_TYPE_LANE, + CLASS_TYPE_LINE_STRING, + CLASS_TYPE_NEIGHBOR, + CLASS_TYPE_POLYGON, + CLASS_TYPE_ROUTE, + INPUT_T, + LINE_STRING_TYPE_NUM, + POLYGON_TYPE_NUM, +) +from oneplanner.models.planner.mixer import MixerBlock + +# Width of the ORIGINAL DPlanner one-hot class vocabulary (ids 0..9, the +# constants above). Deliberately NOT ``constants.CLASS_TYPE_NUM`` (= 11): that +# includes the legacy "perception" slot (id 10) used only by ``E2EEncoder``, +# which pads 10 -> 11 at ``e2e_encoder._pad_pos``. Changing this value widens +# every pos tensor and ``pos_emb`` (Linear(4+10, D) here) and breaks every +# DPlanner checkpoint — see constants.py for the full story. +CLASS_TYPE_NUM = 10 + + +def add_class_type(x, class_type): + """ + Add class type to the input tensor. + Args: + x: Tensor of shape (B, T, D=4) where D=4 represents (x, y, cos, sin) + class_type: Class type to add (int) + Returns: + x: Tensor with class type added at the end + """ + B, T, D = x.shape + assert D == 4, "Input tensor must have 4 features (x, y, cos, sin)" + class_type_tensor = torch.zeros((B, T, CLASS_TYPE_NUM), device=x.device, dtype=x.dtype) + class_type_tensor[..., class_type] = 1.0 + return torch.cat([x, class_type_tensor], dim=-1) + + +class Encoder(nn.Module): + def __init__(self, config): + super().__init__() + + self.hidden_dim = config.hidden_dim + + self.use_ego_history = config.use_ego_history + self.ego_history_dropout_rate = config.ego_history_dropout_rate + self.use_turn_indicators = config.use_turn_indicators + + ego_num = 1 + goal_pose_num = 1 + ego_shape_num = 1 + turn_indicator_num = 1 + # agent_num (neighbors) removed from token count — neighbor branch no longer used + self.token_num = ( + ego_num + + config.lane_num + + config.route_num + + config.polygon_num + + config.line_string_num + + goal_pose_num + + ego_shape_num + + turn_indicator_num + ) + + self.ego_encoder = EgoEncoder( + config.time_len, + drop_path_rate=config.encoder_drop_path_rate, + hidden_dim=config.hidden_dim, + depth=config.encoder_mixer_depth, + ) + # neighbor_encoder removed — neighbor branch no longer used + self.lane_encoder = LaneEncoder( + config.lane_len, + class_type=CLASS_TYPE_LANE, + drop_path_rate=config.encoder_drop_path_rate, + hidden_dim=config.hidden_dim, + depth=config.encoder_mixer_depth, + ) + self.route_encoder = LaneEncoder( + config.route_len, + class_type=CLASS_TYPE_ROUTE, + drop_path_rate=config.encoder_drop_path_rate, + hidden_dim=config.hidden_dim, + depth=config.encoder_mixer_depth, + ) + self.polygon_encoder = LineEncoder( + config.polygon_len, + class_type=CLASS_TYPE_POLYGON, + drop_path_rate=config.encoder_drop_path_rate, + hidden_dim=config.hidden_dim, + depth=config.encoder_mixer_depth, + point_dim=2 + POLYGON_TYPE_NUM, + ) + self.line_string_encoder = LineEncoder( + config.line_string_len, + class_type=CLASS_TYPE_LINE_STRING, + drop_path_rate=config.encoder_drop_path_rate, + hidden_dim=config.hidden_dim, + depth=config.encoder_mixer_depth, + point_dim=2 + LINE_STRING_TYPE_NUM, + ) + self.goal_pose_encoder = GoalPoseEncoder( + drop_path_rate=config.encoder_drop_path_rate, + hidden_dim=config.hidden_dim, + ) + self.ego_shape_encoder = FloatsEncoder( + num_float=3, + drop_path_rate=config.encoder_drop_path_rate, + hidden_dim=config.hidden_dim, + ) + self.turn_indicator_encoder = FloatsEncoder( + num_float=INPUT_T, + drop_path_rate=config.encoder_drop_path_rate, + hidden_dim=config.hidden_dim, + ) + + self.fusion = FusionEncoder( + hidden_dim=config.hidden_dim, + num_heads=config.num_heads, + drop_path_rate=config.encoder_drop_path_rate, + depth=config.encoder_fusion_depth, + ) + + # position embedding encode x, y, cos, sin, type + self.pos_emb = nn.Linear(4 + CLASS_TYPE_NUM, config.hidden_dim) + + # positional embedding for route + self.route_position_embedding = nn.Parameter( + torch.randn(1, config.route_num, config.hidden_dim) + ) + + # Initialize transformer layers: + def _basic_init(m): + if isinstance(m, nn.Linear): + torch.nn.init.xavier_uniform_(m.weight) + if isinstance(m, nn.Linear) and m.bias is not None: + nn.init.constant_(m.bias, 0) + elif isinstance(m, nn.LayerNorm): + nn.init.constant_(m.bias, 0) + nn.init.constant_(m.weight, 1.0) + elif isinstance(m, nn.Embedding): + nn.init.normal_(m.weight, mean=0.0, std=0.02) + + self.apply(_basic_init) + + # Initialize embedding MLP: + nn.init.normal_(self.pos_emb.weight, std=0.02) + nn.init.normal_(self.lane_encoder.speed_limit_emb.weight, std=0.02) + nn.init.normal_(self.lane_encoder.attribute_emb.weight, std=0.02) + + def forward(self, inputs): + B = inputs["ego_agent_past"].shape[0] + encodings, masks, positions = self._encode_segments(inputs) + return self._fuse_segments(encodings, masks, positions, B) + + def _encode_segments(self, inputs): + """Run every per-stream encoder; return ordered (encodings, masks, positions). + + The 8 segments are returned in the fixed planner order + (ego, lanes, route, polygon, line_string, goal_pose, ego_shape, + turn_indicator) — the single source of truth the fusion + bridge rely on. + """ + # ego agent — clone to avoid in-place mutation of shared batch tensors + # (critical for DPO / multi-sample inference which reuse the same batch) + ego = inputs["ego_agent_past"].clone() # (B, T=INPUT_T + 1, D=4) + if not self.use_ego_history: + ego = torch.zeros_like(ego) + ego[:, 6:] *= 0.0 # Only keep the current + first 5 steps of ego history + + # neighbor_agents_past removed — neighbor branch no longer used + + # vector maps + lanes = inputs["lanes"] # (B, P=70, V=20, D=13) + lanes_speed_limit = inputs["lanes_speed_limit"] # (B, P=70, V=20, D=1) + lanes_has_speed_limit = inputs["lanes_has_speed_limit"] # (B, P=70, V=20, D=1) + + # route + route = inputs["route_lanes"] # (B, P=25, V=20, D=13) + route_speed_limit = inputs["route_lanes_speed_limit"] # (B, P=25, V=20, D=1) + route_has_speed_limit = inputs["route_lanes_has_speed_limit"] # (B, P=25, V=20, D=1) + + # polygons + polygons = inputs["polygons"] # (B, P=10, V=40, D=2) + + # line strings + line_strings = inputs["line_strings"] # (B, P=10, V=20, D=2) + + # goal pose + goal_pose = inputs["goal_pose"] # (B, D=4) + + # ego shape + ego_shape = inputs["ego_shape"] # (B, D=3) + + # turn indicator + turn_indicator = inputs["turn_indicators"][:, :-1] # (B, T) + turn_indicator = turn_indicator.float() + if not self.use_turn_indicators: + turn_indicator = torch.zeros_like(turn_indicator) + + B = ego.shape[0] + + encoding_ego, ego_mask, ego_pos = self.ego_encoder(ego) + + if self.ego_history_dropout_rate > 0: + encoding_ego = F.dropout( + encoding_ego, p=self.ego_history_dropout_rate, training=self.training + ) + + encoding_lanes, lanes_mask, lane_pos = self.lane_encoder( + lanes, lanes_speed_limit, lanes_has_speed_limit + ) + encoding_route, route_mask, route_pos = self.route_encoder( + route, route_speed_limit, route_has_speed_limit + ) + encoding_polygon, polygon_mask, polygon_pos = self.polygon_encoder(polygons) + encoding_line_string, line_string_mask, line_string_pos = self.line_string_encoder( + line_strings + ) + + # add positional embedding for route + route_num = encoding_route.shape[1] + route_position_emb = self.route_position_embedding[:, :route_num] # (1, P, hidden_dim) + route_position_emb = route_position_emb.expand(B, -1, -1) # (B, P, hidden_dim) + valid_route_mask = ~route_mask + encoding_route = ( + encoding_route + route_position_emb * valid_route_mask.unsqueeze(-1).float() + ) + + encoding_goal_pose, goal_pose_mask, goal_pose_pos = self.goal_pose_encoder(goal_pose) + encoding_ego_shape, ego_shape_mask, ego_shape_pos = self.ego_shape_encoder(ego_shape) + encoding_turn_indicator, turn_indicator_mask, turn_indicator_pos = ( + self.turn_indicator_encoder(turn_indicator) + ) + + encodings = [ + encoding_ego, + encoding_lanes, + encoding_route, + encoding_polygon, + encoding_line_string, + encoding_goal_pose, + encoding_ego_shape, + encoding_turn_indicator, + ] + masks = [ + ego_mask, + lanes_mask, + route_mask, + polygon_mask, + line_string_mask, + goal_pose_mask, + ego_shape_mask, + turn_indicator_mask, + ] + positions = [ + ego_pos, + lane_pos, + route_pos, + polygon_pos, + line_string_pos, + goal_pose_pos, + ego_shape_pos, + turn_indicator_pos, + ] + return encodings, masks, positions + + def _fuse_segments(self, encodings, masks, positions, B): + """Concat per-stream tokens, add the masked positional embedding, fuse.""" + encoding_input = torch.cat(encodings, dim=1) + encoding_mask = torch.cat(masks, dim=1).view(-1) + encoding_pos = torch.cat(positions, dim=1).view(B * self.token_num, -1) + encoding_pos = self.pos_emb(encoding_pos[~encoding_mask]) + encoding_pos_result = torch.zeros( + (B * self.token_num, self.hidden_dim), + device=encoding_pos.device, + dtype=encoding_pos.dtype, + ) + encoding_pos_result[~encoding_mask] = encoding_pos # Fill in valid parts + + encoding_input = encoding_input + encoding_pos_result.view(B, self.token_num, -1) + + return self.fusion(encoding_input, encoding_mask.view(B, self.token_num)) + + +class SelfAttentionBlock(nn.Module): + def __init__(self, dim, heads, dropout): + super().__init__() + mlp_ratio = 4.0 + + self.norm1 = nn.LayerNorm(dim) + self.attn = nn.MultiheadAttention(dim, heads, dropout, batch_first=True) + + self.drop_path = DropPath(dropout) if dropout > 0.0 else nn.Identity() + self.norm2 = nn.LayerNorm(dim) + mlp_hidden_dim = int(dim * mlp_ratio) + self.mlp = Mlp( + in_features=dim, hidden_features=mlp_hidden_dim, act_layer=nn.GELU, drop=dropout + ) + # SDPA fast path (perf.sdpa_fusion_encoder, flipped by + # training/compile.py): need_weights=False skips materializing the + # [B*heads, N, N] attention probs per layer. Default False == + # need_weights=True == the current default argument — bit-identical. + self.sdpa_only: bool = False + + def forward(self, x, mask): + x = x + self.drop_path( + self.attn(self.norm1(x), x, x, key_padding_mask=mask, need_weights=not self.sdpa_only)[ + 0 + ] + ) + x = x + self.drop_path(self.mlp(self.norm2(x))) + return x + + +class EgoEncoder(nn.Module): + def __init__(self, time_len, drop_path_rate, hidden_dim, depth): + super().__init__() + tokens_mlp_dim = 64 + channels_mlp_dim = 128 + + self._hidden_dim = hidden_dim + + self.channel_pre_project = Mlp( + in_features=4, + hidden_features=channels_mlp_dim, + out_features=channels_mlp_dim, + act_layer=nn.GELU, + drop=0.0, + ) + self.token_pre_project = Mlp( + in_features=time_len, + hidden_features=tokens_mlp_dim, + out_features=tokens_mlp_dim, + act_layer=nn.GELU, + drop=0.0, + ) + + self.blocks = nn.ModuleList( + [MixerBlock(tokens_mlp_dim, channels_mlp_dim, drop_path_rate) for i in range(depth)] + ) + + self.norm = nn.LayerNorm(channels_mlp_dim) + self.emb_project = Mlp( + in_features=channels_mlp_dim, + hidden_features=hidden_dim, + out_features=hidden_dim, + act_layer=nn.GELU, + drop=drop_path_rate, + ) + + def forward(self, x): + """ + x: B, T=21, D=4 (x, y, cos, sin) + """ + B, T, D = x.shape + pos = x[:, -1].clone() # (B, D=4[x, y, cos, sin]) + pos = pos.unsqueeze(1) # (B, 1, D=4) + pos = add_class_type(pos, CLASS_TYPE_EGO) + + mask = torch.zeros((B, 1), dtype=torch.bool, device=x.device) + + x = self.channel_pre_project(x) + x = x.permute(0, 2, 1) + x = self.token_pre_project(x) + x = x.permute(0, 2, 1) + + for block in self.blocks: + x = block(x) + + # pooling + x = torch.mean(x, dim=1, keepdim=True) # (B, 1, C=channels_mlp_dim) + + x = self.emb_project(self.norm(x)) # (B, hidden_dim) + + return x, mask, pos + + +class NeighborEncoder(nn.Module): + def __init__(self, time_len, drop_path_rate, hidden_dim, depth): + super().__init__() + tokens_mlp_dim = 64 + channels_mlp_dim = 128 + + self._hidden_dim = hidden_dim + + self.type_emb = nn.Linear(3, channels_mlp_dim) + + self.channel_pre_project = Mlp( + in_features=8 + 1, + hidden_features=channels_mlp_dim, + out_features=channels_mlp_dim, + act_layer=nn.GELU, + drop=0.0, + ) + self.token_pre_project = Mlp( + in_features=time_len, + hidden_features=tokens_mlp_dim, + out_features=tokens_mlp_dim, + act_layer=nn.GELU, + drop=0.0, + ) + + self.blocks = nn.ModuleList( + [MixerBlock(tokens_mlp_dim, channels_mlp_dim, drop_path_rate) for i in range(depth)] + ) + + self.norm = nn.LayerNorm(channels_mlp_dim) + self.emb_project = Mlp( + in_features=channels_mlp_dim, + hidden_features=hidden_dim, + out_features=hidden_dim, + act_layer=nn.GELU, + drop=drop_path_rate, + ) + + def forward(self, x): + """ + x: B, P, V, D (x, y, cos, sin, vx, vy, w, l, type(3)) + """ + neighbor_type = x[:, :, -1, 8:] + x = x[..., :8] + + pos = x[:, :, -1, :4].clone() # x, y, cos, sin + pos = add_class_type(pos, CLASS_TYPE_NEIGHBOR) + + B, P, V, _ = x.shape + mask_v = torch.sum(torch.ne(x[..., :8], 0), dim=-1).to(x.device) == 0 + mask_p = torch.sum(~mask_v, dim=-1) == 0 + x = torch.cat([x, (~mask_v).float().unsqueeze(-1)], dim=-1) + x = x.view(B * P, V, -1) + x[..., 4:6] *= 0.0 # Zero out velocity features + + valid_indices = ~mask_p.view(-1) + x = x[valid_indices] + + x = self.channel_pre_project(x) + x = x.permute(0, 2, 1) + x = self.token_pre_project(x) + x = x.permute(0, 2, 1) + for block in self.blocks: + x = block(x) + + # pooling + x = torch.mean(x, dim=1) + + neighbor_type = neighbor_type.view(B * P, -1) + neighbor_type = neighbor_type[valid_indices] + type_embedding = self.type_emb(neighbor_type) # Type embedding for valid data + x = x + type_embedding + + x = self.emb_project(self.norm(x)) + + x_result = torch.zeros((B * P, x.shape[-1]), device=x.device, dtype=x.dtype) + x_result[valid_indices] = x.to(x_result.dtype) # Fill in valid parts + + return x_result.view(B, P, -1), mask_p.reshape(B, -1), pos.view(B, P, -1) + + +class LaneEncoder(nn.Module): + def __init__(self, lane_len, class_type, drop_path_rate, hidden_dim, depth): + super().__init__() + tokens_mlp_dim = 64 + channels_mlp_dim = 128 + + assert class_type in [CLASS_TYPE_LANE, CLASS_TYPE_ROUTE], ( + "Invalid class type for LaneEncoder" + ) + + self._lane_len = lane_len + self._class_type = class_type + + self.speed_limit_emb = nn.Linear(1, channels_mlp_dim) + self.unknown_speed_emb = nn.Embedding(1, channels_mlp_dim) + self.attribute_emb = nn.Linear(5 + 2 * 10, channels_mlp_dim) # traffic_light and line type + + self.channel_pre_project = Mlp( + in_features=8, + hidden_features=channels_mlp_dim, + out_features=channels_mlp_dim, + act_layer=nn.GELU, + drop=0.0, + ) + self.token_pre_project = Mlp( + in_features=lane_len, + hidden_features=tokens_mlp_dim, + out_features=tokens_mlp_dim, + act_layer=nn.GELU, + drop=0.0, + ) + + self.blocks = nn.ModuleList( + [MixerBlock(tokens_mlp_dim, channels_mlp_dim, drop_path_rate) for i in range(depth)] + ) + + self.norm = nn.LayerNorm(channels_mlp_dim) + self.emb_project = Mlp( + in_features=channels_mlp_dim, + hidden_features=hidden_dim, + out_features=hidden_dim, + act_layer=nn.GELU, + drop=drop_path_rate, + ) + + def forward(self, x, speed_limit, has_speed_limit): + """ + x: B, P, V, D (x, y, x'-x, y'-y, x_left-x, y_left-y, + x_right-x, y_right-y, traffic(5) + line_type(2 * 10)) + speed_limit: B, P, 1 + has_speed_limit: B, P, 1 + """ + attribute = x[:, :, 0, 8:] + x = x[..., :8] + + pos = x[:, :, int(self._lane_len / 2), :4].clone() # x, y, x'-x, y'-y + heading = torch.atan2(pos[..., 3], pos[..., 2]) + pos[..., 2] = torch.cos(heading) + pos[..., 3] = torch.sin(heading) + pos = add_class_type(pos, self._class_type) + + B, P, V, _ = x.shape + mask_v = torch.sum(torch.ne(x[..., :8], 0), dim=-1).to(x.device) == 0 + mask_p = torch.sum(~mask_v, dim=-1) == 0 + valid_indices = ~mask_p.view(-1) + + x = x.view(B * P, V, -1) + + # Use torch.where instead of indexing to maintain fixed size + x = torch.where(valid_indices.view(-1, 1, 1), x, torch.zeros_like(x)) + + x = self.channel_pre_project(x) + x = x.permute(0, 2, 1) + x = self.token_pre_project(x) + x = x.permute(0, 2, 1) + for block in self.blocks: + x = block(x) + + x = torch.mean(x, dim=1) + + # Reshape speed_limit and traffic to match flattened dimensions + speed_limit = speed_limit.view(B * P, 1) + has_speed_limit = has_speed_limit.view(B * P, 1) + attribute = attribute.view(B * P, -1) + + # Create embeddings for all positions + speed_limit_emb = self.speed_limit_emb(speed_limit) + unknown_speed_emb = self.unknown_speed_emb( + torch.zeros(B * P, dtype=torch.long, device=x.device) + ) + speed_limit_embedding = torch.where(has_speed_limit.bool(), speed_limit_emb, unknown_speed_emb) + + # Process traffic lights for all positions + traffic_light_embedding = self.attribute_emb(attribute) + + x = x + speed_limit_embedding + traffic_light_embedding + x = self.emb_project(self.norm(x)) + + # Apply mask to zero out invalid positions + x = x * valid_indices.float().unsqueeze(-1) + + return x.view(B, P, -1), mask_p.reshape(B, -1), pos.view(B, P, -1) + + +class LineEncoder(nn.Module): + def __init__(self, line_len, class_type, drop_path_rate, hidden_dim, depth, point_dim=2): + super().__init__() + self._class_type = class_type + tokens_mlp_dim = 64 + channels_mlp_dim = 128 + + self._line_len = line_len + + self.channel_pre_project = Mlp( + in_features=point_dim + 2, # point_dim (x, y, type_one_hot...) + dx + dy + hidden_features=channels_mlp_dim, + out_features=channels_mlp_dim, + act_layer=nn.GELU, + drop=0.0, + ) + self.token_pre_project = Mlp( + in_features=line_len, + hidden_features=tokens_mlp_dim, + out_features=tokens_mlp_dim, + act_layer=nn.GELU, + drop=0.0, + ) + + self.blocks = nn.ModuleList( + [MixerBlock(tokens_mlp_dim, channels_mlp_dim, drop_path_rate) for i in range(depth)] + ) + + self.norm = nn.LayerNorm(channels_mlp_dim) + self.emb_project = Mlp( + in_features=channels_mlp_dim, + hidden_features=hidden_dim, + out_features=hidden_dim, + act_layer=nn.GELU, + drop=drop_path_rate, + ) + + def forward(self, x): + """ + x: B, P, V, D(x, y) + """ + B, P, V, D = x.shape + # diffを取る + diff_x = x[:, :, 1:, 0] - x[:, :, :-1, 0] # (B, P, V-1) + diff_y = x[:, :, 1:, 1] - x[:, :, :-1, 1] # (B, P, V-1) + diff_x = torch.cat([diff_x, torch.zeros_like(diff_x[:, :, :1])], dim=2) # (B, P, V) + diff_x = diff_x.view(B, P, V, 1) + diff_y = torch.cat([diff_y, torch.zeros_like(diff_y[:, :, :1])], dim=2) # (B, P, V) + diff_y = diff_y.view(B, P, V, 1) + x = torch.concat([x, diff_x, diff_y], dim=-1) # (B, P, V, D+2) + + pos = x[:, :, int(self._line_len / 2), :4].clone() # x, y, x'-x, y'-y + heading = torch.atan2(pos[..., 3], pos[..., 2]) + pos[..., 2] = torch.cos(heading) + pos[..., 3] = torch.sin(heading) + pos = add_class_type(pos, self._class_type) + + B, P, V, _ = x.shape + mask_v = torch.sum(torch.ne(x[..., :4], 0), dim=-1).to(x.device) == 0 + mask_p = torch.sum(~mask_v, dim=-1) == 0 + valid_indices = ~mask_p.view(-1) + + x = x.view(B * P, V, -1) + + # Use torch.where instead of indexing to maintain fixed size + x = torch.where(valid_indices.view(-1, 1, 1), x, torch.zeros_like(x)) + + x = self.channel_pre_project(x) + x = x.permute(0, 2, 1) + x = self.token_pre_project(x) + x = x.permute(0, 2, 1) + for block in self.blocks: + x = block(x) + + x = torch.mean(x, dim=1) + + x = self.emb_project(self.norm(x)) + + # Apply mask to zero out invalid positions + x = x * valid_indices.float().unsqueeze(-1) + + return x.view(B, P, -1), mask_p.reshape(B, -1), pos.view(B, P, -1) + + +class GoalPoseEncoder(nn.Module): + def __init__(self, drop_path_rate, hidden_dim): + super().__init__() + channels_mlp_dim = 128 + + self._hidden_dim = hidden_dim + + self.channel_pre_project = Mlp( + in_features=4, + hidden_features=channels_mlp_dim, + out_features=channels_mlp_dim, + act_layer=nn.GELU, + drop=0.0, + ) + + self.norm = nn.LayerNorm(channels_mlp_dim) + self.emb_project = Mlp( + in_features=channels_mlp_dim, + hidden_features=hidden_dim, + out_features=hidden_dim, + act_layer=nn.GELU, + drop=drop_path_rate, + ) + + def forward(self, x): + """ + x: B, D=4 (x, y, cos, sin) + """ + B, D = x.shape + pos = x.clone() # (B, D=4[x, y, cos, sin]) + pos = pos.unsqueeze(1) # (B, 1, D=4) + pos = add_class_type(pos, CLASS_TYPE_GOAL_POSE) + + mask = torch.zeros((B, 1), dtype=torch.bool, device=x.device) + + x = self.channel_pre_project(x) # (B, C=channels_mlp_dim) + x = x.unsqueeze(1) # (B, 1, C=channels_mlp_dim) + + x = self.emb_project(self.norm(x)) # (B, 1, hidden_dim) + + return x, mask, pos + + +class FloatsEncoder(nn.Module): + def __init__(self, num_float, drop_path_rate, hidden_dim): + super().__init__() + channels_mlp_dim = 128 + + self._hidden_dim = hidden_dim + + self.channel_pre_project = Mlp( + in_features=num_float, + hidden_features=channels_mlp_dim, + out_features=channels_mlp_dim, + act_layer=nn.GELU, + drop=0.0, + ) + + self.norm = nn.LayerNorm(channels_mlp_dim) + self.emb_project = Mlp( + in_features=channels_mlp_dim, + hidden_features=hidden_dim, + out_features=hidden_dim, + act_layer=nn.GELU, + drop=drop_path_rate, + ) + + def forward(self, x): + """ + x: B, D + """ + B, D = x.shape + pos = torch.zeros((B, 4), device=x.device, dtype=x.dtype) # (B, D=4[x, y, cos, sin]) + pos[:, 2] = 1.0 # cos(0) = 1 + pos = pos.unsqueeze(1) # (B, 1, D=4) + pos = add_class_type(pos, CLASS_TYPE_EGO_SHAPE) + + mask = torch.zeros((B, 1), dtype=torch.bool, device=x.device) + + x = self.channel_pre_project(x) # (B, C=channels_mlp_dim) + x = x.unsqueeze(1) # (B, 1, C=channels_mlp_dim) + + x = self.emb_project(self.norm(x)) # (B, 1, hidden_dim) + + return x, mask, pos + + +class FusionEncoder(nn.Module): + def __init__(self, hidden_dim, num_heads, drop_path_rate, depth): + super().__init__() + + dpr = drop_path_rate + + self.blocks = nn.ModuleList( + [SelfAttentionBlock(hidden_dim, num_heads, dropout=dpr) for i in range(depth)] + ) + + self.norm = nn.LayerNorm(hidden_dim) + + def forward(self, x, mask): + mask[:, 0] = False + + for b in self.blocks: + x = b(x, mask) + + return self.norm(x) diff --git a/src/wizard/patches/oneplanner/model_loader.py b/src/wizard/patches/oneplanner/model_loader.py new file mode 100644 index 00000000..2d29fa44 --- /dev/null +++ b/src/wizard/patches/oneplanner/model_loader.py @@ -0,0 +1,187 @@ +"""Model loading utilities for E2EPlanner. + +Handles both fresh E2E checkpoints and legacy DiffusionPlanner checkpoints +(the latter need a ``pos_emb`` column migration from 14 → 15). +""" + +import logging +import types + +import torch + +from oneplanner.deployment.checkpoint import extract_state_dict, normalize_state_dict + +log = logging.getLogger(__name__) + + +def load_e2e_planner_weights(model, checkpoint_path: str, strict: bool = False): + """Load weights into E2EPlanner from a checkpoint. + + Handles: + - E2E checkpoint (direct load) + - Legacy DiffusionPlanner checkpoint (pos_emb 14 → 15 column migration) + + Any state-dict entries for the removed ``perception_bridge`` module are + silently discarded — older checkpoints may still carry them. + """ + ckpt = torch.load(checkpoint_path, map_location="cpu", weights_only=False) + sd = extract_state_dict(ckpt) + + # Normalize DDP "module." AND Lightning "model." prefixes. The old code stripped + # only "module.", so a Lightning E2E checkpoint (weights stored under "model.") + # loaded with EVERY key still prefixed -> nothing matched -> the model was left + # mostly randomly initialized. normalize_state_dict mirrors train_e2e's warm-start. + sd = normalize_state_dict(sd) + + # Drop any dangling perception_bridge.* keys from legacy Mode 2 checkpoints. + sd = {k: v for k, v in sd.items() if "perception_bridge" not in k} + + new_sd = {} + for k, v in sd.items(): + # pos_emb migration: 14→15 + if "pos_emb.weight" in k and v.ndim == 2 and v.shape[1] == 14: + padded = torch.zeros(v.shape[0], 15, dtype=v.dtype) + padded[:, :14] = v + v = padded + new_sd[k] = v + + missing, unexpected = model.load_state_dict(new_sd, strict=strict) + + if missing: + # WARNING, not info: missing keys mean layers at RANDOM INIT — in a + # deployment context that must stay visible even when the caller never + # configured logging (warnings reach stderr via logging's last-resort + # handler; info is silently dropped). The print this replaced always + # surfaced it (codex-connector on #82). + log.warning("%d keys randomly initialised: %s", len(missing), missing[:5]) + if unexpected: + log.warning("%d unexpected ckpt keys ignored: %s", len(unexpected), unexpected[:5]) + + return model + + +def build_e2e_planner_from_checkpoint(checkpoint_path: str, config=None): + """Build and load an E2EPlanner or DiffusionPlanner from a checkpoint. + + Constructs an E2EPlanner and loads the checkpoint on top. + + Parameters + ---------- + checkpoint_path : str + Path to the ``.pth`` checkpoint file. + config : namespace, optional + Model configuration. When *None* a minimal default config is + constructed from the checkpoint's ``config`` entry (if present) + or from hard-coded defaults matching the standard architecture. + + Returns + ------- + nn.Module + The model with weights loaded, ready for training or inference. + """ + from oneplanner.models.e2e import E2EPlanner + from oneplanner.models.planner.normalizer import ( + ObservationNormalizer, + StateNormalizer, + ) + + ckpt = torch.load(checkpoint_path, map_location="cpu", weights_only=False) + + # Try to recover config from checkpoint + if config is None: + config = ckpt.get("config", None) + + # Build a minimal config if none was found + if config is None: + config = _default_config() + + # Ensure normalizers exist on the config. + # + # HOTFIX (alpasim): the shipped ``model_loader`` defaulted state_normalizer / + # observation_normalizer to identity + empty, but the checkpoint was trained + # with ``configs/planner/normalization.json`` (ego mean=[10,0,0,0], + # std=[20,20,1,1] etc.). Identity normalizers at inference broke the input + # scale seen by the DiT and made the decoder collapse to near-stationary + # predictions regardless of inputs. Prefer the JSON built into the image; + # fall back to identity only if the file is missing. + _norm_json = getattr(config, "normalization_file_path", None) + if not _norm_json: + import os as _os + _candidate = "/opt/oneplanner/configs/planner/normalization.json" + if _os.path.exists(_candidate): + _norm_json = _candidate + config.normalization_file_path = _candidate + + if not hasattr(config, "state_normalizer") or config.state_normalizer is None: + if _norm_json: + log.info("Loading StateNormalizer from %s", _norm_json) + config.state_normalizer = StateNormalizer.from_json(config) + else: + log.warning( + "No normalization JSON found; falling back to identity " + "StateNormalizer. Predictions may be miscalibrated." + ) + config.state_normalizer = StateNormalizer( + mean=[[[0.0, 0.0, 0.0, 0.0]]], + std=[[[1.0, 1.0, 1.0, 1.0]]], + ) + if not hasattr(config, "observation_normalizer") or config.observation_normalizer is None: + if _norm_json: + log.info("Loading ObservationNormalizer from %s", _norm_json) + config.observation_normalizer = ObservationNormalizer.from_json(config) + else: + config.observation_normalizer = ObservationNormalizer({}) + + model = E2EPlanner.from_config(config, bevfusion_head=None) + + # Load weights + model = load_e2e_planner_weights(model, checkpoint_path, strict=False) + + return model + + +def _default_config(): + """Return a minimal config namespace for model construction. + + Values match the reference Diffusion-Planner argparse defaults in + ``train_predictor.py``. + """ + from oneplanner.constants import OUTPUT_T + + cfg = types.SimpleNamespace() + cfg.hidden_dim = 256 + cfg.num_heads = 8 + cfg.query_dim = 128 + + # Encoder config (matching train_predictor.py defaults) + cfg.encoder_mixer_depth = 6 + cfg.encoder_fusion_depth = 6 + cfg.encoder_drop_path_rate = 0.1 + cfg.use_ego_history = True + cfg.ego_history_dropout_rate = 0.6 + cfg.use_turn_indicators = True + + # Decoder config + cfg.decoder_depth = 3 + cfg.decoder_drop_path_rate = 0.1 + cfg.future_len = OUTPUT_T + cfg.diffusion_model_type = "x_start" + cfg.use_velocity_representation = False + cfg.guidance_fn = None + cfg.guidance_scale = 0.5 + cfg.state_normalizer = None + cfg.observation_normalizer = None + + # Normalization file path (default relative path, same as train_predictor.py) + cfg.normalization_file_path = None + + # Deployment loads INFERENCE models: the latent world model is a + # training-only auxiliary and grid-sample BEV needs a live BEV feature + # map wired by the runtime. Pin both OFF so the train-side ON-defaults + # (E2EConfig, 2026-06-11) never leak into checkpoint-only loading — + # fill_defaults would otherwise set use_latent_wm=True and from_config + # would raise on the missing latent_action_dim (Codex on #92). + cfg.use_latent_wm = False + cfg.latent_action_dim = None + + return cfg From 1b07a08ee618dca993834fd3201b1fb0cb2f7473 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Tue, 21 Jul 2026 09:59:23 +0900 Subject: [PATCH 13/13] Add roof LiDAR rig offset and OnePlanner alignment debug tooling lidar.py: set sensor_pose_delta to an approximate PANDAR128 rig->lidar transform so the LiDAR renders from roof height instead of ground level, which otherwise gives OnePlanner an unusable point cloud. driver_preprocessing.py: add env-gated ALIGN_DEBUG logging and NPZ sample dumping (default off) to compare alpasim's local frame against OnePlanner's alignment frame while debugging the road_border/ego frame mismatch. Co-Authored-By: Claude Opus 4.8 (1M context) --- src/runtime/alpasim_runtime/events/lidar.py | 16 +++- .../oneplanner/driver_preprocessing.py | 78 ++++++++++++++++++- 2 files changed, 92 insertions(+), 2 deletions(-) diff --git a/src/runtime/alpasim_runtime/events/lidar.py b/src/runtime/alpasim_runtime/events/lidar.py index 7c2fa7bc..580799fb 100644 --- a/src/runtime/alpasim_runtime/events/lidar.py +++ b/src/runtime/alpasim_runtime/events/lidar.py @@ -8,6 +8,9 @@ import logging from typing import Any +import numpy as np +from utils_rs import Pose + from alpasim_runtime.broadcaster import MessageBroadcaster from alpasim_runtime.events.base import Event, EventPriority, EventQueue from alpasim_runtime.events.camera import _traffic_trajectories @@ -18,6 +21,17 @@ logger = logging.getLogger(__name__) +# Roof-mounted PANDAR128 approximate rig→lidar transform for hyperion-class rigs. +# The renderer (splatsim) treats sensor_pose as the sensor's world pose; without +# this offset the LiDAR sits at rig origin (ground level) and downward rays hit +# ground immediately, so OnePlanner sees an unusable point cloud and predicts +# a stationary ego. TODO(closed-loop): source this from scene extrinsics rather +# than hardcoding once the renderer exposes an AvailableLidars-style RPC. +_RIG_TO_LIDAR_TOP = Pose( + np.array([0.5, 0.0, 1.9], dtype=np.float64), + np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float64), +) + class LidarFrameEvent(Event): """Render one LiDAR sweep and submit the point cloud to the driver. @@ -59,7 +73,7 @@ async def handle(self, rollout_state: RolloutState, queue: EventQueue) -> None: traffic_trajectories=_traffic_trajectories(rollout_state), lidar_logical_id=self.lidar.logical_id, lidar_type=self.lidar.device_type, - sensor_pose_delta=None, + sensor_pose_delta=_RIG_TO_LIDAR_TOP, trigger=self.trigger, scene_id=rollout_state.unbound.scene_id, ) diff --git a/src/wizard/patches/oneplanner/driver_preprocessing.py b/src/wizard/patches/oneplanner/driver_preprocessing.py index 3a74dd73..3a504149 100644 --- a/src/wizard/patches/oneplanner/driver_preprocessing.py +++ b/src/wizard/patches/oneplanner/driver_preprocessing.py @@ -14,6 +14,8 @@ from __future__ import annotations +import logging +import os from dataclasses import dataclass, field from typing import Sequence @@ -27,6 +29,13 @@ build_map_tensors, ) +_ALIGN_DEBUG_LOG = logging.getLogger("oneplanner.deployment.align_debug") +_ALIGN_DEBUG_LOG.setLevel(logging.INFO) +_ALIGN_DEBUG_TICK = 0 +_ALIGN_DEBUG_EVERY_N = int(os.environ.get("ONEPLANNER_ALIGN_DEBUG_EVERY_N", "0") or 0) +_SAMPLE_DUMP_DIR = os.environ.get("ONEPLANNER_SAMPLE_DUMP_DIR", "") +_SAMPLE_DUMP_EVERY_N = int(os.environ.get("ONEPLANNER_SAMPLE_DUMP_EVERY_N", "0") or 0) + @dataclass class EgoPoseSample: @@ -382,6 +391,58 @@ def build_sample( lanes_has_sl = map_tensors["lanes_has_speed_limit"] polygons = map_tensors["polygons"] line_strings = map_tensors["line_strings"] + + # Debug: verify alpasim's "local" frame matches OnePlanner's alignment "local". + # Root cause suspicion: rerun shows ego_history + prediction crossing road_borders, + # which implies road_border draws in a different frame than ego. Log the pieces so + # we can compare numerically. Print every N-th call (default off unless env set). + global _ALIGN_DEBUG_TICK + if _ALIGN_DEBUG_EVERY_N > 0 and _ALIGN_DEBUG_TICK % _ALIGN_DEBUG_EVERY_N == 0: + local_to_rig_mat = np.asarray(current.local_to_rig, dtype=np.float64) + ego_in_local = local_to_rig_mat[:3, 3] + _yaw_deg_local_to_rig = float(np.degrees(np.arctan2(local_to_rig_mat[1, 0], local_to_rig_mat[0, 0]))) + _ALIGN_DEBUG_LOG.info( + "ALIGN_DEBUG tick=%d local_to_rig yaw_deg=%.3f rot=%s translation=%s", + _ALIGN_DEBUG_TICK, _yaw_deg_local_to_rig, + np.round(local_to_rig_mat[:3, :3], 4).tolist(), + np.round(local_to_rig_mat[:3, 3], 3).tolist(), + ) + map_origin_in_local = np.asarray(alignment.local_to_map, dtype=np.float64)[:3, 3] + local_origin_in_map = np.asarray(alignment.map_to_local, dtype=np.float64)[:3, 3] + ep = np.asarray(ego_past, dtype=np.float64) + ep_xy_min = ep[..., :2].min(axis=tuple(range(ep.ndim - 1))).tolist() + ep_xy_max = ep[..., :2].max(axis=tuple(range(ep.ndim - 1))).tolist() + ls = np.asarray(line_strings, dtype=np.float64) + # first non-zero road_border point (any slot, any point) in ego-rig frame + mask = np.any(ls[..., :2] != 0, axis=-1) + if mask.any(): + idx = np.argwhere(mask)[0] + first_ls_pt = ls[idx[0], idx[1], :2] + ls_xy_min = ls[..., :2][mask].min(axis=0) + ls_xy_max = ls[..., :2][mask].max(axis=0) + else: + first_ls_pt = np.zeros(2) + ls_xy_min = np.zeros(2) + ls_xy_max = np.zeros(2) + _ALIGN_DEBUG_LOG.info( + "ALIGN_DEBUG tick=%d ego_in_local=%s map_origin_in_local=%s " + "local_origin_in_map=%s dist_ego_to_map_origin_in_local=%.3fm " + "ego_past xy_range_in_rig=[%s .. %s] " + "line_strings first_pt_in_rig=%s xy_range_in_rig=[%s .. %s] " + "n_nonzero_pts=%d", + _ALIGN_DEBUG_TICK, + np.round(ego_in_local, 3).tolist(), + np.round(map_origin_in_local, 3).tolist(), + np.round(local_origin_in_map, 3).tolist(), + float(np.linalg.norm(ego_in_local - map_origin_in_local)), + [round(v, 3) for v in ep_xy_min], + [round(v, 3) for v in ep_xy_max], + np.round(first_ls_pt, 3).tolist(), + np.round(ls_xy_min, 3).tolist(), + np.round(ls_xy_max, 3).tolist(), + int(mask.sum()), + ) + _ALIGN_DEBUG_TICK += 1 else: lanes = np.zeros( (C.NUM_SEGMENTS_IN_LANE, C.POINTS_PER_LANELET, C.SEGMENT_POINT_DIM), @@ -398,7 +459,7 @@ def build_sample( route_sl = np.zeros((C.NUM_SEGMENTS_IN_ROUTE, 1), dtype=np.float32) route_has_sl = np.zeros((C.NUM_SEGMENTS_IN_ROUTE, 1), dtype=np.float32) - return { + sample = { "ego_agent_past": ego_past, "ego_current_state": ego_now, "ego_agent_future": np.zeros((_FUTURE_LEN, 3), dtype=np.float32), @@ -415,3 +476,18 @@ def build_sample( "goal_pose": goal, "points": points, } + if _SAMPLE_DUMP_DIR and _SAMPLE_DUMP_EVERY_N > 0: + # Save the sample dict as NPZ every Nth call so we can render offline + # with viz.py and compare with what appears in the .rrd file. Uses the + # module-level tick counter that ALIGN_DEBUG also increments upstream. + _tick_for_dump = _ALIGN_DEBUG_TICK - 1 if _ALIGN_DEBUG_TICK > 0 else 0 + if _tick_for_dump % _SAMPLE_DUMP_EVERY_N == 0: + try: + dump_dir = _SAMPLE_DUMP_DIR + os.makedirs(dump_dir, exist_ok=True) + # NPZ-safe: cast non-numpy leaves to arrays as needed + out_path = os.path.join(dump_dir, f"sample_tick_{_tick_for_dump:06d}.npz") + np.savez(out_path, **{k: np.asarray(v) for k, v in sample.items()}) + except Exception as exc: # noqa: BLE001 — dumps must not break RPC + _ALIGN_DEBUG_LOG.warning("sample dump failed: %s", exc) + return sample