diff --git a/CLAUDE.md b/CLAUDE.md index 637ac54..88d41b1 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -81,10 +81,18 @@ first failure: cargo fmt --all --check cargo clippy --workspace --all-targets --all-features -- -D warnings RUSTDOCFLAGS=-D warnings cargo doc --workspace --no-deps +cargo xtask package-fmus cargo test --workspace cargo run -p continuo-examples --example traffic ``` +Packaging comes before the tests because the demo's traffic drives on the +controller FMU, so neither the tests nor the demo can start without a +package. It costs a release build of the FMU crate whenever a law changed, +and well under a second when nothing did. Leaving it out would let an edited +law pass here on the copy packaged before it and fail in CI. It needs +`cargo install cargo-fmi` once. + It adds the viewer's `ruff check .`, `ruff format --check .` and `pytest` when the viewer is installed, and says they were skipped when it is not, so a Rust-only change is not held up by a Python environment nobody set up. @@ -107,7 +115,7 @@ covers the lot locally. A change reaching an FMU crate or the laws it links needs more than that, because a `.fmu` carries its own compiled copy of everything it links and the -checks above compare nothing against it: +checks above drive that copy without comparing it against anything: ```sh cargo xtask verify-fmus @@ -115,16 +123,16 @@ cargo xtask verify-fmus It packages every FMU, validates each with fmpy, and runs the tests that check the packaged copy against the laws it was built from, asking for the -feature each FMU crate holds those tests behind. `verify` leaves all of it -out on purpose: packaging costs a release build of the FMU crate whenever a -law changed, and the tests say nothing without it, so the pair belongs with a -change that reaches a law rather than in an editing loop. Validation is -skipped, and says so, when fmpy is not installed. - -`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, and validating needs -fmpy in the environment the viewer's checks run in. +feature each FMU crate holds those tests behind. `verify` leaves the +validation and the comparison out on purpose: the feature rebuilds much of +the graph, so the pair belongs with a change that reaches a law rather than +in an editing loop. Validation is skipped, and says so, when fmpy is not +installed. + +`continuo-actors` is the one to watch, since a control law there is compiled +into the FMU as well as linked natively, and only this comparison says the +two still agree. Validating needs fmpy in the environment the viewer's checks +run in. ## Turn a mistake into a check diff --git a/Cargo.lock b/Cargo.lock index 507cdc8..3a41479 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -475,9 +475,12 @@ dependencies = [ "continuo-actors", "continuo-conductor", "continuo-core", + "continuo-fmi", + "continuo-fmu-controller-idm", "continuo-transport", "continuo-viz-bridge", - "tracing", + "serde_json", + "thiserror 2.0.18", "tracing-subscriber", ] diff --git a/DECISIONS.md b/DECISIONS.md index e84fffe..11e795f 100644 --- a/DECISIONS.md +++ b/DECISIONS.md @@ -1515,3 +1515,78 @@ it got there, including the roads not taken. that makes a rerun fail the same way until the fix. CI now fetches the dependency crates with the lock held before its first build, so a stale lock fails the job at the cause. + +- **2026-10-12**: **Traffic drives on the controller FMU, and the world + hash moved for it.** Every car the spawner asks for is now `[radar, + controller, physics]`, the controller being the packaged FMU, so it + follows the car ahead in its lane and wants its spawn speed back on a + free road. The ego and `traffic_scale`'s cars stay `[controller, + physics]` at constant speed: the ego until it shares a lane with + traffic, and the scaling run because it measures the conductor, and an + FMU per car is an experiment of its own. + - **What joined the fingerprint**: `eccd08f9a316bbbc` to + `580e8d37b14c6abd`, for every traffic car's scans and acceleration + commands and the poses they changed. Every traffic car slowed + somewhat, since IDM brakes for a lead well before it is close: with a + faster one 65 m ahead, a car at its desired speed brakes at 13 percent + of its maximum acceleration. The slowest fell from 16.2 to 13.3 m/s. + The ego's poses did not move, and neither did the turnover: fourteen + cars, eight retired. + - **A free road is not exactly zero.** `FREE_ROAD` is a long finite + range, so the interaction term is tiny rather than absent, and a car + at its desired speed is commanded about -3e-16. Nothing depends on + exact zero, but a test written against it failed, which is how this + was found. + - **The FMU registers as `controller`**, so a following car's paths read + like a native car's, and the leave order is radar, controller, + physics. + - **One spawn state feeds both halves of a car.** At the instant a car + joins, its controller steps before its physics has published, so the + mapping gives the FMU's pose and speed inputs the state the physics is + built from as their starting values. Without them it would steer from + the origin and accelerate from a standstill it is not in. + - **The mapping's keys are concrete.** A native component wildcards the + world, because its constructor does not know it; a scenario does, so + the FMU subscribes and publishes exactly. + - **Two tests guard following and one guards the meeting point.** No + two same-lane cars overlap at any instant of the run, and some traffic + car changes speed by more than 1 m/s. Pointing the radar binding at a + key nobody publishes fails both, with two origins 0.56 m apart. A unit + test steps one FMU controller and decodes what it published as the + plant's own `AccelCmd` and `SteerCmd`, which is the place a rename on + either side would otherwise surface only as a car holding its speed. + - **`cargo xtask verify` packages the FMUs before testing.** It had left + packaging to `verify-fmus` so the editing loop stayed Rust-only, which + held while nothing in the demo loaded an FMU. Now the tests and the + demo cannot start without a package, and an edited law would leave the + demo driving the copy packaged before it, passing locally and failing + in CI. Packaging costs 0.7 s when nothing changed. Validation and the + feature-gated comparison stay in `verify-fmus`. + - **Building the scenario can now fail in two new ways**, a missing + package and an FMU that will not instantiate, so the setup and run + functions return a `ScenarioError` wrapping those beside + `ConductorError`. + - **The examples drop an FMU's trace of its own calls from the log.** + `fmi-export` reports every FMI call under a `trace` category, one line + per car per step, about 1,700 a run, and `fmi` logs them at `INFO` with + the category as the target. Turning logging off at instantiation would + silence the FMU's errors too, since they travel the same way, so the + examples' subscriber drops a message only when its target is exactly + `trace` and its module path is the `fmu` that `fmi` gives everything an + FMU logs. `Targets` was the obvious tool and is wrong here: it matches a + target as a prefix and knows nothing of module paths, so it would + silence any target beginning with the word, from any crate. The three + lifecycle lines a car logs share their category with the errors and + stay. + - **Joining costs 70 ms a car on Windows, measured and deferred.** `fmi` + 0.8 loads a fresh copy of the DLL for each instance. The demo's + release run went from 0.07 s to 1.27 s, 1.0 s of it the fourteen + joins, and PLAN.md's deferred list carries the shared archive that + would fix it. + - **The spawner can place a car right beside one already there**, left + for the milestone's ego-lane change. Its frontier is where it last + spawned, but cars drive on after spawning, so two of the run's eight + mid-run spawns landed 1.73 m and 0.99 m bumper to bumper from a car in + the same lane. IDM brakes and nothing overlaps, but nothing prevents a + spawn landing inside a car; measuring from the furthest car the + spawner can see in that lane would. diff --git a/PLAN.md b/PLAN.md index a811bcd..3dd0e1f 100644 --- a/PLAN.md +++ b/PLAN.md @@ -849,6 +849,24 @@ under "Wire format", rather than churning the fingerprint once apiece. which `traffic_scale` is the instrument for measuring. Nothing at demo scale needs it. +- **An FMU archive opened once and shared by every instance built from + it.** `fmi` 0.8 extracts the archive and loads its library for each + `FmuComponent`, so every following car loads a fresh copy of the + controller's DLL. On Windows that measured about 70 ms per car joining, + against 3 ms for the extraction: the demo's fourteen traffic joins cost + 1.0 s of a 1.27 s release run, where the same run with native traffic took + 0.07 s. Other platforms were not measured, and a scan of each newly + written DLL is the likely reason Windows pays it. + - An opened archive held behind an `Arc` would load the library once per + run, since loading a path already loaded costs next to nothing. An FMU + declaring `canBeInstantiatedOnlyOncePerProcess` would still need an + archive of its own per instance, so the sharing is the FMU's call. + - `TrafficRequestHandler` is where the demo would hold it, since it builds + the cars and lives exactly as long as the run; a process-wide static + would never clean up its temporary directory. + - Deferred because the demo still runs at 24x real time. It bites when a + scenario joins cars often, or when `traffic_scale` takes the FMU car. + - **Road-network importer**: which format (OpenDRIVE, Lanelet2, other) lowers into the world spec; decide when realistic road scenarios are needed. @@ -883,11 +901,12 @@ under "Wire format", rather than churning the fingerprint once apiece. - **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 + count different things: membership is published per component, so a + traffic car joining raises three events, one each for its radar, its + controller and its physics. A demo run reports 46 joins and 24 leaves + against 15 actors, which reads as a discrepancy and is not one: fourteen + traffic cars at three components each and the ego at two, 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. diff --git a/README.md b/README.md index 5d11581..d408fab 100644 --- a/README.md +++ b/README.md @@ -132,7 +132,17 @@ Requires a recent stable Rust toolchain (edition 2024, rust ≥ 1.85) and libclang, the workspace's only native-code prerequisite. See [Installing libclang](#installing-libclang) below. +The demo's traffic drives on a controller packaged as an FMU, so the tests +and the demo need that package built first, and built again after a control +law changes. `cargo xtask verify` does it every time. + ```sh +# Once, for the packaging subcommand +cargo install cargo-fmi + +# Package the controller FMU into target/fmu +cargo xtask package-fmus + # Build everything cargo build --workspace @@ -177,18 +187,17 @@ anything that runs several lives in [`xtask`](xtask/), reached as how many tests ran. ```sh -# What to run before a commit: format, lint, docs, tests and the demo, in -# that order, cheapest first, stopping at the first failure. The viewer's -# ruff and pytest run when the viewer is installed and say they were skipped -# when it is not. A quick check rather than a thorough one, since CI is the -# authority on whether a commit is good. +# What to run before a commit: format, lint, docs, packaging, tests and the +# demo, in that order, cheapest first, stopping at the first failure. The +# viewer's ruff and pytest run when the viewer is installed and say they were +# skipped when it is not. A quick check rather than a thorough one, since CI +# is the authority on whether a commit is good. cargo xtask verify -# What a change reaching an FMU crate needs on top: package every FMU into -# target/fmu, validate each with fmpy, and run the tests that check the -# packaged copy against the laws it was built from. An .fmu links those laws -# statically, so it carries a snapshot of them. Needs -# `cargo install cargo-fmi` once, and fmpy for the validation. +# What a change reaching an FMU crate needs on top: validate every packaged +# FMU with fmpy, and run the tests that check the packaged copy against the +# laws it was built from. An .fmu links those laws statically, so it carries +# a snapshot of them. Needs fmpy for the validation. cargo xtask verify-fmus # The packaging on its own, which is what CI calls before its own tests. @@ -196,7 +205,7 @@ cargo xtask package-fmus ``` The demo logs every live car's pose once per sim-second and finishes in a -fraction of a wall-second. Traffic appears as it is spawned, so the roll +few wall-seconds. Traffic appears as it is spawned, so the roll changes as the run goes on: ``` @@ -204,16 +213,19 @@ INFO initial pose sim_time=0.0 key="continuo/demo/actor/ego/pose" x=0.00 y=0.00 INFO initial pose sim_time=0.5 key="continuo/demo/actor/traffic1/pose" x=80.36 y=-3.50 yaw_deg=0.0 ... INFO pose sim_time=4.0 key="continuo/demo/actor/ego/pose" x=120.00 y=0.00 yaw_deg=0.0 -INFO pose sim_time=4.0 key="continuo/demo/actor/traffic1/pose" x=145.38 y=-3.50 yaw_deg=0.0 -INFO pose sim_time=4.0 key="continuo/demo/actor/traffic2/pose" x=190.87 y=-3.50 yaw_deg=0.0 +INFO pose sim_time=4.0 key="continuo/demo/actor/traffic1/pose" x=143.47 y=-3.50 yaw_deg=0.0 +INFO pose sim_time=4.0 key="continuo/demo/actor/traffic2/pose" x=189.79 y=-3.50 yaw_deg=0.0 ... done: world 'demo' reached sim time 30.0 in 3031 ticks (free-run) -actual time: 0.404 s (74x real-time), world hash eccd08f9a316bbbc +actual time: 2.295 s (13x real-time), world hash 580e8d37b14c6abd ``` -The ego holds the centre lane at 30 m/s; traffic runs 16-22 m/s in the lanes -either side, so the ego spends the run overtaking. Nothing here models a -collision, which is why traffic never shares the ego's lane. A car is +The ego holds the centre lane at 30 m/s; traffic spawns at 16-22 m/s in the +lanes either side, so the ego spends the run overtaking. Each traffic car +drives on the controller FMU, following the car ahead in its lane with IDM +and steering with pure pursuit, so a faster car closes on a slower one and +settles behind it rather than driving through it. Nothing here yet watches +for a collision, which is why traffic never shares the ego's lane. A car is retired once it falls 60 m behind, and a replacement spawns ahead. Over 30 sim-seconds fourteen different cars pass through a world that holds six at a time, eight of them retired along the way. diff --git a/crates/continuo-examples/Cargo.toml b/crates/continuo-examples/Cargo.toml index 03bdfcd..a665ab6 100644 --- a/crates/continuo-examples/Cargo.toml +++ b/crates/continuo-examples/Cargo.toml @@ -20,10 +20,13 @@ continuo-core = { workspace = true } continuo-transport = { workspace = true } continuo-conductor = { workspace = true } continuo-actors = { workspace = true } +continuo-fmi = { workspace = true } +# For finding the packaged `.fmu` and the array length it was built with, +# not for the FMU itself, which a car loads from the archive at run time. +continuo-fmu-controller-idm = { workspace = true } continuo-viz-bridge = { workspace = true, optional = true } - -[dev-dependencies] -tracing = { workspace = true } +serde_json = { workspace = true } +thiserror = { workspace = true } tracing-subscriber = { workspace = true } [[example]] diff --git a/crates/continuo-examples/examples/traffic.rs b/crates/continuo-examples/examples/traffic.rs index 1d576ba..6732b3b 100644 --- a/crates/continuo-examples/examples/traffic.rs +++ b/crates/continuo-examples/examples/traffic.rs @@ -11,10 +11,7 @@ use continuo_examples::traffic_world::{self, TrafficRequestHandler}; use continuo_transport::InProcTransport; fn main() -> Result<(), Box> { - tracing_subscriber::fmt() - .with_max_level(tracing::Level::INFO) - .with_target(false) - .init(); + continuo_examples::init_logging(); let traffic_request_handler = TrafficRequestHandler::default(); let mut conductor = Conductor::new( diff --git a/crates/continuo-examples/examples/traffic_realtime.rs b/crates/continuo-examples/examples/traffic_realtime.rs index 0fb8e50..695195f 100644 --- a/crates/continuo-examples/examples/traffic_realtime.rs +++ b/crates/continuo-examples/examples/traffic_realtime.rs @@ -19,10 +19,7 @@ use continuo_examples::traffic_world::{self, TrafficRequestHandler}; use continuo_transport::InProcTransport; fn main() -> Result<(), Box> { - tracing_subscriber::fmt() - .with_max_level(tracing::Level::INFO) - .with_target(false) - .init(); + continuo_examples::init_logging(); let seconds: i64 = std::env::args() .nth(1) diff --git a/crates/continuo-examples/examples/traffic_record.rs b/crates/continuo-examples/examples/traffic_record.rs index 0fb165b..159ff59 100644 --- a/crates/continuo-examples/examples/traffic_record.rs +++ b/crates/continuo-examples/examples/traffic_record.rs @@ -13,10 +13,7 @@ use continuo_examples::traffic_world::{self, TrafficRequestHandler}; use continuo_transport::{InProcTransport, MonitorTransport}; fn main() -> Result<(), Box> { - tracing_subscriber::fmt() - .with_max_level(tracing::Level::INFO) - .with_target(false) - .init(); + continuo_examples::init_logging(); let file = std::env::args() .nth(1) diff --git a/crates/continuo-examples/examples/traffic_resim.rs b/crates/continuo-examples/examples/traffic_resim.rs index 00c0882..ae31333 100644 --- a/crates/continuo-examples/examples/traffic_resim.rs +++ b/crates/continuo-examples/examples/traffic_resim.rs @@ -29,10 +29,7 @@ use continuo_transport::InProcTransport; const WHAT_IF_EGO_SPEED: f64 = 36.0; fn main() -> Result<(), Box> { - tracing_subscriber::fmt() - .with_max_level(tracing::Level::INFO) - .with_target(false) - .init(); + continuo_examples::init_logging(); let file = std::env::args() .nth(1) diff --git a/crates/continuo-examples/examples/traffic_verify.rs b/crates/continuo-examples/examples/traffic_verify.rs index 3532756..37727cc 100644 --- a/crates/continuo-examples/examples/traffic_verify.rs +++ b/crates/continuo-examples/examples/traffic_verify.rs @@ -15,10 +15,7 @@ use continuo_examples::traffic_world::{self, TrafficRequestHandler}; use continuo_transport::{InProcTransport, MonitorTransport}; fn main() -> Result<(), Box> { - tracing_subscriber::fmt() - .with_max_level(tracing::Level::INFO) - .with_target(false) - .init(); + continuo_examples::init_logging(); let file = std::env::args() .nth(1) diff --git a/crates/continuo-examples/examples/traffic_viz.rs b/crates/continuo-examples/examples/traffic_viz.rs index c265416..d72fd57 100644 --- a/crates/continuo-examples/examples/traffic_viz.rs +++ b/crates/continuo-examples/examples/traffic_viz.rs @@ -27,10 +27,7 @@ use continuo_transport::InProcTransport; use continuo_viz_bridge::{VizBridge, ZenohSink}; fn main() -> Result<(), Box> { - tracing_subscriber::fmt() - .with_max_level(tracing::Level::INFO) - .with_target(false) - .init(); + continuo_examples::init_logging(); let seconds: i64 = std::env::args() .nth(1) diff --git a/crates/continuo-examples/src/lib.rs b/crates/continuo-examples/src/lib.rs index b4024d4..59fdcc3 100644 --- a/crates/continuo-examples/src/lib.rs +++ b/crates/continuo-examples/src/lib.rs @@ -11,3 +11,36 @@ //! - `traffic_resim`: a live ego re-run against played-back traffic pub mod traffic_world; + +use tracing_subscriber::filter::{LevelFilter, filter_fn}; +use tracing_subscriber::prelude::*; + +/// The module path `fmi` gives every message an FMU logs, which is what +/// tells an FMU's log apart from the host's own. +const FMU_MODULE_PATH: &str = "fmu"; + +/// The logging category an FMU built with `fmi-export` reports each FMI call +/// under. `fmi` passes the category through as the log target. +const FMU_TRACE_CATEGORY: &str = "trace"; + +/// Sends the examples' log to standard output at `INFO`, without an FMU's +/// trace of the calls it receives. +/// +/// The controller FMU logs every call under its `trace` category, one line +/// per car per step. Switching its logging off when it is instantiated would +/// silence its errors as well, which travel the same way and are the only +/// account of why it refused a call, so this drops exactly the messages an +/// FMU logged under that category and lets everything else through. The +/// match is exact on both, since `Targets` would match the target as a +/// prefix and silence anything whose target began with the same word. +pub fn init_logging() { + let not_an_fmu_trace = filter_fn(|metadata| { + metadata.target() != FMU_TRACE_CATEGORY || metadata.module_path() != Some(FMU_MODULE_PATH) + }); + + tracing_subscriber::registry() + .with(tracing_subscriber::fmt::layer().with_target(false)) + .with(LevelFilter::INFO) + .with(not_an_fmu_trace) + .init(); +} diff --git a/crates/continuo-examples/src/traffic_world.rs b/crates/continuo-examples/src/traffic_world.rs index 1e1352e..6e623aa 100644 --- a/crates/continuo-examples/src/traffic_world.rs +++ b/crates/continuo-examples/src/traffic_world.rs @@ -24,10 +24,11 @@ use std::sync::{Arc, Mutex, OnceLock}; +use continuo_actors::control_laws::{FREE_ROAD, IdmParams, PurePursuitParams}; use continuo_actors::{ - CarState, DespawnTrafficRequest, PathFollowController, PlantLimits, PoseLogger, - SpawnTrafficRequest, TrafficSpawner, UnicyclePhysics, Waypoints, road_pose, straight_road, - traffic_despawn_key, traffic_spawn_key, + CarState, DespawnTrafficRequest, PathFollowController, PlantLimits, PoseLogger, RadarSensor, + SpawnTrafficRequest, TrafficSpawner, UnicyclePhysics, Waypoints, accel_cmd_key, pose_key, + radar_key, road_pose, steer_cmd_key, straight_road, traffic_despawn_key, traffic_spawn_key, }; use continuo_conductor::record::LogEvent; use continuo_conductor::{ @@ -35,7 +36,10 @@ use continuo_conductor::{ Verifier, WORLD_LEVEL, }; use continuo_core::{Component, ComponentId, CoreError, Message, SimDuration, SimTime}; +use continuo_fmi::{FmuComponent, FmuConstructionError, FmuMapping, InputBinding, OutputBinding}; +use continuo_fmu_controller_idm::{MAX_WAYPOINTS, PackageError, packaged_fmu_path}; use continuo_transport::{MonitorTransport, Transport}; +use serde_json::json; /// One seed for the whole demo family: record, verify, and resim must all /// agree on it. @@ -79,6 +83,51 @@ const SPAWN_GAP: (f64, f64) = (20.0, 50.0); /// What a full command is worth on every car in this world. const CAR_LIMITS: PlantLimits = PlantLimits::highway_car(); +/// How often a car decides what to do: its radar scans and its controller +/// commands at this period, and its physics integrates ten times between. +const CONTROL_PERIOD: SimDuration = SimDuration::from_millis(100); +const PHYSICS_PERIOD: SimDuration = SimDuration::from_millis(10); + +/// How far ahead a following car's radar sees, in meters. Well past the +/// gap IDM keeps at highway speed, so a car brakes for a slower one long +/// before it would have to brake hard. +const RADAR_MAX_RANGE: f64 = 120.0; + +/// Half a lane either side, so a car counts as sharing the radar's lane +/// when it is nearer that lane's center than any other. +const RADAR_LANE_TOLERANCE: f64 = LANE_WIDTH / 2.0; + +/// How a car decides its speed. +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +enum Guidance { + /// Holds the speed it started at whatever is ahead, steering with + /// the native controller: `[controller, physics]`. + ConstantSpeed, + /// Follows the car ahead in its lane on the controller FMU, and wants + /// the speed it started at whenever the road ahead is free: + /// `[radar, controller, physics]`. + IdmFollowing, +} + +/// Why a traffic scenario could not be built or run. +#[derive(Debug, thiserror::Error)] +pub enum ScenarioError { + #[error(transparent)] + Conductor(#[from] ConductorError), + + /// Nobody packaged the controller FMU, so no following car can be + /// built. The message names the command that fixes it. + #[error(transparent)] + NotPackaged(#[from] PackageError), + + #[error("cannot build the controller FMU for {actor_name}: {source}")] + Controller { + actor_name: String, + #[source] + source: FmuConstructionError, + }, +} + /// A handle on the one road every car in this world drives, built once and /// handed out, not rebuilt per call, which is what keeps a run that spawns /// twenty cars from carrying twenty copies of the same geometry. @@ -111,49 +160,78 @@ pub fn config_paced(pacing: Pacing) -> ConductorConfig { } } -/// Registers one car as the composite `{actor_name} = [controller, -/// physics]`. Declared order matters: the controller is registered before -/// the physics, so its command reaches the physics same-instant when both -/// are due. +/// Registers one car as a composite under `actor_name`, guided as +/// `guidance` says, starting `start_s` along the road in the lane at +/// `lane_offset` and doing `speed`. +/// +/// Declared order matters. A following car registers its radar, then its +/// controller, then its physics, so at every control instant the scan +/// reaches the controller and the command reaches the physics within the +/// same instant. A constant-speed car has no radar and the same order +/// otherwise. /// -/// `speed` goes to the physics rather than the controller, because the -/// physics owns it. Nothing here commands an acceleration, so what the car -/// starts at is what it holds for the whole run. +/// `speed` goes to the physics, which owns it. A constant-speed car +/// commands no acceleration, so it holds that speed for the whole run; a +/// following car wants it back whenever the road ahead is free. /// -/// Controller and physics both take [`CAR_LIMITS`], since a normalized -/// command means whatever the plant says it means and a controller -/// working from a different set would command the wrong rate. +/// Every part takes [`CAR_LIMITS`], since a normalized command means +/// whatever the plant says it means and a controller working from a +/// different set would command the wrong rate. fn add_car( conductor: &mut Conductor, actor_name: &str, + guidance: Guidance, start_s: f64, lane_offset: f64, speed: f64, first_due: SimTime, -) -> Result<(), ConductorError> { +) -> Result<(), ScenarioError> { // A lane is the lateral offset the controller holds, not geometry of - // its own, so both components of every car work off the one road. + // its own, so every component of every car works off the one road. let road = shared_road(); - let initial_pose = road_pose(&road, start_s, lane_offset); - let components: [Box; 2] = [ - Box::new(PathFollowController::new( - actor_name, - road.clone(), - lane_offset, - SimDuration::from_millis(100), - 6.0, // lookahead, m - 1.5, // heading gain, 1/s - CAR_LIMITS.yaw_rate_max, // command turns up to the car's limit - CAR_LIMITS, - initial_pose, - )), - Box::new(UnicyclePhysics::new( - actor_name, - SimDuration::from_millis(10), - CAR_LIMITS, - CarState::new(initial_pose, speed), - )), - ]; + let initial = CarState::new(road_pose(&road, start_s, lane_offset), speed); + let physics = Box::new(UnicyclePhysics::new( + actor_name, + PHYSICS_PERIOD, + CAR_LIMITS, + initial, + )); + let components: Vec> = match guidance { + Guidance::ConstantSpeed => vec![ + Box::new(PathFollowController::new( + actor_name, + road, + lane_offset, + CONTROL_PERIOD, + 6.0, // lookahead, m + 1.5, // heading gain, 1/s + CAR_LIMITS.yaw_rate_max, // command turns up to the car's limit + CAR_LIMITS, + initial.pose(), + )), + physics, + ], + Guidance::IdmFollowing => { + let mapping = + controller_mapping(conductor.world_name(), actor_name, lane_offset, initial); + let controller = FmuComponent::new("controller", packaged_fmu_path()?, mapping) + .map_err(|source| ScenarioError::Controller { + actor_name: actor_name.to_string(), + source, + })?; + vec![ + Box::new(RadarSensor::new( + actor_name, + road, + CONTROL_PERIOD, + RADAR_MAX_RANGE, + RADAR_LANE_TOLERANCE, + )), + Box::new(controller), + physics, + ] + } + }; for component in components { conductor.add_component(JoinMetadata::at(actor_name, first_due), component)?; } @@ -162,6 +240,115 @@ fn add_car( Ok(()) } +/// The sheet wiring one following car's controller FMU into the world: +/// which messages feed its inputs, where its commands go, and the numbers +/// it is built with. +/// +/// The keys are concrete, which is why `world_name` is an argument. A +/// native component cannot do that, since its constructor does not know +/// the world and so it wildcards the world in its subscriptions; a +/// scenario does know it. +/// +/// Four kinds of thing, kept apart below: +/// +/// - The pose inputs read the car's own pose and name no pointers, since +/// `position.x` spells `/position/x` on its own. +/// - The radar inputs read the car's own scan through one pattern each. +/// The scan carries only the cars detected, so every slot past them +/// reads as [`FREE_ROAD`]. +/// - The parameters feed no message and are set once: the road, as the +/// points [`shared_road`] was built from, so the FMU and the radar +/// measure one geometry; the steering and following tunings; and the +/// plant's limits, which the FMU divides by. +/// - The outputs publish on the plant's two command keys, each payload +/// field named after the output variable, which is what the plant +/// decodes. +/// +/// `initial` also gives the pose inputs their starting values. At the +/// instant a car joins, its controller steps before its physics has +/// published anything, so without them it would steer from the origin and +/// accelerate from a standstill it is not in. It is the state the physics +/// is built from, so the two halves of a car cannot be handed different +/// ones. +fn controller_mapping( + world_name: &str, + actor_name: &str, + lane_offset: f64, + initial: CarState, +) -> FmuMapping { + let road = shared_road(); + 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 pursuit = PurePursuitParams::highway_car(lane_offset); + let idm = IdmParams::highway_car(initial.speed); + + let pose = pose_key(world_name, actor_name); + let radar = radar_key(world_name, actor_name); + let 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)), + ]; + let outputs = vec![ + OutputBinding::new("accel_cmd", accel_cmd_key(world_name, actor_name)), + OutputBinding::new("steer_cmd", steer_cmd_key(world_name, actor_name)), + ]; + let parameters = [ + ("road_x", json!(road_x)), + ("road_y", json!(road_y)), + ("road_point_count", json!(road.points().len())), + ("road_closed", json!(road.is_closed())), + ("lateral_tgt", json!(pursuit.lateral_tgt)), + ("lookahead", json!(pursuit.lookahead)), + ("gain_yaw_rate", json!(pursuit.gain_yaw_rate)), + ("max_yaw_rate", json!(pursuit.max_yaw_rate)), + ("v0_speed_tgt", json!(idm.v0_speed_tgt)), + ("t_headway", json!(idm.t_headway)), + ("s0_gap_min", json!(idm.s0_gap_min)), + ("a_accel_max", json!(idm.a_accel_max)), + ("b_decel_comfort", json!(idm.b_decel_comfort)), + ("plant_accel_max", json!(CAR_LIMITS.accel_max)), + ("plant_decel_max", json!(CAR_LIMITS.decel_max)), + ("plant_yaw_rate_max", json!(CAR_LIMITS.yaw_rate_max)), + ]; + let starting_inputs = [ + ("position.x", json!(initial.position.x)), + ("position.y", json!(initial.position.y)), + ("orientation.w", json!(initial.orientation.w)), + ("orientation.x", json!(initial.orientation.x)), + ("orientation.y", json!(initial.orientation.y)), + ("orientation.z", json!(initial.orientation.z)), + ("speed", json!(initial.speed)), + ]; + + // Return the sheet. It is data rather than code, so a scenario file + // could carry it once scenarios come from files. + FmuMapping { + period: CONTROL_PERIOD, + inputs, + outputs, + initial_values: parameters + .into_iter() + .chain(starting_inputs) + .map(|(name, value)| (name.to_string(), value)) + .collect(), + } +} + /// Registers the ego with the initial conditions given: starting `start_s` /// along the road, in the lane at `lane_offset`, holding `speed`. /// @@ -179,11 +366,12 @@ fn add_ego( start_s: f64, lane_offset: f64, speed: f64, -) -> Result<(), ConductorError> { +) -> Result<(), ScenarioError> { // Return once the ego is registered. add_car( conductor, EGO_NAME, + Guidance::ConstantSpeed, start_s, lane_offset, speed, @@ -227,7 +415,7 @@ fn add_spawner(conductor: &mut Conductor) -> Result<(), Conduct /// spawner, and cars played back from a log rather than chosen. pub fn setup_live_traffic_scenario( conductor: &mut Conductor, -) -> Result<(), ConductorError> { +) -> Result<(), ScenarioError> { add_ego(conductor, EGO_START, EGO_LANE, EGO_SPEED)?; add_spawner(conductor)?; add_logger(conductor)?; @@ -272,7 +460,7 @@ pub fn setup_scale_scenario( conductor: &mut Conductor, cars: usize, lanes: usize, -) -> Result<(), ConductorError> { +) -> Result<(), ScenarioError> { assert!(lanes > 0, "a world needs at least one lane"); for index in 0..cars { @@ -282,6 +470,7 @@ pub fn setup_scale_scenario( add_car( conductor, &format!("car{index}"), + Guidance::ConstantSpeed, start_s, lane_offset, speed, @@ -310,7 +499,7 @@ pub fn setup_playback_traffic_scenario( conductor: &mut Conductor, recorded: &EventLog, ego_speed: f64, -) -> Result { +) -> Result { add_ego(conductor, EGO_START, EGO_LANE, ego_speed)?; // Every playback double is registered now, even though most of their @@ -468,7 +657,7 @@ impl TrafficRequestHandler { /// calling this would run a different world from the recorded one, so /// the only way to drive this scenario is /// [`run_live_traffic_scenario`], which cannot forget. - fn apply(&self, conductor: &mut Conductor) -> Result<(), ConductorError> { + fn apply(&self, conductor: &mut Conductor) -> Result<(), ScenarioError> { // Before anything is applied, since a request that could not be read // means the collected ones are an incomplete account of what the sim // asked for. @@ -478,7 +667,7 @@ impl TrafficRequestHandler { .expect("request mutex is never poisoned") .take() { - return Err(ConductorError::Core(error)); + return Err(ConductorError::Core(error).into()); } let collected = std::mem::take( @@ -492,6 +681,7 @@ impl TrafficRequestHandler { Request::Spawn(spawn) => add_car( conductor, &spawn.actor_name, + Guidance::IdmFollowing, spawn.start_s, spawn.lane_offset, spawn.speed, @@ -504,7 +694,7 @@ impl TrafficRequestHandler { Request::Despawn(despawn) => { match conductor.remove_component_now(&despawn.actor_name) { Ok(()) | Err(ConductorError::UnknownPath(_)) => {} - Err(other) => return Err(other), + Err(other) => return Err(other.into()), } } } @@ -542,7 +732,7 @@ pub fn run_live_traffic_scenario( traffic_request_handler: &TrafficRequestHandler, end: SimTime, verifier: Option<&Verifier>, -) -> Result<(), ConductorError> { +) -> Result<(), ScenarioError> { // Once diverged, every later event is compared against a log the run // has already left behind, so the first difference is the only one // worth reporting. @@ -578,3 +768,44 @@ pub fn run_playback_traffic_scenario( // Return once the recorded schedule has been played out to `end`. conductor.run_until(end) } + +#[cfg(test)] +mod tests { + use continuo_actors::{AccelCmd, SteerCmd}; + use continuo_core::{KeyExpr, StepCtx}; + + use super::*; + + #[test] + fn the_controller_fmu_publishes_what_the_plant_decodes() { + // The FMU names its outputs and the plant names its payload fields, + // and nothing compiles one against the other. A car whose commands + // went to the wrong key would hold its speed in silence, so this + // checks the meeting point directly: one step of a following car's + // controller, read back the way the physics reads it. + let initial = CarState::new(road_pose(&shared_road(), 100.0, LANE_WIDTH), 20.0); + let mapping = controller_mapping(WORLD_NAME, "car", LANE_WIDTH, initial); + let mut controller = FmuComponent::new( + "controller", + packaged_fmu_path().unwrap_or_else(|missing| panic!("{missing}")), + mapping, + ) + .expect("the controller FMU builds"); + + let mut ctx = StepCtx::new(SimTime::ZERO, None, WORLD_NAME, 0, Vec::new()); + controller.step(&mut ctx).expect("the controller steps"); + let outbox = ctx.take_outbox(); + + let published = |key: KeyExpr| { + &outbox + .iter() + .find(|(published_key, _)| *published_key == key) + .unwrap_or_else(|| panic!("nothing was published on {key}")) + .1 + }; + serde_json::from_slice::(published(accel_cmd_key(WORLD_NAME, "car"))) + .expect("the plant decodes the acceleration command"); + serde_json::from_slice::(published(steer_cmd_key(WORLD_NAME, "car"))) + .expect("the plant decodes the steering command"); + } +} diff --git a/crates/continuo-examples/tests/highway.rs b/crates/continuo-examples/tests/highway.rs index feec082..49fe907 100644 --- a/crates/continuo-examples/tests/highway.rs +++ b/crates/continuo-examples/tests/highway.rs @@ -7,6 +7,9 @@ //! come from inside the sim, which is the part that has to stay //! deterministic. +use std::collections::BTreeMap; + +use continuo_actors::{CAR_LENGTH, CarState}; use continuo_conductor::record::LogEvent; use continuo_conductor::{Conductor, Divergence, EventLog, Recorder, Verifier}; use continuo_core::SimTime; @@ -91,9 +94,106 @@ fn traffic_arrives_and_retires_while_the_run_is_under_way() { ); } +/// Every car's published state, by the instant it was published and then +/// by actor, from a recorded log. +/// +/// Every physics steps on the same 10 ms grid, since every car joins on a +/// spawner instant, so grouping by instant lines up one state per car. +fn car_states(log: &EventLog) -> BTreeMap> { + let mut states: BTreeMap> = BTreeMap::new(); + for event in &log.events { + let LogEvent::Msg(message) = event else { + continue; + }; + let Some(actor) = message + .key + .strip_prefix("continuo/demo/actor/") + .and_then(|rest| rest.strip_suffix("/pose")) + else { + continue; + }; + let state: CarState = + serde_json::from_str(message.payload.get()).expect("a pose payload decodes"); + states + .entry(message.sim_time) + .or_default() + .insert(actor.to_string(), state); + } + + // Return the states, earliest instant first. + states +} + +#[test] +fn no_two_same_lane_cars_ever_overlap() { + // The demo's road runs along x, so x is the distance along it and y the + // offset across it. Two cars share a lane when they are nearer to each + // other's lane center than to any other lane's. + let log = record_highway(traffic_world::SIM_SECONDS); + let mut pairs_compared = 0; + for (instant, cars) in car_states(&log) { + let cars: Vec<(&String, &CarState)> = cars.iter().collect(); + for (index, (name, state)) in cars.iter().enumerate() { + for (other_name, other) in &cars[index + 1..] { + if (state.position.y - other.position.y).abs() >= 1.75 { + continue; + } + pairs_compared += 1; + let gap = (state.position.x - other.position.x).abs(); + assert!( + gap >= CAR_LENGTH, + "{name} and {other_name} overlap at {instant}: their origins are \ + {gap} m apart along the road, and a car is {CAR_LENGTH} m long" + ); + } + } + } + + // Without this the test would pass on a run whose lanes never held two + // cars at once, which proves nothing about following. + assert!( + pairs_compared > 1000, + "only {pairs_compared} same-lane pairs were compared" + ); +} + +#[test] +fn following_actually_engages_somewhere() { + // A traffic car on a free road wants the speed it spawned at and + // drifts from it by rounding alone, so a car whose speed moved by more + // than a meter per second was slowed by one ahead. Nobody slowing at all would mean the radar never + // reached the controller, and every car would be driving a free road + // forever while overlapping whatever it caught. + let log = record_highway(traffic_world::SIM_SECONDS); + let mut speed_ranges: BTreeMap = BTreeMap::new(); + for cars in car_states(&log).into_values() { + for (name, state) in cars { + if name.starts_with("traffic") { + let range = speed_ranges + .entry(name) + .or_insert((state.speed, state.speed)); + range.0 = range.0.min(state.speed); + range.1 = range.1.max(state.speed); + } + } + } + + let widest = speed_ranges + .iter() + .map(|(name, (low, high))| (name, high - low)) + .max_by(|a, b| a.1.total_cmp(&b.1)) + .expect("traffic drove"); + assert!( + widest.1 > 1.0, + "no traffic car changed speed by more than 1 m/s; the most was {} at {:.3} m/s", + widest.0, + widest.1 + ); +} + #[test] fn a_car_leaves_as_a_whole_actor() { - // Both halves of a composite go together, adjacent in the log, because + // Every part of a composite goes together, adjacent in the log, because // the driver removes the actor rather than its parts. let log = record_highway(traffic_world::SIM_SECONDS); let left = left(&log); @@ -104,10 +204,11 @@ fn a_car_leaves_as_a_whole_actor() { assert_eq!( pair, vec![ + &format!("{actor}/radar").as_str(), &format!("{actor}/controller").as_str(), &format!("{actor}/physics").as_str() ], - "one leave per leaf, controller before physics, in declaration order" + "one leave per leaf, radar then controller then physics, in declaration order" ); } @@ -136,12 +237,16 @@ fn the_dynamic_world_reproduces_exactly() { /// `cargo test` runs on every CI agent. /// /// There is something to catch. The hash reaches `sin`, `cos`, `asin`, and -/// `atan2` through the unicycle integration and the quaternion conversions, -/// and IEEE 754 requires correct rounding for `sqrt` but for none of those. +/// `atan2` through the unicycle integration, the quaternion conversions and +/// the steering law, and IEEE 754 requires correct rounding for `sqrt` but +/// for none of those. /// /// It moves when the scenario, the seed, or the hashing moves, each a -/// deliberate act. Update it and README.md's sample output together. -const DEMO_WORLD_HASH: u64 = 0xeccd_08f9_a316_bbbc; +/// deliberate act. Update it and README.md's sample output together. It +/// also moves when the packaged controller FMU is older than the laws it +/// was built from, since traffic drives on that copy rather than on this +/// build's, and `cargo xtask package-fmus` is the fix for that one. +const DEMO_WORLD_HASH: u64 = 0x580e_8d37_b14c_6abd; #[test] fn the_demo_world_hashes_to_the_same_value_on_every_platform() { diff --git a/crates/continuo-examples/tests/unreadable_request.rs b/crates/continuo-examples/tests/unreadable_request.rs index 52c217f..fc0f280 100644 --- a/crates/continuo-examples/tests/unreadable_request.rs +++ b/crates/continuo-examples/tests/unreadable_request.rs @@ -50,7 +50,7 @@ impl Component for Saboteur { } /// Runs two seconds of the demo, with or without the saboteur in the world. -fn run_demo(sabotaged: bool) -> Result<(), continuo_conductor::ConductorError> { +fn run_demo(sabotaged: bool) -> Result<(), traffic_world::ScenarioError> { let traffic_request_handler = TrafficRequestHandler::default(); let mut conductor = Conductor::new( traffic_world::config(), diff --git a/xtask/src/verify.rs b/xtask/src/verify.rs index 9dbaff5..1ff95e8 100644 --- a/xtask/src/verify.rs +++ b/xtask/src/verify.rs @@ -11,11 +11,14 @@ //! before a push, so what matters most about it is that it is fast enough to //! sit in an editing loop. //! -//! That is why it packages no FMUs and asks for no features, which -//! `cargo xtask verify-fmus` is for. Packaging costs a release build of the -//! FMU crate whenever a law changed, 13 seconds against 0.7 when nothing did, -//! and the tests it feeds say nothing without it, so the two belong with a -//! change that reaches a law rather than in an editing loop. +//! It does package the FMUs, because the demo's traffic drives on one and +//! neither its tests nor its smoke run can start without it. Packaging +//! costs a release build of the FMU crate whenever a law changed, 13 +//! seconds against 0.7 when nothing did, and leaving it out would cost +//! more: an edited law would leave the demo driving the copy packaged +//! before it, passing here and failing in CI. Validating the packages and +//! the tests comparing them with the laws stay in `cargo xtask verify-fmus`, +//! which asks for the feature those tests sit behind. use crate::task::{Progress, answers, run_command, workspace_root}; @@ -131,6 +134,15 @@ const VERIFY_COMMANDS: &[VerifyCommand] = &[ skip_unless: Some(RUFF_IS_INSTALLED), runs_tests: false, }, + // Before the tests, since the demo's traffic loads the controller FMU + // from the package and fails to build without one. + VerifyCommand { + argv: &["cargo", "xtask", "package-fmus"], + dir: Dir::Root, + env: &[], + skip_unless: None, + runs_tests: false, + }, VerifyCommand { argv: &["cargo", "test", "--workspace"], dir: Dir::Root, diff --git a/xtask/src/verify_fmus.rs b/xtask/src/verify_fmus.rs index 8fa1849..6bbb7a1 100644 --- a/xtask/src/verify_fmus.rs +++ b/xtask/src/verify_fmus.rs @@ -1,11 +1,12 @@ -//! The checks a change reaching an FMU crate needs, which `verify` leaves out. +//! The checks a change reaching an FMU crate needs on top of `verify`. //! -//! A `.fmu` carries its own compiled copy of everything it links, so the -//! tests against it read whatever was packaged last, and a law edited -//! without packaging again leaves the two disagreeing while -//! `cargo test --workspace` stays green. Packaging costs a release build of -//! the FMU crate, which is why it does not belong in a check meant to sit in -//! an editing loop and does belong here. +//! A `.fmu` carries its own compiled copy of everything it links. `verify` +//! packages them, since the demo drives on one, but nothing there asks +//! whether an archive is a valid FMU or whether its copy of the laws +//! answers as the laws do here. This does both: it validates each package +//! with fmpy, then runs the tests that compare a package with its laws. +//! Those sit behind a feature, and asking for it rebuilds much of the +//! graph, which is the cost that keeps them out of an editing loop. //! //! The tests ask for the one feature they sit behind rather than for all of //! them, because this task is about the FMUs. Asking for all of