Skip to content
Open
Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
17 commits
Select commit Hold shift + click to select a range
32eb04e
Autonomous: add rules-compliant autocross course generation
tanushchauhan Nov 3, 2026
835aa75
Autonomous: add fixed extreme autocross scenarios
tanushchauhan Nov 3, 2026
80c716c
Autonomous: import audited AMZ competition cone layouts
tanushchauhan Nov 7, 2026
a5a2f73
Merge main into competition layout imports
tanushchauhan Nov 12, 2026
142eb3a
Audit imported gate geometry before smoothing
tanushchauhan Nov 12, 2026
ae6399e
Merge main into extreme autocross scenarios
tanushchauhan Nov 12, 2026
2edc1e7
Keep extreme gate rows on the current sensor stack
tanushchauhan Nov 12, 2026
ac251be
Merge remote-tracking branch 'origin/main' into tanush/competition-la…
tanushchauhan Nov 16, 2026
ff35ee3
Fix imported-course campaign planning and noisy geometry audits
tanushchauhan Nov 16, 2026
343c3f2
Integrate competition layouts and pin extreme-course review regressions
tanushchauhan Nov 16, 2026
c1bf194
Test the narrow-course road clearance instead of cone-pair separation
tanushchauhan Nov 16, 2026
dea7f99
Space extreme-course cones by boundary arc and preserve complete appr…
tanushchauhan Nov 16, 2026
80f4bbd
Merge main into competition track layouts
tanushchauhan Nov 20, 2026
9e76ef6
Update the extreme autocross base branch
tanushchauhan Nov 20, 2026
3a9e68e
Pin extreme course widths and cone density
tanushchauhan Nov 20, 2026
93f23ea
Reconcile extreme autocross with merged competition tracks
tanushchauhan Nov 20, 2026
a5379ae
Assign distinct IMU seeds to fixed-course gate runs
tanushchauhan Nov 20, 2026
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 12 additions & 0 deletions autonomy/ros2/src/lhr_gazebo/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -747,3 +747,15 @@ within the original 200 m plane serialize unchanged.
Metrics scores the actual world's paired centerline without a style-specific
route shortcut. These courses can expose failures in perception and planning;
passing the geometry checks does not imply the autonomy gate passes.

## Fixed extreme worlds

The [world catalog](worlds/README.md#extreme-autocross-set) provides seven
committed, rules-checked `extreme_*_seed1.sdf` scenes for #212. Select their
style directly through `track_style`; both perception branches retain the
same fixed scene and pairing width, and metrics uses that saved reference.
`generate_world.py --style <name>` regenerates each scene with twelve-decimal
XY coordinates and validates its serialized geometry. Other styles retain
their prior precision. Rebuild after regeneration; freshness and geometry
checks run in `lhr_gazebo`'s tests. The catalog lists usage and the intended
failure modes; a valid scene is not a successful autonomy gate run.
15 changes: 12 additions & 3 deletions autonomy/ros2/src/lhr_gazebo/scripts/generate_world.py
Original file line number Diff line number Diff line change
Expand Up @@ -25,8 +25,10 @@
sys.path.insert(0, _trackgen_pkg)

from lhr_trackgen.cone_geometry import CONE_SPECS # noqa: E402
from lhr_trackgen.extreme_autocross import SCENARIOS # noqa: E402
from lhr_trackgen.publish_cones import ( # noqa: E402
ACCEL_STAGING_M, cone_kinds, GENERATORS)
from lhr_trackgen.rules_autocross import check_envelope # noqa: E402

# Gazebo model for each lhr_trackgen cone kind (the CONE_SPECS keys).
CONE_MODELS = {
Expand All @@ -53,13 +55,13 @@ def _vehicle_nose_m() -> float:


def _cone_include(model_name: str, instance_name: str,
x: float, y: float, z: float) -> str:
x: float, y: float, z: float, precision: int = 4) -> str:
"""Return an SDF <include> block for a cone."""
return textwrap.dedent(f"""\
<include>
<uri>model://{model_name}</uri>
<name>{instance_name}</name>
<pose>{x:.4f} {y:.4f} {z:.4f} 0 0 0</pose>
<pose>{x:.{precision}f} {y:.{precision}f} {z:.4f} 0 0 0</pose>
</include>""")


Expand Down Expand Up @@ -107,6 +109,7 @@ def generate_world_sdf(
model_dir: str = '',
left_kinds=None,
right_kinds=None,
coordinate_precision: int = 4,
) -> str:
"""Build a complete Gazebo world SDF string."""
# Auto-compute spawn from track geometry if not explicitly set
Expand All @@ -131,7 +134,7 @@ def generate_world_sdf(
counts[model] = index + 1
cone_blocks.append(_cone_include(
model, f'{model}_{index}', cx, cy,
CONE_SPECS[kind].height_m / 2.0))
CONE_SPECS[kind].height_m / 2.0, coordinate_precision))

