diff --git a/.cargo/config.toml b/.cargo/config.toml new file mode 100644 index 0000000..b24d6be --- /dev/null +++ b/.cargo/config.toml @@ -0,0 +1,6 @@ +# `cargo xtask ` runs the workspace's own tool. Cargo has no +# user-defined targets and its aliases cannot chain commands, so a task +# that is more than one invocation lives in a binary, and this is what +# makes it something a person types. +[alias] +xtask = "run --package xtask --" diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index d0a89cb..fef9088 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -41,23 +41,37 @@ jobs: steps: - uses: actions/checkout@v7 - - name: Install Rust (stable) + # Everything a step later needs is installed first, so a missing tool is + # reported before any of the work rather than partway through it. All + # three are cached; only the second ever compiles anything. + - name: "Install: Rust (stable)" uses: dtolnay/rust-toolchain@stable with: components: rustfmt, clippy - uses: Swatinem/rust-cache@v2 - # Ordered cheapest first, so an unformatted file is reported in seconds - # rather than after the workspace has been compiled. Steps only run when - # every earlier one succeeded. - - name: "Lint: format" - run: cargo fmt --all --check + # Pinned by version, since a packager that changed under us would + # rewrite modelDescription.xml on a commit that touched nothing. The + # cache key carries the runner because the binary is native, and a miss + # costs a release build of cargo-fmi, which is why it is cached at all. + - name: "Install: cargo-fmi (cached)" + uses: actions/cache@v6 + id: cargo-fmi-cache + with: + path: | + ~/.cargo/bin/cargo-fmi + ~/.cargo/bin/cargo-fmi.exe + key: cargo-fmi-0.3.0-${{ runner.os }}-${{ runner.arch }} + + - name: "Install: cargo-fmi (on a cache miss)" + if: steps.cargo-fmi-cache.outputs.cache-hit != 'true' + run: cargo install cargo-fmi --version 0.3.0 --locked # The viewer installs with plain `pip`, not `uv`. `uv.lock` is committed # as a convenience and nothing requires it, so installing the ordinary # way is what keeps that true. - - name: Install Python + - name: "Install: Python (cached)" uses: actions/setup-python@v7 with: python-version: "3.12" @@ -68,14 +82,35 @@ jobs: working-directory: python run: python -m pip install -e . pytest ruff - - name: "Lint: ruff" - working-directory: python + # fmpy validates what the FMU steps below package, and is nothing to do + # with the viewer, so it gets an environment of its own. It brings numpy + # and lxml among others, and sharing one would run the viewer's tests + # against a tree nobody who installs the viewer has, where an accidental + # import of any of it passes here and fails for them. + - name: "Install: fmpy" + shell: bash run: | - ruff check . - ruff format --check . + # Install fmpy in a virtual environment, named for the later steps. + python -m venv .venv-fmpy + if [ -x .venv-fmpy/bin/python ]; then + fmpy_python=.venv-fmpy/bin/python # Linux and macOS + else + fmpy_python=.venv-fmpy/Scripts/python # Windows + fi + "$fmpy_python" -m pip install fmpy + echo "FMPY_PYTHON=$fmpy_python" >> "$GITHUB_ENV" + # Lints run before anything is built for real, cheapest first, so an + # unformatted file is reported in seconds rather than after a compile. + # Steps only run when every earlier one succeeded. + - name: "Lint: format" + run: cargo fmt --all --check + + # `--all-features` so a target held behind one is still linted. The + # packaged-FMU comparison is such a target, and it would otherwise be + # the one file in the workspace nothing here reads. - name: "Lint: clippy" - run: cargo clippy --workspace --all-targets -- -D warnings + run: cargo clippy --workspace --all-targets --all-features -- -D warnings # Catches broken intra-doc links, which nothing else does: the crates # cross-reference each other heavily, and a renamed item leaves a link @@ -85,20 +120,98 @@ jobs: env: RUSTDOCFLAGS: -D warnings + - name: "Lint: ruff" + working-directory: python + run: | + ruff check . + ruff format --check . + + # A build of its own, so a compile error is reported as one rather than + # as whichever test step reached it first. `--all-features` for the same + # reason clippy takes it: a target behind a feature compiles here rather + # than in whichever later step first needs it. + - name: "Build: debug" + run: cargo build --workspace --all-targets --all-features + + # Before the tests, because one of them loads the `.fmu` this writes. + # Packaging after them would need a second cargo run for that one + # test, and asking for a single target resolves fewer packages, which + # changes their dependencies' features and rebuilds much of the graph. + # + # Names no package: the task finds every `continuo-fmu-*` crate + # itself, so a second FMU crate needs no edit here. + - name: "FMU: package" + run: cargo xtask package-fmus + + # Validate with fmpy to get an independent check on the correctness of + # the packaged FMUs. The glob is guarded because one matching nothing + # expands to itself, and naming the empty directory beats fmpy failing + # to open a path with a star in it. + - name: "FMU: validate" + shell: bash + run: | + # Validate every packaged FMU with fmpy. + packaged=(target/fmu/*.fmu) + if [ ! -e "${packaged[0]}" ]; then + echo "ERROR: no .fmu in target/fmu; the packaging step produced none" + exit 1 + fi + for fmu in "${packaged[@]}"; do + "$FMPY_PYTHON" -m fmpy validate "$fmu" + done + # `--lib` and `--test '*'` partition the suite: the first runs the unit # tests inside each lib, the second the integration targets in tests/. # Note `--tests` would *not* work as the second half, because it # includes lib unit tests too, so the two steps would re-run them. + # + # `--all-features` matches the build above, so neither step compiles + # anything: a changed feature list alone makes cargo rebuild a crate. + # It is also what runs the packaged-FMU comparison, one of the + # integration targets. - name: "Test: unit" - run: cargo test --workspace --lib + run: cargo test --workspace --all-features --lib - name: "Test: integration" - run: cargo test --workspace --test '*' + run: cargo test --workspace --all-features --test '*' - name: "Test: viewer" working-directory: python run: pytest -v + # Release from here on, so the smokes run against the profile the + # packaged FMU was built at and will be loaded beside. Packaging above + # asks for `--release` itself, since a host loads the binary it finds + # and cannot rebuild it. + - name: "Build: release" + run: cargo build --release --workspace --examples + + # The world hash is pinned by a test that runs debug, and everything + # after this point runs release, so this is what carries the pin across + # to the other profile. rustc does not reassociate float arithmetic at + # any optimization level, which is why the two agree; this is what would + # notice if that stopped being true, on whichever agent it stopped on. + - name: "Test: the world hash holds in release too" + shell: bash + run: | + # Run the demo under both profiles and compare the hash it prints. + debug_hash=$(cargo run -q -p continuo-examples --example traffic \ + | sed -n 's/.*world hash \([0-9a-f]*\).*/\1/p') + release_hash=$(cargo run -q --release -p continuo-examples --example traffic \ + | sed -n 's/.*world hash \([0-9a-f]*\).*/\1/p') + echo "debug $debug_hash, release $release_hash" + + # Two empty captures would otherwise match, so a demo that stopped + # printing its hash would pass this rather than fail it. + if [ -z "$debug_hash" ]; then + echo "ERROR: no world hash in the demo's output; this check would be vacuous" + exit 1 + fi + if [ "$debug_hash" != "$release_hash" ]; then + echo "ERROR: the world hash differs between profiles" + exit 1 + fi + # Smoke steps run the shipped examples end to end, covering what the # test suites cannot: that the examples themselves build and run, and # that the artefacts one produces are what the next consumes. They are @@ -108,13 +221,13 @@ jobs: # # New end-to-end checks belong here, with the same "Smoke:" prefix. - name: "Smoke: traffic demo" - run: cargo run -p continuo-examples --example traffic + run: cargo run --release -p continuo-examples --example traffic - name: "Smoke: record a run and verify the unmodified log" shell: bash run: | - cargo run -q -p continuo-examples --example traffic_record -- ci-run.jsonl - cargo run -q -p continuo-examples --example traffic_verify -- ci-run.jsonl + cargo run -q --release -p continuo-examples --example traffic_record -- ci-run.jsonl + cargo run -q --release -p continuo-examples --example traffic_verify -- ci-run.jsonl # `verification_stops_at_the_first_divergence` in the highway tests # already checks that a tampered log is rejected, where it can be run @@ -124,27 +237,29 @@ jobs: # # Neither `if` below can become `! cmd`. Bash exempts an inverted # command from `set -e`, so the short form would never fail the step. + # + # The awk corrupts one pose x-coordinate mid-run, at the first match on + # or after line 6000 (roughly sim time 14 s of 30), keeping the JSON + # valid, so verification checks a long clean prefix before diverging + # and stops there rather than running to completion. - name: "Smoke: verification detects a modified log" shell: bash run: | - # Corrupt one pose x-coordinate mid-run, at the first match on or - # after line 6000 (roughly sim time 14 s of 30), keeping the JSON - # valid, so verification checks a long clean prefix before - # diverging (and stops there rather than running to completion). + # Corrupt one pose mid-run, then check verification rejects the log. awk 'NR>=6000 && !done && /"x":/ { sub(/"x":[-0-9.eE]*/, "\"x\":99.9"); done=1 } { print }' \ ci-run.jsonl > ci-tampered.jsonl if cmp -s ci-run.jsonl ci-tampered.jsonl; then echo "ERROR: awk matched nothing, log was NOT modified; this check would be vacuous" exit 1 fi - if cargo run -q -p continuo-examples --example traffic_verify -- ci-tampered.jsonl; then + if cargo run -q --release -p continuo-examples --example traffic_verify -- ci-tampered.jsonl; then echo "ERROR: verification PASSED on the modified log; divergence was NOT detected" exit 1 fi - name: "Smoke: open-loop resimulation from the recorded log" shell: bash - run: cargo run -q -p continuo-examples --example traffic_resim -- ci-run.jsonl + run: cargo run -q --release -p continuo-examples --example traffic_resim -- ci-run.jsonl # The viewer's own tests build their logs from synthetic events. This is # the only check that it reads what the conductor actually writes, which @@ -188,12 +303,22 @@ jobs: # A smoke, not a benchmark: a shared runner cannot hold a rate steady # enough to assert one, so this checks that a large world still runs and # leaves the numbers for a human reading the log. - # - # Release, because the numbers it prints mean nothing from a debug build, - # and `--no-default-features` so that costs a release build of 60 crates - # rather than the 911 the viz feature brings in. This example needs none - # of them. - name: "Smoke: a scaled world" - run: > - cargo run --release -p continuo-examples - --no-default-features --example traffic_scale + run: cargo run --release -p continuo-examples --example traffic_scale + + # Last, because publishing is the one thing here that leaves the job. + # A step only runs when every earlier one succeeded, so an FMU reaches + # the merge workflow only from a run that passed all of it. + # + # Every FMU the workspace packages, each carrying this platform's + # binary alone. One artifact per platform rather than per FMU, because + # an upload step produces one artifact and a workflow cannot loop over + # steps, so a name per FMU would mean a step per FMU to keep updated. + # The merge workflow groups them by file name instead. + - name: "FMU: upload" + uses: actions/upload-artifact@v7 + with: + name: fmus-${{ runner.os }}-${{ runner.arch }} + path: target/fmu/*.fmu + # A step that collected nothing would otherwise pass. + if-no-files-found: error diff --git a/.github/workflows/fmu-merge.yml b/.github/workflows/fmu-merge.yml new file mode 100644 index 0000000..4a8f28e --- /dev/null +++ b/.github/workflows/fmu-merge.yml @@ -0,0 +1,72 @@ +name: Merge FMUs + +# CI packages every FMU crate on each of its four agents, and each archive +# carries only the binary for the platform it ran on. This combines each FMU's +# four builds into the one archive FMI 3.0 expects, so a user downloads a +# single file per FMU that loads wherever they are rather than picking the +# right one of four. +# +# Run by hand, naming the CI run whose artifacts to take. The `workflow_run` +# trigger that fires it on every green run on main follows separately: a +# workflow cannot be dispatched until it sits on the default branch, so landing +# both at once would make the first run of this an automatic one that nothing +# had tried. A dispatch against a real CI run is what tries it. +on: + workflow_dispatch: + inputs: + run_id: + description: "The CI run whose FMU artifacts to merge" + required: true + +# Reading another run's artifacts needs `actions: read`, which the default +# token does not carry. Naming the two it uses also drops the rest. +permissions: + actions: read + contents: read + +jobs: + merge_fmus: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v7 + + - name: "Install: Python" + uses: actions/setup-python@v7 + with: + python-version: "3.12" + + - name: "Install: fmpy" + run: python -m pip install fmpy + + # One directory per platform, named as CI named them. + - name: "Download: every platform's FMUs" + uses: actions/download-artifact@v8 + with: + pattern: fmus-* + path: artifacts + run-id: ${{ inputs.run_id }} + github-token: ${{ secrets.GITHUB_TOKEN }} + + # Given the download directory rather than each artifact inside it, so + # no shell has to expand a glob, and the script fails by itself when + # nothing was downloaded. + - name: "Merge: each FMU's builds into one" + working-directory: python + run: python scripts/merge_fmus.py ../artifacts --out-dir ../merged + + # The same second opinion CI asks of each platform's FMU, asked again of + # the ones assembled from them, since merging rewrites the archive. + - name: "Validate: the merged FMUs" + shell: bash + run: | + # Validate every merged FMU with fmpy. + for fmu in merged/*.fmu; do + python -m fmpy validate "$fmu" + done + + - name: "Upload: the merged FMUs" + uses: actions/upload-artifact@v7 + with: + name: fmus-all-platforms + path: merged/*.fmu + if-no-files-found: error diff --git a/.gitignore b/.gitignore index 1d94342..47d4518 100644 --- a/.gitignore +++ b/.gitignore @@ -5,7 +5,7 @@ # the same reasoning that pins the Rust ones. It is only a convenience for uv, # and nothing here requires uv, since `pyproject.toml` alone is what pip or any # other installer reads. -.venv/ +.venv*/ __pycache__/ *.pyc .pytest_cache/ diff --git a/CLAUDE.md b/CLAUDE.md index ac1ac83..45c01ed 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -65,7 +65,7 @@ rather than after the workspace has compiled: ```sh cargo fmt --all --check -cargo clippy --workspace --all-targets -- -D warnings +cargo clippy --workspace --all-targets --all-features -- -D warnings RUSTDOCFLAGS=-D warnings cargo doc --workspace --no-deps cargo test --workspace cargo run -p continuo-examples --example traffic @@ -77,6 +77,19 @@ and a renamed item leaves a broken intra-doc link that still compiles. CI splits the test step in two, `--lib` then `--test '*'`, so neither reruns the other's tests. One `cargo test --workspace` covers both locally. +A change reaching an FMU crate or the laws it links needs two more commands, +in this order, because a `.fmu` carries its own compiled copy of everything it +links and the suite above compares nothing against it: + +```sh +cargo xtask package-fmus +cargo test --workspace --all-features +``` + +`continuo-actors` is the one to watch, since editing a control law there +leaves the packaged FMU a build behind and `cargo test --workspace` stays +green. Packaging needs `cargo install cargo-fmi` once. + Python changes also need `ruff check .`, `ruff format --check .` and `pytest` from `python/`. diff --git a/Cargo.lock b/Cargo.lock index 3443538..742e637 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -138,6 +138,36 @@ dependencies = [ "syn 3.0.3", ] +[[package]] +name = "attribute-derive" +version = "0.10.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "05832cdddc8f2650cc2cc187cc2e952b8c133a48eb055f35211f61ee81502d77" +dependencies = [ + "attribute-derive-macro", + "derive-where", + "manyhow", + "proc-macro2", + "quote", + "syn 2.0.119", +] + +[[package]] +name = "attribute-derive-macro" +version = "0.10.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0a7cdbbd4bd005c5d3e2e9c885e6fa575db4f4a3572335b974d8db853b6beb61" +dependencies = [ + "collection_literals", + "interpolator", + "manyhow", + "proc-macro-utils", + "proc-macro2", + "quote", + "quote-use", + "syn 2.0.119", +] + [[package]] name = "autocfg" version = "1.5.1" @@ -332,6 +362,12 @@ version = "0.5.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "0c9ea0ac24bc397ab3c98583a3c9ba74fa56b09a4449bbe172b9b1ddb016027a" +[[package]] +name = "collection_literals" +version = "1.0.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2550f75b8cfac212855f6b1885455df8eaee8fe8e246b647d69146142e016084" + [[package]] name = "combine" version = "4.6.7" @@ -439,6 +475,19 @@ dependencies = [ "tracing-subscriber", ] +[[package]] +name = "continuo-fmu-controller-idm" +version = "0.1.0" +dependencies = [ + "continuo-actors", + "continuo-core", + "continuo-fmi", + "fmi", + "fmi-export", + "serde_json", + "thiserror 2.0.18", +] + [[package]] name = "continuo-transport" version = "0.1.0" @@ -460,6 +509,15 @@ dependencies = [ "zenoh", ] +[[package]] +name = "convert_case" +version = "0.11.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "affbf0190ed2caf063e3def54ff444b449371d55c58e513a95ab98eca50adb49" +dependencies = [ + "unicode-segmentation", +] + [[package]] name = "core-foundation" version = "0.10.1" @@ -674,6 +732,17 @@ dependencies = [ "serde_core", ] +[[package]] +name = "derive-where" +version = "1.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d08b3a0bcc0d079199cd476b2cae8435016ec11d1c0986c6901c5ac223041534" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.119", +] + [[package]] name = "digest" version = "0.10.7" @@ -850,6 +919,37 @@ dependencies = [ "zip", ] +[[package]] +name = "fmi-export" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4cf4aa2c91d183eeab3df6e5f440bc40897974cfec30380970be967ac8e6613a" +dependencies = [ + "chrono", + "document-features", + "fmi", + "fmi-export-derive", + "paste", + "thiserror 2.0.18", +] + +[[package]] +name = "fmi-export-derive" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a3cf5e59493768c414821bfbd08b457fead054f72e6ecc93566fbbcf4a0ef938" +dependencies = [ + "attribute-derive", + "chrono", + "convert_case", + "fmi", + "proc-macro-error2", + "proc-macro2", + "quote", + "syn 2.0.119", + "uuid", +] + [[package]] name = "fmi-schema" version = "0.8.0" @@ -1378,6 +1478,12 @@ dependencies = [ "hybrid-array", ] +[[package]] +name = "interpolator" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "71dd52191aae121e8611f1e8dc3e324dd0dd1dee1e6dd91d10ee07a3cfb4d9d8" + [[package]] name = "ipnetwork" version = "0.20.0" @@ -1675,6 +1781,29 @@ dependencies = [ "sha2", ] +[[package]] +name = "manyhow" +version = "0.11.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b33efb3ca6d3b07393750d4030418d594ab1139cee518f0dc88db70fec873587" +dependencies = [ + "manyhow-macros", + "proc-macro2", + "quote", + "syn 2.0.119", +] + +[[package]] +name = "manyhow-macros" +version = "0.11.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "46fce34d199b78b6e6073abf984c9cf5fd3e9330145a93ee0738a7443e371495" +dependencies = [ + "proc-macro-utils", + "proc-macro2", + "quote", +] + [[package]] name = "matchers" version = "0.2.0" @@ -2134,6 +2263,37 @@ dependencies = [ "toml_edit", ] +[[package]] +name = "proc-macro-error-attr2" +version = "2.0.0" +source = "git+https://github.com/fgardt/proc-macro-error-2?rev=53ff94b61c678593158db1a3d50269c6f0de0db7#53ff94b61c678593158db1a3d50269c6f0de0db7" +dependencies = [ + "proc-macro2", + "quote", +] + +[[package]] +name = "proc-macro-error2" +version = "2.0.1" +source = "git+https://github.com/fgardt/proc-macro-error-2?rev=53ff94b61c678593158db1a3d50269c6f0de0db7#53ff94b61c678593158db1a3d50269c6f0de0db7" +dependencies = [ + "proc-macro-error-attr2", + "proc-macro2", + "quote", + "syn 2.0.119", +] + +[[package]] +name = "proc-macro-utils" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "eeaf08a13de400bc215877b5bdc088f241b12eb42f0a548d3390dc1c56bb7071" +dependencies = [ + "proc-macro2", + "quote", + "smallvec", +] + [[package]] name = "proc-macro2" version = "1.0.106" @@ -2210,6 +2370,28 @@ dependencies = [ "proc-macro2", ] +[[package]] +name = "quote-use" +version = "0.8.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9619db1197b497a36178cfc736dc96b271fe918875fbf1344c436a7e93d0321e" +dependencies = [ + "quote", + "quote-use-macros", +] + +[[package]] +name = "quote-use-macros" +version = "0.8.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "82ebfb7faafadc06a7ab141a6f67bcfb24cb8beb158c6fe933f2f035afa99f35" +dependencies = [ + "proc-macro-utils", + "proc-macro2", + "quote", + "syn 2.0.119", +] + [[package]] name = "r-efi" version = "5.3.0" @@ -2764,6 +2946,12 @@ dependencies = [ "digest 0.11.3", ] +[[package]] +name = "sha1_smol" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bbfa15b3dddfee50a0fff136974b3e1bde555604ba463834a7eb7deb6417705d" + [[package]] name = "sha2" version = "0.11.0" @@ -3399,6 +3587,12 @@ version = "1.0.24" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e6e4313cd5fcd3dad5cafa179702e2b244f760991f45397d14d4ebf38247da75" +[[package]] +name = "unicode-segmentation" +version = "1.13.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c6f5d3c3b1bf09027a88a6bc961fc00497d651009560b5463668dc81b0fa87a8" + [[package]] name = "unicode-xid" version = "0.2.6" @@ -3460,6 +3654,7 @@ checksum = "bf3923a6f5c4c6382e0b653c4117f48d631ea17f38ed86e2a828e6f7412f5239" dependencies = [ "getrandom 0.4.3", "js-sys", + "sha1_smol", "wasm-bindgen", ] @@ -3829,6 +4024,13 @@ version = "0.13.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "66fee0b777b0f5ac1c69bb06d361268faafa61cd4682ae064a171c16c433e9e4" +[[package]] +name = "xtask" +version = "0.1.0" +dependencies = [ + "serde_json", +] + [[package]] name = "yasna" version = "0.5.2" diff --git a/Cargo.toml b/Cargo.toml index ba5cd9b..e7e199c 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,8 @@ [workspace] resolver = "3" -members = ["crates/*"] +# `xtask` sits outside `crates/` because it is tooling for this +# checkout rather than part of what the workspace publishes. +members = ["crates/*", "xtask"] [workspace.package] version = "0.1.0" @@ -15,12 +17,34 @@ continuo-conductor = { path = "crates/continuo-conductor" } continuo-actors = { path = "crates/continuo-actors" } continuo-viz-bridge = { path = "crates/continuo-viz-bridge" } continuo-fmi = { path = "crates/continuo-fmi" } +continuo-fmu-controller-idm = { path = "crates/continuo-fmu-controller-idm" } # Default also pulls FMI 2.0 and the layered LS-BUS standard, neither of which # this workspace imports. fmi = { version = "0.8", default-features = false, features = ["fmi3"] } +# Authoring side of the same upstream workspace. It depends on `fmi` in turn, +# so an FMU crate pays for bindgen exactly as the importer does. +fmi-export = "0.3" serde = { version = "1", features = ["derive"] } serde_json = { version = "1", features = ["arbitrary_precision", "raw_value"] } thiserror = "2" tracing = "0.1" tracing-subscriber = "0.3" zenoh = "1.9" + +# `fmi-export-derive` pulls in `proc-macro-error2`, which re-exports a private +# `extern crate proc_macro`. rustc will make that an error, and until then it +# warns on every build that touches the FMU crate: +# https://github.com/rust-lang/rust/issues/127909 +# +# The crate is archived and will not be updated again, so 2.0.1 is the last +# release there will ever be and there is nothing to upgrade to. This pins the +# two-line fix rustc's own help suggests, from the pull request that was still +# open when the repository was archived: +# https://github.com/GnomedDev/proc-macro-error-2/pull/14 +# +# By revision rather than by branch, so what builds here is the commit that was +# read rather than wherever the branch points later. It goes when `fmi-export` +# stops depending on the crate, which is the only thing that ends this +# properly. +[patch.crates-io] +proc-macro-error2 = { git = "https://github.com/fgardt/proc-macro-error-2", rev = "53ff94b61c678593158db1a3d50269c6f0de0db7" } diff --git a/DECISIONS.md b/DECISIONS.md index 7458fc8..e441fd5 100644 --- a/DECISIONS.md +++ b/DECISIONS.md @@ -864,3 +864,174 @@ it got there, including the roads not taken. `x = 0` on the next line, and runs only at instantiate and reset, before an importer can write a parameter. Setting `x` directly is refused outside Continuous Time Mode and Event Mode. + +- **2026-08-15**: **The demo's FMU is the whole controller, and the laws it + runs stay in `continuo-actors`.** The FMU crate holds the FMI interface + declaration and nothing else, delegating every answer to `idm_accel`, + `nearest_detection` and `pure_pursuit_yaw_rate`. A planner publishing an + acceleration for a component to track was the alternative, and it lost on + reading the signal flow out: its longitudinal half consumed a number and + republished it unchanged, where IDM is the follow controller rather than a + reference for one. + - Only the steering law has a native caller today. + `PathFollowController` calls `pure_pursuit_yaw_rate`, so those two + agree by being one implementation rather than by being kept in step, + which is the whole argument for this arrangement. Nothing native calls + `idm_accel` or `nearest_detection`, because M6 gives traffic its + longitudinal behavior through the FMU and leaves that controller purely + lateral. So the argument is currently half demonstrated, and PLAN.md + defers the component that would finish it. + - The packaging is its own crate because FMI allows one model per shared + library, the model identifier follows the cdylib's name, and + `crate-type` cannot be feature-gated. + - What that costs is a `.fmu` carrying a compiled snapshot of the laws, + since the cdylib links them statically and nothing calls back into the + host. Editing a law without packaging again leaves the copy behind, and + the comparison in `crates/continuo-fmu-controller-idm/tests/` is the + only thing that would notice. Its failures name + `cargo xtask package-fmus` for that reason. + +- **2026-08-15**: **The following law is the published equation with one + departure, and its parameters are the published five.** A wanted gap does + not go below zero. A lead pulling away contributes a negative closing + term, and the equation as written lets that take the wanted gap negative, + where squaring turns it back into braking: at 20 m/s with 20 m of gap and + a lead pulling away to 30, it commands -1.28 m/s^2 where the answer wanted + is +1.20. A guard on the output cannot fix that, since -1.28 is an + ordinary number to command and only its sign is wrong. + - The command is held inside `[-b, a]`, the two rates the equation already + names, so `IdmParams` invents no number of its own. The plan had a + `b_max` of 4.0 for emergency braking and a `GAP_FLOOR` guarding the + division; both are gone, the first because the parameter set should be + the published one and the second because flooring the wanted gap is what + the departure above already does. + - The consequence is that a car brakes no harder than comfortably, which + is right for a law that is here to be representative rather than to + drive well. It does mean the ego-lane work needs more spawn distance + than the plan budgeted, which assumed braking at 4.0. + - Two of the values are Treiber's own, the 1.5 s headway and the 2 m + standstill gap his simulator lists for a car. The acceleration and + comfortable braking there are picked to bring stop-and-go waves out of a + crowded road, which is a different thing to demonstrate, so those two + are this project's, and the plan overstated all of them as calibrated. + - Cross-checked once against highway-env's `IDMVehicle`, which implements + the unclamped form: `desired_gap` has no floor and `acceleration` + applies no limit of its own. Fed our parameters, it agrees with this + implementation to the last bit or two at every sampled point where the + wanted gap stays positive, and parts from it exactly where the two + adaptations say it should. At 20 m/s with 20 m of gap it wants -25.735 m + of room from a lead pulling away to 30 and commands -1.280 m/s^2 where + this commands +1.204, and -54.603 m and -9.977 m/s^2 from one reaching + 35. What was compared is the equation rather than the tuning, so it was + fed ours throughout. Its own parameters differ and were not used: + `DISTANCE_WANTED` is 10 m against our 2, since it folds a car length + into the standstill gap, `COMFORT_ACC_MAX` is 3.0 against our 1.5, and + `COMFORT_ACC_MIN` is -5.0 where we brake at 2.0. + +- **2026-08-15**: **The road crosses into the FMU as fixed arrays with a + count, not as a structurally sized one.** The plan preferred a structural + parameter sizing the arrays, so a road would cross as exactly as many + points as it has. `fmi-export` 0.3.0 cannot: a `[T; N]` field emits a + fixed dimension and `#[variable(...)]` has no key naming a sizing + variable, while a `Vec` field hardcodes value reference 0, which is the + derive's own `time`, and implements neither the get nor the set trait. So + the road is `road_x` and `road_y` at `MAX_WAYPOINTS` with + `road_point_count` beside them, and that count is an ordinary parameter + rather than a structural one, which would claim a role it does not have + and send an importer into configuration mode for nothing. None of this + reaches `continuo-fmi`, which supports both forms because StateSpace + requires it. + +- **2026-08-15**: **The FMU refuses at run time what its model description + cannot state.** `fmi-export` 0.3.0 has no `min` or `max` key, so the + description carries no bounds and a host's own checker has nothing to + check against. A doc comment protects whoever builds the parameters in + Rust and nobody who loads the packaged `.fmu` in another tool, sets the + target speed to zero and gets NaN in every command after. So the model + checks the four parameters its laws divide by, a point count no road could + have, and a road of no length, and reports what it was sent through FMI's + own status and log rather than panicking, since an unwind through the C + interface would take the host process with it. + +- **2026-08-15**: **`cargo xtask package-fmus` packages every + `continuo-fmu-*` crate, and `cargo-fmi` stays a binary rather than a + dependency.** Discovery is by crate-name prefix through `cargo metadata`, + so a second FMU crate is packaged by the task and by CI with no edit + anywhere. An xtask rather than a `build.rs`, because packaging is a real + entry point somebody types rather than a side effect of building, and + rather than a cargo alias, because aliases cannot chain commands. It + passes `--release`, since how a packaged FMU is optimized is settled when + it is packaged and a host loads the binary it finds. + - Each agent packages its own platform's binary, so CI's four artifacts + are four FMUs nobody can hand to anyone else. + `python/scripts/merge_fmus.py` combines any set that differs only in the + binaries it carries, checking first that the model descriptions agree: + two packagings of one commit differ in `generationDateAndTime` alone, + and the instantiation token is a v5 UUID of the model name, so it is the + same everywhere. It lives beside the viewer rather than in the xtask + because merging needs several platforms' output and so has no use on one + machine, and because the job that runs it then needs no Rust toolchain. + - That job arrives with a manual trigger alone, taking the CI run to + read artifacts from as an input. A workflow cannot be dispatched until + it sits on the default branch, so a `workflow_run` trigger arriving + with it would make the first run of it an automatic one that nothing + had tried. That trigger follows once a dispatch has shown the + cross-run download works. + +- **2026-08-15**: **The packaged-FMU comparison sits behind a feature and + runs inside the ordinary integration step.** Everything in it reads a file + `cargo xtask package-fmus` writes, so `packaged-fmu` gates it and a plain + `cargo test --workspace` needs nothing packaged. The gate is a `#![cfg]` + on the file rather than `required-features` in the manifest, because CI + runs its integration tests as `--test '*'`, and a glob names every target + it matches, so cargo refuses the whole step over a named target whose + features are off. + - CI packages before it tests rather than after, which is what lets the + comparison run as one of the integration targets. Running it afterwards + means a second cargo invocation naming that one target, and **asking for + a single target resolves fewer packages**: 142 units against 399 here, + the whole Zenoh tree among the missing. Their dependencies then unify + features differently, so the crate links a different `thiserror` and + every crate between the two rebuilds, which cargo reports as `info of + dependency thiserror changed`. That cost 12 to 31 seconds a run on every + agent until the steps were reordered. + - Worth knowing before optimizing anything else here: the same step + measured 79, 70, 71, 37 and 92 seconds across five runs of materially + identical work, so a single run says almost nothing. + +- **2026-08-15**: **`proc-macro-error2` is patched to its fix rather than + suppressed.** `fmi-export-derive` pulls it in, and it re-exports a private + `extern crate proc_macro`, which rustc will make an error and warns about + meanwhile on every build touching the FMU crate. The crate is archived, so + 2.0.1 is the last release there will be and there is nothing to upgrade + to. `[patch.crates-io]` points at the two-line fix rustc's own help + suggests, from the pull request that was open when the repository was + archived, pinned by revision so what builds is the commit that was read. + `[future-incompat-report] frequency = "never"` was tried and dropped: it + is workspace wide, so it would hide every other dependency's warnings to + quiet one, and the hard error would still be coming. + +- **2026-08-16**: **Each situation in the packaged-FMU comparison is a run + of its own, with a reset between them.** The sweeps drive situations + picked to reach corners of the input space, and no car could drive from + one to the next, so stepping them in sequence asked the model to account + for a trajectory that never happened. That was sound only because these + laws carry nothing across a step, which is a property of the + implementation rather than anything the test established. `fmi3Reset` + takes the assumption out, and it is what the standard offers for exactly + this: a reset instance costs about a hundredth of a fresh one, since + construction extracts the archive and loads the library where a reset + does neither. + - **A reset does not restore an FMU's size.** It returns an instance to + Instantiated, which is before it was ever configured, and Configuration + Mode closes before Initialization Mode opens, so the next step is + already too late. The adapter therefore writes the mapping's structural + parameters again as part of the reset. Without that a reset StateSpace + would come back at the size its description declares while the bindings + went on addressing the size the mapping asked for. + - Each situation is stepped twice, because the first step after a reset + is Initialization Mode and `fmi3DoStep` needs an interval behind it + that a start time does not have. Resetting and stepping once instead + would have taken `fmi3DoStep` out of the sweeps altogether while + costing no measurable time, which is what a safety gain and a coverage + loss look like from the outside. diff --git a/M6-PLAN.md b/M6-PLAN.md index cad0dfe..84c4683 100644 --- a/M6-PLAN.md +++ b/M6-PLAN.md @@ -104,7 +104,7 @@ so nothing in M6 re-tests it per component. the same functions and bit-identity is true by construction. The shell crate holds only the FMI interface declaration (`#[derive(FmuModel)]` struct), `impl UserModel` delegating to the actors functions, - `export_fmu!`, and `bundled_fmu_path()`. A separate crate because FMI + `export_fmu!`, and `packaged_fmu_path()`. A separate crate because FMI allows one model per dylib, the modelIdentifier follows the cdylib name (`continuo_fmu_controller_idm.fmu`), crate-type cannot be feature-gated, and `cargo fmi bundle` builds with default features. `continuo-fmi` @@ -125,7 +125,8 @@ so nothing in M6 re-tests it per component. ceiling a genuinely unbounded road will eventually hit. The fallback, if the derive cannot, is `road_x[MAX_WAYPOINTS]` with an integer `road_point_count` beside it; PR B checks this alongside - the fixed-dimension question. + the fixed-dimension question. **It cannot**, and the PR B entry + below records what was tried. - The detection arrays stay fixed at `MAX_DETECTIONS` either way. There is no count variable to eliminate, since unused slots take the free-road defaults, so a structural parameter would only add a @@ -225,11 +226,65 @@ so nothing in M6 re-tests it per component. `#[model(co_simulation = true)]` must be set explicitly. `UserModel` methods all have defaults; a stateless map implements only `calculate_values` (outputs recompute lazily when read after `do_step` - marks them dirty). Implementation-time check: whether fmi-export itself - pulls fmi-sys/bindgen (hoped not; it implements the API rather than - binding to it). -- Bundling: `cargo install cargo-fmi`, then `cargo fmi --package - bundle` (wrapped by `cargo xtask bundle-fmus`) builds the cdylib + marks them dirty). + - **It does pull fmi-sys and so bindgen**, through a plain `fmi` + dependency, so authoring an FMU costs libclang exactly as importing + one does. Only the `fmi3` feature is enabled, so the workspace's + feature trimming survives. + - **The exporting crate needs `fmi` as a direct dependency of its + own**, because the generated code writes `::fmi::...` paths. Without + it the derive fails with "cannot find `fmi` in the crate root" and a + cascade of missing-trait errors behind it. + - **A field carrying no `#[variable]` is ignored**, which is what lets + the FMU hold ordinary Rust state beside its interface. + - **A dotted `name` override passes through verbatim**, so + `name = "position.x"` declares the structured name the pose inputs + want and no JSON Pointer has to be written for them. + - **Int64 and UInt64 cannot be declared.** The builder covers f32, f64, + the 8, 16 and 32 bit integers, `bool`, `String`, `Binary` and + `Clock`. It is an export-side gap only, since the importer binds all + ten numeric types, and nothing here wants a 64 bit integer. + - **Only the first line of a doc comment becomes the FMI + description**, so a first line has to be a whole sentence and a + field wanting more says the rest on the lines below. + - **An output needs `initial = Calculated` spelled out.** Left off, + the derive lists no initial unknowns and the output keeps whatever + start value it declares, which FMI forbids of a calculated variable. + fmpy refuses to load the result, which is how this was found. + - **`variableNamingConvention` cannot be set** and comes out `flat`. + Dotted names are still legal and fmpy accepts them, so what is lost + is the model description saying out loud that `position.x` is + structured. Nothing here depends on it, since the mapping derives + its pointers by this project's own rule. + - **`canSerializeFMUstate` is false**, so the adapter runs this FMU in + output-hash mode. Nothing in `fmi-export` implements state + serialization. + - **It builds on a crate that would otherwise stop compiling.** + `fmi-export-derive` depends on `proc-macro-error2`, which re-exports a + private `extern crate proc_macro`. That is a future hard error, + [rust-lang/rust#127909](https://github.com/rust-lang/rust/issues/127909), + and it warned on every build touching the FMU crate. The crate is + archived and will not be updated again, last pushed September 2024 + with a dozen issues frozen, so 2.0.1 is the last release there will + ever be and waiting for a fix was never an option. + - The workspace patches it to the two-line change rustc's own help + suggests, from the pull request that was still open when the + repository was archived, + [#14](https://github.com/GnomedDev/proc-macro-error-2/pull/14), + pinned by revision. Suppressing the notice + instead was tried and dropped: `frequency = "never"` is workspace + wide, so it would have hidden every other dependency's warnings too, + and it would have left the hard error still coming. Patching removes + the cause, so the day rustc makes it an error nothing here breaks. + - The cost is a build that depends on a personal fork staying + reachable. Vendoring the crate would remove that and carry somebody + else's source instead. What ends it properly is `fmi-export` + dropping the dependency, which is also the thing to check for when + it next releases. + +- Packaging: `cargo install cargo-fmi`, then `cargo fmi --package + bundle --release` (wrapped by `cargo xtask package-fmus`, and + `--release` because the default is the dev profile) builds the cdylib itself, extracts variable metadata from the built dylib, generates modelDescription.xml, and writes `target/fmu/continuo_fmu_controller_idm.fmu`. cargo-fmi as a @@ -879,13 +934,21 @@ PLAN.md's "native arrays, float64 value references" that survives. `[f64; MAX_DETECTIONS]`, with `const _: () = assert!(..)` against the actors constant so a cap raised without rebuilding the FMU cannot go unnoticed. - - **Check first whether the derive can declare a structural parameter - and a `Vec`-backed dimension.** If it can, the road takes that form - and no count variable exists; if not, it is `[f64; MAX_WAYPOINTS]` - with `road_point_count` beside it and the same compile-time assert. - This is the larger of PR B's two dimension questions, since a - structurally sized array needs runtime sizing rather than a - fixed-length field, and it may simply not exist in a crate at 0.3.0. + - **The derive cannot size an array by a structural parameter**, so + the road takes the fallback form: `[f64; MAX_WAYPOINTS]` with + `road_point_count` beside it and the same compile-time assert. The + count is an ordinary `Parameter`, since a `StructuralParameter` that + sizes nothing would claim a role it does not have and would send the + importer into configuration mode for no reason. + - `causality = StructuralParameter` is accepted and declared, but + nothing connects one to a dimension. A `[T; N]` field declares + `Dimension::Fixed(N)`, and `#[variable(...)]` has no key naming a + sizing variable. + - `Vec` is the only path to a variable dimension, and it is + unusable twice over: it hardcodes value reference 0, which is the + derive's own `time` variable, and it implements neither of the get + and set traits, so the variable could not be read or written even + if the dimension named the right thing. - A non-`#[variable]` `Option` field, invisible to FMI and ordinary Rust state that drops at `fmi3FreeInstance`. `impl UserModel` builds the road **once, on first use**: @@ -903,21 +966,21 @@ PLAN.md's "native arrays, float64 value references" that survives. `nearest_detection`, which returns the free-road default when nothing is detected so there is no empty case to write, and delegates to `pure_pursuit_yaw_rate` and `idm_accel`. - - `export_fmu!`, and `pub fn bundled_fmu_path() -> Result Result` walking `current_exe()` ancestors for `fmu/continuo_fmu_controller_idm.fmu`, its error Display carrying the fix (`cargo install cargo-fmi`, then `cargo xtask - bundle-fmus`). Tests fail with that message rather than skip. + package-fmus`). Tests fail with that message rather than skip. - **The `.fmu` embeds a snapshot of `continuo-actors`**, since the cdylib links it statically: the control laws and the whole `Waypoints` implementation are compiled into the shared library the zip ships, and nothing calls back into the host. Editing `path.rs` or `control_laws` without - re-bundling therefore leaves the native code and the FMU's copy - disagreeing. CI cannot hit this, because bundling runs before the tests + packaging it again therefore leaves the native code and the FMU's copy + disagreeing. CI cannot hit this, because packaging runs before the tests on every job; locally the golden bit-identity tests are the detector and fail loudly, which is the right outcome. What they cannot do is - say *why*, since a stale bundle presents as the IDM math disagreeing + say *why*, since a stale package presents as the IDM math disagreeing with itself. So their failure messages name it: the laws may have changed since the FMU was built, and here is the command to rebuild it. That is the whole guard, deliberately. Anything cleverer, a @@ -925,25 +988,39 @@ PLAN.md's "native arrays, float64 value references" that survives. accuracy this does not need. - **`xtask`**, a workspace member binary plus `.cargo/config.toml` with `[alias] xtask = "run --package xtask --"`, so `cargo xtask - bundle-fmus` packages every FMU in the repo. It discovers them by the + package-fmus` packages every FMU in the repo. It discovers them by the `continuo-fmu-*` crate-name prefix, so `continuo-fmu-controller-ai` - is picked up later with no change anywhere, and CI's bundle step + is picked up later with no change anywhere, and CI's packaging step never has to learn about a second FMU. It shells out to `cargo fmi` and fails with the install command when that is missing, in the same - style as `bundled_fmu_path`. Cargo has no user-defined targets and + style as `packaged_fmu_path`. + - **It passes `--release`.** `cargo fmi bundle` builds the dev profile + otherwise, which would ship an unoptimized `.fmu` running + `calculate_values` for every car at every control instant once PR E + puts one in each. Instantiation cost is irrelevant beside that: it + is once per car, and measures 3.8 us in dev against 23 ns in + release. + - The two profiles answer bit for bit alike, checked through fmpy + rather than assumed, over a following command, an open-road + command, and a steering command that goes through `atan2`, plus a + near-cancellation landing at -1.5e-15. So a release `.fmu` compared + against debug-built native laws is a fair comparison, which is what + the golden tests need it to be. rustc does not reassociate float + arithmetic at any optimization level, which is why this holds, and + the golden tests are what would notice if it ever stopped. Cargo has no user-defined targets and its aliases cannot chain commands, so this is the idiomatic form; it is also a real entry point rather than a hidden side effect of `cargo build`, which is why it beats a `build.rs`. First `xtask` and first `.cargo/config.toml` in the workspace, which is new structure in a repo that has deliberately little, and worth it because CI is the immediate beneficiary. -- README: dev-setup note pointing at `cargo xtask bundle-fmus` (install - `cargo-fmi` once, bundle before `cargo test`, and again after editing +- README: dev-setup note pointing at `cargo xtask package-fmus` (install + `cargo-fmi` once, package before `cargo test`, and again after editing the shared laws), plus bindgen needing libclang (stock on CI images; `winget install LLVM` locally). Crates table row. -Tests: `the_bundled_fmu_reproduces_the_native_idm_bit_for_bit` and -`the_bundled_fmu_steers_bit_identically_to_the_native_pursuit` (dev-dep +Tests: `the_packaged_fmu_reproduces_the_native_idm_bit_for_bit` and +`the_packaged_fmu_steers_bit_identically_to_the_native_pursuit` (dev-dep continuo-fmi; sweep input grids and parameter sets through the imported FMU against the actors functions; `f64::to_bits` equality; also proves the lazy calculate_values path fires; the fallback if it does not is to @@ -967,16 +1044,16 @@ highway-env, noting it uses the unclamped variant). CI (before the test steps, after "Lint: docs"): cache `~/.cargo/bin/cargo-fmi*` keyed on version + os + arch; `cargo install cargo-fmi --version --locked` on miss; then `cargo xtask -bundle-fmus`, which names no package and so needs no edit when the +package-fmus`, which names no package and so needs no edit when the second FMU arrives. DECISIONS titles: The demo FMU is a complete car controller and a drop-in replacement for the native one (why a planner-plus-relay lost; the laws live in actors, the FMU packaging is a shell crate, and the `.fmu` therefore carries a compiled snapshot of those laws, which is what makes -re-bundling part of the edit-test loop, the golden tests its detector, +packaging part of the edit-test loop, the golden tests its detector, and their failure text the only guard worth having). `cargo xtask -bundle-fmus` packages every `continuo-fmu-*` crate (why an xtask rather +package-fmus` packages every `continuo-fmu-*` crate (why an xtask rather than a `build.rs` or an alias, and why CI calls it instead of naming a package). The road crosses as waypoint arrays, a count and a closed flag, and is rebuilt inside the FMU (three pieces of state, only two of @@ -1117,7 +1194,7 @@ all). same-instant at every control instant); `RADAR_MAX_RANGE 120.0` and the sensor capped at `continuo_actors::MAX_DETECTIONS` (64), the same constant the FMU's arrays are built from; `controller_fmu_path()` - OnceLock over `bundled_fmu_path()`. + OnceLock over `packaged_fmu_path()`. - `controller_mapping(world, actor, v0, &UnicycleState)` builds one car's `FmuMapping`, the sheet saying which messages feed which FMU variables and where its outputs go. Four parts, and they are different @@ -1436,7 +1513,7 @@ fixed each; the final numbers). `cargo fmt --all --check`; `cargo clippy --workspace --all-targets -- -D warnings`; `RUSTDOCFLAGS=-D warnings cargo doc --workspace --no-deps`; -`cargo xtask bundle-fmus` (from PR B on); +`cargo xtask package-fmus` (from PR B on); `cargo test --workspace`; demo smoke `cargo run -p continuo-examples --example traffic`. Hash discipline: PRs A, B, D, F, H leave `DEMO_WORLD_HASH` untouched (asserted by the existing test); PRs C, E, G diff --git a/PLAN.md b/PLAN.md index 4210ed8..9a6d717 100644 --- a/PLAN.md +++ b/PLAN.md @@ -769,3 +769,58 @@ than churning the fingerprint twice. - **External (non-deterministic) ego participation**: a relaxed admission mode for a live AV stack under test, after milestone 7. + +- **The viewer's `--check` counts components and calls them joins.** Its + summary prints `joins / leaves` directly above `actors seen`, and the two + count different things: membership is published per component, so a car + joining raises two events, one for its controller and one for its physics. + A demo run reports 32 joins and 16 leaves against 15 actors, which reads as + a discrepancy and is not one: 15 cars at two components each, plus the + logger and the spawner at one, and eight cars retiring. Saying "component + joins / leaves" would settle it. The CI smoke asserts those exact labels, so + the two move together. + +- **A native component that follows, so both control laws have a caller on + each side of the FMI boundary.** The laws live in `continuo-actors` so that + the native side and the FMU call one implementation rather than two kept in + step by hand, and only the steering law actually does: + `PathFollowController` calls `pure_pursuit_yaw_rate`, while `idm_accel` and + `nearest_detection` are reached only from the FMU. The argument for where + the laws live is sound and currently half demonstrated. + - Which shape it takes is the open question. A second component beside + `PathFollowController` keeps that one purely lateral, which M6 made it + on purpose: two small commands travel separately so a learned + longitudinal FMU can run beside native steering, and one component + publishing both would argue against that. Teaching the existing one to + follow is less code and gives that back. + - What it buys beyond symmetry is a native baseline. Today the only thing + that runs IDM in a world is the FMU, so a scenario cannot be run both + ways to see what the boundary costs in throughput, and the packaged-FMU + comparison is the only place the two are checked against each other at + all. + +- **`cargo xtask verify`, running what has to pass before a commit.** + CLAUDE.md lists five commands in CI's order, cheapest first so a formatting + slip is reported in seconds rather than after the workspace has compiled, + and they are typed by hand every time. One task would run them in that + order and stop at the first failure, which is what the order is for. + - Two questions it has to answer rather than assume. Whether it runs the + Python half, `ruff` and `pytest` from `python/`, which needs an + interpreter and an install that a Rust-only change has no reason to have: + running it when it is there and saying it was skipped when it is not + beats either failing or silently covering less than the name claims. + And whether it packages the FMUs first, since the packaged-FMU tests + compare against what `cargo xtask package-fmus` last wrote, so a verify + that skipped it would pass against a stale artifact. + - The case for it is that the local commands and CI's are not the same + commands. CI splits the tests as `--lib` then `--test '*'`, where + CLAUDE.md offers one `cargo test --workspace` as covering both, and it + does cover both until a target is gated behind a feature: a glob names + every target it matches, so cargo refuses the step, where the + unqualified form skips the target and passes. That went green locally + and red on all four agents. A task typing CI's own commands is what + would have caught it, which is worth more than the typing it saves. + - What it must not become is a second definition of what CI runs. CI is + the authority, so this follows that file rather than the other way + round, and anything it grows that CI does not have is a divergence to + fix rather than a feature. diff --git a/README.md b/README.md index 1b6ae22..48c104f 100644 --- a/README.md +++ b/README.md @@ -98,9 +98,11 @@ instant, and repeats. | [`continuo-core`](crates/continuo-core/) | `SimTime`/`SimDuration`, ids and paths, key expressions, `Vec3`/`Quat`/Euler (canonical Z-Y-X conversions), wire messages, the `Component` trait, owned hash/random/seed derivation | | [`continuo-transport`](crates/continuo-transport/) | `Transport` trait, deterministic `InProcTransport`, `MonitorTransport` for out-of-band message recording | | [`continuo-conductor`](crates/continuo-conductor/) | Registry (component tree as data), event schedule, the conductor loop, tick fingerprints, and the event log: `record`, `verify`, `playback` | -| [`continuo-actors`](crates/continuo-actors/) | Sample components: waypoint path, path-follow controller, unicycle physics, pose logger, traffic spawner | +| [`continuo-actors`](crates/continuo-actors/) | Sample components: waypoint path, path-follow controller, unicycle physics, pose logger, traffic spawner. Also the control laws they steer and follow by, IDM and pure pursuit, which the exported FMU compiles in rather than reimplements | | [`continuo-viz-bridge`](crates/continuo-viz-bridge/) | Relays a run's published messages and membership changes to a live viewer, as a transport monitor rather than a component | | [`continuo-fmi`](crates/continuo-fmi/) | Runs an imported FMI 3.0 Co-Simulation FMU as a component, wired to the world by a mapping rather than by code | +| [`continuo-fmu-controller-idm`](crates/continuo-fmu-controller-idm/) | The car controller as an exported FMU: the FMI interface declaration, delegating every answer to the laws in `continuo-actors` | +| [`xtask`](xtask/) | Workspace tasks cargo has no command for, run as `cargo xtask ` | | [`continuo-examples`](crates/continuo-examples/) | Runnable example worlds: `traffic` (base demo), `traffic_realtime`, `traffic_record`, `traffic_verify`, `traffic_resim`, `traffic_viz`, `traffic_scale` | | [`python/continuo_viz`](python/) | The viewer: reads a recorded log or a live Zenoh session, and draws the world top-down | @@ -141,6 +143,17 @@ cargo test --workspace cargo clippy --workspace --all-targets cargo fmt --all +# Package the FMUs into target/fmu. Needs `cargo install cargo-fmi` once. +# An .fmu links the control laws statically, so it carries a snapshot of +# them: run this again after editing anything they reach, or what it +# computes and what the native code computes drift apart. +cargo xtask package-fmus + +# Drive each packaged FMU across the FMI boundary and check it answers what +# the laws answer, to the bit. Behind a feature, so the plain test run above +# needs nothing packaged and skips it. +cargo test --workspace --all-features + # Run the demo: an ego car on a straight highway, traffic spawning ahead of # it and retiring once passed; free-run, 30 sim-seconds cargo run -p continuo-examples --example traffic diff --git a/crates/continuo-actors/src/control_laws.rs b/crates/continuo-actors/src/control_laws.rs new file mode 100644 index 0000000..a517daa --- /dev/null +++ b/crates/continuo-actors/src/control_laws.rs @@ -0,0 +1,604 @@ +//! The control laws themselves: pure functions from what an actor knows +//! to what it commands. +//! +//! Nothing here takes a component, an inbox or a clock, so a law needs +//! only its arguments and answers every caller alike. That is what lets +//! one implementation serve a component in this process and a copy of it +//! compiled into something else, rather than two that have to be kept in +//! step by hand. + +use continuo_core::{Detection, Pose}; + +use crate::path::Waypoints; + +/// How a pure-pursuit follower is tuned: which lane to hold, how far ahead +/// to aim, and how hard to turn toward the aim point. +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct PurePursuitParams { + /// Meters left of the road's centerline to hold, the Frenet `d`. + pub lateral_tgt: f64, + /// How far along the road the aim point sits, in meters. Positive: + /// an aim point at or behind the follower has no direction to give. + pub lookahead: f64, + /// Yaw rate commanded per radian of heading error, in 1/s. + pub gain_yaw_rate: f64, + /// The most yaw rate to command in either direction, rad/s. + pub max_yaw_rate: f64, +} + +impl PurePursuitParams { + /// The set this project steers cars with, taking the lane to hold. + /// + /// The three beyond the lane are scenario tuning rather than anything + /// published: an aim point a car length or so ahead, turning toward + /// it briskly, inside a rate a car could be asked for. + pub const fn highway_car(lateral_tgt: f64) -> Self { + PurePursuitParams { + lateral_tgt, + lookahead: 6.0, + gain_yaw_rate: 1.5, + max_yaw_rate: 1.2, + } + } +} + +/// Yaw rate that steers a follower at `pose` onto the lane +/// `lateral_tgt` meters left of `road`, positive counter-clockwise. +/// +/// Projects the pose onto the road, aims at the point `lookahead` further +/// along it, and turns toward that point in proportion to how far off the +/// heading is, no harder than the follower is allowed to turn. Holding a +/// lane needs no geometry of its own, because the aim point is offset +/// from the one road everything on it shares. +pub fn pure_pursuit_yaw_rate(road: &Waypoints, pose: Pose, params: PurePursuitParams) -> f64 { + let position = pose.position; + let s = road.project(position.x, position.y); + let target = road.point_at_offset(s + params.lookahead, params.lateral_tgt); + let desired_heading = f64::atan2(target.y - position.y, target.x - position.x); + let heading_error = wrap_to_pi(desired_heading - pose.orientation.yaw()); + + // Return the turn toward the aim point, inside the clamp. + (params.gain_yaw_rate * heading_error).clamp(-params.max_yaw_rate, params.max_yaw_rate) +} + +/// The same angle measured the short way round, in [-pi, pi]. +/// +/// Wrapping the magnitude and putting the sign back sends both signs +/// through one calculation, so the answer is exactly odd: +/// `wrap_to_pi(-a)` is `-wrap_to_pi(a)` to the bit. Folding a negative +/// angle up into [0, TAU) and back down instead would round it twice +/// where a positive angle is not rounded at all, and two followers the +/// same distance either side of a lane would steer back at slightly +/// different rates. +/// +/// Nothing here rounds. A remainder is exact, and the subtraction only +/// runs when more than half a turn is left, so its two sides are within +/// a factor of two of each other and it is exact as well. Multiplying by +/// 1 or -1 is exact. An angle of exactly half a turn keeps the sign it +/// arrived with, both ways round being equally short. +fn wrap_to_pi(angle: f64) -> f64 { + let part_turn = angle.abs() % std::f64::consts::TAU; + let short_way = if part_turn > std::f64::consts::PI { + part_turn - std::f64::consts::TAU + } else { + part_turn + }; + + // Return the short way round, pointing the way the angle did. + angle.signum() * short_way +} + +/// The range standing for nothing detected, in meters. +/// +/// A million kilometers, which is less a distance than a number picked +/// against two constraints. Far enough that a following law reads it as +/// an open road: what IDM asks for at this range moves the answer by +/// under 2e-12 m/s^2, for any speed and closing rate a road produces. +/// Small enough to stay finite through whatever is done to it, since a +/// scan travels as JSON, which has no infinity, and something +/// downstream may yet take a car length off it or hand it to a 32-bit +/// float. +pub const FREE_ROAD_RANGE: f64 = 1e9; + +/// Nothing there: [`FREE_ROAD_RANGE`] away and not moving. +/// +/// What an unfilled slot holds in a fixed-length scan, which is how a +/// scan crosses into an FMU. +pub const FREE_ROAD: Detection = Detection { + range: FREE_ROAD_RANGE, + range_rate: 0.0, +}; + +/// The nearest detection in a scan, or [`FREE_ROAD`] if the scan holds +/// nothing nearer. +/// +/// A sensor reports what it found in no particular order, because +/// relevance is the consumer's idea rather than the sensor's. For +/// following, the nearest thing ahead is the one that matters, so +/// picking it out is this function's job and not the radar's. +/// +/// Ties go to the earlier slot, so a scan answers the same way every +/// time it is read, which is what the world hash needs of it. Free-road +/// slots lose to anything real, so a fixed-length scan padded out with +/// them needs no count of what is in it. +pub fn nearest_detection(scan: &[Detection]) -> Detection { + let mut nearest = FREE_ROAD; + for &detection in scan { + if detection.range < nearest.range { + nearest = detection; + } + } + + // Return the nearest, which is the free road until something beats it. + nearest +} + +/// How an IDM follower is tuned: how fast it wants to go, how much room +/// it wants at that speed, and how hard it will work for either. +/// +/// These are the published equation's five and nothing else, each +/// carrying its symbol so the two can be read side by side. +/// +/// Three of them are divisors, so `v0_speed_tgt`, `a_accel_max` and +/// `b_decel_comfort` have to be greater than zero. A zero among them +/// reaches [`idm_accel`] as a NaN rather than as a wrong number, and a +/// NaN spreads into every command the car goes on to make. Whoever takes +/// these from outside checks them there, where the value came in and +/// where there is something to report to. +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct IdmParams { + /// Speed held on an open road, m/s, greater than zero. + pub v0_speed_tgt: f64, + /// Seconds of gap wanted at whatever speed is being held. + pub t_headway: f64, + /// Meters of gap wanted at a standstill. + pub s0_gap_min: f64, + /// The most acceleration commanded, m/s^2, greater than zero. + pub a_accel_max: f64, + /// The braking taken as comfortable, m/s^2 and greater than zero. + /// Also the hardest [`idm_accel`] will ask for, which costs a car the + /// emergency stop it might otherwise make. + pub b_decel_comfort: f64, +} + +impl IdmParams { + /// The set this project drives cars with, taking the speed each one + /// wants to hold. + /// + /// `t_headway` and `s0_gap_min` are the values Treiber's own + /// simulator lists for a car, at + /// . The + /// acceleration and braking there, 0.3 and 3.0, are picked to bring + /// stop-and-go waves out of a crowded road, which is a different + /// thing to show than a car following another car, so `a_accel_max` + /// and `b_decel_comfort` are this project's, within what ordinary + /// driving uses. + pub const fn highway_car(v0_speed_tgt: f64) -> Self { + IdmParams { + v0_speed_tgt, + t_headway: 1.5, + s0_gap_min: 2.0, + a_accel_max: 1.5, + b_decel_comfort: 2.0, + } + } +} + +/// Acceleration the Intelligent Driver Model commands, m/s^2. +/// +/// `gap` is the room ahead in meters. `approach_rate` is how fast that +/// room is closing, in m/s and positive while it closes, which is the +/// paper's own convention: it defines the approaching rate as the +/// follower's speed minus the lead's. A [`Detection`] measures the same +/// quantity the other way up, so a caller holding one passes +/// `-range_rate`. +/// +/// Both are relative, which is what a radar measures and all the law +/// wants: the lead's own speed appears nowhere in it. +/// +/// The room it asks for is `s0_gap_min`, plus `t_headway` seconds of +/// travel, plus enough to shed the approach rate against a brake between +/// comfortable and hardest. It accelerates when it has more room than +/// that and brakes when it has less, easing off either way as it nears +/// `v0_speed_tgt`. An open road is the same expression rather than a case +/// of its own, since at [`FREE_ROAD_RANGE`] the room asked for is +/// nothing beside the room there is. +/// +/// Add, multiply, divide and sqrt only, which IEEE 754 pins, so four +/// platforms agree to the last bit. The fourth power is two squarings +/// rather than `powi`, whose expansion is the compiler's business rather +/// than the standard's. +/// +/// One thing here is not in the published equation, and the body says +/// what it is for: the wanted gap does not go below zero. The command is +/// held inside `[-b_decel_comfort, a_accel_max]`, which adds no +/// parameter of its own, and buys a finite answer for a gap of nothing +/// at the price of the emergency braking a real car would have. +pub fn idm_accel(speed: f64, gap: f64, approach_rate: f64, params: IdmParams) -> f64 { + // The paper's s* reads s0 + s1*sqrt(v/v0) + v*T + v*dv/(2*sqrt(ab)), + // and this is it with s1 = 0, as the common four-parameter form has + // it, under the one adaptation: a wanted gap is a distance, so it + // does not go below zero. + // + // Something has to say that, because a lead pulling away contributes + // a negative closing term, and the equation as published lets that + // take the whole expression negative, where it squares back into + // braking. Then a car brakes as the road ahead clears: at 20 m/s + // with 20 m of gap and a lead pulling away to 30, -1.28 m/s^2 where + // the answer wanted is +1.20. A limit on the output cannot fix that, + // since -1.28 is a perfectly ordinary number to command. What is + // wrong with it is its sign. + let closing_room = + speed * approach_rate / (2.0 * (params.a_accel_max * params.b_decel_comfort).sqrt()); + let gap_wanted = (params.s0_gap_min + speed * params.t_headway + closing_room).max(0.0); + + // How much of the open road is left to take, and how much of it the + // lead has taken. The fourth power is what holds the first term near + // full until the speed is nearly there, rather than easing off from + // a standstill onward. + let speed_ratio = speed / params.v0_speed_tgt; + let speed_ratio_squared = speed_ratio * speed_ratio; + let free_road = 1.0 - speed_ratio_squared * speed_ratio_squared; + // Every real gap divides as it stands. The floor only keeps a zero + // out of the divisor, since a wanted gap of zero over it would be a + // NaN spreading into everything downstream. What comes out of a gap + // of nothing is an enormous quotient, and the clamp takes that to + // the braking limit, which is the answer a collided pair deserves. + let crowding = gap_wanted / gap.max(f64::MIN_POSITIVE); + + // Return the acceleration the equation gives, which is what is left + // of the open road once the lead's share is taken off it, held + // inside the two rates the parameters name. + // + // The lower bound is the one doing the work: as the gap goes to + // nothing the crowding term runs away, taking the command to + // negative infinity, which is a brake without any limit at all. The + // upper bound only guards, since the equation on its own never gives + // more than a_accel_max, that being what a standstill on an empty + // road gives. + (params.a_accel_max * (free_road - crowding * crowding)) + .clamp(-params.b_decel_comfort, params.a_accel_max) +} + +#[cfg(test)] +mod tests { + use continuo_core::{Quat, Vec3}; + + use super::*; + + /// A hundred meters of road along +x, which every test here steers on. + fn road() -> Waypoints { + Waypoints::build_straight((0.0, 0.0), (100.0, 0.0)) + } + + fn pose_at(x: f64, y: f64, yaw: f64) -> Pose { + Pose { + position: Vec3::new(x, y, 0.0), + orientation: Quat::from_yaw(yaw), + } + } + + /// Aims 10 m ahead at the road itself, turning a radian per radian. + fn tuning() -> PurePursuitParams { + PurePursuitParams { + lateral_tgt: 0.0, + lookahead: 10.0, + gain_yaw_rate: 1.0, + max_yaw_rate: 1.0, + } + } + + #[test] + fn a_follower_on_its_lane_and_pointing_along_it_commands_no_turn() { + assert_eq!( + pure_pursuit_yaw_rate(&road(), pose_at(20.0, 0.0, 0.0), tuning()), + 0.0 + ); + } + + #[test] + fn a_follower_off_its_lane_turns_back_toward_it() { + // Left of the lane the aim point is to the right, so the command + // is clockwise, and one as far the other side turns the other way + // by exactly as much. Exactly, with no tolerance, because a + // mirrored geometry that answered only to within rounding would + // mean the law had a preferred side. + let from_the_left = pure_pursuit_yaw_rate(&road(), pose_at(20.0, 3.0, 0.0), tuning()); + let from_the_right = pure_pursuit_yaw_rate(&road(), pose_at(20.0, -3.0, 0.0), tuning()); + assert!(from_the_left < 0.0, "{from_the_left}"); + assert_eq!(from_the_left, -from_the_right); + } + + #[test] + fn a_lane_offset_is_what_the_law_holds_rather_than_the_road() { + let holding_a_lane = PurePursuitParams { + lateral_tgt: 3.0, + ..tuning() + }; + + // On the centerline there is now three meters of error to work + // off, and none once the car is on the lane it was given. + let toward_the_lane = + pure_pursuit_yaw_rate(&road(), pose_at(20.0, 0.0, 0.0), holding_a_lane); + assert!(toward_the_lane > 0.0, "{toward_the_lane}"); + assert_eq!( + pure_pursuit_yaw_rate(&road(), pose_at(20.0, 3.0, 0.0), holding_a_lane), + 0.0 + ); + } + + #[test] + fn the_command_never_leaves_the_clamp() { + let hard_and_capped = PurePursuitParams { + gain_yaw_rate: 10.0, + max_yaw_rate: 0.5, + ..tuning() + }; + + // Pointing across the road, where a quarter turn of error times a + // gain of ten asks for far more than the follower may command. + let quarter_turn = std::f64::consts::FRAC_PI_2; + let facing_right = + pure_pursuit_yaw_rate(&road(), pose_at(20.0, 0.0, -quarter_turn), hard_and_capped); + let facing_left = + pure_pursuit_yaw_rate(&road(), pose_at(20.0, 0.0, quarter_turn), hard_and_capped); + assert_eq!(facing_right, 0.5); + assert_eq!(facing_left, -0.5); + } + + #[test] + fn wrapping_an_angle_answers_its_mirror_exactly() { + // Out to +/- 8 radians, a little past a full turn either side of + // zero, so the sweep covers angles that wrap and angles that do + // not. + const STEP: f64 = 0.04; + const STEPS: i32 = 200; + + // Each angle is a product, so it has a full mantissa. Reaching + // them by summing instead, as -8.0 plus a multiple of the step, + // would leave the low bits clear, and an angle with room to + // spare there comes back from a fold up and down unchanged. The + // sweep would then pass whatever this function did. + for k in -STEPS..=STEPS { + let angle = f64::from(k) * STEP; + let wrapped = wrap_to_pi(angle); + assert!(wrapped.abs() <= std::f64::consts::PI, "{angle} {wrapped}"); + assert_eq!(wrapped, -wrap_to_pi(-angle), "{angle}"); + } + } + + #[test] + fn half_a_turn_keeps_the_sign_it_came_with() { + // The one angle with two equally short ways round, and the only + // input where the answer is a choice rather than a value. + assert_eq!(wrap_to_pi(std::f64::consts::PI), std::f64::consts::PI); + assert_eq!(wrap_to_pi(-std::f64::consts::PI), -std::f64::consts::PI); + } + + #[test] + fn an_error_past_half_a_turn_steers_the_short_way_round() { + let free_to_turn = PurePursuitParams { + max_yaw_rate: 10.0, + ..tuning() + }; + + // Past the end of the road and facing back down it, where the aim + // point is just the far side of half a turn away. Measured the + // long way the error would be most of a full turn, and the command + // a hard turn in the wrong direction. + let yaw_rate = pure_pursuit_yaw_rate(&road(), pose_at(110.0, 0.5, 3.0), free_to_turn); + assert!(yaw_rate > 0.0 && yaw_rate < 0.25, "{yaw_rate}"); + } + + #[test] + fn the_nearest_detection_wins_regardless_of_its_slot() { + let far = Detection { + range: 40.0, + range_rate: -1.0, + }; + let near = Detection { + range: 12.0, + range_rate: -3.0, + }; + let farthest = Detection { + range: 90.0, + range_rate: 5.0, + }; + assert_eq!(nearest_detection(&[far, near, farthest]), near); + + // The same three cars, reported in another order, are the same + // three cars, and the sensor promises no order at all. + assert_eq!(nearest_detection(&[near, farthest, far]), near); + } + + #[test] + fn an_empty_scan_selects_the_free_road() { + assert_eq!(nearest_detection(&[]), FREE_ROAD); + } + + #[test] + fn padding_never_beats_a_car() { + // A fixed-size scan holding one car and nothing else, which is + // the shape it arrives in once it has crossed into an FMU. + let car = Detection { + range: 30.0, + range_rate: -2.0, + }; + let mut scan = [FREE_ROAD; 8]; + scan[5] = car; + assert_eq!(nearest_detection(&scan), car); + + // And one holding nothing is the free road, padding and all. + assert_eq!(nearest_detection(&[FREE_ROAD; 8]), FREE_ROAD); + } + + #[test] + fn a_tie_goes_to_the_earlier_slot() { + // Two cars at exactly one range, one closing and one not. Which + // of them is followed matters less than that the answer is not + // left to the order the slots happen to arrive in. + let closing = Detection { + range: 25.0, + range_rate: -4.0, + }; + let opening = Detection { + range: 25.0, + range_rate: 1.0, + }; + assert_eq!(nearest_detection(&[closing, opening]), closing); + } + + /// A car wanting 30 m/s, tuned the way the demo tunes every car. + fn idm_tuning() -> IdmParams { + IdmParams::highway_car(30.0) + } + + #[test] + fn an_open_road_from_rest_accelerates_at_the_limit() { + // Nothing ahead and no speed yet, which is the whole of what the + // car will do and exactly that, since the room a standstill asks + // for is nothing against a free road. + assert_eq!( + idm_accel(0.0, FREE_ROAD_RANGE, 0.0, idm_tuning()), + idm_tuning().a_accel_max + ); + } + + #[test] + fn an_open_road_at_the_speed_it_wants_commands_nothing() { + let accel = idm_accel(30.0, FREE_ROAD_RANGE, 0.0, idm_tuning()); + assert!(accel.abs() < 1e-12, "{accel}"); + } + + #[test] + fn the_published_equation_gives_the_value_worked_by_hand() { + // Doing 20 behind a lead 60 m off and closing at 5, worked from + // the equation with the numbers highway_car sets: + // room wanted = 2 + 20*1.5 + 20*5 / (2*sqrt(1.5*2)) + // = 60.86751345948129 + // accel = 1.5 * (1 - (20/30)^4 - (60.86751345948129/60)^2) + // = -0.33998554410468607 + // + // Sixty meters rather than forty so the answer lands inside the + // limits, since a clamped value would check the clamp instead of + // the equation. + let accel = idm_accel(20.0, 60.0, 5.0, idm_tuning()); + assert!((accel + 0.339_985_544_104_686_07).abs() < 1e-12, "{accel}"); + } + + #[test] + fn the_equilibrium_gap_commands_nothing() { + // Where IDM settles behind a lead holding a steady speed: the + // two terms cancel, so (s*/gap)^2 is the open-road term and the + // gap is s* over its root. Derived rather than written down, + // since a constant here would be a number nobody could check. + let params = idm_tuning(); + let speed = 20.0; + let gap_wanted = params.s0_gap_min + speed * params.t_headway; + let open_road = 1.0 - (speed / params.v0_speed_tgt).powi(4); + let gap = gap_wanted / open_road.sqrt(); + + // It sits further back than the room the law asks for, because + // at 20 against a wanted 30 the open-road term is already short + // of full, and only a longer gap makes up the difference. + assert!(gap > gap_wanted, "{gap} {gap_wanted}"); + let accel = idm_accel(speed, gap, 0.0, params); + assert!(accel.abs() < 1e-12, "{accel} at {gap} m"); + } + + /// The paper's stated highway values: v0 = 120 km/h, T = 1.6 s, + /// a = 0.73 and b = 1.67 m/s^2, with the jam distance at the 2 m in + /// common use. + fn the_papers_highway_car() -> IdmParams { + IdmParams { + v0_speed_tgt: 120.0 / 3.6, + t_headway: 1.6, + s0_gap_min: 2.0, + a_accel_max: 0.73, + b_decel_comfort: 1.67, + } + } + + #[test] + fn the_papers_own_parameters_settle_where_its_equation_says() { + // The paper tabulates parameters and plots figures, and puts no + // worked acceleration on the page to check a number against. So + // what its parameters can check is its own algebra, over the + // range of speeds they were quoted for. + let params = the_papers_highway_car(); + for &speed in &[5.0, 15.0, 25.0, 33.0] { + let gap_wanted = params.s0_gap_min + speed * params.t_headway; + let open_road = 1.0 - (speed / params.v0_speed_tgt).powi(4); + let gap = gap_wanted / open_road.sqrt(); + let accel = idm_accel(speed, gap, 0.0, params); + assert!(accel.abs() < 1e-12, "{speed} m/s at {gap} m gave {accel}"); + } + + // And the one value the paper does state outright: on an open + // road from rest, a car accelerates at a. + assert_eq!( + idm_accel(0.0, FREE_ROAD_RANGE, 0.0, params), + params.a_accel_max + ); + } + + #[test] + fn a_close_slow_lead_brakes_no_harder_than_the_limit() { + // Thirty meters a second onto something 5 m ahead: the equation + // asks for hundreds, and the car commands what it has. + assert_eq!( + idm_accel(30.0, 5.0, 20.0, idm_tuning()), + -idm_tuning().b_decel_comfort + ); + } + + #[test] + fn a_lead_pulling_away_never_buys_room_below_the_minimum() { + // The wanted gap floors at zero, so however fast a lead pulls + // away, what is left is an open road rather than a negative + // distance squaring back into braking. Two different retreats + // therefore answer alike, and both accelerate. + let fast = idm_accel(20.0, 32.0, -1000.0, idm_tuning()); + let faster = idm_accel(20.0, 32.0, -5000.0, idm_tuning()); + assert_eq!(fast, faster); + assert!(fast > 0.0, "{fast}"); + + // The case that made the floor necessary, which is ordinary + // rather than extreme: a lead pulling away to 30 with 20 m of + // gap. Unfloored the equation commands -1.28 here. + let clearing = idm_accel(20.0, 20.0, -10.0, idm_tuning()); + assert!(clearing > 1.0, "{clearing}"); + } + + #[test] + fn a_gap_of_nothing_brakes_rather_than_answering_nothing() { + // Both of these are already a collision, and the law's business + // with them is only to stay a number. + assert_eq!( + idm_accel(20.0, 0.0, 0.0, idm_tuning()), + -idm_tuning().b_decel_comfort + ); + assert_eq!( + idm_accel(20.0, -3.0, 0.0, idm_tuning()), + -idm_tuning().b_decel_comfort + ); + } + + #[test] + fn the_command_is_always_finite_and_inside_the_limits() { + let params = idm_tuning(); + for &speed in &[0.0, 5.0, 20.0, 30.0, 60.0] { + for &gap in &[-1.0, 0.0, 0.5, 5.0, 40.0, FREE_ROAD_RANGE] { + for &approach_rate in &[-60.0, -1.0, 0.0, 1.0, 60.0] { + let accel = idm_accel(speed, gap, approach_rate, params); + assert!( + accel.is_finite() + && accel <= params.a_accel_max + && accel >= -params.b_decel_comfort, + "{speed} {gap} {approach_rate} gave {accel}" + ); + } + } + } + } +} diff --git a/crates/continuo-actors/src/controller.rs b/crates/continuo-actors/src/controller.rs index e857bfb..867c345 100644 --- a/crates/continuo-actors/src/controller.rs +++ b/crates/continuo-actors/src/controller.rs @@ -5,6 +5,7 @@ use continuo_core::{ }; use serde::{Deserialize, Serialize}; +use crate::control_laws::{PurePursuitParams, pure_pursuit_yaw_rate}; use crate::path::Waypoints; /// Drive command from a controller to its physics sibling. @@ -16,8 +17,9 @@ pub struct Cmd { pub yaw_rate: f64, } -/// Pure-pursuit-flavored path follower: projects the latest pose onto the -/// road, aims at a lookahead point, and commands a clamped yaw rate. +/// Path follower: reads the latest pose, asks +/// [`pure_pursuit_yaw_rate`] where to steer, and publishes that with the +/// speed it was built with. /// /// Follows the road in **Frenet coordinates**: an arc length `s` found by /// projection, and a fixed lateral offset it holds. So every car on a road @@ -29,13 +31,9 @@ pub struct Cmd { pub struct PathFollowController { actor_name: String, road: Arc, - /// Meters left of the road's centerline to hold, the Frenet `d`. - lateral: f64, period: SimDuration, speed: f64, - lookahead: f64, - gain: f64, - max_yaw_rate: f64, + pursuit_params: PurePursuitParams, last_pose: Pose, } @@ -44,39 +42,30 @@ impl PathFollowController { pub fn new( actor_name: impl Into, road: Arc, - lateral: f64, + lateral_tgt: f64, period: SimDuration, speed: f64, lookahead: f64, - gain: f64, + gain_yaw_rate: f64, max_yaw_rate: f64, initial_pose: Pose, ) -> Self { PathFollowController { actor_name: actor_name.into(), road, - lateral, period, speed, - lookahead, - gain, - max_yaw_rate, + pursuit_params: PurePursuitParams { + lateral_tgt, + lookahead, + gain_yaw_rate, + max_yaw_rate, + }, last_pose: initial_pose, } } } -fn wrap_pi(angle: f64) -> f64 { - let wrapped = angle.rem_euclid(std::f64::consts::TAU); - - // Return the equivalent angle in (-pi, pi]. - if wrapped > std::f64::consts::PI { - wrapped - std::f64::consts::TAU - } else { - wrapped - } -} - impl Component for PathFollowController { fn id(&self) -> ComponentId { ComponentId::new("controller").expect("valid id") @@ -99,14 +88,9 @@ impl Component for PathFollowController { self.last_pose = message.decode::()?; } - let position = self.last_pose.position; - let s = self.road.project(position.x, position.y); - let target = self.road.point_at_offset(s + self.lookahead, self.lateral); - let desired_heading = f64::atan2(target.y - position.y, target.x - position.x); - let heading_error = wrap_pi(desired_heading - self.last_pose.orientation.yaw()); let cmd = Cmd { speed: self.speed, - yaw_rate: (self.gain * heading_error).clamp(-self.max_yaw_rate, self.max_yaw_rate), + yaw_rate: pure_pursuit_yaw_rate(&self.road, self.last_pose, self.pursuit_params), }; let key = crate::cmd_key(ctx.world_name(), &self.actor_name); diff --git a/crates/continuo-actors/src/lib.rs b/crates/continuo-actors/src/lib.rs index d92c80f..021d3a3 100644 --- a/crates/continuo-actors/src/lib.rs +++ b/crates/continuo-actors/src/lib.rs @@ -2,6 +2,10 @@ //! path-following controller, unicycle physics, a pose logger, and a //! traffic spawner. //! +//! [`control_laws`] holds the laws themselves, as pure functions over +//! their arguments. A controller component is then the wiring around one: +//! read the inbox, call the law, publish the answer. +//! //! [`PathFollowController`] and [`UnicyclePhysics`] form a car: the //! composite `[controller, physics]`, where the controller runs at the //! slower period and intra-composite same-instant delivery feeds its @@ -20,6 +24,7 @@ //! one freeway scenario; both carry TODOs pointing at the world spec and //! scenario configuration that replace them. +pub mod control_laws; mod controller; mod logger; mod path; @@ -37,6 +42,17 @@ pub use traffic_spawner::{ traffic_despawn_key, traffic_spawn_key, }; +/// How many detections a scan carries at most. +/// +/// A placeholder for demo development, picked well above anything the +/// scenarios here produce so truncation is not a question yet. It settles +/// once a sensor has a range and a scenario has a traffic density, and +/// until then the arguments for any particular number are guesswork. +/// +/// A consumer wanting the scan as a fixed-length array reads it from +/// here, so the cap and the array are one number. +pub const MAX_DETECTIONS: usize = 64; + /// Key for an actor's pose in `world`. pub fn pose_key(world_name: &str, actor_name: &str) -> KeyExpr { KeyExpr::new_rooted(format!("{world_name}/actor/{actor_name}/pose")).expect("valid pose key") diff --git a/crates/continuo-actors/src/path.rs b/crates/continuo-actors/src/path.rs index 21ed341..85d2474 100644 --- a/crates/continuo-actors/src/path.rs +++ b/crates/continuo-actors/src/path.rs @@ -104,6 +104,26 @@ impl Waypoints { Self::build_closed(points) } + /// The points the path was built from, in order. + /// + /// These and [`is_closed`](Self::is_closed()) are the whole of a path, + /// since the arc-length table is derived from them. So the two are what + /// travels when a path has to reach somewhere a Rust value cannot, such + /// as a controller inside an FMU, which rebuilds its own copy through + /// the same builders. + pub fn points(&self) -> &[(f64, f64)] { + &self.points + } + + /// Whether the last point connects back to the first. + /// + /// It travels beside [`points`](Self::points()) because one set of + /// points makes two different paths: a loop wraps at the end where a + /// road stops. + pub fn is_closed(&self) -> bool { + self.is_closed + } + pub fn total_length(&self) -> f64 { self.total } @@ -338,4 +358,67 @@ mod tests { let s = p.project(0.0, 10.0); assert!((s - 30.0).abs() < 1e-12); } + + /// A point as the bits it is made of, so the comparisons below are + /// exact rather than close enough. + fn bits(v: Vec3) -> (u64, u64, u64) { + (v.x.to_bits(), v.y.to_bits(), v.z.to_bits()) + } + + #[test] + fn a_road_rebuilt_from_its_points_is_the_same_road() { + // A standard lane width, so the offset is one a real lane sits at. + const LANE_OFFSET: f64 = 3.5; + + // A bend and a loop, so the copy is checked both where an open + // path clamps and where a closed one wraps. + let roads = [ + Waypoints::build_open(vec![(0.0, 0.0), (30.0, 0.0), (55.0, 18.0), (80.0, 18.0)]), + square(), + ]; + + for road in roads { + let copy = if road.is_closed() { + Waypoints::build_closed(road.points().to_vec()) + } else { + Waypoints::build_open(road.points().to_vec()) + }; + assert_eq!(copy.total_length().to_bits(), road.total_length().to_bits()); + + // Past both ends as well as along it, since that is where the + // two kinds of path answer differently. + for pct in -10..=110 { + let s = road.total_length() * pct as f64 / 100.0; + assert_eq!(bits(copy.point_at(s)), bits(road.point_at(s)), "s = {s}"); + assert_eq!( + copy.heading_at(s).to_bits(), + road.heading_at(s).to_bits(), + "s = {s}" + ); + // The call a lane-following controller makes, and worth + // comparing in its own right: a left normal taken across a + // corner rather than along one segment would answer + // differently here while the centerline and the heading + // still agreed. + assert_eq!( + bits(copy.point_at_offset(s, LANE_OFFSET)), + bits(road.point_at_offset(s, LANE_OFFSET)), + "s = {s}" + ); + } + + // Projection from all round the path, which is where a segment + // table built any other way would show. + for i in -2..=12 { + for j in -3..=3 { + let (x, y) = (10.0 * i as f64, 5.0 * j as f64); + assert_eq!( + copy.project(x, y).to_bits(), + road.project(x, y).to_bits(), + "({x}, {y})" + ); + } + } + } + } } diff --git a/crates/continuo-core/src/lib.rs b/crates/continuo-core/src/lib.rs index 3770dfe..6fb0050 100644 --- a/crates/continuo-core/src/lib.rs +++ b/crates/continuo-core/src/lib.rs @@ -22,7 +22,7 @@ pub use hash::{HashFnv1a64, hash_bytes}; pub use ids::{ComponentId, ComponentPath}; pub use keyexpr::{KEY_ROOT, KeyExpr}; pub use math::{EulerDeg, EulerRad, Quat, Vec3}; -pub use messages::{Message, Pose, TickDone, TickStart}; +pub use messages::{Detection, Message, Pose, TickDone, TickStart}; pub use random::RandomSplitMix64; pub use seed::{derive_component_seed, mix_seeds}; pub use time::{SimDuration, SimTime}; diff --git a/crates/continuo-core/src/messages.rs b/crates/continuo-core/src/messages.rs index 1cc99f9..a5dc96e 100644 --- a/crates/continuo-core/src/messages.rs +++ b/crates/continuo-core/src/messages.rs @@ -34,6 +34,16 @@ pub struct Pose { pub orientation: Quat, } +/// One thing a sensor found ahead of it: how far off it is, and how fast +/// that is changing. +#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)] +pub struct Detection { + /// Meters to it. + pub range: f64, + /// How fast the range is changing, m/s, negative when closing. + pub range_rate: f64, +} + /// A routed unit on the transport: canonical JSON payload bytes plus the /// metadata that makes delivery deterministic. /// diff --git a/crates/continuo-fmi/src/fmu_component.rs b/crates/continuo-fmi/src/fmu_component.rs index 5f2e0e4..9cc6721 100644 --- a/crates/continuo-fmi/src/fmu_component.rs +++ b/crates/continuo-fmi/src/fmu_component.rs @@ -38,13 +38,34 @@ pub struct FmuComponent { outputs: Vec, subscriptions: Vec, period: SimDuration, - /// Variables the mapping gives a starting value. Held until the first - /// step, which is where Initialization Mode happens. + /// Structural parameters the mapping sets, written in Configuration Mode + /// before anything else. Kept rather than spent, since [`Self::reset`] + /// sends the FMU back to where both modes begin and each set is written + /// again on the way through. + structural_vars: Vec<(ResolvedVariable, Value)>, + /// Variables the mapping gives a starting value, written during + /// Initialization Mode. Kept for the same reason as the structural vars + /// above. vars_to_initialize: Vec<(ResolvedVariable, Value)>, - /// When this component last stepped, and `None` until it has. The FMU's - /// own clock: `fmi3DoStep` is told the point it steps from, so the - /// adapter has to remember where it left the model. - last_step: Option, + /// Where this instance stands in FMI's own state machine, as far as the + /// two states an adapter with event mode off ever sees. + phase: Phase, +} + +/// The FMI states an instance passes through here, and the FMU's own clock. +/// +/// The standard's machine has more states than these, but the rest are +/// passed through inside a single call: Configuration Mode opens and closes +/// within [`FmuComponent::new`], and Initialization Mode within the step +/// that performs it. These two are the ones an instance rests in between +/// calls, so they are what a step has to tell apart. +enum Phase { + /// Instantiated and sized, waiting for a step to initialize it. + Uninitialized, + /// In Step Mode, having last stepped at this instant. `fmi3DoStep` is + /// told the point it steps from, so the adapter has to remember where it + /// left the model. + Stepping { last: SimTime }, } impl FmuComponent { @@ -174,37 +195,72 @@ impl FmuComponent { outputs, subscriptions, period, + structural_vars, vars_to_initialize, - last_step: None, + phase: Phase::Uninitialized, }; - - // Configuration Mode is the only state where a structural parameter - // may be written, and it comes before initialization. Entering it for - // nothing would be a call an FMU need not implement, so an FMU with - // no structural parameters in its mapping never sees it. - if !structural_vars.is_empty() { - component - .instance - .enter_configuration_mode() - .map_err(|source| FmuConstructionError::Configure { - instance_name: component.id.to_string(), - reason: format!("{source:?}"), - })?; - component.set_initial_values(&structural_vars)?; - component - .instance - .exit_configuration_mode() - .map_err(|source| FmuConstructionError::Configure { - instance_name: component.id.to_string(), - reason: format!("{source:?}"), - })?; - } + component.size_by_structural_parameters()?; // Return a component sized by its structural parameters and ready to // initialize, which it does on its first step. Ok(component) } + /// Writes the mapping's structural parameters, which decide how large the + /// FMU's arrays are. + /// + /// Configuration Mode is the only state where one may be written, and it + /// comes before initialization. Entering it for nothing would be a call an + /// FMU need not implement, so an FMU with no structural parameters in its + /// mapping never sees it. + fn size_by_structural_parameters(&mut self) -> Result<(), FmuConstructionError> { + if self.structural_vars.is_empty() { + return Ok(()); + } + self.instance.enter_configuration_mode().map_err(|source| { + FmuConstructionError::Configure { + instance_name: self.id.to_string(), + reason: format!("{source:?}"), + } + })?; + Self::set_initial_values(&mut self.instance, &self.id, &self.structural_vars)?; + self.instance.exit_configuration_mode().map_err(|source| { + FmuConstructionError::Configure { + instance_name: self.id.to_string(), + reason: format!("{source:?}"), + } + })?; + + Ok(()) + } + + /// Puts the FMU back to how it was instantiated, so the next step + /// initializes it again. + /// + /// This is what `fmi3Reset` is for: running a model again without paying + /// to load it a second time. It does not initialize, because + /// Initialization Mode needs a start time and only a step carries one. + /// The next step writes the mapping's starting values on its way through, + /// so a second run begins where the first did. + /// + /// Its structural parameters go in here instead, because Configuration + /// Mode is the only state that accepts one and it closes before + /// Initialization Mode opens. Without that a reset StateSpace would come + /// back at the size its description declares while the bindings still + /// addressed the size the mapping asked for. + pub fn reset(&mut self) -> Result<(), CoreError> { + self.instance + .reset() + .map_err(|source| step_failure(&self.id, "", "fmi3Reset", &source))?; + self.size_by_structural_parameters() + .map_err(|error| CoreError::ComponentFailure { + reason: error.to_string(), + })?; + self.phase = Phase::Uninitialized; + + Ok(()) + } + /// Writes values the mapping wrote out straight into the FMU. /// /// The mapping supplies these itself rather than binding them to a @@ -214,8 +270,13 @@ impl FmuComponent { /// Used twice, in the two modes the standard allows: structural /// parameters during configuration, and everything else during /// initialization. + /// + /// Takes the instance and the id rather than the whole component, so a + /// caller can pass a field of that same component as the values without + /// borrowing it twice. fn set_initial_values( - &mut self, + instance: &mut InstanceCS, + id: &ComponentId, initial_vals: &[(ResolvedVariable, Value)], ) -> Result<(), FmuConstructionError> { for (variable, value) in initial_vals { @@ -228,7 +289,7 @@ impl FmuComponent { dimensions: variable.dimensions.clone(), }); } - set_values(&mut self.instance, &self.id, variable, &elements).map_err(|source| { + set_values(instance, id, variable, &elements).map_err(|source| { FmuConstructionError::InitialValue { variable: variable.name.clone(), reason: source.to_string(), @@ -390,12 +451,12 @@ impl Component for FmuComponent { fn step(&mut self, ctx: &mut StepCtx) -> Result { let now = ctx.now(); - match self.last_step { - // Nothing to step across yet, so the first step initializes and + match self.phase { + // Nothing to step across yet, so this step initializes and // publishes what the FMU starts out holding. Initialization ends // in Step Mode, since this instance declared it does not handle // events. - None => { + Phase::Uninitialized => { self.instance .enter_initialization_mode(None, now.as_secs_f64(), None) .map_err(|source| { @@ -405,8 +466,7 @@ impl Component for FmuComponent { // The mapping's values first, then the inbox, so a message // arriving at instant zero wins over a start value written // for the case where none does. - let vars_to_initialize = std::mem::take(&mut self.vars_to_initialize); - self.set_initial_values(&vars_to_initialize) + Self::set_initial_values(&mut self.instance, &self.id, &self.vars_to_initialize) .map_err(|error| CoreError::ComponentFailure { reason: error.to_string(), })?; @@ -415,7 +475,7 @@ impl Component for FmuComponent { step_failure(&self.id, "", "exit_initialization_mode", &source) })?; } - Some(last) => { + Phase::Stepping { last } => { self.apply_inbox(ctx.inbox())?; let mut event_handling_needed = false; @@ -488,7 +548,7 @@ impl Component for FmuComponent { } self.publish_outputs(ctx)?; - self.last_step = Some(now); + self.phase = Phase::Stepping { last: now }; // Return the next instant this FMU is due. Ok(now + self.period) diff --git a/crates/continuo-fmi/tests/reference_fmus.rs b/crates/continuo-fmi/tests/reference_fmus.rs index b5d3f2f..95cab23 100644 --- a/crates/continuo-fmi/tests/reference_fmus.rs +++ b/crates/continuo-fmi/tests/reference_fmus.rs @@ -331,6 +331,43 @@ fn two_identical_fmu_runs_publish_identically() { assert_eq!(run(), run()); } +#[test] +fn a_reset_instance_runs_again_from_where_it_first_started() { + // A gravity the fixture does not declare, so a second run answering like + // the first says the mapping's own values were written again rather than + // the FMU's start values quietly standing in for them. + let mut mapping = empty_mapping(10); + mapping.initial_values = vec![("g".to_string(), json!(-20.0))]; + mapping.outputs = vec![OutputBinding::new("h", key("ball"))]; + let mut ball = FmuComponent::new("ball", fixture_path("BouncingBall"), mapping).expect("build"); + + let run = |ball: &mut FmuComponent| -> Vec { + (0..50) + .map(|tick| { + step(ball, SimTime::from_millis(tick * 10), Vec::new())[0].payload["h"] + .as_f64() + .expect("a height") + }) + .collect() + }; + + let first = run(&mut ball); + ball.reset().expect("the FMU resets"); + let second = run(&mut ball); + + // A ball that only fell would leave less behind for a reset to undo than + // one that bounced, whose velocity changed sign along the way. + assert!( + first.windows(2).any(|pair| pair[1] > pair[0]), + "the run bounces, so what reset has to undo is more than a fall: {first:?}" + ); + assert_eq!( + first.iter().map(|h| h.to_bits()).collect::>(), + second.iter().map(|h| h.to_bits()).collect::>(), + "a reset ball falls from where it was dropped rather than from where it had got to" + ); +} + #[test] fn an_fmu_handles_its_own_events_when_event_mode_is_off() { // Instantiated with `event_mode_used = false`, so an FMU must deal with @@ -454,6 +491,31 @@ fn a_structural_parameter_set_at_construction_resizes_its_arrays() { ); } +#[test] +fn a_reset_instance_is_sized_by_its_structural_parameters_again() { + // Reset drops an FMU back to Instantiated, which is before it was ever + // sized, and StateSpace sizes every array from `m`, `n` and `r`. Left + // there, those would return to the 3 its description declares, so `u` + // and `y` would each hold three elements while the binding goes on + // writing the two the mapping asked for. Configuration Mode closes + // before Initialization Mode opens, so the next step is already too + // late to put it right. + let mut mapping = state_space(&[("m", 2), ("n", 2), ("r", 2)], identity(2)); + mapping.inputs = vec![InputBinding::new("u", key("u")).with_pointer("/u/*")]; + let mut fmu = FmuComponent::new("ss", fixture_path("StateSpace"), mapping).expect("build"); + + let fed = |fmu: &mut FmuComponent, now| { + step(fmu, now, vec![message("u", json!({"u": [4.0, 5.0]}))])[0].payload["y"].clone() + }; + + let before = fed(&mut fmu, SimTime::ZERO); + fmu.reset().expect("the FMU resets"); + let after = fed(&mut fmu, SimTime::from_millis(1000)); + + assert_eq!(before, json!([4.0, 5.0]), "sized before the reset"); + assert_eq!(after, before, "and sized the same way after it"); +} + #[test] fn an_array_binding_whose_pointer_count_misses_the_dimension_fails_at_construction() { // The check that stops a rebuilt FMU and a stale mapping from drifting diff --git a/crates/continuo-fmu-controller-idm/Cargo.toml b/crates/continuo-fmu-controller-idm/Cargo.toml new file mode 100644 index 0000000..ae1284a --- /dev/null +++ b/crates/continuo-fmu-controller-idm/Cargo.toml @@ -0,0 +1,57 @@ +[package] +name = "continuo-fmu-controller-idm" +# This is the FMU's own description, not just the crate's: the packager +# writes it into modelDescription.xml, so it is what a host reads. +description = "A controller for traffic cars, following with IDM and turning with pure pursuit" +version.workspace = true +edition.workspace = true +license.workspace = true +rust-version.workspace = true +# Nothing outside this workspace has a use for it, and the artifact that +# matters is the packaged .fmu rather than the crate. +publish = false + +# cdylib is what FMI loads; rlib is what `packaged_fmu_path` and the comparison +# in tests/ link against. Neither can be feature-gated, since `cargo fmi +# bundle`, which packages the FMU, builds with default features. +[lib] +crate-type = ["cdylib", "rlib"] + +[features] +# Turns on the tests in `tests/packaged_fmu.rs`, which read what +# `cargo xtask package-fmus` writes and so need it run first. Off by default, +# which is what makes them opt in rather than a step every test run pays for. +# CI turns it on after packaging. +# +# The gate is a `#![cfg]` in that file rather than `required-features` here +# because CI runs `--test '*'`, and a glob names every target it matches, so +# cargo refuses the step over a named target whose features are off. +packaged-fmu = [] + +[dependencies] +continuo-actors = { workspace = true } +continuo-core = { workspace = true } +# The derive writes `::fmi::...` paths, so the exporting crate needs `fmi` +# of its own even though nothing here names it. +fmi = { workspace = true } +fmi-export = { workspace = true } +thiserror = { workspace = true } + +# What `tests/packaged_fmu.rs` takes to import the packaged FMU and drive it as +# a component, which is the only way to reach it across the boundary rather +# than beside it. +[dev-dependencies] +continuo-fmi = { workspace = true } +serde_json = { workspace = true } + +# `export_fmu!` expands to functions marked for a coverage build, and a cfg +# inside a macro is checked against the crate it lands in rather than the one +# it came from. Naming it here says it is expected, which silences three dozen +# warnings without turning the lint off. +# +# `check-cfg` in a lint table is Cargo 1.80 and later. An editor calling it +# invalid is validating against a schema older than the feature; a build +# script emitting `cargo::rustc-check-cfg` would say the same thing, and is +# machinery bought to satisfy a schema rather than the compiler. +[lints.rust] +unexpected_cfgs = { level = "warn", check-cfg = ['cfg(coverage_nightly)'] } diff --git a/crates/continuo-fmu-controller-idm/src/error.rs b/crates/continuo-fmu-controller-idm/src/error.rs new file mode 100644 index 0000000..2cfe264 --- /dev/null +++ b/crates/continuo-fmu-controller-idm/src/error.rs @@ -0,0 +1,49 @@ +//! What this crate refuses, on both sides of the boundary. +//! +//! [`BadInput`] is what the FMU tells a host at run time, in the one +//! channel FMI gives it: a status and a line of text. [`PackageError`] is +//! what a Rust caller gets when the packaged `.fmu` has not been built. +//! They share nothing but being refusals, and they sit together for the +//! same reason every crate here keeps its errors in one place: it is +//! where a reader looks for what can go wrong. + +use std::path::PathBuf; + +use crate::MAX_WAYPOINTS; +use crate::package_fmu::{FMU_DIRECTORY, FMU_FILE_NAME}; + +/// Something the FMU was handed that no controller could run on. +/// +/// Each carries the value that arrived, because a host setting a +/// parameter from another tool has no other way to see what it sent. The +/// text is the whole of what crosses back, since FMI carries a status and +/// a log line rather than anything structured. +#[derive(Debug, Clone, Copy, PartialEq, thiserror::Error)] +pub(crate) enum BadInput { + #[error("road_point_count is {given}, and {kind} roads need {least} to {MAX_WAYPOINTS}")] + PointCount { + given: usize, + least: usize, + kind: &'static str, + }, + + #[error("the first {count} road points are all the same place, which is a road of no length")] + RoadOfNoLength { count: usize }, + + #[error("{name} is {given}, and it has to be a positive number")] + NotPositive { name: &'static str, given: f64 }, +} + +/// Why the packaged FMU could not be found. +#[derive(Debug, thiserror::Error)] +pub enum PackageError { + #[error("cannot find the running executable to search from: {0}")] + ExeNotFound(#[source] std::io::Error), + + #[error( + "no {FMU_DIRECTORY}/{FMU_FILE_NAME} in {} or any directory above it: \ + run `cargo install cargo-fmi` once, then `cargo xtask package-fmus`", + searched_from.display() + )] + NotPackaged { searched_from: PathBuf }, +} diff --git a/crates/continuo-fmu-controller-idm/src/fmu_controller.rs b/crates/continuo-fmu-controller-idm/src/fmu_controller.rs new file mode 100644 index 0000000..7788bc4 --- /dev/null +++ b/crates/continuo-fmu-controller-idm/src/fmu_controller.rs @@ -0,0 +1,692 @@ +//! The controller behind the FMI boundary: what crosses it, and what +//! the laws make of that. +//! +//! Nothing here decides anything. The declaration below says what a host +//! may set and read, and every answer comes from +//! [`continuo_actors::control_laws`], which the native controller calls +//! too. So the two agree because they are one implementation rather than +//! because somebody keeps them in step. + +use continuo_actors::control_laws::{ + FREE_ROAD, IdmParams, PurePursuitParams, idm_accel, nearest_detection, pure_pursuit_yaw_rate, +}; +use continuo_actors::{MAX_DETECTIONS, Waypoints}; +use continuo_core::{Detection, Pose, Quat, Vec3}; +use fmi::fmi3::{Fmi3Error, Fmi3Res}; +use fmi_export::FmuModel; +use fmi_export::fmi3::{Context, DefaultLoggingCategory, UserModel}; + +use crate::error::BadInput; + +/// How many points of road this FMU can be handed. +/// +/// It lives here because it is a fact about this interface rather than +/// about roads: `fmi-export` 0.3.0 cannot size an array by a parameter, +/// so the array is fixed and `road_point_count` says how much of it is +/// real. Padding the tail by repeating the last point instead would hand +/// [`Waypoints::project`] a segment of no length, so the padding has to +/// be ignored rather than read. +/// +/// It covers the demo's two points with room for a polyline drawn by +/// hand. A road built from a map will want more, and this is where that +/// question first shows. +pub const MAX_WAYPOINTS: usize = 64; + +/// The speed a car holds until a host says otherwise, m/s. +/// +/// The only tuning number this crate picks. Everything else it starts +/// from is a tuning set [`continuo_actors::control_laws`] already names, +/// so the FMU and the native controller start from the same places. +const DEFAULT_TARGET_SPEED: f64 = 30.0; + +/// What the following law's parameters hold until a host sets them. +const DEFAULT_IDM: IdmParams = IdmParams::highway_car(DEFAULT_TARGET_SPEED); + +/// What the steering law's parameters hold until a host sets them, +/// holding the road's own centerline. +const DEFAULT_PURSUIT: PurePursuitParams = PurePursuitParams::highway_car(0.0); + +/// A parameter the laws divide by, checked before they see it. +/// +/// Rejects a zero, a negative, an infinity and a NaN alike, since what +/// they have in common is that no answer computed from them means +/// anything. +fn require_positive(name: &'static str, given: f64) -> Result { + if given.is_finite() && given > 0.0 { + Ok(given) + } else { + Err(BadInput::NotPositive { name, given }) + } +} + +/// A controller for traffic cars, following with IDM and turning with +/// pure pursuit, from one pose and one radar scan. +/// +/// Each variable's **first doc line is its FMI description**, and the +/// packager takes that line alone. So a first line has to be a whole +/// sentence, and anything longer goes on the lines below it, which stay +/// here for whoever reads the code. +// `user_model = false` because the `impl UserModel` below is written by +// hand. Left true, the derive writes one whose `calculate_values` does +// nothing, and this FMU would export cleanly and command zero forever. +#[derive(FmuModel, Debug)] +#[model(co_simulation = true, model_exchange = false, user_model = false)] +pub struct FmuController { + /// Where the car is, meters east. + #[variable(causality = Input, name = "position.x", start = Self::default().position_x)] + position_x: f64, + /// Where the car is, meters north. + #[variable(causality = Input, name = "position.y", start = Self::default().position_y)] + position_y: f64, + /// Which way the car points: the quaternion's scalar part. + #[variable(causality = Input, name = "orientation.w", start = Self::default().orientation_w)] + orientation_w: f64, + /// Which way the car points: the quaternion's x part. + #[variable(causality = Input, name = "orientation.x", start = Self::default().orientation_x)] + orientation_x: f64, + /// Which way the car points: the quaternion's y part. + #[variable(causality = Input, name = "orientation.y", start = Self::default().orientation_y)] + orientation_y: f64, + /// Which way the car points: the quaternion's z part. + #[variable(causality = Input, name = "orientation.z", start = Self::default().orientation_z)] + orientation_z: f64, + /// How fast the car is going, m/s, as the car itself reports it. + /// + /// A radar measures nothing about the car carrying it, so this + /// arrives from the pose, standing in for a wheel speed sensor. + #[variable(causality = Input, start = Self::default().speed)] + speed: f64, + + /// Meters to each thing the radar found ahead, free road past those. + /// + /// A slot the radar did not fill holds [`FREE_ROAD`], which loses to + /// anything real, so nothing has to say how many are worth reading. + #[variable(causality = Input, start = Self::default().range)] + range: [f64; MAX_DETECTIONS], + /// How fast each range is changing, m/s, negative while closing. + #[variable(causality = Input, start = Self::default().range_rate)] + range_rate: [f64; MAX_DETECTIONS], + + // The road is fixed where the law parameters below are tunable, and + // the difference is real rather than cautious. Each instance builds + // its `Waypoints` once and keeps it, so a road changed after + // initialization would be accepted and then ignored. Declaring it + // fixed is what tells a host that before it tries. + /// The road's points, meters east, the first road_point_count real. + #[variable(causality = Parameter, variability = Fixed, start = Self::default().road_x)] + road_x: [f64; MAX_WAYPOINTS], + /// The road's points, meters north, the first road_point_count real. + #[variable(causality = Parameter, variability = Fixed, start = Self::default().road_y)] + road_y: [f64; MAX_WAYPOINTS], + /// How many of those points are the road, the rest being padding. + #[variable(causality = Parameter, variability = Fixed, start = Self::default().road_point_count)] + road_point_count: u32, + /// Whether the road wraps at its end rather than stopping there. + /// + /// True when the last point joins back to the first. Two roads with + /// the same points and different answers here are different roads. + #[variable(causality = Parameter, variability = Fixed, start = Self::default().road_closed)] + road_closed: bool, + + // Tunable, so a host may change any of these between steps: a car + // wanting a different speed, or a different lane, says so here. That + // is why a step reads them rather than keeping a copy taken at + // initialization, and why the check on them runs there too. + /// Pure pursuit: meters left of the centerline to hold, making a lane. + #[variable(causality = Parameter, variability = Tunable, start = Self::default().lateral_tgt)] + lateral_tgt: f64, + /// Pure pursuit: how far along the road to aim, in meters. + #[variable(causality = Parameter, variability = Tunable, start = Self::default().lookahead)] + lookahead: f64, + /// Pure pursuit: yaw rate per radian of heading error, in 1/s. + #[variable(causality = Parameter, variability = Tunable, start = Self::default().gain_yaw_rate)] + gain_yaw_rate: f64, + /// Pure pursuit: the most yaw rate to command either way, rad/s. + #[variable(causality = Parameter, variability = Tunable, start = Self::default().max_yaw_rate)] + max_yaw_rate: f64, + + /// IDM: speed held on an open road, m/s. + #[variable(causality = Parameter, variability = Tunable, start = Self::default().v0_speed_tgt)] + v0_speed_tgt: f64, + /// IDM: seconds of gap wanted at whatever speed is held. + #[variable(causality = Parameter, variability = Tunable, start = Self::default().t_headway)] + t_headway: f64, + /// IDM: meters of gap wanted at a standstill. + #[variable(causality = Parameter, variability = Tunable, start = Self::default().s0_gap_min)] + s0_gap_min: f64, + /// IDM: the most acceleration commanded, m/s^2. + #[variable(causality = Parameter, variability = Tunable, start = Self::default().a_accel_max)] + a_accel_max: f64, + /// IDM: comfortable braking, m/s^2 and positive, the hardest asked for. + #[variable(causality = Parameter, variability = Tunable, start = Self::default().b_decel_comfort)] + b_decel_comfort: f64, + + /// Acceleration to hold, m/s^2. + /// + /// `initial = Calculated`, since it is worked out from the inputs + /// rather than begun at anything. Saying so is what lists it under + /// `InitialUnknowns`, and a `start` attribute beside it would be a + /// second answer to the same question, which FMI forbids and a + /// checking importer refuses to load. + #[variable(causality = Output, initial = Calculated)] + accel_cmd: f64, + /// Yaw rate to hold, rad/s, positive counter-clockwise. + #[variable(causality = Output, initial = Calculated)] + yaw_rate_cmd: f64, + + /// The road, built from the parameters above and kept. + /// + /// No `#[variable]`, so FMI never sees it: it is ordinary Rust state + /// living as long as the instance. Rebuilding it per step would + /// reallocate and walk the whole polyline again for every car at + /// every control instant, to get the same road back each time. + road: Option, +} + +impl Default for FmuController { + /// Every start value this FMU has, written once. + /// + /// The declarations above read their `start` from here, so what a + /// host finds in the model description and what an instance holds + /// before anything sets it cannot come apart. That costs building one + /// of these per variable while the metadata is assembled, which + /// happens at packaging and at instantiation and never in a step. + fn default() -> Self { + FmuController { + position_x: 0.0, + position_y: 0.0, + orientation_w: 1.0, + orientation_x: 0.0, + orientation_y: 0.0, + orientation_z: 0.0, + speed: 0.0, + range: [FREE_ROAD.range; MAX_DETECTIONS], + range_rate: [FREE_ROAD.range_rate; MAX_DETECTIONS], + road_x: [0.0; MAX_WAYPOINTS], + road_y: [0.0; MAX_WAYPOINTS], + road_point_count: 2, + road_closed: false, + lateral_tgt: DEFAULT_PURSUIT.lateral_tgt, + lookahead: DEFAULT_PURSUIT.lookahead, + gain_yaw_rate: DEFAULT_PURSUIT.gain_yaw_rate, + max_yaw_rate: DEFAULT_PURSUIT.max_yaw_rate, + v0_speed_tgt: DEFAULT_IDM.v0_speed_tgt, + t_headway: DEFAULT_IDM.t_headway, + s0_gap_min: DEFAULT_IDM.s0_gap_min, + a_accel_max: DEFAULT_IDM.a_accel_max, + b_decel_comfort: DEFAULT_IDM.b_decel_comfort, + accel_cmd: 0.0, + yaw_rate_cmd: 0.0, + road: None, + } + } +} + +impl FmuController { + /// The road its parameters describe, or why they describe none. + /// + /// The count is checked before anything is built, because + /// [`Waypoints`] asserts its own minimum and a panic unwinding out + /// through the C interface would take the host process with it. The + /// same information travels as a refusal instead, which the standard + /// can carry. + /// + /// A road of no length is refused for a plainer reason. The point + /// arrays start full of zeros, so a host that sets the count and + /// forgets the points sends a road whose every point is the origin. + /// Nothing in `Waypoints` objects to that, and a car steering along + /// it holds still at a single spot. Better to say so. + fn road_from_parameters(&self) -> Result { + let given = self.road_point_count as usize; + let (least, kind) = if self.road_closed { + (3, "closed") + } else { + (2, "open") + }; + if given < least || given > MAX_WAYPOINTS { + return Err(BadInput::PointCount { given, least, kind }); + } + let points: Vec<(f64, f64)> = (0..given) + .map(|i| (self.road_x[i], self.road_y[i])) + .collect(); + let road = if self.road_closed { + Waypoints::build_closed(points) + } else { + Waypoints::build_open(points) + }; + if road.total_length() <= 0.0 { + return Err(BadInput::RoadOfNoLength { count: given }); + } + + // Return the road, built by the code that built the native + // controller's road, so the two are one geometry rather than two + // readings of the same numbers. + Ok(road) + } + + /// The road, reported to the host if there is none to build. + fn build_road(&self, context: &dyn Context) -> Result { + self.road_from_parameters() + .map_err(|bad| self.refuse(context, bad)) + } + + /// Reports to the host what it handed over that cannot be run on. + fn refuse(&self, context: &dyn Context, bad: BadInput) -> Fmi3Error { + context.log( + Fmi3Error::Error.into(), + DefaultLoggingCategory::LogAll, + format_args!("{bad}"), + ); + + Fmi3Error::Error + } + + /// The steering law's parameters, checked. + /// + /// Read afresh per step rather than kept, because these are tunable + /// and a host may have changed one since the last. Keeping a copy + /// would take the change and act on the old value, and checking once + /// would let a new zero through. + fn pursuit_params(&self) -> Result { + // Return the set, once the aim point is somewhere ahead. An aim + // point on top of the follower gives no direction to steer + // toward, and one behind it gives the wrong one. + Ok(PurePursuitParams { + lateral_tgt: self.lateral_tgt, + lookahead: require_positive("lookahead", self.lookahead)?, + gain_yaw_rate: self.gain_yaw_rate, + max_yaw_rate: self.max_yaw_rate, + }) + } + + /// The following law's parameters, checked, and read per step for + /// the same reason [`Self::pursuit_params`] is. + fn idm_params(&self) -> Result { + // Return the set, once the three the equation divides by are + // numbers it can divide by. + Ok(IdmParams { + v0_speed_tgt: require_positive("v0_speed_tgt", self.v0_speed_tgt)?, + t_headway: self.t_headway, + s0_gap_min: self.s0_gap_min, + a_accel_max: require_positive("a_accel_max", self.a_accel_max)?, + b_decel_comfort: require_positive("b_decel_comfort", self.b_decel_comfort)?, + }) + } + + /// What the laws command, given the road they command along. + /// + /// Every number here comes from [`continuo_actors::control_laws`]. + /// What this adds is the translation either side of it: a pose out of + /// seven scalars, and a scan out of two arrays. + fn calculate_commands(&self, road: &Waypoints) -> Result<(f64, f64), BadInput> { + let pose = Pose { + position: Vec3::new(self.position_x, self.position_y, 0.0), + orientation: Quat { + w: self.orientation_w, + x: self.orientation_x, + y: self.orientation_y, + z: self.orientation_z, + }, + }; + let yaw_rate_cmd = pure_pursuit_yaw_rate(road, pose, self.pursuit_params()?); + + // The two arrays back into the detections they were taken from, + // which is the shape the law reads. Padding loses to anything + // real, so the count the sensor knew is not needed here. + let scan: [Detection; MAX_DETECTIONS] = std::array::from_fn(|i| Detection { + range: self.range[i], + range_rate: self.range_rate[i], + }); + let lead = nearest_detection(&scan); + // A range closes as it falls and an approach rate rises as it + // closes, so each is the other's negative. + let accel_cmd = idm_accel(self.speed, lead.range, -lead.range_rate, self.idm_params()?); + + // Return what to hold: how hard to push, and how fast to turn. + Ok((accel_cmd, yaw_rate_cmd)) + } +} + +impl UserModel for FmuController { + /// The categories a host can switch on: `logAll` and `trace`, which + /// is what `fmi-export` offers unless a model defines an enum of its + /// own. One message is raised here and it belongs under `logAll`, + /// since a refusal is not tracing. + type LoggingCategory = DefaultLoggingCategory; + + /// Builds the road and commands from it, once the parameters are + /// final. + /// + /// `fmi3ExitInitializationMode` calls this last, which is the first + /// moment the parameters mean what the host meant and the last + /// before an output is read for real. It is also the only place the + /// road is ever built, so nothing downstream has to wonder which + /// parameters it was built from. + fn configurate(&mut self, context: &dyn Context) -> Result<(), Fmi3Error> { + self.road = Some(self.build_road(context)?); + // The outputs answer for the road just built, which is what a + // host reading them straight afterward expects. + self.calculate_values(context)?; + + Ok(()) + } + + /// Keeps the two output variables answering for the inputs. + /// + /// There is no road until `configurate` builds one, and this runs + /// before it does. Both happen inside `fmi3ExitInitializationMode`, + /// which recomputes whatever was dirtied and only then configures, + /// and a host reading an output during initialization arrives + /// earlier still. With no road to steer along there is nothing to + /// command, so the outputs keep the values they were made with until + /// there is one. + fn calculate_values(&mut self, context: &dyn Context) -> Result { + let Some(road) = self.road.as_ref() else { + return Ok(Fmi3Res::OK); + }; + let (accel_cmd, yaw_rate_cmd) = self + .calculate_commands(road) + .map_err(|bad| self.refuse(context, bad))?; + + self.accel_cmd = accel_cmd; + self.yaw_rate_cmd = yaw_rate_cmd; + + Ok(Fmi3Res::OK) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + /// A controller holding `road` in its parameters, the way a host + /// hands one across. + fn controller_given(road: &Waypoints) -> FmuController { + let mut controller = FmuController { + road_point_count: road.points().len() as u32, + road_closed: road.is_closed(), + ..FmuController::default() + }; + for (i, &(x, y)) in road.points().iter().enumerate() { + controller.road_x[i] = x; + controller.road_y[i] = y; + } + + // Return it with the road written into the arrays and everything + // past the road left as it was. + controller + } + + /// Where a road puts a car and which way it points it, sampled + /// along and past both ends, as the bits of the answers. + fn sampled_along(road: &Waypoints) -> Vec<(u64, u64, u64)> { + (-10..=110) + .map(|pct| { + let s = road.total_length() * pct as f64 / 100.0; + let point = road.point_at(s); + ( + point.x.to_bits(), + point.y.to_bits(), + road.heading_at(s).to_bits(), + ) + }) + .collect() + } + + #[test] + fn a_road_arrives_as_the_road_it_was_sent_as() { + // A bend and a loop, so the flag is asked for as well as the + // points. + for sent in [ + Waypoints::build_open(vec![(0.0, 0.0), (30.0, 0.0), (55.0, 18.0), (80.0, 18.0)]), + Waypoints::build_closed(vec![(0.0, 0.0), (10.0, 0.0), (10.0, 10.0), (0.0, 10.0)]), + ] { + let rebuilt = controller_given(&sent) + .road_from_parameters() + .expect("the count is the road's own"); + assert_eq!(rebuilt.is_closed(), sent.is_closed()); + assert_eq!(sampled_along(&rebuilt), sampled_along(&sent)); + } + } + + #[test] + fn the_padding_past_the_count_is_never_read() { + let road = Waypoints::build_straight((0.0, 0.0), (100.0, 0.0)); + let mut controller = controller_given(&road); + // Somewhere no road would go, in every slot the road did not + // fill. A reader that trusted the array over the count would + // come back with a road hundreds of meters long. + for i in road.points().len()..MAX_WAYPOINTS { + controller.road_x[i] = 500.0 + i as f64; + controller.road_y[i] = -500.0; + } + + let rebuilt = controller + .road_from_parameters() + .expect("two points are enough for an open road"); + assert_eq!(rebuilt.total_length(), road.total_length()); + assert_eq!(sampled_along(&rebuilt), sampled_along(&road)); + } + + #[test] + fn a_count_no_road_could_have_is_refused_rather_than_asserted() { + let straight = Waypoints::build_straight((0.0, 0.0), (100.0, 0.0)); + + // One point is not a road, and `Waypoints` would assert on it. + let mut too_few = controller_given(&straight); + too_few.road_point_count = 1; + assert_eq!( + too_few.road_from_parameters().unwrap_err(), + BadInput::PointCount { + given: 1, + least: 2, + kind: "open", + } + ); + + // Two points close into nothing, so a loop needs three. + let mut not_a_loop = controller_given(&straight); + not_a_loop.road_closed = true; + assert_eq!( + not_a_loop.road_from_parameters().unwrap_err(), + BadInput::PointCount { + given: 2, + least: 3, + kind: "closed", + } + ); + + // More points than arrived would read past what the host sent. + let mut too_many = controller_given(&straight); + too_many.road_point_count = MAX_WAYPOINTS as u32 + 1; + assert_eq!( + too_many.road_from_parameters().unwrap_err(), + BadInput::PointCount { + given: MAX_WAYPOINTS + 1, + least: 2, + kind: "open", + } + ); + } + + #[test] + fn a_count_set_over_points_nobody_sent_is_refused() { + // What a host gets by setting the count and forgetting the + // points: the arrays start at zero, so every point of the road + // is the origin. It builds without complaint, and a car given it + // would sit still at one spot. + let mut forgot_the_road = FmuController { + road_point_count: 4, + ..FmuController::default() + }; + assert_eq!( + forgot_the_road.road_from_parameters().unwrap_err(), + BadInput::RoadOfNoLength { count: 4 } + ); + + // A closed road of repeated points goes the same way. + forgot_the_road.road_closed = true; + assert_eq!( + forgot_the_road.road_from_parameters().unwrap_err(), + BadInput::RoadOfNoLength { count: 4 } + ); + } + + /// Asserts a controller commands nothing, naming the parameter it + /// refused and quoting the value back. + fn check_refusal(controller: &FmuController, road: &Waypoints, name: &str, given: f64) { + match controller.calculate_commands(road) { + Err(BadInput::NotPositive { + name: refused, + given: quoted, + }) => { + assert_eq!(refused, name); + // Bits, because a NaN is not equal to itself. + assert_eq!(quoted.to_bits(), given.to_bits()); + } + other => panic!("{name} = {given} gave {other:?}"), + } + } + + #[test] + fn a_parameter_the_laws_divide_by_is_refused_unless_it_is_positive() { + let road = Waypoints::build_straight((0.0, 0.0), (100.0, 0.0)); + + // A host in another tool can send any of these, and the model + // description cannot warn it off, since `fmi-export` 0.3.0 + // declares no bounds for a variable. + for given in [0.0, -1.0, f64::NAN, f64::INFINITY] { + let mut target_speed = controller_given(&road); + target_speed.v0_speed_tgt = given; + check_refusal(&target_speed, &road, "v0_speed_tgt", given); + + let mut acceleration = controller_given(&road); + acceleration.a_accel_max = given; + check_refusal(&acceleration, &road, "a_accel_max", given); + + let mut braking = controller_given(&road); + braking.b_decel_comfort = given; + check_refusal(&braking, &road, "b_decel_comfort", given); + + let mut aim_point = controller_given(&road); + aim_point.lookahead = given; + check_refusal(&aim_point, &road, "lookahead", given); + } + } + + #[test] + fn a_parameter_changed_between_steps_changes_the_command() { + // What tunable buys, and what a step reading its parameters + // afresh is for: a car told to want a different speed wants it + // from the next command, without being rebuilt. + const SPEED: f64 = 20.0; + + let road = Waypoints::build_straight((0.0, 0.0), (1000.0, 0.0)); + let mut controller = controller_given(&road); + controller.speed = SPEED; + + let (wanting_default, _) = controller.calculate_commands(&road).expect("a sound set"); + controller.v0_speed_tgt = SPEED; + let (wanting_this_speed, _) = controller.calculate_commands(&road).expect("a sound set"); + controller.v0_speed_tgt = SPEED * 2.0; + let (wanting_twice_it, _) = controller.calculate_commands(&road).expect("a sound set"); + + // Wanting exactly the speed it holds asks for nothing, and + // wanting twice that asks for more than the default target did. + assert!( + wanting_this_speed.abs() < 1e-12, + "at its target: {wanting_this_speed}" + ); + assert!(wanting_twice_it > wanting_default); + } + + #[test] + fn every_parameter_the_laws_divide_by_is_checked() { + // The commands come out finite for the defaults, so the check + // above is refusing what is wrong rather than everything. + let road = Waypoints::build_straight((0.0, 0.0), (100.0, 0.0)); + let (accel_cmd, yaw_rate_cmd) = controller_given(&road) + .calculate_commands(&road) + .expect("the defaults are a set the laws can run on"); + assert!(accel_cmd.is_finite() && yaw_rate_cmd.is_finite()); + } + + #[test] + fn the_commands_are_what_the_laws_answer_to_the_bit() { + // A car off its lane behind a closing lead, so both laws have + // something to say. The slot is nowhere near the front, so + // picking the lead out of the scan is part of what is checked. + const POSITION: (f64, f64) = (25.0, 1.5); + const ORIENTATION: (f64, f64) = (0.9, 0.1); + const SPEED: f64 = 22.0; + const LANE: f64 = 3.5; + const SLOT: usize = 37; + const GAP: f64 = 40.0; + const CLOSING_AT: f64 = 4.0; + + let road = Waypoints::build_open(vec![(0.0, 0.0), (60.0, 0.0), (110.0, 40.0)]); + let mut controller = controller_given(&road); + controller.position_x = POSITION.0; + controller.position_y = POSITION.1; + controller.orientation_w = ORIENTATION.0; + controller.orientation_z = ORIENTATION.1; + controller.speed = SPEED; + controller.lateral_tgt = LANE; + controller.range[SLOT] = GAP; + controller.range_rate[SLOT] = -CLOSING_AT; + + let (accel_cmd, yaw_rate_cmd) = controller + .calculate_commands(&road) + .expect("the defaults are a set the laws can run on"); + + let pose = Pose { + position: Vec3::new(POSITION.0, POSITION.1, 0.0), + orientation: Quat { + w: ORIENTATION.0, + x: 0.0, + y: 0.0, + z: ORIENTATION.1, + }, + }; + let expected_yaw_rate = pure_pursuit_yaw_rate( + &road, + pose, + PurePursuitParams { + lateral_tgt: LANE, + ..DEFAULT_PURSUIT + }, + ); + let expected_accel = idm_accel(SPEED, GAP, CLOSING_AT, DEFAULT_IDM); + + assert_eq!(yaw_rate_cmd.to_bits(), expected_yaw_rate.to_bits()); + assert_eq!(accel_cmd.to_bits(), expected_accel.to_bits()); + // The approach rate is the range rate's negative, so a lead + // closing must not read as one pulling away. + assert!( + accel_cmd < idm_accel(SPEED, GAP, -CLOSING_AT, DEFAULT_IDM), + "a closing lead should be braked for harder than a receding one" + ); + } + + #[test] + fn an_empty_scan_drives_the_open_road() { + // Under the speed it wants, whatever that has been set to, so + // the assertion below holds for any target rather than for 30. + let speed = DEFAULT_IDM.v0_speed_tgt / 3.0; + let road = Waypoints::build_straight((0.0, 0.0), (500.0, 0.0)); + let mut controller = controller_given(&road); + controller.speed = speed; + + let (accel_cmd, _) = controller + .calculate_commands(&road) + .expect("the defaults are a set the laws can run on"); + + // Nothing was detected, so every slot holds the free road, and + // the answer is the one an open road gives at this speed. + assert_eq!( + accel_cmd.to_bits(), + idm_accel(speed, FREE_ROAD.range, 0.0, DEFAULT_IDM).to_bits() + ); + assert!(accel_cmd > 0.0, "below its target it should accelerate"); + } +} diff --git a/crates/continuo-fmu-controller-idm/src/lib.rs b/crates/continuo-fmu-controller-idm/src/lib.rs new file mode 100644 index 0000000..99b849a --- /dev/null +++ b/crates/continuo-fmu-controller-idm/src/lib.rs @@ -0,0 +1,34 @@ +//! A controller for traffic cars, following with IDM and turning with +//! pure pursuit, exported as an FMI 3.0 Co-Simulation FMU. +//! +//! Nothing here decides anything. [`FmuController`] declares what crosses +//! the boundary, and every answer comes from +//! [`continuo_actors::control_laws`], which the native controller calls +//! too. So the two agree because they are one implementation rather than +//! because somebody keeps them in step. +//! +//! What the boundary costs is worth seeing plainly. A road cannot cross +//! as a `Waypoints`, so it crosses as the numbers it was built from and +//! each instance builds its own copy. A scan cannot cross as a list, so +//! it crosses as two arrays of a fixed length, padded out with the free +//! road. Both are the FMI data model rather than a choice, and both are +//! why this crate exists at all: the laws stay where they are, and only +//! the packaging lives here. +//! +//! The `.fmu` carries a compiled snapshot of `continuo-actors`, since the +//! cdylib links it statically. Editing a law without packaging it again +//! therefore leaves this copy behind, and `cargo xtask package-fmus` is +//! what puts it back. + +mod error; +mod fmu_controller; +mod package_fmu; + +pub use error::PackageError; +pub use fmu_controller::{FmuController, MAX_WAYPOINTS}; +pub use package_fmu::{FMU_FILE_NAME, packaged_fmu_path}; + +// The C entry points FMI loads, generated for the model above. They +// export by name rather than by module path, so this stands as the +// crate's front door whichever file the model itself lives in. +fmi_export::export_fmu!(FmuController); diff --git a/crates/continuo-fmu-controller-idm/src/package_fmu.rs b/crates/continuo-fmu-controller-idm/src/package_fmu.rs new file mode 100644 index 0000000..4e8a981 --- /dev/null +++ b/crates/continuo-fmu-controller-idm/src/package_fmu.rs @@ -0,0 +1,42 @@ +//! Finding the packaged `.fmu` that this crate is built into. +//! +//! Nothing in the FMU itself uses any of this. It is for the Rust side of +//! the boundary: the tests that run the packaged FMU against the laws it +//! was built from, and the demo scenario that hands its path to an +//! `FmuComponent`. Both need the same answer to the same question, so they +//! ask it here rather than each spelling out a path. + +use std::path::PathBuf; + +use crate::error::PackageError; + +/// The file `cargo xtask package-fmus` writes, named after the cdylib +/// because FMI takes its model identifier from the shared library. +pub const FMU_FILE_NAME: &str = "continuo_fmu_controller_idm.fmu"; + +/// The directory the packaged FMU is written to, under `target`. +pub(crate) const FMU_DIRECTORY: &str = "fmu"; + +/// Where `cargo xtask package-fmus` left the packaged FMU. +/// +/// The search starts at the running executable and walks up its +/// directories, so it lands on `target/fmu` from a test binary buried in +/// `target/debug/deps` as readily as from an example, whichever profile +/// built either. Taking a path fixed at build time instead would tie the +/// answer to how the caller was compiled. +/// +/// Failing carries the command that fixes it, because a missing package +/// means a step was not run rather than anything being broken. +pub fn packaged_fmu_path() -> Result { + let exe = std::env::current_exe().map_err(PackageError::ExeNotFound)?; + for directory in exe.ancestors() { + let candidate = directory.join(FMU_DIRECTORY).join(FMU_FILE_NAME); + if candidate.is_file() { + return Ok(candidate); + } + } + + // Return the failure naming where the search began, which is the + // only part of it a caller cannot work out. + Err(PackageError::NotPackaged { searched_from: exe }) +} diff --git a/crates/continuo-fmu-controller-idm/tests/packaged_fmu.rs b/crates/continuo-fmu-controller-idm/tests/packaged_fmu.rs new file mode 100644 index 0000000..2cf43ef --- /dev/null +++ b/crates/continuo-fmu-controller-idm/tests/packaged_fmu.rs @@ -0,0 +1,888 @@ +//! The packaged FMU against the laws it was built from. +//! +//! The `.fmu` links `continuo-actors` statically, so the shared library it +//! ships carries its own compiled copy of the control laws rather than +//! calling back into this process. These tests drive that copy across the +//! FMI boundary and compare it, to the bit, with what the laws answer +//! here. Nothing is stored to compare against: both sides are computed +//! afresh, and the claim is that two builds of one implementation agree, +//! which is a claim only running both can settle. +//! +//! Two things can part them, and both are worth catching. A packaged FMU +//! older than the laws is the likelier by far, since editing a law is +//! ordinary work and packaging one is a separate command. The other is the +//! boundary itself losing something: a pose that arrives as seven scalars, +//! a scan that arrives as two arrays, and a road that arrives as the +//! numbers it was built from all have somewhere to go wrong that no +//! compiler checks. +//! +//! Driven through [`Component::step`] rather than through a conductor, as +//! `continuo-fmi`'s own suite is, since what is under test is one +//! component's answers rather than how a world schedules it. + +// Everything here reads a file `cargo xtask package-fmus` writes, so this +// file compiles away to nothing unless the `packaged-fmu` feature is enabled. +// What that costs is the run where it matters most: a law edited without +// packaging again goes unnoticed by `cargo test --workspace`, and it takes +// both of those commands to see it. +#![cfg(feature = "packaged-fmu")] + +use std::path::PathBuf; + +use continuo_actors::control_laws::{ + FREE_ROAD, IdmParams, PurePursuitParams, idm_accel, nearest_detection, pure_pursuit_yaw_rate, +}; +use continuo_actors::{MAX_DETECTIONS, Waypoints}; +use continuo_core::{ + Component, ComponentPath, CoreError, Detection, KeyExpr, Message, Pose, Quat, RandomSplitMix64, + SimDuration, SimTime, StepCtx, +}; +use continuo_fmi::{FmuComponent, FmuMapping, InputBinding, OutputBinding}; +use continuo_fmu_controller_idm::{MAX_WAYPOINTS, packaged_fmu_path}; +use serde_json::{Value, json}; + +const WORLD: &str = "continuo/test"; + +/// How often the controller steps. The laws are a map from inputs to +/// commands rather than anything integrated, so no answer here depends on +/// this, but a step still has to happen somewhere on a clock. +const PERIOD_MS: i64 = 100; + +/// What a disagreement most likely means. +/// +/// Named in every comparison's failure text, because a stale package +/// presents as the IDM math disagreeing with itself, which says nothing +/// about where to look. +const MAYBE_STALE: &str = + "the laws may have changed since the FMU was packaged: run `cargo xtask package-fmus`"; + +/// The tuning the demo's traffic will run, near enough that these tests +/// exercise the numbers the demo does. +const PURSUIT: PurePursuitParams = PurePursuitParams::highway_car(3.5); +const IDM: IdmParams = IdmParams::highway_car(30.0); + +fn key(name: &str) -> KeyExpr { + KeyExpr::new(format!("{WORLD}/{name}")).unwrap() +} + +/// Where `cargo xtask package-fmus` left the FMU, or why it is not there. +/// +/// Failing rather than skipping, since a suite that quietly tests nothing +/// is worse than one that says what to run. +fn packaged() -> PathBuf { + packaged_fmu_path().unwrap_or_else(|missing| panic!("{missing}")) +} + +/// One message on `key` carrying `payload`, as a publisher would send it. +fn message(key_name: &str, payload: Value) -> Message { + Message { + key: key(key_name), + publisher: ComponentPath::parse("car").unwrap(), + seq: 0, + sim_time: SimTime::ZERO, + payload: serde_json::to_vec(&payload).unwrap(), + } +} + +/// What a car knows about itself and what is ahead of it. +#[derive(Clone)] +struct Situation { + pose: Pose, + speed: f64, + scan: Vec, +} + +impl Situation { + /// One line naming what the car was in, for a failure to be read by. + /// + /// The scan is reduced to the detection the law would have followed, + /// because it is mostly padding by construction and sixty-four free + /// roads printed out bury the numbers that decided anything. + fn summary(&self) -> String { + let position = self.pose.position; + let lead = nearest_detection(&self.scan); + + // Return the line, in the quantities the laws are written in + // rather than the seven scalars a pose crosses as. + format!( + "at ({}, {}) facing {} rad, {} m/s, nearest {} m closing at {} m/s", + position.x, + position.y, + self.pose.orientation.yaw(), + self.speed, + lead.range, + -lead.range_rate, + ) + } +} + +/// A car at `s` along the road, `lateral` meters left of the centerline, +/// pointing `yaw_error` radians off the road's own heading. +fn car_at(road: &Waypoints, s: f64, lateral: f64, yaw_error: f64) -> Pose { + Pose { + position: road.point_at_offset(s, lateral), + orientation: Quat::from_yaw(road.heading_at(s) + yaw_error), + } +} + +/// The wiring an FMU car will be given: a pose and a radar scan in, the +/// two commands out on one key, and everything the laws are tuned by set +/// once as parameters. +/// +/// The pose inputs name no pointers, since FMI's structured names and JSON +/// Pointer describe the same shape and `position.x` spells `/position/x` +/// on its own. The radar's do, because nothing derives +/// `/detections/0/range` from a variable named `range`. +fn controller_mapping(road: &Waypoints, pursuit: PurePursuitParams, idm: IdmParams) -> FmuMapping { + let mut road_x = vec![0.0; MAX_WAYPOINTS]; + let mut road_y = vec![0.0; MAX_WAYPOINTS]; + for (slot, &(x, y)) in road.points().iter().enumerate() { + road_x[slot] = x; + road_y[slot] = y; + } + + let pose = key("pose"); + let radar = key("radar"); + let cmd = key("cmd"); + + // Return the sheet, which is data rather than code: adding this FMU to + // a world writes one of these instead of compiling anything. + FmuMapping { + period: SimDuration::from_millis(PERIOD_MS), + inputs: vec![ + InputBinding::new("position.x", pose.clone()), + InputBinding::new("position.y", pose.clone()), + InputBinding::new("orientation.w", pose.clone()), + InputBinding::new("orientation.x", pose.clone()), + InputBinding::new("orientation.y", pose.clone()), + InputBinding::new("orientation.z", pose.clone()), + InputBinding::new("speed", pose), + InputBinding::new("range", radar.clone()) + .with_pointer("/detections/*/range") + .when_missing(json!(FREE_ROAD.range)), + InputBinding::new("range_rate", radar) + .with_pointer("/detections/*/range_rate") + .when_missing(json!(FREE_ROAD.range_rate)), + ], + outputs: vec![ + OutputBinding::new("accel_cmd", cmd.clone()), + OutputBinding::new("yaw_rate_cmd", cmd), + ], + initial_values: vec![ + ("road_x".to_string(), json!(road_x)), + ("road_y".to_string(), json!(road_y)), + ("road_point_count".to_string(), json!(road.points().len())), + ("road_closed".to_string(), json!(road.is_closed())), + ("lateral_tgt".to_string(), json!(pursuit.lateral_tgt)), + ("lookahead".to_string(), json!(pursuit.lookahead)), + ("gain_yaw_rate".to_string(), json!(pursuit.gain_yaw_rate)), + ("max_yaw_rate".to_string(), json!(pursuit.max_yaw_rate)), + ("v0_speed_tgt".to_string(), json!(idm.v0_speed_tgt)), + ("t_headway".to_string(), json!(idm.t_headway)), + ("s0_gap_min".to_string(), json!(idm.s0_gap_min)), + ("a_accel_max".to_string(), json!(idm.a_accel_max)), + ("b_decel_comfort".to_string(), json!(idm.b_decel_comfort)), + ], + } +} + +/// Replaces one parameter's value, for a test sending something the +/// builder above never would. +fn set_parameter(mapping: &mut FmuMapping, name: &str, value: Value) { + let slot = mapping + .initial_values + .iter_mut() + .find(|(parameter, _)| parameter == name) + .unwrap_or_else(|| panic!("{name} is not a parameter this mapping sets")); + slot.1 = value; +} + +/// Runs `situation` through the packaged FMU as a run of its own, and +/// returns what it commanded. +/// +/// A reset first, since these situations reach corners of the input space +/// no car could drive between, and stepping them in sequence would ask the +/// model to account for a trajectory that never happened. +/// +/// Then two steps, at the same pair of instants whatever the situation, +/// since a reset FMU starts over rather than carrying on the last one's +/// clock. The first is Initialization Mode, which has no interval behind +/// it to step over, so `fmi3DoStep` is what the second reaches. +/// +/// The same inputs and the same road are behind both, so both must command +/// the same thing. Initializing is the more elaborate path, the one that +/// writes the parameters and builds the road, and this is where it is held +/// to what a plain step answers. +fn fmu_commands(fmu: &mut FmuComponent, situation: &Situation) -> (f64, f64) { + fmu.reset().expect("the FMU resets"); + let period = SimDuration::from_millis(PERIOD_MS); + let initializing = step_once(fmu, SimTime::ZERO, None, situation); + let stepping = step_once(fmu, SimTime::ZERO + period, Some(period), situation); + + // No `MAYBE_STALE` here. A package older than the laws puts both of + // these wrong together, so it cannot be why they differ from one + // another. + assert_eq!( + (initializing.0.to_bits(), initializing.1.to_bits()), + (stepping.0.to_bits(), stepping.1.to_bits()), + "the FMU commands {initializing:?} out of initialization and \ + {stepping:?} out of a step, {}", + situation.summary(), + ); + + // Return what both paths answered. + stepping +} + +/// Steps the FMU once at `now` in `situation` and returns what it commanded. +/// +/// `dt` is what a conductor would pass: `None` on a first step and the +/// interval since the last one after that. This FMU does not read it, but a +/// context claiming otherwise is one no conductor builds. +/// +/// Both outputs are bound to one key, so one message comes back carrying +/// both, which is the shape [`OutputBinding`] merges them into. +fn step_once( + fmu: &mut FmuComponent, + now: SimTime, + dt: Option, + situation: &Situation, +) -> (f64, f64) { + let inbox = vec![ + message( + "pose", + json!({ + "position": situation.pose.position, + "orientation": situation.pose.orientation, + "speed": situation.speed, + }), + ), + message("radar", json!({ "detections": situation.scan })), + ]; + + let mut ctx = StepCtx::new(now, dt, WORLD, 0, inbox); + fmu.step(&mut ctx).expect("the FMU steps"); + let outbox = ctx.take_outbox(); + assert_eq!(outbox.len(), 1, "both commands share one key"); + let payload: Value = serde_json::from_slice(&outbox[0].1).expect("the payload decodes"); + + // Return the pair, read back through JSON as a consumer would. The + // shortest decimal that round trips is what serde writes, so a number + // that survives this is the same number to the bit. + ( + payload["accel_cmd"].as_f64().expect("an acceleration"), + payload["yaw_rate_cmd"].as_f64().expect("a yaw rate"), + ) +} + +/// What the laws answer in `situation`, called here in this process. +fn native_commands( + road: &Waypoints, + pursuit: PurePursuitParams, + idm: IdmParams, + situation: &Situation, +) -> (f64, f64) { + let lead = nearest_detection(&situation.scan); + + // Return the same pair the FMU is asked for. A range closes as it + // falls and an approach rate rises as it closes, so each is the + // other's negative, and getting that backwards inside the FMU is one + // of the things these tests are here to catch. + ( + idm_accel(situation.speed, lead.range, -lead.range_rate, idm), + pure_pursuit_yaw_rate(road, situation.pose, pursuit), + ) +} + +/// Runs every situation through the packaged FMU and through the laws, +/// asserting the two agree to the bit, and returns what was commanded so a +/// caller can check the comparison had something to compare. +/// +/// One instance serves the whole sweep, since [`fmu_commands`] resets it +/// before each situation. What that saves is the import: building an +/// `FmuComponent` extracts the archive and loads the library it holds, +/// which measures around a hundred times what a reset costs. +fn commands_over( + road: &Waypoints, + pursuit: PurePursuitParams, + idm: IdmParams, + situations: &[Situation], +) -> Vec<(f64, f64)> { + let mapping = controller_mapping(road, pursuit, idm); + let mut fmu = FmuComponent::new("controller", packaged(), mapping).expect("the FMU builds"); + + // Return each situation's commands, having agreed at every one. + situations + .iter() + .map(|situation| { + let from_fmu = fmu_commands(&mut fmu, situation); + let from_laws = native_commands(road, pursuit, idm, situation); + assert_eq!( + from_fmu.0.to_bits(), + from_laws.0.to_bits(), + "accel_cmd: the FMU says {} where the laws say {}, {}\n{MAYBE_STALE}", + from_fmu.0, + from_laws.0, + situation.summary(), + ); + assert_eq!( + from_fmu.1.to_bits(), + from_laws.1.to_bits(), + "yaw_rate_cmd: the FMU says {} where the laws say {}, {}\n{MAYBE_STALE}", + from_fmu.1, + from_laws.1, + situation.summary(), + ); + + // Return what the laws answered, which the FMU has agreed with. + from_laws + }) + .collect() +} + +/// Asserts a sweep asked for real commands, both ways in each. +/// +/// A comparison of two zeros passes whatever is wrong on either side, and +/// a road every car sits exactly on would give nothing but zeros from the +/// steering law. So each sweep says what it managed to provoke. +/// +/// A NaN satisfies neither comparison, so a command that diverged cannot +/// stand as evidence that anything was commanded. +fn assert_both_laws_had_something_to_say(commands: &[(f64, f64)]) { + assert!( + commands.iter().any(|&(accel, _)| accel > 0.0), + "nothing accelerated" + ); + assert!( + commands.iter().any(|&(accel, _)| accel < 0.0), + "nothing braked" + ); + assert!( + commands.iter().any(|&(_, yaw_rate)| yaw_rate > 0.0), + "nothing turned left" + ); + assert!( + commands.iter().any(|&(_, yaw_rate)| yaw_rate < 0.0), + "nothing turned right" + ); +} + +/// What a car meets on `road`: the cases somebody chose, then a batch +/// nobody did. +/// +/// Both halves are wanted. A chosen case is aimed at something, and says +/// what in a comment beside it, so a failure there names its own subject. +/// A random one is aimed at nothing, which is the point: the corners worth +/// finding are the ones nobody thought to write down. +fn situations_on(road: &Waypoints) -> Vec { + let mut situations = chosen_situations(road); + situations.extend(random_situations(road)); + + // Return both, which every sweep runs together. + situations +} + +/// Every situation worth putting a car in deliberately: along the road, +/// either side of its lane, pointing off it, at four speeds, seeing seven +/// different things ahead. +/// +/// One cross product rather than a sweep per law, because a controller +/// answers both at once and an interaction between them is exactly what a +/// hand-picked list would step around. +fn chosen_situations(road: &Waypoints) -> Vec { + // Fractions of the road's length, so the same list means the same + // places on a road of any size. The last is near enough to the end + // that a lookahead runs past it, which is where a loop wraps and a + // road stops. + const ALONG: [f64; 4] = [0.0, 0.25, 0.6, 0.98]; + // Meters left of the centerline. The lane is 3.5 to the left, so these + // put the car well right of it, on it, and past it. + const LATERAL: [f64; 3] = [-2.0, 3.5, 6.0]; + // Radians off the road's own heading, which is what the steering law + // turns to correct. + const YAW_ERROR: [f64; 3] = [-0.35, 0.0, 0.5]; + const SPEEDS: [f64; 4] = [0.0, 8.0, 20.0, 33.0]; + + let scans = [ + // An open road, where every slot holds the free road and the + // padding is the whole of what crosses. + Vec::new(), + // One lead, in a slot nowhere near the front, so picking the + // nearest out of the array is part of what is being compared. + scan_of(&[(37, 40.0, -4.0)]), + // Close and closing hard, which is the braking end of the law. + scan_of(&[(0, 6.0, -12.0)]), + // Pulling away, which is the sign the wanted gap has to be floored + // at zero for. + scan_of(&[(63, 25.0, 9.0)]), + // Three at once, the nearest neither first nor last, so a reader + // that took the first real slot or the last would answer from the + // wrong car. A single detection cannot tell those apart from + // picking correctly, since there is only one to pick. + scan_of(&[(2, 55.0, -1.0), (19, 18.0, -6.0), (44, 31.0, 2.0)]), + // Two at the same range, closing at different rates, so which one + // wins is observable in the command rather than a detail. Ties go + // to the earlier slot, and either side deciding otherwise is a + // disagreement this catches. + scan_of(&[(5, 22.0, -3.0), (40, 22.0, 7.0)]), + // Every slot real, so nothing is padding and the whole array is + // set from the message rather than from the mapping's default. + full_scan(), + ]; + + let mut situations = Vec::new(); + for fraction in ALONG { + for lateral in LATERAL { + for yaw_error in YAW_ERROR { + let pose = car_at(road, road.total_length() * fraction, lateral, yaw_error); + for speed in SPEEDS { + for scan in &scans { + situations.push(Situation { + pose, + speed, + scan: scan.clone(), + }); + } + } + } + } + } + + // Return the lot: 36 poses, at each of 4 speeds, seeing each of 7 + // things ahead. + situations +} + +/// Situations picked at random, to reach what the grid above steps around. +/// +/// From a fixed seed, because a failure nobody can reproduce is a failure +/// nobody can fix. The same situations come out on every agent and every +/// run, so a disagreement found here can be walked back into rather than +/// chased, and the workspace's own generator is what picks them, since it +/// is integer arithmetic and answers alike on all four platforms. +/// +/// The ranges are wider than anything a road produces. A car may sit past +/// either end of it, twelve meters off to the side, pointing any direction +/// at all, and a gap may be smaller than a car, which is where the +/// following law's floor on the gap it divides by starts to matter. None +/// of that is a world this project runs; all of it is a value a host in +/// another tool can set. +fn random_situations(road: &Waypoints) -> Vec { + // Enough to reach corners, few enough that the suite runs in about a + // second, which is what keeps it something to run while editing. Each + // one costs a reset and two steps. + const SITUATIONS: usize = 250; + // The most detections one situation holds. Past a handful, another + // detection only adds a slot for the nearest not to be in, and the + // chosen cases already fill every slot in one of theirs. + const MOST_DETECTIONS: u64 = 5; + // Arbitrary. What matters is that it never changes, not what it is. + const SEED: u64 = 0x1D3A_7B91_C0DE_4F62; + + let mut random = RandomSplitMix64::new(SEED); + let length = road.total_length(); + + // Return one situation per draw, each independent of the last. + (0..SITUATIONS) + .map(|_| { + let pose = car_at( + road, + random.range_f64(-0.2 * length, 1.2 * length), + random.range_f64(-12.0, 12.0), + random.range_f64(-std::f64::consts::PI, std::f64::consts::PI), + ); + let detections = random.next_u64() % (MOST_DETECTIONS + 1); + let scan: Vec<(usize, f64, f64)> = (0..detections) + .map(|_| { + ( + (random.next_u64() % MAX_DETECTIONS as u64) as usize, + random.range_f64(-2.0, 300.0), + random.range_f64(-25.0, 25.0), + ) + }) + .collect(); + + Situation { + pose, + speed: random.range_f64(0.0, 45.0), + scan: if scan.is_empty() { + Vec::new() + } else { + scan_of(&scan) + }, + } + }) + .collect() +} + +/// A scan holding a detection in each `(slot, range, range_rate)` given, +/// free road in the slots between them. +/// +/// The free road is padding rather than an absence, which is how a +/// fixed-length array carries a scan that is mostly empty: a free-road +/// slot loses to anything real, so nothing has to say how many are worth +/// reading. +fn scan_of(detections: &[(usize, f64, f64)]) -> Vec { + let last = detections + .iter() + .map(|&(slot, _, _)| slot) + .max() + .expect("a scan of nothing is written as an empty vector"); + let mut scan = vec![FREE_ROAD; last + 1]; + for &(slot, range, range_rate) in detections { + scan[slot] = Detection { range, range_rate }; + } + + // Return the scan, which stops after the last slot named, so the + // mapping's own default fills the rest of the FMU's arrays. + scan +} + +/// A scan filling every slot the FMU declares, so nothing about it is +/// padding. +/// +/// Ranges fall toward slot 29 and rise again, putting the nearest where +/// neither end of the array is, and the rates differ with them so which +/// one wins reaches the command. +fn full_scan() -> Vec { + // Return one detection per slot, arranged around a nearest that has to + // be searched for. + (0..MAX_DETECTIONS) + .map(|slot| { + let from_nearest = (slot as f64 - 29.0).abs(); + Detection { + range: 12.0 + from_nearest * 3.0, + range_rate: -6.0 + from_nearest * 0.25, + } + }) + .collect() +} + +#[test] +fn the_packaged_fmu_commands_what_the_laws_command() { + // The demo's road: straight, and long enough that a car can sit + // anywhere along it. + let road = Waypoints::build_straight((0.0, 0.0), (1200.0, 0.0)); + let commands = commands_over(&road, PURSUIT, IDM, &situations_on(&road)); + assert_both_laws_had_something_to_say(&commands); +} + +#[test] +fn a_curved_road_commands_what_the_laws_command() { + // What the demo's straight road cannot show. On it the heading is the + // same everywhere and a projection lands on the one segment there is, + // so a road that crossed as its first two points alone would pass. A + // polyline bending both ways puts every segment of the arc-length + // table in play. + let road = Waypoints::build_open(vec![ + (0.0, 0.0), + (60.0, 0.0), + (110.0, 40.0), + (170.0, 40.0), + (210.0, -10.0), + (280.0, -10.0), + ]); + let commands = commands_over(&road, PURSUIT, IDM, &situations_on(&road)); + assert_both_laws_had_something_to_say(&commands); +} + +#[test] +fn a_closed_road_wraps_inside_the_fmu_as_it_does_natively() { + // `road_closed` is the field whose absence would have been silent: the + // demo's road is open, so nothing there would ever notice a loop + // arriving as a polyline that stops. A car near the end of a lap is + // where the two part, since its aim point is past the end and a loop + // puts that back at the start. + let road = Waypoints::ellipse((0.0, 0.0), 120.0, 80.0, 24); + assert!(road.is_closed(), "the fixture is the case under test"); + + let commands = commands_over(&road, PURSUIT, IDM, &situations_on(&road)); + assert_both_laws_had_something_to_say(&commands); +} + +#[test] +fn the_padding_past_the_count_is_never_read() { + // The arrays are as long as the FMU declares whatever the road is, so + // a host fills the tail with something. Here that something is + // somewhere no road would go: a reader trusting the array over the + // count comes back with a road hundreds of meters long, and every + // command differs. + let road = Waypoints::build_open(vec![(0.0, 0.0), (80.0, 0.0), (140.0, 45.0)]); + let situations = situations_on(&road); + + let mut mapping = controller_mapping(&road, PURSUIT, IDM); + let junk: Vec = (0..MAX_WAYPOINTS) + .map(|slot| 500.0 + slot as f64 * 17.0) + .collect(); + let mut road_x = junk.clone(); + let mut road_y: Vec = junk.iter().map(|value| -value).collect(); + for (slot, &(x, y)) in road.points().iter().enumerate() { + road_x[slot] = x; + road_y[slot] = y; + } + set_parameter(&mut mapping, "road_x", json!(road_x)); + set_parameter(&mut mapping, "road_y", json!(road_y)); + + let mut fmu = FmuComponent::new("controller", packaged(), mapping).expect("the FMU builds"); + for situation in &situations { + let from_fmu = fmu_commands(&mut fmu, situation); + let from_laws = native_commands(&road, PURSUIT, IDM, situation); + assert_eq!( + (from_fmu.0.to_bits(), from_fmu.1.to_bits()), + (from_laws.0.to_bits(), from_laws.1.to_bits()), + "the padding reached the road, {}\n{MAYBE_STALE}", + situation.summary(), + ); + } +} + +/// Builds a controller whose parameters carry `bad`, and returns why its +/// first step failed. +/// +/// Every refusal lands in the same place. A parameter is set during +/// initialization, and both the road and the checks on the tuning sets run +/// when `fmi3ExitInitializationMode` calls `configurate`, which is the +/// first moment the parameters mean what the host meant. +fn refusal(bad: &[(&str, Value)]) -> String { + let road = Waypoints::build_straight((0.0, 0.0), (400.0, 0.0)); + let mut mapping = controller_mapping(&road, PURSUIT, IDM); + for (name, value) in bad { + set_parameter(&mut mapping, name, value.clone()); + } + + let mut fmu = FmuComponent::new("controller", packaged(), mapping).expect("the FMU builds"); + let mut ctx = StepCtx::new(SimTime::ZERO, None, WORLD, 0, Vec::new()); + let refused = match fmu.step(&mut ctx) { + Err(CoreError::ComponentFailure { reason }) => reason, + other => panic!("{bad:?} should halt the world, and gave {other:?}"), + }; + assert!( + ctx.take_outbox().is_empty(), + "a step that failed published nothing" + ); + + // Return the reason, which names the instance and the call that + // refused. What was wrong with the value travels separately, on the + // log channel FMI gives a model for saying so, since a status code is + // the whole of what a call itself can return. + refused +} + +#[test] +fn a_road_the_fmu_cannot_build_halts_the_world_rather_than_panicking() { + // `Waypoints` asserts its own minimum, and a panic unwinding out + // through the C interface would take the host process with it. So the + // count is checked first and the same information travels as a + // refusal, which the standard can carry. + for count in [0, 1, MAX_WAYPOINTS + 1] { + let reason = refusal(&[("road_point_count", json!(count))]); + assert!( + reason.contains("exit_initialization_mode"), + "{count} points: {reason}" + ); + } + + // Two points close into nothing, so a loop needs three, and a road set + // closed without enough points for one is refused the same way. + let reason = refusal(&[("road_closed", json!(true)), ("road_point_count", json!(2))]); + assert!(reason.contains("exit_initialization_mode"), "{reason}"); + + // What a host gets by setting the count and forgetting the points: the + // arrays start at zero, so every point of the road is the origin. + // Nothing in `Waypoints` objects, and a car given it holds still. + let reason = refusal(&[ + ("road_x", json!(vec![0.0; MAX_WAYPOINTS])), + ("road_y", json!(vec![0.0; MAX_WAYPOINTS])), + ("road_point_count", json!(4)), + ]); + assert!(reason.contains("exit_initialization_mode"), "{reason}"); +} + +#[test] +fn a_parameter_the_laws_divide_by_halts_the_world_unless_it_is_positive() { + // The model description states no bounds, because `fmi-export` 0.3.0 + // has no key for one, so a host in another tool can send any of these + // and nothing warns it off. Without the check they reach the laws as a + // NaN, which then spreads into every command the car goes on to make. + for name in [ + "v0_speed_tgt", + "a_accel_max", + "b_decel_comfort", + "lookahead", + ] { + for given in [json!(0.0), json!(-1.0)] { + let reason = refusal(&[(name, given.clone())]); + assert!( + reason.contains("exit_initialization_mode"), + "{name} = {given}: {reason}" + ); + } + } +} + +#[test] +fn a_tuning_parameter_changed_between_steps_changes_the_command() { + // What declaring the law parameters tunable buys, checked where a host + // actually stands: they are set again between steps rather than at + // initialization, and the command answers to the new value. Keeping a + // copy taken during initialization would pass every other test here + // and quietly ignore the change. + const SPEED: f64 = 20.0; + + let road = Waypoints::build_straight((0.0, 0.0), (600.0, 0.0)); + let mut mapping = controller_mapping(&road, PURSUIT, IDM); + mapping + .inputs + .push(InputBinding::new("v0_speed_tgt", key("tuning"))); + let mut fmu = FmuComponent::new("controller", packaged(), mapping).expect("the FMU builds"); + + let situation = Situation { + pose: car_at(&road, 100.0, PURSUIT.lateral_tgt, 0.0), + speed: SPEED, + scan: Vec::new(), + }; + let mut wanted = |tick: i64, v0_speed_tgt: f64| { + let now = SimTime::from_millis(tick * PERIOD_MS); + let inbox = vec![ + message( + "pose", + json!({ + "position": situation.pose.position, + "orientation": situation.pose.orientation, + "speed": situation.speed, + }), + ), + message("tuning", json!({ "v0_speed_tgt": v0_speed_tgt })), + ]; + // These steps really do follow one another, so only the first has no + // interval behind it, which is what a conductor would say too. + let dt = (tick > 0).then(|| SimDuration::from_millis(PERIOD_MS)); + let mut ctx = StepCtx::new(now, dt, WORLD, 0, inbox); + fmu.step(&mut ctx).expect("the FMU steps"); + let payload: Value = + serde_json::from_slice(&ctx.take_outbox()[0].1).expect("the payload decodes"); + payload["accel_cmd"].as_f64().expect("an acceleration") + }; + + // Wanting exactly the speed it holds asks for nothing, and wanting + // twice that asks for more than the default target did. + let at_the_default = wanted(0, IDM.v0_speed_tgt); + let at_this_speed = wanted(1, SPEED); + let at_twice_it = wanted(2, SPEED * 2.0); + + assert_eq!( + at_this_speed.to_bits(), + idm_accel( + SPEED, + FREE_ROAD.range, + 0.0, + IdmParams { + v0_speed_tgt: SPEED, + ..IDM + } + ) + .to_bits(), + "{MAYBE_STALE}" + ); + assert!( + at_this_speed.abs() < 1e-12, + "at its target: {at_this_speed}" + ); + assert!(at_twice_it > at_the_default); +} + +#[test] +fn the_nearest_of_several_detections_is_the_one_followed() { + // The sweeps run `nearest_detection` on both sides of the boundary, so + // they would agree on the wrong car as readily as on the right one. + // Here the answer is named rather than searched for: the command has to + // be the one the nearest detection alone would have given. + const SPEED: f64 = 20.0; + + let road = Waypoints::build_straight((0.0, 0.0), (600.0, 0.0)); + let mut fmu = FmuComponent::new( + "controller", + packaged(), + controller_mapping(&road, PURSUIT, IDM), + ) + .expect("the FMU builds"); + let situation = |scan: Vec| Situation { + pose: car_at(&road, 100.0, PURSUIT.lateral_tgt, 0.0), + speed: SPEED, + scan, + }; + + // Three ahead, the nearest in neither the first slot nor the last, so + // taking either end would be a different command rather than the same + // one by luck. + let scan = scan_of(&[(2, 55.0, -1.0), (19, 18.0, -6.0), (44, 31.0, 2.0)]); + let (accel_cmd, _) = fmu_commands(&mut fmu, &situation(scan)); + assert_eq!( + accel_cmd.to_bits(), + idm_accel(SPEED, 18.0, 6.0, IDM).to_bits(), + "followed something other than the nearest\n{MAYBE_STALE}" + ); + for (range, approach_rate) in [(55.0, 1.0), (31.0, -2.0)] { + assert_ne!( + accel_cmd.to_bits(), + idm_accel(SPEED, range, approach_rate, IDM).to_bits(), + "the assertion above cannot tell {range} m apart from the nearest" + ); + } + + // A tie goes to the earlier slot, which is what makes a scan answer the + // same way every time it is read. Both carry the same range and + // different rates, so which one won reaches the command. + let tied = scan_of(&[(5, 22.0, -3.0), (40, 22.0, 7.0)]); + let (accel_cmd, _) = fmu_commands(&mut fmu, &situation(tied)); + assert_eq!( + accel_cmd.to_bits(), + idm_accel(SPEED, 22.0, 3.0, IDM).to_bits(), + "a tie went to the later slot\n{MAYBE_STALE}" + ); + assert_ne!( + accel_cmd.to_bits(), + idm_accel(SPEED, 22.0, -7.0, IDM).to_bits(), + "the assertion above cannot tell the two tied slots apart" + ); +} + +#[test] +fn a_scan_shorter_than_the_arrays_reads_as_free_road_past_its_end() { + // The radar publishes what it found, so a scan is almost always + // shorter than the arrays it crosses in. What fills the rest is the + // mapping's own default rather than anything the sensor said, and + // getting it wrong reads as a phantom car sitting at whatever the tail + // of the buffer held. + let road = Waypoints::build_straight((0.0, 0.0), (400.0, 0.0)); + let lead = Detection { + range: 30.0, + range_rate: -2.0, + }; + let situation = |scan: Vec| Situation { + pose: car_at(&road, 50.0, PURSUIT.lateral_tgt, 0.0), + speed: 22.0, + scan, + }; + + // One detection against a full array holding the same detection and + // free road everywhere else: the FMU cannot tell them apart, because + // padding is what the short one becomes on the way in. + let mut padded = vec![FREE_ROAD; MAX_DETECTIONS]; + padded[0] = lead; + let commands = commands_over( + &road, + PURSUIT, + IDM, + &[ + situation(vec![lead]), + situation(padded), + situation(Vec::new()), + ], + ); + assert_eq!(commands[0], commands[1], "a short scan is a padded one"); + assert!( + commands[0].0 < commands[2].0, + "a lead 30 m ahead should be braked for where an empty scan is not: {commands:?}" + ); +} diff --git a/python/continuo_viz/render.py b/python/continuo_viz/render.py index 8a2cf8e..d752759 100644 --- a/python/continuo_viz/render.py +++ b/python/continuo_viz/render.py @@ -273,8 +273,13 @@ def __init__( self._pygame = pygame self._onscreen = onscreen - pygame.init() + # Only the parts that draw. `pygame.init()` starts every subsystem it + # has, including the mixer, which goes looking for a sound card and + # says so at length on a machine without one. Nothing here makes a + # sound, and a recording run has no display either. + pygame.font.init() if onscreen: + pygame.display.init() self.surface = pygame.display.set_mode((width, height)) pygame.display.set_caption(title) else: diff --git a/python/pyproject.toml b/python/pyproject.toml index 4b400b2..409320c 100644 --- a/python/pyproject.toml +++ b/python/pyproject.toml @@ -43,3 +43,6 @@ select = [ [tool.pytest.ini_options] testpaths = ["tests"] +# `scripts/` holds repo tools rather than package modules, so it is not +# importable on its own. Naming it here is what lets their tests import them. +pythonpath = ["scripts"] diff --git a/python/scripts/merge_fmus.py b/python/scripts/merge_fmus.py new file mode 100644 index 0000000..ffa7e1e --- /dev/null +++ b/python/scripts/merge_fmus.py @@ -0,0 +1,235 @@ +"""Combine FMUs that carry different platforms' binaries into one. + +FMI 3.0 expects a single archive to hold every platform it supports, under +``binaries//``. Anything that builds one platform at a time leaves a +set of archives that each describe the same model and carry one binary apiece, +and this puts them back together. + +That is what packaging on several machines produces, which is why it exists: +each agent writes an FMU carrying only the binary for the platform it ran on. +Nothing here depends on that, and any set of FMUs meeting the same description +merges the same way. + +The sources are grouped by file name, which is the model's library name and so +identifies which archives belong together, and each group becomes one merged +FMU. A directory holding several models' FMUs therefore merges all of them. + +Everything outside ``binaries/`` is shared within a group, and identical +between its members but for ``generationDateAndTime``: the instantiation token +is a UUID derived from the model's name rather than from the build. So one +copy is kept and the rest are checked against it. + +Point it at whatever holds the builds and it searches for them: + + python scripts/merge_fmus.py downloaded-fmus --out-dir merged + +Both default to the shell's working directory, so standing in the folder the +builds were downloaded into needs no arguments at all. The search reaches down +into whatever subdirectories they sit in, and each merged FMU is written to +that working directory: + + cd downloaded-fmus + python ../continuo/python/scripts/merge_fmus.py + +The only thing the layout has to satisfy is that each build sits somewhere of +its own, which it does by construction: builds of one FMU share a file name, +so they cannot be in the same directory. How they got there does not matter, +whether that is `gh run download`, an unzipped download, or four folders +arranged by hand. +""" + +from __future__ import annotations + +import argparse +import re +import sys +import zipfile +from collections import defaultdict +from pathlib import Path + +# What every archive in a group agrees on except this, which is a timestamp +# and so differs between two packagings of the same source. +_GENERATED_AT = re.compile(rb'\s*generationDateAndTime="[^"]*"') + +BINARIES = "binaries/" +MODEL_DESCRIPTION = "modelDescription.xml" + + +class MergeError(Exception): + """Something about the inputs that no merged FMU could come from.""" + + +def find_fmus(source_fmu_dir: Path) -> list[Path]: + """Every archive under one directory, at any depth, in path order. + + Searched rather than listed, so the caller names one folder instead of + each build inside it. That keeps a shell out of it: a glob would leave + this working everywhere but Windows, where neither the shell nor Python + expands one. Builds spread across several places want their common parent. + """ + if not source_fmu_dir.is_dir(): + raise MergeError(f"{source_fmu_dir} is not a directory") + found = sorted(source_fmu_dir.rglob("*.fmu")) + if not found: + raise MergeError(f"no .fmu under {source_fmu_dir}") + + # Return them in path order, so a run reports what it found the same way + # however the filesystem lists it. + return found + + +def group_by_fmu(fmus: list[Path]) -> dict[str, list[Path]]: + """The archives gathered into one group per FMU. + + Grouped by file name, which is the crate's library name, so one FMU's + builds come together wherever they were found. A workspace packaging two + FMU crates puts a build of each beside every platform's other, so the + groups run across the directories rather than within them. + """ + groups: dict[str, list[Path]] = defaultdict(list) + for fmu in fmus: + groups[fmu.name].append(fmu) + + # Return the groups in name order, so a run reports them the same way + # whichever order they were found in. + return {name: groups[name] for name in sorted(groups)} + + +def inputs_to_merge(fmus: list[Path], out: Path) -> list[Path]: + """The group's archives, less the one this run is about to write. + + Writing into the directory being searched is the ordinary case, since + both default to the current directory, so a second run finds what the + first one wrote. It is + a merge of the others and would collide with all of them, and it is about + to be replaced anyway, so it is passed over. + + An archive at that path carrying one platform is a different thing: an + input that happens to sit where the output goes. Overwriting it would drop + its platform from the result without saying so, and it is the only file a + run could destroy, so it stops instead. + """ + keep = [] + for fmu in fmus: + if fmu.resolve() != out.resolve(): + keep.append(fmu) + continue + with zipfile.ZipFile(fmu) as archive: + if len(platforms_in(archive)) < 2: + raise MergeError( + f"{fmu} is where the merged FMU would go and carries one " + "platform, so merging would overwrite an input; write " + "somewhere else with --out-dir" + ) + + # Return what is left, which is every archive that is not this run's own + # output from a previous one. + return keep + + +def platforms_in(archive: zipfile.ZipFile) -> set[str]: + """The platform directories an archive carries binaries for.""" + return { + name[len(BINARIES) :].split("/")[0] + for name in archive.namelist() + if name.startswith(BINARIES) and name.count("/") >= 2 + } + + +def _comparable(model_description: bytes) -> bytes: + """The description without the one attribute that is a timestamp.""" + return _GENERATED_AT.sub(b"", model_description) + + +def merge(fmus: list[Path], out: Path) -> dict[str, list[str]]: + """Writes one FMU carrying every input's binaries. Returns what it took. + + The first archive supplies everything outside ``binaries/``. The rest + supply only their binaries, after their description is checked against the + first, since two archives describing different models would merge into one + that misdescribes both. + """ + contents: dict[str, bytes] = {} + description: bytes | None = None + taken: dict[str, list[str]] = {} + + for index, fmu in enumerate(fmus): + with zipfile.ZipFile(fmu) as source: + if MODEL_DESCRIPTION not in source.namelist(): + raise MergeError(f"{fmu} has no {MODEL_DESCRIPTION}") + here = source.read(MODEL_DESCRIPTION) + if description is None: + description = here + elif _comparable(here) != _comparable(description): + raise MergeError( + f"{fmu} describes a different model from {fmus[0]}; " + "these are not builds of one FMU" + ) + + platforms = platforms_in(source) + if not platforms: + raise MergeError(f"{fmu} carries no {BINARIES} entries") + # Keyed by path, since every build of one FMU has the same name. + taken[str(fmu)] = sorted(platforms) + + for item in source.infolist(): + if item.is_dir(): + continue + if item.filename.startswith(BINARIES): + if item.filename in contents: + raise MergeError( + f"{item.filename} appears in more than one input" + ) + contents[item.filename] = source.read(item) + elif index == 0: + contents[item.filename] = source.read(item) + + out.parent.mkdir(parents=True, exist_ok=True) + with zipfile.ZipFile(out, "w", zipfile.ZIP_DEFLATED) as merged: + # Sorted, so the layout comes from the merge rather than from the + # order the artifacts happened to arrive in. + for name in sorted(contents): + merged.writestr(name, contents[name]) + + return taken + + +def main(argv: list[str] | None = None) -> int: + # Spelled out rather than taken from `__doc__`, which is None under `-OO`. + parser = argparse.ArgumentParser( + description="Combine FMUs that carry different platforms' binaries into one." + ) + parser.add_argument( + "source_fmu_dir", + nargs="?", + default=Path("."), + type=Path, + help="the root of a directory holding the FMUs to merge, searched at " + "any depth (default: the current directory)", + ) + parser.add_argument( + "--out-dir", + type=Path, + default=Path("."), + help="where to write the merged FMUs (default: the current directory)", + ) + args = parser.parse_args(argv) + + try: + groups = group_by_fmu(find_fmus(args.source_fmu_dir)) + for name, fmus in groups.items(): + out = args.out_dir / name + taken = merge(inputs_to_merge(fmus, out), out) + print(f"{name}") + for source, platforms in sorted(taken.items()): + print(f" {source}: {', '.join(platforms)}") + except MergeError as error: + print(f"error: {error}", file=sys.stderr) + return 1 + + print(f"wrote {len(groups)} FMU(s) to {args.out_dir}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/python/tests/test_merge_fmus.py b/python/tests/test_merge_fmus.py new file mode 100644 index 0000000..d552385 --- /dev/null +++ b/python/tests/test_merge_fmus.py @@ -0,0 +1,169 @@ +"""The multi-platform FMU merge.""" + +import zipfile + +import pytest +from merge_fmus import MergeError, find_fmus, group_by_fmu, inputs_to_merge, merge + +# A stand-in for what a packager writes. The token is invented: the merge +# never reads one, and only cares that a group's descriptions match. +DESCRIPTION = ( + b'' + b'' +) + + +def write_fmu(path, platform, stamp="2026-01-01T00:00:00Z", model=DESCRIPTION): + """One platform's FMU, as the packager writes it.""" + with zipfile.ZipFile(path, "w") as fmu: + fmu.writestr("modelDescription.xml", model.replace(b"{stamp}", stamp.encode())) + fmu.writestr(f"binaries/{platform}/demo.bin", f"code for {platform}".encode()) + fmu.writestr("sources/buildDescription.xml", b"") + return path + + +def test_the_merged_fmu_carries_every_platforms_binary(tmp_path): + platforms = ["aarch64-darwin", "aarch64-linux", "x86_64-linux", "x86_64-windows"] + fmus = [write_fmu(tmp_path / f"{p}.fmu", p) for p in platforms] + + out = tmp_path / "merged.fmu" + merge(fmus, out) + + with zipfile.ZipFile(out) as merged: + names = merged.namelist() + for platform in platforms: + assert f"binaries/{platform}/demo.bin" in names + assert merged.read(f"binaries/{platform}/demo.bin") == ( + f"code for {platform}".encode() + ) + # Everything outside binaries/ is carried once, from the first input. + assert names.count("modelDescription.xml") == 1 + assert names.count("sources/buildDescription.xml") == 1 + + +def test_the_merged_fmu_keeps_one_description(tmp_path): + # The packager stamps each build with the time it ran, so the four + # descriptions differ there and nowhere else. That is not a disagreement. + fmus = [ + write_fmu(tmp_path / "a.fmu", "x86_64-linux", stamp="2026-01-01T00:00:00Z"), + write_fmu(tmp_path / "b.fmu", "x86_64-windows", stamp="2026-06-30T12:34:56Z"), + ] + + out = tmp_path / "merged.fmu" + merge(fmus, out) + + with zipfile.ZipFile(out) as merged: + assert merged.read("modelDescription.xml") == zipfile.ZipFile(fmus[0]).read( + "modelDescription.xml" + ) + + +def test_the_entries_are_ordered_the_same_whichever_order_the_inputs_come_in(tmp_path): + # CI hands over one directory per platform and promises nothing about + # the order, so the layout has to come from the merge rather than from + # the arguments. + platforms = ["x86_64-windows", "aarch64-linux", "x86_64-linux"] + fmus = [write_fmu(tmp_path / f"{p}.fmu", p) for p in platforms] + + forward = tmp_path / "forward.fmu" + backward = tmp_path / "backward.fmu" + merge(fmus, forward) + merge(list(reversed(fmus)), backward) + + with zipfile.ZipFile(forward) as a, zipfile.ZipFile(backward) as b: + assert a.namelist() == b.namelist() + assert a.namelist() == sorted(a.namelist()) + + +def test_two_builds_of_different_models_are_refused(tmp_path): + # Merging these would produce one FMU whose description fits neither. + other = DESCRIPTION.replace(b'modelName="demo"', b'modelName="something-else"') + fmus = [ + write_fmu(tmp_path / "a.fmu", "x86_64-linux"), + write_fmu(tmp_path / "b.fmu", "x86_64-windows", model=other), + ] + + with pytest.raises(MergeError, match="different model"): + merge(fmus, tmp_path / "merged.fmu") + + +def test_each_fmu_is_merged_from_its_own_builds(tmp_path): + # A workspace packaging two FMU crates puts both in every platform's + # artifact, so the sets to merge run across the directories rather than + # within them. Grouping by anything but the name would cross them. + platforms = ["x86_64-linux", "x86_64-windows"] + for platform in platforms: + artifact = tmp_path / f"fmus-{platform}" + artifact.mkdir() + write_fmu(artifact / "controller_idm.fmu", platform, model=DESCRIPTION) + write_fmu( + artifact / "controller_ai.fmu", + platform, + model=DESCRIPTION.replace(b'modelName="demo"', b'modelName="learned"'), + ) + + groups = group_by_fmu(find_fmus(tmp_path)) + + assert list(groups) == ["controller_ai.fmu", "controller_idm.fmu"] + for name, fmus in groups.items(): + merged = tmp_path / "merged" / name + merge(fmus, merged) + with zipfile.ZipFile(merged) as archive: + assert sorted(platforms_of(archive)) == platforms + + +def platforms_of(archive): + """The platforms an archive carries binaries for.""" + return { + name.split("/")[1] + for name in archive.namelist() + if name.startswith("binaries/") + } + + +def test_two_builds_for_the_same_platform_are_refused(tmp_path): + # One would silently overwrite the other, so the merged FMU would carry + # a binary from a build nobody chose. + fmus = [ + write_fmu(tmp_path / "a.fmu", "x86_64-linux"), + write_fmu(tmp_path / "b.fmu", "x86_64-linux"), + ] + + with pytest.raises(MergeError, match="more than one input"): + merge(fmus, tmp_path / "merged.fmu") + + +def test_merging_in_place_passes_over_the_last_run(tmp_path): + # Both paths default to here, so a second run finds the first run's + # output. It is a merge of the others, would collide with all of them, + # and is about to be replaced. + platforms = ["x86_64-linux", "x86_64-windows"] + for platform in platforms: + (tmp_path / platform).mkdir() + write_fmu(tmp_path / platform / "demo.fmu", platform) + + out = tmp_path / "demo.fmu" + merge(inputs_to_merge(group_by_fmu(find_fmus(tmp_path))["demo.fmu"], out), out) + again = group_by_fmu(find_fmus(tmp_path))["demo.fmu"] + merge(inputs_to_merge(again, out), out) + + with zipfile.ZipFile(out) as merged: + assert sorted(platforms_of(merged)) == platforms + + +def test_an_input_where_the_output_goes_is_not_overwritten(tmp_path): + # One platform's FMU sitting at the output path is an input rather than + # a previous merge, and overwriting it would drop its platform from the + # result without saying so. + (tmp_path / "linux").mkdir() + write_fmu(tmp_path / "linux" / "demo.fmu", "x86_64-linux") + in_the_way = write_fmu(tmp_path / "demo.fmu", "x86_64-windows") + + fmus = group_by_fmu(find_fmus(tmp_path))["demo.fmu"] + with pytest.raises(MergeError, match="would overwrite an input"): + inputs_to_merge(fmus, tmp_path / "demo.fmu") + + with zipfile.ZipFile(in_the_way) as untouched: + assert platforms_of(untouched) == {"x86_64-windows"} diff --git a/xtask/Cargo.toml b/xtask/Cargo.toml new file mode 100644 index 0000000..bb43ea1 --- /dev/null +++ b/xtask/Cargo.toml @@ -0,0 +1,13 @@ +[package] +name = "xtask" +description = "Workspace tasks that cargo has no command for" +version.workspace = true +edition.workspace = true +license.workspace = true +rust-version.workspace = true +# A tool for this checkout, not a crate anyone installs. +publish = false + +[dependencies] +# For reading `cargo metadata`, which is how the FMU crates are found. +serde_json = { workspace = true } diff --git a/xtask/src/main.rs b/xtask/src/main.rs new file mode 100644 index 0000000..7c787a2 --- /dev/null +++ b/xtask/src/main.rs @@ -0,0 +1,146 @@ +//! Workspace tasks that cargo has no command for. +//! +//! Cargo has no user-defined targets and its aliases cannot chain +//! commands, so anything more than one invocation lives in a binary like +//! this one. `.cargo/config.toml` aliases `cargo xtask` to running it, +//! which is what makes it a real entry point somebody types rather than a +//! side effect hidden inside `cargo build`. + +use std::io; +use std::process::{Command, ExitCode}; + +/// The crate-name prefix an FMU crate carries. +/// +/// Discovery goes by name rather than by a list, so a second FMU crate, +/// such as one carrying a learned model, is packaged by this and by CI +/// with no edit anywhere. +const FMU_CRATE_PREFIX: &str = "continuo-fmu-"; + +/// What to install when the packaging subcommand is missing. +const INSTALL_CARGO_FMI: &str = "cargo install cargo-fmi"; + +fn main() -> ExitCode { + let task = std::env::args().nth(1); + let result = match task.as_deref() { + Some("package-fmus") => package_fmus(), + Some(unknown) => Err(format!( + "unknown task `{unknown}`\nusage: cargo xtask package-fmus" + )), + None => Err("no task given\nusage: cargo xtask package-fmus".to_string()), + }; + + // Return the failure as a message rather than a panic, since these + // are things a person is meant to read and act on. + match result { + Ok(()) => ExitCode::SUCCESS, + Err(message) => { + eprintln!("error: {message}"); + ExitCode::FAILURE + } + } +} + +/// Packages every FMU crate in the workspace. +fn package_fmus() -> Result<(), String> { + let fmu_crates = fmu_crates()?; + if fmu_crates.is_empty() { + return Err(format!("no {FMU_CRATE_PREFIX}* crate in this workspace")); + } + require_cargo_fmi()?; + for name in &fmu_crates { + println!("packaging {name}"); + package(name)?; + } + + // Return once every one is packaged, having stopped at the first + // that would not, since a half-packaged target directory is worse + // than none. + Ok(()) +} + +/// The FMU crates this workspace holds, in name order. +/// +/// `--no-deps` keeps the answer to workspace members, so a dependency +/// that happened to match the prefix could not join the list. Sorting is +/// for the reader and the log: cargo's order is its own business, and a +/// packaging run that names its crates in a different order each time +/// invites the question of what else moved. +fn fmu_crates() -> Result, String> { + let output = cargo() + .args(["metadata", "--no-deps", "--format-version", "1"]) + .output() + .map_err(|error| format!("cannot run cargo: {error}"))?; + if !output.status.success() { + return Err(format!( + "cargo metadata failed ({})\n{}", + output.status, + String::from_utf8_lossy(&output.stderr).trim() + )); + } + let metadata: serde_json::Value = serde_json::from_slice(&output.stdout) + .map_err(|error| format!("cannot read cargo metadata: {error}"))?; + let packages = metadata["packages"] + .as_array() + .ok_or("cargo metadata carried no packages")?; + let mut names: Vec = packages + .iter() + .filter_map(|package| package["name"].as_str()) + .filter(|name| name.starts_with(FMU_CRATE_PREFIX)) + .map(str::to_string) + .collect(); + names.sort(); + + // Return the FMU crates, in the order they will be packaged. + Ok(names) +} + +/// Fails with the install command unless `cargo fmi` is there to call. +/// +/// Asked once, before anything is packaged, so a missing subcommand is +/// reported as itself rather than as a packaging failure carrying a +/// guess about the cause. `--help` is the probe because it exits zero +/// when the subcommand exists and cargo exits 101 when it does not, +/// which reading cargo's error text would only approximate. +fn require_cargo_fmi() -> Result<(), String> { + let probed = cargo() + .args(["fmi", "--help"]) + .stdout(std::process::Stdio::null()) + .stderr(std::process::Stdio::null()) + .status(); + match probed { + Ok(status) if status.success() => Ok(()), + Ok(_) => Err(format!("`cargo fmi` is not available: {INSTALL_CARGO_FMI}")), + Err(error) if error.kind() == io::ErrorKind::NotFound => { + Err("cannot find cargo on the path".to_string()) + } + Err(error) => Err(format!("cannot run cargo: {error}")), + } +} + +/// Packages one FMU crate into `target/fmu`. +/// +/// `--release` because the default is the debug profile, and how a +/// packaged FMU is optimized is settled when it is packaged: a host +/// loads the binary it finds and cannot rebuild it. The two profiles +/// answer bit for bit alike, which is what lets a release FMU be compared +/// against natively built laws. +fn package(name: &str) -> Result<(), String> { + let status = cargo() + .args(["fmi", "--package", name, "bundle", "--release"]) + .status() + .map_err(|error| format!("cannot run cargo: {error}"))?; + if !status.success() { + return Err(format!("packaging {name} failed ({status})")); + } + + Ok(()) +} + +/// The cargo that invoked this, or whatever one is on the path. +/// +/// `CARGO` is set for anything cargo runs, so a toolchain chosen by +/// `rustup run` or a `+toolchain` argument is the one used throughout +/// rather than being swapped halfway. +fn cargo() -> Command { + Command::new(std::env::var("CARGO").unwrap_or_else(|_| "cargo".to_string())) +}