cones_xml = '\n'.join(cone_blocks)
# Long legal courses can extend beyond the legacy 200 m ground plane.
Expand Down Expand Up @@ -280,13 +283,19 @@ def main():
vy = 0.0 if vy is None else vy
vyaw = 0.0 if vyaw is None else vyaw

precision = 12 if args.style in SCENARIOS else 4
if args.style in SCENARIOS:
saved = [[[round(v, precision) for v in p] for p in side] for side in (left, right)]
print(f'Rules envelope: {check_envelope(*saved)}')

sdf = generate_world_sdf(
left, right,
vehicle_x=vx,
vehicle_y=vy,
vehicle_yaw=vyaw,
left_kinds=left_kinds,
right_kinds=right_kinds,
coordinate_precision=precision,
)

if args.output is None:
Expand Down
131 changes: 131 additions & 0 deletions autonomy/ros2/src/lhr_gazebo/test/test_extreme_courses.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,131 @@
"""Keep saved edge-case worlds, launch branches and gate coverage consistent."""

import csv
import math
import os
from pathlib import Path
import subprocess

from launch import LaunchContext
from lhr_metrics.reference import (
CircuitProgress, paired_centerline, ReferenceTrack, world_geometry)
from lhr_track_builder.track_builder_node import TrackBuilder
from lhr_trackgen.extreme_autocross import generate_extreme_track, SCENARIOS
from lhr_trackgen.publish_cones import ConePublisher, GENERATORS, pairing_width_m
from lhr_trackgen.rules_autocross import check_envelope
import pytest
from test_acceleration_event import _launch, _params
from test_launch_param_types import _start
from test_rules_autocross import load

PACKAGE = Path(__file__).parents[1]
TOOLS = Path(__file__).parents[4]/'testing/tools/gate'


@pytest.mark.parametrize('name', SCENARIOS)
def test_saved_world_is_current_legal_and_has_a_complete_reference(name, tmp_path):
committed = PACKAGE/f'worlds/{name}_seed1.sdf'
left, right, cones = world_geometry(committed)
report = check_envelope(left, right)
assert len(cones) == len(left)+len(right)
expected = generate_extreme_track(name)
for actual_side, expected_side in zip((left, right), expected):
assert len(actual_side) == len(expected_side)
assert max(math.dist(a, b) for a, b in zip(actual_side, expected_side)) < 1e-8
if name in ('extreme_narrow', 'extreme_combined'):
assert 3 <= report['minimum_clear_width_m'] < 3.01
if name in ('extreme_hairpin', 'extreme_combined'):
assert report['longest_straight_m'] == pytest.approx(80., abs=1e-8)
assert report['minimum_radius_m'] == pytest.approx(4.5, abs=1e-8)
if name == 'extreme_500m':
assert 499.9999 < report['lap_length_m'] <= 500
# Regenerate through the public CLI, not just the in-memory helper.
output = tmp_path/'regenerated.sdf'
subprocess.run(['python3', str(PACKAGE/'scripts/generate_world.py'),
'--style', name, '--output', str(output)], check=True, capture_output=True)
regenerated = world_geometry(output)
for actual_side, expected_side in zip(regenerated[:2], (left, right)):
assert len(actual_side) == len(expected_side)
assert max(math.dist(a, b) for a, b in zip(actual_side, expected_side)) < 1e-8
track = ReferenceTrack(paired_centerline(left, right))
progress = CircuitProgress(track)
completed = False
for i, (x, y) in enumerate(track.points+track.points[:2]):
completed |= progress.observe(x, y, i*.4, 5.)
assert completed
assert not progress.invalid_reason


@pytest.mark.parametrize('name', SCENARIOS)
@pytest.mark.parametrize('perception', ['sim', 'lidar'])
@pytest.mark.parametrize('estimator', ['truth', 'ekf'])
def test_fixed_style_reaches_both_perception_branches(monkeypatch, name, perception, estimator):
assert GENERATORS[name]() == generate_extreme_track(name)
context, nodes = _launch(monkeypatch, track_style=name, perception=perception,
estimator=estimator)
builder = _params(context, nodes, 'track_builder')
width = pairing_width_m(name)
assert builder['track_width'] == width
assert _start(TrackBuilder, builder, ['track_width'])['track_width'] == width
assert _params(context, nodes, 'metrics_node')['track_style'] == name
if perception == 'sim':
params = _params(context, nodes, 'publish_cones')
assert _start(ConePublisher, params, ['track_style'])['track_style'] == name
module = load('extreme_launch', PACKAGE/'launch/gazebo_demo.launch.py')
launch_context = LaunchContext()
launch_context.launch_configurations.update(track_style=name, seed='1')
assert Path(module._find_world(launch_context)).name == f'{name}_seed1.sdf'


def plan(tmp_path, extremes):
script = (TOOLS/'run_gate.sh').read_text().split('# Skidpad joins the matrix')[0]
script = script.replace('HERE=$(cd "$(dirname "$0")" && pwd)', f'HERE="{TOOLS}"')
env = dict(os.environ, SEEDS='', EXTREMES=extremes, REPEATS='2', BASELINE='1',
ACCEL_REPEATS='0', SKIDPAD_REPEATS='0', OUT=str(tmp_path))
return subprocess.run(['bash', '-c', script+'\nSKIDPAD_WORLD=/missing\nplan'],
env=env, text=True, capture_output=True)


def test_gate_plans_all_cases_and_keeps_matching_baselines(tmp_path):
result = plan(tmp_path, 'all')
assert result.returncode == 0, result.stderr
lines = result.stdout.splitlines()
assert len(lines) == 3*len(SCENARIOS)
assert len({line.split()[0] for line in lines}) == len(lines)
for line in lines:
if 'perception:=lidar' in line:
assert 'lidar_source:=mid360' in line
assert 'enable_stereo_camera:=true' in line
assert 'enable_camera_lidar_fusion:=true' in line
else:
assert 'enable_stereo_camera:=false' in line
scorer = load('extreme_gate', TOOLS/'score_gate.py')
for name in SCENARIOS:
expected = [f'baseline_{name}_s1', f'{name}_s1_r1', f'{name}_s1_r2']
actual = [line.split()[0] for line in lines if f'track_style:={name} ' in line]
assert actual == expected
row = scorer.score(str(tmp_path/'missing'), expected[1])
assert row['event'] == 'autocross'
assert row['track_style'] == name
row.update(outcome='lap', stop_dist_m=1., finished=True, max_v_after_stop=0.,
max_cte=0., max_pos_error=0., off_track_count='0', cones_hit='0',
lap_time_s=100.)
scorer.judge(row, {(name, '1'): 100., ('autocross', '1'): 10.})
assert row['passed']
# Every named planned row must fail if a campaign never actually ran it.
(tmp_path/'expected.txt').write_text('\n'.join(line.split()[0] for line in lines))
assert scorer.main(str(tmp_path)) == 1
with (tmp_path/'results.csv').open() as stream:
rows = list(csv.DictReader(stream))
assert len(rows) == len(lines)
assert all(row['outcome'] == 'missing' for row in rows)
assert all(row['passed'] == 'False' for row in rows if row['kind'] == 'gate')


def test_gate_accepts_subsets_and_rejects_unknown_names(tmp_path):
subset = plan(tmp_path, 'extreme_hairpin extreme_500m')
assert subset.returncode == 0
assert len(subset.stdout.splitlines()) == 6
invalid = plan(tmp_path, 'extreme_typo')
assert invalid.returncode == 2
assert 'unsupported extreme' in invalid.stderr
88 changes: 88 additions & 0 deletions autonomy/ros2/src/lhr_gazebo/worlds/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,88 @@
# Gazebo worlds

Committed SDF scenes use the same ordered cone pairs as
[lhr_trackgen](../../lhr_trackgen/README.md). Launch finds
`<track_style>_seed<seed>.sdf` in this directory or the installed package.
An explicit `world:=...` overrides that lookup; metrics and the viewer read
that actual file. These are synthetic courses, not competition recordings.

## Extreme autocross set

These seven fixed courses exercise DD.4.4.1 edges from
[the track rules reference](../../../../docs/rules/fsae-track-and-events.md).
Every named style has **seed 1 only**. Each saved world passes the shared
rules-envelope check, including segment crossings and return-lane separation.

| Style | Limit or feature | Measured reference | Stack behavior stressed |
| --- | --- | --- | --- |
| `extreme_narrow` | 3 m clear width throughout | 226.658 m lap; clear width 3.000–3.005 m | Side classification (#85), pairing, lateral clearance |
| `extreme_hairpin` | 80 m straight directly into a 9 m turning-diameter half-turn | 201.563 m lap; 80.000 m straight; 4.500 m radius | Speed-profile braking, pursuit lookahead and steering |
| `extreme_decreasing` | Turn tightens from roughly 9 m radius toward a 4.5 m design apex | 206.019 m lap; sampled minimum radius 4.554 m | Curvature preview, speed reduction and steering response |
| `extreme_slalom_chicane` | Alternating slalom and a separate chicane | 259.305 m lap; minimum radius 4.630 m | Side classifier, rapid curvature reversals, track chaining |
| `extreme_doubleback` | Opposite-direction lanes 9 m apart | 218.395 m lap; 4.500 m turns | Cross-lane association, planner shortcuts and reference progress |
| `extreme_500m` | Maximum lap length | 499.99999 m reference lap | Mapping, accumulated EKF error and lap bookkeeping |
| `extreme_combined` | 3 m corridor, 80 m approach, 9 m turning diameter, slalom/chicane and nearby return lane | 329.109 m lap; 3.000–3.005 m clear width | Interaction of speed planning, pursuit, pairing and limited clearance |

Clear width uses the square cone bases' circumscribed diameter, matching the
conservative checker from #211. A tiny chord reserve makes the 3 m corridor
at most 1 cm wider between paired stations; it does not use the 0.10 m reserve
of the random generator. Turning diameter refers to the centerline. A legal
layout does not establish vehicle drivability or full event compliance:
staging, timing gates and exit lanes are not supplied by these fixtures.

The 500 m target is the closed polyline metrics scores. Its 10 micrometre
reserve keeps serialization inside the upper limit. Extreme worlds store XY
at 12 decimal places so four-decimal rounding cannot tighten the 4.5 m
hairpins below the limit. Existing worlds retain their four-decimal output.

## Regenerate and run

From `autonomy/ros2`, in the ROS environment:

```bash
python3 src/lhr_gazebo/scripts/generate_world.py --style extreme_hairpin
ros2 launch lhr_gazebo gazebo_demo.launch.py track_style:=extreme_hairpin \
seed:=1 perception:=lidar estimator:=ekf gui:=false rviz:=false
```

Repeat the generator command for each style in the table to regenerate the
set, then rebuild `lhr_gazebo` so installed scenes agree. The generator checks
coordinates at their saved precision before writing. `colcon test
--packages-select lhr_trackgen lhr_gazebo` also checks exact limit dimensions,
world freshness, matching simplified cone publication, launch parameter
startup, and complete progress around the saved references.

Fixed styles ignore generic `width_m`, `cone_spacing_m`, radius, jitter and
waypoint flags; their named dimensions and adaptive cone sampling are part
of the fixture. Seed values other than 1 fail. Both perception branches use
the corresponding cone-center pairing width. Metrics remains on the
`autocross` mission and scores the selected world's ordered reference;
perception and control receive no route from that reference.

To run just this set with course-derived reference laps:

```bash
SEEDS= EXTREMES=all REPEATS=1 ACCEL_REPEATS=0 SKIDPAD_REPEATS=0 \
RUN_S=900 OUT="$HOME/lhr-test-data/extreme-autocross" \
../testing/tools/gate/run_gate.sh
```

`EXTREMES` also accepts a space-separated subset of style names. See the
[gate tools](../../../../testing/tools/gate/README.md). The extension uses
existing scorer thresholds and does not alter the approved MVP defaults.
The [Sim v2 criteria](../../../../docs/plans/sim-v2-pass-criteria.md) include
these scenarios. Each row selects the current Mid-360 and camera-fusion stack.
`BASELINE=1` adds optional truth-stack comparisons; lap scoring uses the course
reference. `RUN_S` counts simulation seconds. Geometry acceptance is
not a claim that any course has passed the driving gate.

## Validation

The affected trackgen, Gazebo and metrics suites pass 568 tests, including
all seven saved references and gate plan/scorer coverage. Strict MkDocs also
passes. A bounded headless `extreme_hairpin` run with LiDAR and EKF reached
`DRIVING`; a 30-second observer received 909 truth odometry messages, 111
clouds and 90 planner paths, measuring 89.3 m of travel and 0.585 m maximum
centerline distance during that window. Safety and metrics were disabled
for this smoke run. It establishes scene integration, not a complete lap,
cone-contact result or full-set gate pass.
Loading
Loading