From e44017d0773720dd4c0c5405d0aaf832b64681d9 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Tue, 11 Aug 2026 20:32:16 -1000 Subject: [PATCH 01/70] Move the steering law out of the component that publishes it Pure pursuit lived inside `PathFollowController::step`, where the only way to reach it was to run a component. Milestone 6's controller FMU runs the same law inside a shared library built from this crate, and a second copy of an equation is a second thing to get wrong, so it moves to a `control_laws` module: pure functions over their arguments, with no inbox, clock or component in the signature. `PurePursuitParams` names the four numbers the law is tuned by, and the controller holds one of those in place of four loose fields, so its struct now separates what steers from what it takes to be a component. The module is public rather than flattened into the crate root the way the rest of this crate is. It is the surface something outside this process compiles against, and a named module is what says which functions those are. It is also what makes splitting the laws into their own crate a file move, once a second FMU crate makes that worth doing. None of the arithmetic changed, the same operations in the same order, so the demo world hash is untouched and the existing pin says so. The law had no tests of its own before, being reachable only through a running world, and has five now: a car on its lane commands nothing, a car off it turns back, a lane offset is what gets held rather than the road, the clamp holds in both directions, and an error past half a turn steers the short way round. That last one is what `wrap_pi` is for, and it moved along with the law. Mirrored positions do not give exactly mirrored commands, which that test allows for rather than hides: a negative heading error comes back through `rem_euclid` and a positive one does not, so the two part company in the last bits. Co-authored-by: Claude Opus 5 (1M context) --- crates/continuo-actors/src/control_laws.rs | 159 +++++++++++++++++++++ crates/continuo-actors/src/controller.rs | 40 ++---- crates/continuo-actors/src/lib.rs | 5 + 3 files changed, 176 insertions(+), 28 deletions(-) create mode 100644 crates/continuo-actors/src/control_laws.rs diff --git a/crates/continuo-actors/src/control_laws.rs b/crates/continuo-actors/src/control_laws.rs new file mode 100644 index 0000000..22dc691 --- /dev/null +++ b/crates/continuo-actors/src/control_laws.rs @@ -0,0 +1,159 @@ +//! The control laws themselves: pure functions from what a car 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::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: f64, + /// How far along the road the aim point sits, in meters. + pub lookahead: f64, + /// Yaw rate commanded per radian of heading error, in 1/s. + pub gain: f64, + /// The most yaw rate to command in either direction, rad/s. + pub max_yaw_rate: f64, +} + +/// Yaw rate that steers a car at `pose` onto the lane `lateral` 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 car is allowed to turn. Holding a lane +/// needs no geometry of its own, because the aim point is offset from the +/// one road every car 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); + let desired_heading = f64::atan2(target.y - position.y, target.x - position.x); + let heading_error = wrap_pi(desired_heading - pose.orientation.yaw()); + + // Return the turn toward the aim point, inside the clamp. + (params.gain * heading_error).clamp(-params.max_yaw_rate, params.max_yaw_rate) +} + +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 + } +} + +#[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: 0.0, + lookahead: 10.0, + gain: 1.0, + max_yaw_rate: 1.0, + } + } + + #[test] + fn a_car_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_car_off_its_lane_turns_back_toward_it() { + // Left of the lane the aim point is to the right, so the command + // is clockwise, and a car as far the other side turns the other + // way by as much. Only as much to within rounding, since the + // negative error comes back through the wrap and the positive one + // does not. + 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}"); + let mirrored = from_the_left + from_the_right; + assert!(mirrored.abs() < 1e-12, "{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: 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: 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 car 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 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}"); + } +} diff --git a/crates/continuo-actors/src/controller.rs b/crates/continuo-actors/src/controller.rs index e857bfb..f0ff443 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: PurePursuitParams, last_pose: Pose, } @@ -55,28 +53,19 @@ impl PathFollowController { PathFollowController { actor_name: actor_name.into(), road, - lateral, period, speed, - lookahead, - gain, - max_yaw_rate, + pursuit: PurePursuitParams { + lateral, + lookahead, + gain, + 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), }; 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..3d96b7f 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; From cf7f8426cea4c762c684787aed7291253c6aeebd Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Wed, 12 Aug 2026 16:31:40 -1000 Subject: [PATCH 02/70] Write the laws for whatever obeys them, not for cars Pure pursuit turns anything that follows a path, so the module's prose should not narrow it to the one caller the demo happens to have. An FMU compiling these functions into a shared library makes that concrete: what crosses the boundary is a law and its arguments, with no car in sight. Co-authored-by: Claude Opus 5 (1M context) --- crates/continuo-actors/src/control_laws.rs | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/crates/continuo-actors/src/control_laws.rs b/crates/continuo-actors/src/control_laws.rs index 22dc691..11096b8 100644 --- a/crates/continuo-actors/src/control_laws.rs +++ b/crates/continuo-actors/src/control_laws.rs @@ -1,5 +1,5 @@ -//! The control laws themselves: pure functions from what a car knows to -//! what it commands. +//! 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 @@ -25,14 +25,14 @@ pub struct PurePursuitParams { pub max_yaw_rate: f64, } -/// Yaw rate that steers a car at `pose` onto the lane `lateral` meters -/// left of `road`, positive counter-clockwise. +/// Yaw rate that steers a follower at `pose` onto the lane `lateral` +/// 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 car is allowed to turn. Holding a lane -/// needs no geometry of its own, because the aim point is offset from the -/// one road every car on it shares. +/// 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); @@ -84,7 +84,7 @@ mod tests { } #[test] - fn a_car_on_its_lane_and_pointing_along_it_commands_no_turn() { + 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 @@ -92,9 +92,9 @@ mod tests { } #[test] - fn a_car_off_its_lane_turns_back_toward_it() { + 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 a car as far the other side turns the other + // is clockwise, and one as far the other side turns the other // way by as much. Only as much to within rounding, since the // negative error comes back through the wrap and the positive one // does not. @@ -132,7 +132,7 @@ mod tests { }; // Pointing across the road, where a quarter turn of error times a - // gain of ten asks for far more than the car may command. + // 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); From b18f97fceeff48b838c6a39e7142d217dcb247f4 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Wed, 12 Aug 2026 16:33:50 -1000 Subject: [PATCH 03/70] Name the pursuit parameters after the quantities they are A struct of tuning constants is read one field at a time, often far from the doc comment, so each name has to carry its own units and meaning. "lateral" alone says a direction rather than a quantity, and "gain" says nothing about what it scales in a law that could gain several things. "lateral_tgt" and "gain_yaw_rate" say the target offset and the yaw rate per radian of error. The field holding the set is "pursuit_params" for the same reason: it holds a parameter block, not a pursuit. Inert: renames only, and the pinned demo hash still passes. Co-authored-by: Claude Opus 5 (1M context) --- crates/continuo-actors/src/control_laws.rs | 20 ++++++++++---------- crates/continuo-actors/src/controller.rs | 14 +++++++------- 2 files changed, 17 insertions(+), 17 deletions(-) diff --git a/crates/continuo-actors/src/control_laws.rs b/crates/continuo-actors/src/control_laws.rs index 11096b8..c92f4fb 100644 --- a/crates/continuo-actors/src/control_laws.rs +++ b/crates/continuo-actors/src/control_laws.rs @@ -16,17 +16,17 @@ use crate::path::Waypoints; #[derive(Debug, Clone, Copy, PartialEq)] pub struct PurePursuitParams { /// Meters left of the road's centerline to hold, the Frenet `d`. - pub lateral: f64, + pub lateral_tgt: f64, /// How far along the road the aim point sits, in meters. pub lookahead: f64, /// Yaw rate commanded per radian of heading error, in 1/s. - pub gain: f64, + pub gain_yaw_rate: f64, /// The most yaw rate to command in either direction, rad/s. pub max_yaw_rate: f64, } -/// Yaw rate that steers a follower at `pose` onto the lane `lateral` -/// meters left of `road`, positive counter-clockwise. +/// 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 @@ -36,12 +36,12 @@ pub struct PurePursuitParams { 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); + 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_pi(desired_heading - pose.orientation.yaw()); // Return the turn toward the aim point, inside the clamp. - (params.gain * heading_error).clamp(-params.max_yaw_rate, params.max_yaw_rate) + (params.gain_yaw_rate * heading_error).clamp(-params.max_yaw_rate, params.max_yaw_rate) } fn wrap_pi(angle: f64) -> f64 { @@ -76,9 +76,9 @@ mod tests { /// Aims 10 m ahead at the road itself, turning a radian per radian. fn tuning() -> PurePursuitParams { PurePursuitParams { - lateral: 0.0, + lateral_tgt: 0.0, lookahead: 10.0, - gain: 1.0, + gain_yaw_rate: 1.0, max_yaw_rate: 1.0, } } @@ -108,7 +108,7 @@ mod tests { #[test] fn a_lane_offset_is_what_the_law_holds_rather_than_the_road() { let holding_a_lane = PurePursuitParams { - lateral: 3.0, + lateral_tgt: 3.0, ..tuning() }; @@ -126,7 +126,7 @@ mod tests { #[test] fn the_command_never_leaves_the_clamp() { let hard_and_capped = PurePursuitParams { - gain: 10.0, + gain_yaw_rate: 10.0, max_yaw_rate: 0.5, ..tuning() }; diff --git a/crates/continuo-actors/src/controller.rs b/crates/continuo-actors/src/controller.rs index f0ff443..867c345 100644 --- a/crates/continuo-actors/src/controller.rs +++ b/crates/continuo-actors/src/controller.rs @@ -33,7 +33,7 @@ pub struct PathFollowController { road: Arc, period: SimDuration, speed: f64, - pursuit: PurePursuitParams, + pursuit_params: PurePursuitParams, last_pose: Pose, } @@ -42,11 +42,11 @@ 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 { @@ -55,10 +55,10 @@ impl PathFollowController { road, period, speed, - pursuit: PurePursuitParams { - lateral, + pursuit_params: PurePursuitParams { + lateral_tgt, lookahead, - gain, + gain_yaw_rate, max_yaw_rate, }, last_pose: initial_pose, @@ -90,7 +90,7 @@ impl Component for PathFollowController { let cmd = Cmd { speed: self.speed, - yaw_rate: pure_pursuit_yaw_rate(&self.road, self.last_pose, self.pursuit), + 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); From 2e4b9479e6e5a46bc5166bff817b2d860b77a66f Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Wed, 12 Aug 2026 16:52:05 -1000 Subject: [PATCH 04/70] Wrap an angle by peeling its sign off first Folding a negative angle up into [0, TAU) and back down rounds it twice, where a positive angle is not rounded at all, so a mirrored pair of heading errors disagreed in the last bits. That is what the mirror test's tolerance was hiding: two followers the same distance either side of a lane steered back at slightly different rates. Wrapping the magnitude and putting the sign back sends both signs through one calculation, so the answer is exactly odd, and nothing in it rounds. A remainder is exact, the subtraction runs only when more than half a turn is left so its two sides are within a factor of two of each other, and 1 and -1 multiply exactly. Positive angles come back with the bits they had before. Taking off the nearest whole turn would be odd too, but its quotient rounds at the boundary, so an angle a hair under half a turn would come back a hair over it with the sign flipped. The name is wrap_to_pi now. An angle is what gets wrapped, and pi is what names the range it lands in. This is not inert. Over angles formed the way the law forms them, the two wraps disagree on about a quarter of inputs, by up to 4.44e-16 rad. The demo hash holds anyway, and not because that is small: the demo's road is straight and every car spawns exactly on its lane, so the heading error is identically zero and this code never sees an angle to wrap. A temporary assertion in the law proved that rather than leaving it inferred, which also means the pin guards nothing here and these unit tests are all the law has. All three tests touching the wrap fail against the old implementation. One of them nearly did not: an angle summed as -8.0 plus a multiple of the step has its low bits already clear and survives a fold up and back unchanged, so the sweep only discriminates once its angles are products. Co-authored-by: Claude Opus 5 (1M context) --- crates/continuo-actors/src/control_laws.rs | 74 +++++++++++++++++----- 1 file changed, 59 insertions(+), 15 deletions(-) diff --git a/crates/continuo-actors/src/control_laws.rs b/crates/continuo-actors/src/control_laws.rs index c92f4fb..7a03c6e 100644 --- a/crates/continuo-actors/src/control_laws.rs +++ b/crates/continuo-actors/src/control_laws.rs @@ -38,21 +38,37 @@ pub fn pure_pursuit_yaw_rate(road: &Waypoints, pose: Pose, params: PurePursuitPa 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_pi(desired_heading - pose.orientation.yaw()); + 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) } -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 +/// 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 { - wrapped - } + part_turn + }; + + // Return the short way round, pointing the way the angle did. + angle.signum() * short_way } #[cfg(test)] @@ -94,15 +110,14 @@ mod tests { #[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 as much. Only as much to within rounding, since the - // negative error comes back through the wrap and the positive one - // does not. + // 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}"); - let mirrored = from_the_left + from_the_right; - assert!(mirrored.abs() < 1e-12, "{from_the_left} {from_the_right}"); + assert_eq!(from_the_left, -from_the_right); } #[test] @@ -142,6 +157,35 @@ mod tests { 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 { From d197d40162842d1c4522064c133b48d6921f7d0f Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Wed, 12 Aug 2026 17:02:01 -1000 Subject: [PATCH 05/70] Let the follower pick its own lead out of a scan A sensor reports what it found in no particular order, because relevance is the consumer's idea and not the sensor's. Car-following means the nearest thing ahead, a learned successor may want the set sorted some other way or not sorted at all, so the choosing belongs with whoever is following. Two things fall out of standing for nothing detected with a range of 1e9 rather than with an absent value. A free road computes as an ordinary lead so far away that the room the law wants is nothing beside it, so no following law needs an empty case. And an array padded out to a fixed length reads correctly with no count beside it, since a padded slot loses to anything real. That is what the scan looks like once it has crossed into an FMU, where an array has one length and always that length. Ties go to the earlier slot. Which car gets followed matters less than that a scan answers the same way every time it is read, since the answer reaches the world hash. Co-authored-by: Claude Opus 5 (1M context) --- crates/continuo-actors/src/control_laws.rs | 87 ++++++++++++++++++++++ 1 file changed, 87 insertions(+) diff --git a/crates/continuo-actors/src/control_laws.rs b/crates/continuo-actors/src/control_laws.rs index 7a03c6e..e2b5364 100644 --- a/crates/continuo-actors/src/control_laws.rs +++ b/crates/continuo-actors/src/control_laws.rs @@ -71,6 +71,40 @@ fn wrap_to_pi(angle: f64) -> f64 { angle.signum() * short_way } +/// The range standing for nothing detected, in meters. +/// +/// Far enough that a following law computes the free road from it +/// without being told the road is free: the gap is so large that the +/// room the law wants is nothing beside it. So an empty scan, a slot no +/// sensor filled, and a lead a kilometer off are all one case, and there +/// is no emptiness to test for anywhere downstream. +pub const FREE_ROAD_RANGE: f64 = 1e9; + +/// The nearest detection in a scan, as `(range, range_rate)`, or the +/// 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. +/// +/// The two slices are read in step and a shorter one ends the scan. 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. A slot at +/// [`FREE_ROAD_RANGE`] loses to anything real, so a fixed-size array +/// padded out to its length needs no separate count of what is in it. +pub fn nearest_detection(ranges: &[f64], range_rates: &[f64]) -> (f64, f64) { + let mut nearest = (FREE_ROAD_RANGE, 0.0); + for (&range, &range_rate) in ranges.iter().zip(range_rates) { + if range < nearest.0 { + nearest = (range, range_rate); + } + } + + // Return the nearest, which is the free road until something beats it. + nearest +} + #[cfg(test)] mod tests { use continuo_core::{Quat, Vec3}; @@ -200,4 +234,57 @@ mod tests { 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() { + assert_eq!( + nearest_detection(&[40.0, 12.0, 90.0], &[-1.0, -3.0, 5.0]), + (12.0, -3.0) + ); + + // 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(&[12.0, 90.0, 40.0], &[-3.0, 5.0, -1.0]), + (12.0, -3.0) + ); + } + + #[test] + fn an_empty_scan_selects_the_free_road() { + assert_eq!(nearest_detection(&[], &[]), (FREE_ROAD_RANGE, 0.0)); + } + + #[test] + fn padding_never_beats_a_car() { + // A fixed-size array holding one car and nothing else, which is + // the shape a scan arrives in once it has crossed into an FMU. + let mut ranges = [FREE_ROAD_RANGE; 8]; + let mut range_rates = [0.0; 8]; + ranges[5] = 30.0; + range_rates[5] = -2.0; + assert_eq!(nearest_detection(&ranges, &range_rates), (30.0, -2.0)); + + // And one holding nothing is the free road, padding and all. + assert_eq!( + nearest_detection(&[FREE_ROAD_RANGE; 8], &[0.0; 8]), + (FREE_ROAD_RANGE, 0.0) + ); + } + + #[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. + assert_eq!(nearest_detection(&[25.0, 25.0], &[-4.0, 1.0]), (25.0, -4.0)); + } + + #[test] + fn a_scan_ends_where_the_shorter_of_its_two_slices_does() { + // Nothing publishes a mismatched pair. Reading past the end of + // one of them would be reading whichever detection last held + // that slot. + assert_eq!(nearest_detection(&[50.0, 5.0], &[-1.0]), (50.0, -1.0)); + } } From 72996f5f7055557c5290201eb5934274fefd0ab4 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Wed, 12 Aug 2026 17:05:13 -1000 Subject: [PATCH 06/70] Give the follower the law it follows with idm_accel is the Intelligent Driver Model: a gap and an approach rate in, an acceleration out. Both inputs are relative, which is what a radar measures. The lead's own speed appears nowhere in the law, so a Doppler measurement is the natural input rather than a concession. The room it asks for is a minimum gap, plus a headway of travel, plus enough to shed the approach rate against a brake between comfortable and hardest. An open road is the same expression rather than a case of its own, since at the free-road range the room asked for is nothing beside the room there is. Two of the six parameters are the values Treiber's own simulator lists for a car, the headway and the minimum gap. Its acceleration and braking are picked to bring stop-and-go waves out of a crowded road, which is a different thing to show than one car following another, so those two are this project's. So is the bound on braking, IDM having none, and so is the floor under the gap, which does nothing but keep a division finite for a pair that has already collided. The doc comments say which is which, because a parameter that reads as a knob is where a wiring mistake goes to hide. Add, multiply, divide and sqrt only, which IEEE 754 pins. The fourth power is two squarings rather than powi, whose expansion is the compiler's business rather than the standard's, and this law is headed for a world hash that four platforms have to agree on. The spot value is worked from the published equation by hand. Dropping the 2 from 2*sqrt(a*b), taking the exponent from four to two, and halving the headway each fail a test, so the equation is pinned and not merely exercised. Co-authored-by: Claude Opus 5 (1M context) --- crates/continuo-actors/src/control_laws.rs | 199 +++++++++++++++++++++ 1 file changed, 199 insertions(+) diff --git a/crates/continuo-actors/src/control_laws.rs b/crates/continuo-actors/src/control_laws.rs index e2b5364..76aebf2 100644 --- a/crates/continuo-actors/src/control_laws.rs +++ b/crates/continuo-actors/src/control_laws.rs @@ -105,6 +105,108 @@ pub fn nearest_detection(ranges: &[f64], range_rates: &[f64]) -> (f64, f64) { nearest } +/// The smallest gap [`idm_accel`] will divide by, in meters. +/// +/// Not IDM's. The published equation divides by the gap and says nothing +/// about a gap of nothing, so this is what keeps the answer a number. +/// What it is set to hardly matters: a run that reaches it has put two +/// bodies in one place already, which is a problem for something other +/// than the follower to report. +const GAP_FLOOR: f64 = 0.1; + +/// 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. +/// +/// Each field gives its name in the published equation, since that +/// equation is what the law here is written from. +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct IdmParams { + /// Speed held on an open road, m/s. IDM's `v0`. + pub speed_tgt: f64, + /// Seconds of gap wanted at whatever speed is being held. IDM's `T`. + pub time_headway: f64, + /// Meters of gap wanted at a standstill. IDM's `s0`. + pub gap_min: f64, + /// The most acceleration commanded, m/s^2. IDM's `a`. + pub accel_max: f64, + /// The braking taken as comfortable, m/s^2 and positive. IDM's `b`. + pub decel_comfort: f64, + /// The most braking commanded, m/s^2 and positive. Not IDM's, which + /// bounds braking nowhere. A law free to ask for anything is a poor + /// thing to put in front of an actuator. + pub decel_max: f64, +} + +impl IdmParams { + /// The set this project drives cars with, taking the speed each one + /// wants to hold. + /// + /// `time_headway` and `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 `accel_max` + /// and `decel_comfort` are this project's, within what ordinary + /// driving uses. So is `decel_max`, at firm braking rather than + /// emergency braking. + /// + /// None of them are knobs. A scenario misbehaving with these is a + /// reason to look at the wiring, because numbers fitted until a demo + /// looks right are where a wiring mistake goes to hide. + pub fn highway_car(speed_tgt: f64) -> Self { + IdmParams { + speed_tgt, + time_headway: 1.5, + gap_min: 2.0, + accel_max: 1.5, + decel_comfort: 2.0, + decel_max: 4.0, + } + } +} + +/// Acceleration the Intelligent Driver Model commands, m/s^2. +/// +/// `gap` is the room ahead in meters and `approach_rate` is how fast +/// that room is closing, positive when closing. Both are relative, which +/// is what a radar measures and all this wants: the lead's own speed +/// appears nowhere in the law. +/// +/// The room it asks for is `gap_min`, plus `time_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 +/// `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. +pub fn idm_accel(speed: f64, gap: f64, approach_rate: f64, params: IdmParams) -> f64 { + // A lead pulling away asks for no extra room at all, which is what + // the floor at zero says. Without it a fast enough retreat would + // talk the law into wanting less room than gap_min. + let closing_room = + speed * approach_rate / (2.0 * (params.accel_max * params.decel_comfort).sqrt()); + let gap_wanted = params.gap_min + (speed * params.time_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.speed_tgt; + let speed_ratio_squared = speed_ratio * speed_ratio; + let free_road = 1.0 - speed_ratio_squared * speed_ratio_squared; + let crowding = gap_wanted / gap.max(GAP_FLOOR); + + // Return what the road leaves once the lead has had its share. + (params.accel_max * (free_road - crowding * crowding)) + .clamp(-params.decel_max, params.accel_max) +} + #[cfg(test)] mod tests { use continuo_core::{Quat, Vec3}; @@ -280,6 +382,103 @@ mod tests { assert_eq!(nearest_detection(&[25.0, 25.0], &[-4.0, 1.0]), (25.0, -4.0)); } + /// A car wanting 30 m/s, tuned the way the demo tunes every car. + fn following() -> 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, following()), + following().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, following()); + assert!(accel.abs() < 1e-12, "{accel}"); + } + + #[test] + fn the_published_equation_gives_the_value_worked_by_hand() { + // Doing 20 behind a lead 40 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/40)^2) + // = -2.2695971038651734 + let accel = idm_accel(20.0, 40.0, 5.0, following()); + assert!((accel + 2.269_597_103_865_173_4).abs() < 1e-12, "{accel}"); + } + + #[test] + fn the_equilibrium_gap_commands_nothing() { + // Where IDM settles behind a lead holding a steady speed. It is + // further back than the room the law asks for, because the open + // road term is already short of full at 20 against a wanted 30, + // and the two only cancel where the gap makes up the difference. + let accel = idm_accel(20.0, 35.722_003_561_692_034, 0.0, following()); + assert!(accel.abs() < 1e-12, "{accel}"); + } + + #[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, following()), + -following().decel_max + ); + } + + #[test] + fn a_lead_pulling_away_never_buys_room_below_the_minimum() { + // The closing allowance floors at zero, so no retreat, however + // fast, talks the law into wanting less than gap_min and a + // headway. Two different retreats therefore answer alike. + let fast = idm_accel(20.0, 32.0, -1000.0, following()); + let faster = idm_accel(20.0, 32.0, -5000.0, following()); + assert_eq!(fast, faster); + assert!(fast > 0.0, "{fast}"); + } + + #[test] + fn the_gap_floor_keeps_a_touching_lead_finite() { + // 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, following()), + -following().decel_max + ); + assert_eq!( + idm_accel(20.0, -3.0, 0.0, following()), + -following().decel_max + ); + } + + #[test] + fn the_command_is_always_finite_and_inside_the_limits() { + let params = following(); + 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.accel_max + && accel >= -params.decel_max, + "{speed} {gap} {approach_rate} gave {accel}" + ); + } + } + } + } + #[test] fn a_scan_ends_where_the_shorter_of_its_two_slices_does() { // Nothing publishes a mismatched pair. Reading past the end of From 7a663ffb7f0233689d77906399f29f670e332fd9 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Wed, 12 Aug 2026 20:51:25 -1000 Subject: [PATCH 07/70] Keep a detection's two numbers in one value A range and a range rate describe one thing a sensor found, so passing them as two slices was the wrong shape twice over. It admitted a pair that cannot mean anything, a scan one range longer than its rates, with nothing to say about which half is wrong. And it made both numbers positional at every call, where a range arriving as a rate puts a lead somewhere it is not. A Detection carries them together, so neither mistake can be written down and nothing has to check for it or warn. The type is plain data that a sensor fills and a law reads, so it sits in core beside Pose rather than at either end. That also settles a dependency before it is load-bearing: the radar lands next to the laws in continuo-actors for now, and when the laws split into a crate of their own, neither has to reach for the other to name what passes between them. The free-road range keeps its value and loses a wrong claim: 1e9 meters is a million kilometers rather than the kilometer the doc said. What it is, is a number picked against two constraints, and the doc gives them now. Far enough that IDM reads it as an open road, which at this range costs under 2e-12 m/s^2. 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 take a car length off it or hand it to a 32-bit float. The approach rate's sign moves to the law whose convention it is. The paper defines the approaching rate as the follower's speed minus the lead's, so it is positive while the gap closes, and a detection measures the same quantity the other way up. Co-authored-by: Claude Opus 5 (1M context) --- crates/continuo-actors/src/control_laws.rs | 123 ++++++++++++--------- crates/continuo-core/src/lib.rs | 2 +- crates/continuo-core/src/messages.rs | 10 ++ 3 files changed, 83 insertions(+), 52 deletions(-) diff --git a/crates/continuo-actors/src/control_laws.rs b/crates/continuo-actors/src/control_laws.rs index 76aebf2..963936f 100644 --- a/crates/continuo-actors/src/control_laws.rs +++ b/crates/continuo-actors/src/control_laws.rs @@ -7,7 +7,7 @@ //! compiled into something else, rather than two that have to be kept in //! step by hand. -use continuo_core::Pose; +use continuo_core::{Detection, Pose}; use crate::path::Waypoints; @@ -73,31 +73,42 @@ fn wrap_to_pi(angle: f64) -> f64 { /// The range standing for nothing detected, in meters. /// -/// Far enough that a following law computes the free road from it -/// without being told the road is free: the gap is so large that the -/// room the law wants is nothing beside it. So an empty scan, a slot no -/// sensor filled, and a lead a kilometer off are all one case, and there -/// is no emptiness to test for anywhere downstream. +/// 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; -/// The nearest detection in a scan, as `(range, range_rate)`, or the -/// free road if the scan holds nothing nearer. +/// 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. /// -/// The two slices are read in step and a shorter one ends the scan. 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. A slot at -/// [`FREE_ROAD_RANGE`] loses to anything real, so a fixed-size array -/// padded out to its length needs no separate count of what is in it. -pub fn nearest_detection(ranges: &[f64], range_rates: &[f64]) -> (f64, f64) { - let mut nearest = (FREE_ROAD_RANGE, 0.0); - for (&range, &range_rate) in ranges.iter().zip(range_rates) { - if range < nearest.0 { - nearest = (range, range_rate); +/// 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; } } @@ -168,10 +179,15 @@ impl IdmParams { /// Acceleration the Intelligent Driver Model commands, m/s^2. /// -/// `gap` is the room ahead in meters and `approach_rate` is how fast -/// that room is closing, positive when closing. Both are relative, which -/// is what a radar measures and all this wants: the lead's own speed -/// appears nowhere in the law. +/// `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 `gap_min`, plus `time_headway` seconds of /// travel, plus enough to shed the approach rate against a brake between @@ -339,39 +355,44 @@ mod tests { #[test] fn the_nearest_detection_wins_regardless_of_its_slot() { - assert_eq!( - nearest_detection(&[40.0, 12.0, 90.0], &[-1.0, -3.0, 5.0]), - (12.0, -3.0) - ); + 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(&[12.0, 90.0, 40.0], &[-3.0, 5.0, -1.0]), - (12.0, -3.0) - ); + assert_eq!(nearest_detection(&[near, farthest, far]), near); } #[test] fn an_empty_scan_selects_the_free_road() { - assert_eq!(nearest_detection(&[], &[]), (FREE_ROAD_RANGE, 0.0)); + assert_eq!(nearest_detection(&[]), FREE_ROAD); } #[test] fn padding_never_beats_a_car() { - // A fixed-size array holding one car and nothing else, which is - // the shape a scan arrives in once it has crossed into an FMU. - let mut ranges = [FREE_ROAD_RANGE; 8]; - let mut range_rates = [0.0; 8]; - ranges[5] = 30.0; - range_rates[5] = -2.0; - assert_eq!(nearest_detection(&ranges, &range_rates), (30.0, -2.0)); + // 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_RANGE; 8], &[0.0; 8]), - (FREE_ROAD_RANGE, 0.0) - ); + assert_eq!(nearest_detection(&[FREE_ROAD; 8]), FREE_ROAD); } #[test] @@ -379,7 +400,15 @@ mod tests { // 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. - assert_eq!(nearest_detection(&[25.0, 25.0], &[-4.0, 1.0]), (25.0, -4.0)); + 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. @@ -478,12 +507,4 @@ mod tests { } } } - - #[test] - fn a_scan_ends_where_the_shorter_of_its_two_slices_does() { - // Nothing publishes a mismatched pair. Reading past the end of - // one of them would be reading whichever detection last held - // that slot. - assert_eq!(nearest_detection(&[50.0, 5.0], &[-1.0]), (50.0, -1.0)); - } } 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. /// From 7a65d17689d92c2b4a092d91196947613f63a978 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Wed, 12 Aug 2026 20:53:37 -1000 Subject: [PATCH 08/70] Carry the equation's own symbols in the parameter names A reader checking this law against the paper has to match six numbers to six symbols, and a doc comment saying which is which only helps where the doc comment is. Prefixing each name with the symbol carries that to every use: v0_speed_tgt is the paper's v0, and there is nothing to remember at the call site. It also marks the one parameter that is ours. Five names carry a symbol and decel_max does not, because the equation has no bound on braking, so the odd one out says so by its shape. s0 keeps "min" rather than becoming a target. It is the gap held at a standstill, a floor under everything else the law asks for, while the gap actually aimed at is s*, which moves with speed and closing rate. Calling s0 the target would take the name of the thing that is one. The line about the parameters not being knobs goes. Where they came from is already written above it, and what to do when a demo misbehaves is a matter for the scenario that misbehaves rather than for the struct. Co-authored-by: Claude Opus 5 (1M context) --- crates/continuo-actors/src/control_laws.rs | 73 +++++++++++----------- 1 file changed, 35 insertions(+), 38 deletions(-) diff --git a/crates/continuo-actors/src/control_laws.rs b/crates/continuo-actors/src/control_laws.rs index 963936f..dc5cd2b 100644 --- a/crates/continuo-actors/src/control_laws.rs +++ b/crates/continuo-actors/src/control_laws.rs @@ -128,22 +128,23 @@ const GAP_FLOOR: f64 = 0.1; /// 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. /// -/// Each field gives its name in the published equation, since that -/// equation is what the law here is written from. +/// Every parameter the published equation has carries its symbol, so +/// the two can be read side by side. The one without a symbol is the one +/// the equation does not have. #[derive(Debug, Clone, Copy, PartialEq)] pub struct IdmParams { - /// Speed held on an open road, m/s. IDM's `v0`. - pub speed_tgt: f64, - /// Seconds of gap wanted at whatever speed is being held. IDM's `T`. - pub time_headway: f64, - /// Meters of gap wanted at a standstill. IDM's `s0`. - pub gap_min: f64, - /// The most acceleration commanded, m/s^2. IDM's `a`. - pub accel_max: f64, - /// The braking taken as comfortable, m/s^2 and positive. IDM's `b`. - pub decel_comfort: f64, - /// The most braking commanded, m/s^2 and positive. Not IDM's, which - /// bounds braking nowhere. A law free to ask for anything is a poor + /// Speed held on an open road, m/s. + 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. + pub a_accel_max: f64, + /// The braking taken as comfortable, m/s^2 and positive. + pub b_decel_comfort: f64, + /// The most braking commanded, m/s^2 and positive. IDM bounds + /// braking nowhere, and a law free to ask for anything is a poor /// thing to put in front of an actuator. pub decel_max: f64, } @@ -152,26 +153,22 @@ impl IdmParams { /// The set this project drives cars with, taking the speed each one /// wants to hold. /// - /// `time_headway` and `gap_min` are the values Treiber's own + /// `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 `accel_max` - /// and `decel_comfort` are this project's, within what ordinary + /// 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. So is `decel_max`, at firm braking rather than /// emergency braking. - /// - /// None of them are knobs. A scenario misbehaving with these is a - /// reason to look at the wiring, because numbers fitted until a demo - /// looks right are where a wiring mistake goes to hide. - pub fn highway_car(speed_tgt: f64) -> Self { + pub fn highway_car(v0_speed_tgt: f64) -> Self { IdmParams { - speed_tgt, - time_headway: 1.5, - gap_min: 2.0, - accel_max: 1.5, - decel_comfort: 2.0, + v0_speed_tgt, + t_headway: 1.5, + s0_gap_min: 2.0, + a_accel_max: 1.5, + b_decel_comfort: 2.0, decel_max: 4.0, } } @@ -189,11 +186,11 @@ impl IdmParams { /// 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 `gap_min`, plus `time_headway` seconds of +/// 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 -/// `speed_tgt`. An open road is the same expression rather than a case +/// `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. /// @@ -204,23 +201,23 @@ impl IdmParams { pub fn idm_accel(speed: f64, gap: f64, approach_rate: f64, params: IdmParams) -> f64 { // A lead pulling away asks for no extra room at all, which is what // the floor at zero says. Without it a fast enough retreat would - // talk the law into wanting less room than gap_min. + // talk the law into wanting less room than s0_gap_min. let closing_room = - speed * approach_rate / (2.0 * (params.accel_max * params.decel_comfort).sqrt()); - let gap_wanted = params.gap_min + (speed * params.time_headway + closing_room).max(0.0); + 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.speed_tgt; + 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; let crowding = gap_wanted / gap.max(GAP_FLOOR); // Return what the road leaves once the lead has had its share. - (params.accel_max * (free_road - crowding * crowding)) - .clamp(-params.decel_max, params.accel_max) + (params.a_accel_max * (free_road - crowding * crowding)) + .clamp(-params.decel_max, params.a_accel_max) } #[cfg(test)] @@ -423,7 +420,7 @@ mod tests { // for is nothing against a free road. assert_eq!( idm_accel(0.0, FREE_ROAD_RANGE, 0.0, following()), - following().accel_max + following().a_accel_max ); } @@ -468,7 +465,7 @@ mod tests { #[test] fn a_lead_pulling_away_never_buys_room_below_the_minimum() { // The closing allowance floors at zero, so no retreat, however - // fast, talks the law into wanting less than gap_min and a + // fast, talks the law into wanting less than s0_gap_min and a // headway. Two different retreats therefore answer alike. let fast = idm_accel(20.0, 32.0, -1000.0, following()); let faster = idm_accel(20.0, 32.0, -5000.0, following()); @@ -499,7 +496,7 @@ mod tests { let accel = idm_accel(speed, gap, approach_rate, params); assert!( accel.is_finite() - && accel <= params.accel_max + && accel <= params.a_accel_max && accel >= -params.decel_max, "{speed} {gap} {approach_rate} gave {accel}" ); From 40bbf4e6ee33f2607aec2afed3bb93cf61c6b9b7 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Wed, 12 Aug 2026 20:55:19 -1000 Subject: [PATCH 09/70] Floor the gap at a number rather than at a distance A tenth of a meter is a real distance, close enough that two cars can be that far apart without having touched, so a law that quietly used it in place of what it was handed was answering a question nobody asked. The divisor is floored at the smallest positive double instead. That is not a distance and cannot be mistaken for one, and every gap a road produces now divides as it stands. What the floor is for is keeping a zero out of the divisor, since a wanted gap of zero over a gap of zero is a NaN that spreads into the physics, the hash and everything after. A gap of nothing gives an enormous quotient instead, and the clamp takes that to the braking limit. Nothing observable changes. Swept over speeds, closing rates and every gap from below zero up through a tenth of a meter, the two floors give the same answer at every point, because with these parameters anything that close is already braking as hard as it may. Co-authored-by: Claude Opus 5 (1M context) --- crates/continuo-actors/src/control_laws.rs | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/crates/continuo-actors/src/control_laws.rs b/crates/continuo-actors/src/control_laws.rs index dc5cd2b..c601125 100644 --- a/crates/continuo-actors/src/control_laws.rs +++ b/crates/continuo-actors/src/control_laws.rs @@ -116,15 +116,6 @@ pub fn nearest_detection(scan: &[Detection]) -> Detection { nearest } -/// The smallest gap [`idm_accel`] will divide by, in meters. -/// -/// Not IDM's. The published equation divides by the gap and says nothing -/// about a gap of nothing, so this is what keeps the answer a number. -/// What it is set to hardly matters: a run that reaches it has put two -/// bodies in one place already, which is a problem for something other -/// than the follower to report. -const GAP_FLOOR: f64 = 0.1; - /// 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. /// @@ -213,7 +204,12 @@ pub fn idm_accel(speed: f64, gap: f64, approach_rate: f64, params: IdmParams) -> 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; - let crowding = gap_wanted / gap.max(GAP_FLOOR); + // 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 what the road leaves once the lead has had its share. (params.a_accel_max * (free_road - crowding * crowding)) @@ -474,7 +470,7 @@ mod tests { } #[test] - fn the_gap_floor_keeps_a_touching_lead_finite() { + 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!( From 9e65fcfc621496c5e42a660889c8bbfb59d26ffb Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Wed, 12 Aug 2026 21:29:36 -1000 Subject: [PATCH 10/70] Keep the equation as published under one adaptation The law carried two departures from the published equation, both of the kind other implementations make. For an algorithm that is here to be representative rather than to drive well, one departure is better than two, and it should be one this project can justify on its own terms. The one that stays is that a wanted gap does not go below zero, stated of the whole expression rather than of the speed-dependent part inside it. Something has to say it. A lead pulling away contributes a negative closing term, and the equation as published lets that take the wanted gap negative, where it squares back into braking, so a car brakes as the road ahead clears. That is not an extreme case: at 20 m/s with 20 m of gap and a lead pulling away to 30, the published form commands -1.28 where the answer wanted is +1.20, and -9.98 if the lead reaches 35. A limit on the output cannot fix it, since -1.28 is an ordinary number to command and only its sign is wrong. The braking bound is no longer a parameter. The command is held inside the two rates the equation already names, so the parameter set is the published five exactly, and the law invents no number of its own. What that costs is the emergency stop: a car now brakes no harder than comfortably, which is a worse driver and a truer transcription. The spawn distances in the ego-lane work will have to account for it. The spot check moves from a 40 m gap to 60 m, since the tighter bound clamps the old one, and a clamped value would check the clamp rather than the equation. Co-authored-by: Claude Opus 5 (1M context) --- crates/continuo-actors/src/control_laws.rs | 89 +++++++++++++++------- 1 file changed, 60 insertions(+), 29 deletions(-) diff --git a/crates/continuo-actors/src/control_laws.rs b/crates/continuo-actors/src/control_laws.rs index c601125..6f0e7aa 100644 --- a/crates/continuo-actors/src/control_laws.rs +++ b/crates/continuo-actors/src/control_laws.rs @@ -119,9 +119,8 @@ pub fn nearest_detection(scan: &[Detection]) -> Detection { /// 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. /// -/// Every parameter the published equation has carries its symbol, so -/// the two can be read side by side. The one without a symbol is the one -/// the equation does not have. +/// These are the published equation's five and nothing else, each +/// carrying its symbol so the two can be read side by side. #[derive(Debug, Clone, Copy, PartialEq)] pub struct IdmParams { /// Speed held on an open road, m/s. @@ -132,12 +131,10 @@ pub struct IdmParams { pub s0_gap_min: f64, /// The most acceleration commanded, m/s^2. pub a_accel_max: f64, - /// The braking taken as comfortable, m/s^2 and positive. + /// The braking taken as comfortable, m/s^2 and positive. Also the + /// hardest [`idm_accel`] will ask for, which costs a car the + /// emergency stop it might otherwise make. pub b_decel_comfort: f64, - /// The most braking commanded, m/s^2 and positive. IDM bounds - /// braking nowhere, and a law free to ask for anything is a poor - /// thing to put in front of an actuator. - pub decel_max: f64, } impl IdmParams { @@ -151,8 +148,7 @@ impl IdmParams { /// 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. So is `decel_max`, at firm braking rather than - /// emergency braking. + /// driving uses. pub fn highway_car(v0_speed_tgt: f64) -> Self { IdmParams { v0_speed_tgt, @@ -160,7 +156,6 @@ impl IdmParams { s0_gap_min: 2.0, a_accel_max: 1.5, b_decel_comfort: 2.0, - decel_max: 4.0, } } } @@ -189,13 +184,29 @@ impl IdmParams { /// 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 { - // A lead pulling away asks for no extra room at all, which is what - // the floor at zero says. Without it a fast enough retreat would - // talk the law into wanting less room than s0_gap_min. + // 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); + 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 @@ -211,9 +222,18 @@ pub fn idm_accel(speed: f64, gap: f64, approach_rate: f64, params: IdmParams) -> // the braking limit, which is the answer a collided pair deserves. let crowding = gap_wanted / gap.max(f64::MIN_POSITIVE); - // Return what the road leaves once the lead has had its share. + // 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.decel_max, params.a_accel_max) + .clamp(-params.b_decel_comfort, params.a_accel_max) } #[cfg(test)] @@ -428,14 +448,18 @@ mod tests { #[test] fn the_published_equation_gives_the_value_worked_by_hand() { - // Doing 20 behind a lead 40 m off and closing at 5, worked from + // 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/40)^2) - // = -2.2695971038651734 - let accel = idm_accel(20.0, 40.0, 5.0, following()); - assert!((accel + 2.269_597_103_865_173_4).abs() < 1e-12, "{accel}"); + // 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, following()); + assert!((accel + 0.339_985_544_104_686_07).abs() < 1e-12, "{accel}"); } #[test] @@ -454,19 +478,26 @@ mod tests { // asks for hundreds, and the car commands what it has. assert_eq!( idm_accel(30.0, 5.0, 20.0, following()), - -following().decel_max + -following().b_decel_comfort ); } #[test] fn a_lead_pulling_away_never_buys_room_below_the_minimum() { - // The closing allowance floors at zero, so no retreat, however - // fast, talks the law into wanting less than s0_gap_min and a - // headway. Two different retreats therefore answer alike. + // 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, following()); let faster = idm_accel(20.0, 32.0, -5000.0, following()); 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, following()); + assert!(clearing > 1.0, "{clearing}"); } #[test] @@ -475,11 +506,11 @@ mod tests { // with them is only to stay a number. assert_eq!( idm_accel(20.0, 0.0, 0.0, following()), - -following().decel_max + -following().b_decel_comfort ); assert_eq!( idm_accel(20.0, -3.0, 0.0, following()), - -following().decel_max + -following().b_decel_comfort ); } @@ -493,7 +524,7 @@ mod tests { assert!( accel.is_finite() && accel <= params.a_accel_max - && accel >= -params.decel_max, + && accel >= -params.b_decel_comfort, "{speed} {gap} {approach_rate} gave {accel}" ); } From 84b7100ae44a4f645de16b315a3aed645e67430a Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Wed, 12 Aug 2026 21:31:48 -1000 Subject: [PATCH 11/70] Derive what the tests were asserting from constants The equilibrium test held a gap of 35.722003561692034 m with nothing to say where it came from, which is a number a reader can only take on trust. It is derived in the test now, from the same cancellation that defines it: the two terms balance when the crowding term equals the open-road term, so the gap is the wanted gap over that term's root. The derivation is the paper's algebra rather than a second call into the law, so it still checks something. A second case runs the same balance under the paper's own stated highway values rather than this project's, over the range of speeds it quotes them for, and pins the one number the paper states outright: from rest on an open road, a car accelerates at a. The paper tabulates parameters and plots figures, and puts no worked acceleration on the page, so its algebra with its numbers is as close to a reference condition as it offers. Checking against an independent implementation is a separate job and stays on the list. The tuning helper is idm_tuning, beside the pure-pursuit tuning it sits next to. "following" named the maneuver rather than the thing it returns. Co-authored-by: Claude Opus 5 (1M context) --- crates/continuo-actors/src/control_laws.rs | 88 ++++++++++++++++------ 1 file changed, 67 insertions(+), 21 deletions(-) diff --git a/crates/continuo-actors/src/control_laws.rs b/crates/continuo-actors/src/control_laws.rs index 6f0e7aa..93e2dc1 100644 --- a/crates/continuo-actors/src/control_laws.rs +++ b/crates/continuo-actors/src/control_laws.rs @@ -425,7 +425,7 @@ mod tests { } /// A car wanting 30 m/s, tuned the way the demo tunes every car. - fn following() -> IdmParams { + fn idm_tuning() -> IdmParams { IdmParams::highway_car(30.0) } @@ -435,14 +435,14 @@ mod tests { // 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, following()), - following().a_accel_max + 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, following()); + let accel = idm_accel(30.0, FREE_ROAD_RANGE, 0.0, idm_tuning()); assert!(accel.abs() < 1e-12, "{accel}"); } @@ -458,18 +458,64 @@ mod tests { // 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, following()); + 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. It is - // further back than the room the law asks for, because the open - // road term is already short of full at 20 against a wanted 30, - // and the two only cancel where the gap makes up the difference. - let accel = idm_accel(20.0, 35.722_003_561_692_034, 0.0, following()); - assert!(accel.abs() < 1e-12, "{accel}"); + // 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] @@ -477,8 +523,8 @@ mod tests { // 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, following()), - -following().b_decel_comfort + idm_accel(30.0, 5.0, 20.0, idm_tuning()), + -idm_tuning().b_decel_comfort ); } @@ -488,15 +534,15 @@ mod tests { // 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, following()); - let faster = idm_accel(20.0, 32.0, -5000.0, following()); + 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, following()); + let clearing = idm_accel(20.0, 20.0, -10.0, idm_tuning()); assert!(clearing > 1.0, "{clearing}"); } @@ -505,18 +551,18 @@ mod tests { // 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, following()), - -following().b_decel_comfort + 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, following()), - -following().b_decel_comfort + 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 = following(); + 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] { From e489735a19eb3fbaae1733ca48fa4c972630e3a1 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Thu, 13 Aug 2026 09:26:59 -1000 Subject: [PATCH 12/70] Let a road cross as the data it was built from A controller inside an FMU cannot be handed a `Waypoints`, so the road travels as values and is rebuilt on the far side. Every field is private and only two of them are inputs: the arc-length table is derived by the builders, which leaves the points and the closed flag as the whole of a path. The flag is the half that would go missing quietly. The demo's road is open, and a loop rebuilt as a road answers the same everywhere except off its ends, so nothing here would notice for a while. So the test compares by bits rather than by tolerance, over a bend and a closed square, sampling past both ends where the two kinds part company. That is also what would catch any future `Waypoints` state that its builders do not derive. Co-authored-by: Claude Opus 5 (1M context) --- crates/continuo-actors/src/path.rs | 83 ++++++++++++++++++++++++++++++ 1 file changed, 83 insertions(+) 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})" + ); + } + } + } + } } From 43f783e3012a6058c238d38cba087be6fd4403dd Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Thu, 13 Aug 2026 09:34:10 -1000 Subject: [PATCH 13/70] Settle how long the FMU's arrays may be The plan preferred a structural parameter sizing the road arrays, so the road would cross as exactly as many points as it has. `fmi-export` 0.3.0 cannot do it, which a probe against the crate says rather than its documentation: the causality is accepted and declared, but nothing joins one to a dimension, and the only path to a variable dimension is a `Vec` field that names value reference 0, the derive's own `time` variable, and implements neither of the get and set traits. So the road takes the fallback form, a fixed array with a count beside it, and the count is an ordinary parameter rather than a structural one, which would claim a role it does not have. Four smaller findings from the same probe are recorded beside it, since each is a compile error waiting for whoever writes the FMU crate: the exporting crate needs `fmi` itself as a dependency, `fmi-export` pulls bindgen in through it, a dotted `name` gives the pose inputs their structured names for free, and there is no way to declare a 64 bit integer. Co-authored-by: Claude Opus 5 (1M context) --- M6-PLAN.md | 46 +++++++++++++++++++++++++++++++++++----------- 1 file changed, 35 insertions(+), 11 deletions(-) diff --git a/M6-PLAN.md b/M6-PLAN.md index cad0dfe..7d57c89 100644 --- a/M6-PLAN.md +++ b/M6-PLAN.md @@ -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,9 +226,24 @@ 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). + 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. - Bundling: `cargo install cargo-fmi`, then `cargo fmi --package bundle` (wrapped by `cargo xtask bundle-fmus`) builds the cdylib itself, extracts @@ -879,13 +895,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**: From c1b57a868dd4dea1441152474cf094160ae73659 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Thu, 13 Aug 2026 10:10:08 -1000 Subject: [PATCH 14/70] Package the control laws as an FMU The laws already answer every caller alike, so what was missing was the packaging: a crate whose whole job is to say what crosses an FMI boundary and to translate either side of it. Nothing here decides anything. A pose comes back together out of seven scalars, a scan out of two arrays, and every number after that comes from `control_laws`, which is what makes the native controller and this copy agree by construction rather than by somebody keeping them in step. Two constants join `continuo-actors`, where the radar will read them too. Both are bounds rather than working numbers, and `MAX_WAYPOINTS` exists only because an array cannot be trimmed: the count beside it says where the road stops and the padding starts, since repeating the last point through the tail would hand `project` a segment of zero length. `IdmParams::highway_car` becomes a `const fn` so the declared start values name those numbers rather than copying them. The road is built at two places on purpose. `configurate` runs as initialization ends, when the parameters are final, but `calculate_values` can run before it in that same call and earlier still if a host reads an output during initialization, so it builds the road when it has none and `configurate` replaces what it built. The count is checked before `Waypoints` sees it, because a failed assertion unwinding through the C interface would take the host process with it, and a refusal carries the same information where the standard can hold it. Testing through the exported C interface would mean `unsafe`, which this workspace does not have, so the substance sits in two pure functions and the FMI-facing methods are wrappers over them. The arrays are declared from the constants themselves, so there is no second number to assert against; the hazard worth guarding is a stale bundle, which no compile-time check can see and which the importer already catches when a mapping's pointer count meets the declared dimension. Bundling and fmpy found what the Rust side did not. Both outputs declared a start value while being calculated, no initial unknowns were listed because the derive wants `initial = Calculated` spelled out, and every description was cut off at the first line of its doc comment. All three are fixed, and what upstream does is written down in M6-PLAN.md. The barrier is `python -m fmpy validate`, which exits nonzero on exactly these, and it lands with the CI bundling step that has somewhere to run it. The demo hash does not move, since nothing loads this yet. Co-authored-by: Claude Opus 5 (1M context) --- Cargo.lock | 195 +++++++ Cargo.toml | 4 + M6-PLAN.md | 15 + crates/continuo-actors/src/control_laws.rs | 2 +- crates/continuo-actors/src/lib.rs | 25 + crates/continuo-fmu-controller-idm/Cargo.toml | 32 + crates/continuo-fmu-controller-idm/src/lib.rs | 550 ++++++++++++++++++ 7 files changed, 822 insertions(+), 1 deletion(-) create mode 100644 crates/continuo-fmu-controller-idm/Cargo.toml create mode 100644 crates/continuo-fmu-controller-idm/src/lib.rs diff --git a/Cargo.lock b/Cargo.lock index 3443538..becf9b0 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,17 @@ dependencies = [ "tracing-subscriber", ] +[[package]] +name = "continuo-fmu-controller-idm" +version = "0.1.0" +dependencies = [ + "continuo-actors", + "continuo-core", + "fmi", + "fmi-export", + "thiserror 2.0.18", +] + [[package]] name = "continuo-transport" version = "0.1.0" @@ -460,6 +507,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 +730,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 +917,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 +1476,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 +1779,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 +2261,39 @@ dependencies = [ "toml_edit", ] +[[package]] +name = "proc-macro-error-attr2" +version = "2.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "96de42df36bb9bba5542fe9f1a054b8cc87e172759a1868aa05c1f3acc89dfc5" +dependencies = [ + "proc-macro2", + "quote", +] + +[[package]] +name = "proc-macro-error2" +version = "2.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "11ec05c52be0a07b08061f7dd003e7d7092e0472bc731b4af7bb1ef876109802" +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", ] diff --git a/Cargo.toml b/Cargo.toml index ba5cd9b..001d6ac 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -15,9 +15,13 @@ 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" diff --git a/M6-PLAN.md b/M6-PLAN.md index 7d57c89..4f05068 100644 --- a/M6-PLAN.md +++ b/M6-PLAN.md @@ -244,6 +244,21 @@ so nothing in M6 re-tests it per component. 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. - Bundling: `cargo install cargo-fmi`, then `cargo fmi --package bundle` (wrapped by `cargo xtask bundle-fmus`) builds the cdylib itself, extracts diff --git a/crates/continuo-actors/src/control_laws.rs b/crates/continuo-actors/src/control_laws.rs index 93e2dc1..f546ebd 100644 --- a/crates/continuo-actors/src/control_laws.rs +++ b/crates/continuo-actors/src/control_laws.rs @@ -149,7 +149,7 @@ impl IdmParams { /// 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 fn highway_car(v0_speed_tgt: f64) -> Self { + pub const fn highway_car(v0_speed_tgt: f64) -> Self { IdmParams { v0_speed_tgt, t_headway: 1.5, diff --git a/crates/continuo-actors/src/lib.rs b/crates/continuo-actors/src/lib.rs index 3d96b7f..c0302f9 100644 --- a/crates/continuo-actors/src/lib.rs +++ b/crates/continuo-actors/src/lib.rs @@ -42,6 +42,31 @@ pub use traffic_spawner::{ traffic_despawn_key, traffic_spawn_key, }; +/// How many detections a scan carries at most, and how long the arrays +/// are that carry one into an FMU. +/// +/// A bound rather than a working number. A scan reaches about 120 m down +/// one lane, which holds 26 cars nose to tail at four and a half meters +/// each, and moving traffic leaves gaps, so no world this project runs +/// gets near it. Nothing pays for the headroom on the wire, since a scan +/// carries only what was found and the padding out to a fixed length +/// happens inside the FMU, where a slot past the end holds the free road. +pub const MAX_DETECTIONS: usize = 64; + +/// How many points a road may have to cross into an FMU, which declares +/// arrays this long and a count of how many it filled. +/// +/// The count exists because the array cannot be trimmed: `fmi-export` +/// 0.3.0 has no way to size one by a parameter. Repeating the last point +/// through the tail instead would hand [`Waypoints::project`] a segment +/// of zero length, so the padding has to be ignored rather than +/// interpreted, and something has to say where it starts. +/// +/// 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; + /// 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-fmu-controller-idm/Cargo.toml b/crates/continuo-fmu-controller-idm/Cargo.toml new file mode 100644 index 0000000..69a9378 --- /dev/null +++ b/crates/continuo-fmu-controller-idm/Cargo.toml @@ -0,0 +1,32 @@ +[package] +name = "continuo-fmu-controller-idm" +description = "The demo's car controller, exported as an FMI 3.0 Co-Simulation FMU" +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 bundled .fmu rather than the crate. +publish = false + +# cdylib is what FMI loads; rlib is what `bundled_fmu_path` and the golden +# tests link against. Neither can be feature-gated, since `cargo fmi bundle` +# builds with default features. +[lib] +crate-type = ["cdylib", "rlib"] + +[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 } + +# `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. +[lints.rust] +unexpected_cfgs = { level = "warn", check-cfg = ['cfg(coverage_nightly)'] } 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..458cfed --- /dev/null +++ b/crates/continuo-fmu-controller-idm/src/lib.rs @@ -0,0 +1,550 @@ +//! The demo's car controller, exported as an FMI 3.0 Co-Simulation FMU. +//! +//! Nothing here decides anything. The interface declaration below says +//! 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 rebundling therefore +//! leaves this copy behind, and `cargo xtask bundle-fmus` is what puts it +//! back. + +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, MAX_WAYPOINTS, Waypoints}; +use continuo_core::{Detection, Pose, Quat, Vec3}; +use fmi::fmi3::{Fmi3Error, Fmi3Res}; +use fmi_export::fmi3::{Context, DefaultLoggingCategory, UserModel}; +use fmi_export::{FmuModel, export_fmu}; + +/// The file `cargo xtask bundle-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"; + +/// What the following parameters hold until a host sets them. +/// +/// A host always does: the scenario knows how fast a car means to go, and +/// which lane it holds, where this crate cannot. Three of these must be +/// nonzero whatever happens, since the equation divides by the target +/// speed and by the root of the two rates, so they are named here rather +/// than left at zero. +const DEFAULT_IDM: IdmParams = IdmParams::highway_car(30.0); + +/// What the steering parameters hold until a host sets them: the road's +/// own centerline, aimed a few car lengths ahead. +const DEFAULT_PURSUIT: PurePursuitParams = PurePursuitParams { + lateral_tgt: 0.0, + lookahead: 6.0, + gain_yaw_rate: 1.5, + max_yaw_rate: 1.2, +}; + +/// A car controller: IDM for how fast to go, pure pursuit for where to +/// point, from one pose and one radar scan. +/// +/// Each variable's **first doc line is its FMI description**, and the +/// bundler 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. +#[derive(FmuModel, Debug)] +#[model(co_simulation = true, model_exchange = false, user_model = false)] +pub struct IdmController { + /// Where the car is, meters east. + #[variable(causality = Input, name = "position.x", start = 0.0)] + position_x: f64, + /// Where the car is, meters north. + #[variable(causality = Input, name = "position.y", start = 0.0)] + position_y: f64, + /// Which way the car points: the quaternion's scalar part. + #[variable(causality = Input, name = "orientation.w", start = 1.0)] + orientation_w: f64, + /// Which way the car points: the quaternion's x part. + #[variable(causality = Input, name = "orientation.x", start = 0.0)] + orientation_x: f64, + /// Which way the car points: the quaternion's y part. + #[variable(causality = Input, name = "orientation.y", start = 0.0)] + orientation_y: f64, + /// Which way the car points: the quaternion's z part. + #[variable(causality = Input, name = "orientation.z", start = 0.0)] + 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 = 0.0)] + 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 = [FREE_ROAD.range; MAX_DETECTIONS])] + range: [f64; MAX_DETECTIONS], + /// How fast each range is changing, m/s, negative while closing. + #[variable(causality = Input, start = [FREE_ROAD.range_rate; MAX_DETECTIONS])] + range_rate: [f64; MAX_DETECTIONS], + + /// The road's points, meters east, the first road_point_count real. + #[variable(causality = Parameter, variability = Fixed, start = [0.0; MAX_WAYPOINTS])] + road_x: [f64; MAX_WAYPOINTS], + /// The road's points, meters north, the first road_point_count real. + #[variable(causality = Parameter, variability = Fixed, start = [0.0; MAX_WAYPOINTS])] + road_y: [f64; MAX_WAYPOINTS], + /// How many of those points are the road, the rest being padding. + #[variable(causality = Parameter, variability = Fixed, start = 2u32)] + 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 = false)] + road_closed: bool, + + /// Meters left of the centerline to hold, which is what makes a lane. + #[variable(causality = Parameter, variability = Fixed, start = DEFAULT_PURSUIT.lateral_tgt)] + lateral_tgt: f64, + /// How far along the road to aim, in meters. + #[variable(causality = Parameter, variability = Fixed, start = DEFAULT_PURSUIT.lookahead)] + lookahead: f64, + /// Yaw rate per radian of heading error, in 1/s. + #[variable(causality = Parameter, variability = Fixed, start = DEFAULT_PURSUIT.gain_yaw_rate)] + gain_yaw_rate: f64, + /// The most yaw rate to command either way, rad/s. + #[variable(causality = Parameter, variability = Fixed, start = DEFAULT_PURSUIT.max_yaw_rate)] + max_yaw_rate: f64, + + /// Speed held on an open road, m/s. + #[variable(causality = Parameter, variability = Fixed, start = DEFAULT_IDM.v0_speed_tgt)] + v0_speed_tgt: f64, + /// Seconds of gap wanted at whatever speed is held. + #[variable(causality = Parameter, variability = Fixed, start = DEFAULT_IDM.t_headway)] + t_headway: f64, + /// Meters of gap wanted at a standstill. + #[variable(causality = Parameter, variability = Fixed, start = DEFAULT_IDM.s0_gap_min)] + s0_gap_min: f64, + /// The most acceleration commanded, m/s^2. + #[variable(causality = Parameter, variability = Fixed, start = DEFAULT_IDM.a_accel_max)] + a_accel_max: f64, + /// Comfortable braking, m/s^2 and positive, the hardest commanded. + #[variable(causality = Parameter, variability = Fixed, start = DEFAULT_IDM.b_decel_comfort)] + b_decel_comfort: f64, + + /// Acceleration to hold, m/s^2. + /// + /// Calculated rather than started, since it is worked out from the + /// inputs. Saying so is what lists it among the initial unknowns, and + /// a start value beside it would be a second answer to the same + /// question, which 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 IdmController { + /// The same values the variables declare as their starts. + /// + /// Both are needed and neither is the other's copy: the declared + /// starts are what a host reads out of the model description, and + /// these are what the struct holds before anything sets it. Naming + /// the constants twice is what keeps them one set of numbers. + fn default() -> Self { + IdmController { + 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, + } + } +} + +/// A `road_point_count` no road could have: what arrived, and the fewest +/// that would have worked. +#[derive(Debug, Clone, Copy, PartialEq)] +struct BadPointCount { + given: usize, + least: usize, +} + +impl IdmController { + /// 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. + fn road_from_parameters(&self) -> Result { + let given = self.road_point_count as usize; + let least = if self.road_closed { 3 } else { 2 }; + if given < least || given > MAX_WAYPOINTS { + return Err(BadPointCount { given, least }); + } + let points: Vec<(f64, f64)> = (0..given) + .map(|i| (self.road_x[i], self.road_y[i])) + .collect(); + + // 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(if self.road_closed { + Waypoints::build_closed(points) + } else { + Waypoints::build_open(points) + }) + } + + /// 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| { + context.log( + Fmi3Error::Error.into(), + DefaultLoggingCategory::default(), + format_args!( + "road_point_count is {}, and a {} road needs {} to {MAX_WAYPOINTS}", + bad.given, + if self.road_closed { "closed" } else { "open" }, + bad.least, + ), + ); + + Fmi3Error::Error + }) + } + + /// 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 commands(&self, road: &Waypoints) -> (f64, f64) { + 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, + PurePursuitParams { + lateral_tgt: self.lateral_tgt, + lookahead: self.lookahead, + gain_yaw_rate: self.gain_yaw_rate, + max_yaw_rate: self.max_yaw_rate, + }, + ); + + // 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, + IdmParams { + v0_speed_tgt: self.v0_speed_tgt, + t_headway: self.t_headway, + s0_gap_min: self.s0_gap_min, + a_accel_max: self.a_accel_max, + b_decel_comfort: self.b_decel_comfort, + }, + ); + + // Return what to hold: how hard to push, and how fast to turn. + (accel_cmd, yaw_rate_cmd) + } +} + +impl UserModel for IdmController { + type LoggingCategory = DefaultLoggingCategory; + + /// Builds the road, at the point where the parameters are final. + /// + /// This runs as initialization ends, so what it reads is what the + /// host meant. It replaces whatever `calculate_values` may have built + /// already, which is what makes a road built from half-set parameters + /// harmless rather than something to avoid. + fn configurate(&mut self, context: &dyn Context) -> Result<(), Fmi3Error> { + self.road = Some(self.build_road(context)?); + + Ok(()) + } + + /// Both commands, from one pose and one scan. + /// + /// It builds the road if it has none, because this can run before + /// `configurate` does: leaving initialization recomputes whatever was + /// dirtied first, and a host reading an output before that gets here + /// earlier still. + fn calculate_values(&mut self, context: &dyn Context) -> Result { + if self.road.is_none() { + self.road = Some(self.build_road(context)?); + } + let road = self.road.as_ref().expect("the road was just built"); + let (accel_cmd, yaw_rate_cmd) = self.commands(road); + + self.accel_cmd = accel_cmd; + self.yaw_rate_cmd = yaw_rate_cmd; + + Ok(Fmi3Res::OK) + } +} + +export_fmu!(IdmController); + +/// Why the bundled FMU could not be found. +#[derive(Debug, thiserror::Error)] +pub enum BundleError { + #[error("cannot find this test's own executable: {0}")] + CurrentExe(#[source] std::io::Error), + + #[error( + "no fmu/{FMU_FILE_NAME} above {}: run `cargo install cargo-fmi` once, \ + then `cargo xtask bundle-fmus`", + searched_from.display() + )] + NotBundled { searched_from: PathBuf }, +} + +/// Where `cargo xtask bundle-fmus` left the packaged FMU. +/// +/// Found by walking up from the running executable rather than from a +/// build-time path, so it works the same from a test binary, an example +/// and a tool, whichever profile built them. +pub fn bundled_fmu_path() -> Result { + let exe = std::env::current_exe().map_err(BundleError::CurrentExe)?; + for directory in exe.ancestors() { + let candidate = directory.join("fmu").join(FMU_FILE_NAME); + if candidate.is_file() { + return Ok(candidate); + } + } + + // Return the failure carrying the command that fixes it, since a + // missing bundle is a step not run rather than anything broken. + Err(BundleError::NotBundled { searched_from: exe }) +} + +#[cfg(test)] +mod tests { + use super::*; + + /// A controller carrying `road` as its parameters, the way a host + /// hands one across. + fn carrying(road: &Waypoints) -> IdmController { + let mut controller = IdmController { + road_point_count: road.points().len() as u32, + road_closed: road.is_closed(), + ..IdmController::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 + } + + /// Every answer a road gives, as the bits it gives them in. + fn answers(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 arrived = carrying(&sent) + .road_from_parameters() + .expect("the count is the road's own"); + assert_eq!(arrived.is_closed(), sent.is_closed()); + assert_eq!(answers(&arrived), answers(&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 = carrying(&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 arrived = controller + .road_from_parameters() + .expect("two points are enough for an open road"); + assert_eq!(arrived.total_length(), road.total_length()); + assert_eq!(answers(&arrived), answers(&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 = carrying(&straight); + too_few.road_point_count = 1; + assert_eq!( + too_few.road_from_parameters().unwrap_err(), + BadPointCount { given: 1, least: 2 } + ); + + // Two points close into nothing, so a loop needs three. + let mut not_a_loop = carrying(&straight); + not_a_loop.road_closed = true; + assert_eq!( + not_a_loop.road_from_parameters().unwrap_err(), + BadPointCount { given: 2, least: 3 } + ); + + // More points than arrived would read past what the host sent. + let mut too_many = carrying(&straight); + too_many.road_point_count = MAX_WAYPOINTS as u32 + 1; + assert_eq!( + too_many.road_from_parameters().unwrap_err(), + BadPointCount { + given: MAX_WAYPOINTS + 1, + least: 2, + } + ); + } + + #[test] + fn the_commands_are_what_the_laws_answer_to_the_bit() { + let road = Waypoints::build_open(vec![(0.0, 0.0), (60.0, 0.0), (110.0, 40.0)]); + let mut controller = carrying(&road); + controller.position_x = 25.0; + controller.position_y = 1.5; + controller.orientation_z = 0.1; + controller.orientation_w = 0.9; + controller.speed = 22.0; + controller.lateral_tgt = 3.5; + // A lead 40 m off and closing, in a slot nowhere near the front, + // so picking it out is part of what is being checked. + controller.range[37] = 40.0; + controller.range_rate[37] = -4.0; + + let (accel_cmd, yaw_rate_cmd) = controller.commands(&road); + + let pose = Pose { + position: Vec3::new(25.0, 1.5, 0.0), + orientation: Quat { + w: 0.9, + x: 0.0, + y: 0.0, + z: 0.1, + }, + }; + let expected_yaw_rate = pure_pursuit_yaw_rate( + &road, + pose, + PurePursuitParams { + lateral_tgt: 3.5, + lookahead: DEFAULT_PURSUIT.lookahead, + gain_yaw_rate: DEFAULT_PURSUIT.gain_yaw_rate, + max_yaw_rate: DEFAULT_PURSUIT.max_yaw_rate, + }, + ); + let expected_accel = idm_accel(22.0, 40.0, 4.0, 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 at 4 m/s must not read as one pulling away. + assert!( + accel_cmd < idm_accel(22.0, 40.0, -4.0, DEFAULT_IDM), + "a closing lead should be braked for harder than a receding one" + ); + } + + #[test] + fn an_empty_scan_drives_the_open_road() { + let road = Waypoints::build_straight((0.0, 0.0), (500.0, 0.0)); + let mut controller = carrying(&road); + controller.speed = 10.0; + + let (accel_cmd, _) = controller.commands(&road); + + // 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(10.0, FREE_ROAD.range, 0.0, DEFAULT_IDM).to_bits() + ); + assert!(accel_cmd > 0.0, "an open road below v0 should accelerate"); + } +} From 39821a0d63101c43973afd0749e978971d649d73 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Thu, 13 Aug 2026 15:58:46 -1000 Subject: [PATCH 15/70] Keep each array bound where its reason lives `MAX_WAYPOINTS` was never about roads. It exists because `fmi-export` 0.3.0 cannot size an array by a parameter, which is a fact about one interface, and the FMU is its only user. So it moves into the crate that declares the array, where the reason for it is in front of whoever reads it. `MAX_DETECTIONS` stays in actors, since the radar reads it too, but its doc stops arguing for the number. The lane length and the car count were worked backward from a sensor range nothing has settled, which reads as evidence when it is guesswork. What can be said now is that it is a placeholder for demo development, picked high enough that truncation is not a question yet. Co-authored-by: Claude Opus 5 (1M context) --- crates/continuo-actors/src/lib.rs | 30 +++++-------------- crates/continuo-fmu-controller-idm/src/lib.rs | 16 +++++++++- 2 files changed, 23 insertions(+), 23 deletions(-) diff --git a/crates/continuo-actors/src/lib.rs b/crates/continuo-actors/src/lib.rs index c0302f9..021d3a3 100644 --- a/crates/continuo-actors/src/lib.rs +++ b/crates/continuo-actors/src/lib.rs @@ -42,30 +42,16 @@ pub use traffic_spawner::{ traffic_despawn_key, traffic_spawn_key, }; -/// How many detections a scan carries at most, and how long the arrays -/// are that carry one into an FMU. +/// How many detections a scan carries at most. /// -/// A bound rather than a working number. A scan reaches about 120 m down -/// one lane, which holds 26 cars nose to tail at four and a half meters -/// each, and moving traffic leaves gaps, so no world this project runs -/// gets near it. Nothing pays for the headroom on the wire, since a scan -/// carries only what was found and the padding out to a fixed length -/// happens inside the FMU, where a slot past the end holds the free road. -pub const MAX_DETECTIONS: usize = 64; - -/// How many points a road may have to cross into an FMU, which declares -/// arrays this long and a count of how many it filled. +/// 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. /// -/// The count exists because the array cannot be trimmed: `fmi-export` -/// 0.3.0 has no way to size one by a parameter. Repeating the last point -/// through the tail instead would hand [`Waypoints::project`] a segment -/// of zero length, so the padding has to be ignored rather than -/// interpreted, and something has to say where it starts. -/// -/// 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; +/// 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 { diff --git a/crates/continuo-fmu-controller-idm/src/lib.rs b/crates/continuo-fmu-controller-idm/src/lib.rs index 458cfed..843adfc 100644 --- a/crates/continuo-fmu-controller-idm/src/lib.rs +++ b/crates/continuo-fmu-controller-idm/src/lib.rs @@ -24,7 +24,7 @@ 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, MAX_WAYPOINTS, Waypoints}; +use continuo_actors::{MAX_DETECTIONS, Waypoints}; use continuo_core::{Detection, Pose, Quat, Vec3}; use fmi::fmi3::{Fmi3Error, Fmi3Res}; use fmi_export::fmi3::{Context, DefaultLoggingCategory, UserModel}; @@ -34,6 +34,20 @@ use fmi_export::{FmuModel, export_fmu}; /// because FMI takes its model identifier from the shared library. pub const FMU_FILE_NAME: &str = "continuo_fmu_controller_idm.fmu"; +/// 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; + /// What the following parameters hold until a host sets them. /// /// A host always does: the scenario knows how fast a car means to go, and From fb04dcf1f9c7d928ed57aedd2be39ca20948a02f Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Thu, 13 Aug 2026 16:06:35 -1000 Subject: [PATCH 16/70] Name the steering tuning set beside the following one `IdmParams::highway_car` already said what this project drives cars with, so the steering half sitting as four literals in the FMU was the odd one out. `PurePursuitParams::highway_car` puts it next to its counterpart, which leaves the FMU picking exactly one number of its own, the speed a car holds when nobody says otherwise. Where a parameter has to be positive now says so on the parameter, since three of IDM's five are divisors and a zero among them arrives as a NaN rather than as a wrong number, which then spreads into every command the car makes. Saying it on the struct serves whoever builds one in Rust. It does nothing for whoever loads the packaged FMU in another tool, and the next commit is what answers them. Co-authored-by: Claude Opus 5 (1M context) --- crates/continuo-actors/src/control_laws.rs | 34 ++++++++++++++++--- crates/continuo-fmu-controller-idm/src/lib.rs | 28 +++++++-------- 2 files changed, 41 insertions(+), 21 deletions(-) diff --git a/crates/continuo-actors/src/control_laws.rs b/crates/continuo-actors/src/control_laws.rs index f546ebd..a517daa 100644 --- a/crates/continuo-actors/src/control_laws.rs +++ b/crates/continuo-actors/src/control_laws.rs @@ -17,7 +17,8 @@ use crate::path::Waypoints; 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. + /// 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, @@ -25,6 +26,22 @@ pub struct PurePursuitParams { 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. /// @@ -121,18 +138,25 @@ pub fn nearest_detection(scan: &[Detection]) -> Detection { /// /// 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. + /// 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. + /// The most acceleration commanded, m/s^2, greater than zero. pub a_accel_max: f64, - /// The braking taken as comfortable, m/s^2 and positive. Also the - /// hardest [`idm_accel`] will ask for, which costs a car the + /// 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, } diff --git a/crates/continuo-fmu-controller-idm/src/lib.rs b/crates/continuo-fmu-controller-idm/src/lib.rs index 843adfc..b7156bd 100644 --- a/crates/continuo-fmu-controller-idm/src/lib.rs +++ b/crates/continuo-fmu-controller-idm/src/lib.rs @@ -48,23 +48,19 @@ pub const FMU_FILE_NAME: &str = "continuo_fmu_controller_idm.fmu"; /// question first shows. pub const MAX_WAYPOINTS: usize = 64; -/// What the following parameters hold until a host sets them. +/// The speed a car holds until a host says otherwise, m/s. /// -/// A host always does: the scenario knows how fast a car means to go, and -/// which lane it holds, where this crate cannot. Three of these must be -/// nonzero whatever happens, since the equation divides by the target -/// speed and by the root of the two rates, so they are named here rather -/// than left at zero. -const DEFAULT_IDM: IdmParams = IdmParams::highway_car(30.0); - -/// What the steering parameters hold until a host sets them: the road's -/// own centerline, aimed a few car lengths ahead. -const DEFAULT_PURSUIT: PurePursuitParams = PurePursuitParams { - lateral_tgt: 0.0, - lookahead: 6.0, - gain_yaw_rate: 1.5, - max_yaw_rate: 1.2, -}; +/// 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 car controller: IDM for how fast to go, pure pursuit for where to /// point, from one pose and one radar scan. From 64e648bb750669ffcebaf70edf516a5616d141f8 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Thu, 13 Aug 2026 16:40:43 -1000 Subject: [PATCH 17/70] Build the road only once its parameters are final Building it lazily meant building it from whatever a host had set so far. `fmi3ExitInitializationMode` recomputes dirtied values before it calls `configurate`, and a host may read an output during initialization earlier still, so the first build could land on half a road and be kept. Replacing it afterward hid that rather than fixing it. So `configurate` is the one place a road is built, and a step with no road to steer along commands nothing and leaves the outputs at the values they were made with. Nothing downstream has to ask which parameters the road came from, because there is only one answer. Co-authored-by: Claude Opus 5 (1M context) --- crates/continuo-fmu-controller-idm/src/lib.rs | 37 +++++++++++-------- 1 file changed, 22 insertions(+), 15 deletions(-) diff --git a/crates/continuo-fmu-controller-idm/src/lib.rs b/crates/continuo-fmu-controller-idm/src/lib.rs index b7156bd..0a980aa 100644 --- a/crates/continuo-fmu-controller-idm/src/lib.rs +++ b/crates/continuo-fmu-controller-idm/src/lib.rs @@ -322,29 +322,36 @@ impl IdmController { impl UserModel for IdmController { type LoggingCategory = DefaultLoggingCategory; - /// Builds the road, at the point where the parameters are final. + /// Builds the road and commands from it, once the parameters are + /// final. /// - /// This runs as initialization ends, so what it reads is what the - /// host meant. It replaces whatever `calculate_values` may have built - /// already, which is what makes a road built from half-set parameters - /// harmless rather than something to avoid. + /// `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(()) } - /// Both commands, from one pose and one scan. + /// Keeps the two output variables answering for the inputs. /// - /// It builds the road if it has none, because this can run before - /// `configurate` does: leaving initialization recomputes whatever was - /// dirtied first, and a host reading an output before that gets here - /// earlier still. - fn calculate_values(&mut self, context: &dyn Context) -> Result { - if self.road.is_none() { - self.road = Some(self.build_road(context)?); - } - let road = self.road.as_ref().expect("the road was just built"); + /// 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.commands(road); self.accel_cmd = accel_cmd; From ab70e8ac78de7bdbbff324c7c3de6e23ae98baff Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Thu, 13 Aug 2026 16:43:34 -1000 Subject: [PATCH 18/70] Refuse a road that is not one A road of no length now fails alongside a count no road could have. 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, which builds without complaint and holds a car still at one spot. That is a wiring mistake, and hearing about it beats driving along it. Two refusals want one type, so `BadPointCount` becomes an enum, which also drops the `least` field from the case that never used it. Its text carries the value that arrived, since a host setting a parameter from another tool has no other way to see what it sent, and FMI gives it a status and a log line rather than anything structured. Writing that text as the error's own Display leaves `refuse` to log whatever it is handed, which is what the next commit needs. Co-authored-by: Claude Opus 5 (1M context) --- crates/continuo-fmu-controller-idm/src/lib.rs | 118 +++++++++++++----- 1 file changed, 87 insertions(+), 31 deletions(-) diff --git a/crates/continuo-fmu-controller-idm/src/lib.rs b/crates/continuo-fmu-controller-idm/src/lib.rs index 0a980aa..2a0421b 100644 --- a/crates/continuo-fmu-controller-idm/src/lib.rs +++ b/crates/continuo-fmu-controller-idm/src/lib.rs @@ -211,12 +211,23 @@ impl Default for IdmController { } } -/// A `road_point_count` no road could have: what arrived, and the fewest -/// that would have worked. -#[derive(Debug, Clone, Copy, PartialEq)] -struct BadPointCount { - given: usize, - least: usize, +/// 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: FMI carries a status and a log +/// line, not a structured error. +#[derive(Debug, Clone, Copy, PartialEq, thiserror::Error)] +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 }, } impl IdmController { @@ -227,42 +238,55 @@ impl IdmController { /// through the C interface would take the host process with it. The /// same information travels as a refusal instead, which the standard /// can carry. - fn road_from_parameters(&self) -> Result { + /// + /// 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 = if self.road_closed { 3 } else { 2 }; + let (least, kind) = if self.road_closed { + (3, "closed") + } else { + (2, "open") + }; if given < least || given > MAX_WAYPOINTS { - return Err(BadPointCount { given, least }); + 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(if self.road_closed { - Waypoints::build_closed(points) - } else { - Waypoints::build_open(points) - }) + 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| { - context.log( - Fmi3Error::Error.into(), - DefaultLoggingCategory::default(), - format_args!( - "road_point_count is {}, and a {} road needs {} to {MAX_WAYPOINTS}", - bad.given, - if self.road_closed { "closed" } else { "open" }, - bad.least, - ), - ); - - Fmi3Error::Error - }) + 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::default(), + format_args!("{bad}"), + ); + + Fmi3Error::Error } /// What the laws command, given the road they command along. @@ -477,7 +501,11 @@ mod tests { too_few.road_point_count = 1; assert_eq!( too_few.road_from_parameters().unwrap_err(), - BadPointCount { given: 1, least: 2 } + BadInput::PointCount { + given: 1, + least: 2, + kind: "open", + } ); // Two points close into nothing, so a loop needs three. @@ -485,7 +513,11 @@ mod tests { not_a_loop.road_closed = true; assert_eq!( not_a_loop.road_from_parameters().unwrap_err(), - BadPointCount { given: 2, least: 3 } + BadInput::PointCount { + given: 2, + least: 3, + kind: "closed", + } ); // More points than arrived would read past what the host sent. @@ -493,13 +525,37 @@ mod tests { too_many.road_point_count = MAX_WAYPOINTS as u32 + 1; assert_eq!( too_many.road_from_parameters().unwrap_err(), - BadPointCount { + 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 = IdmController { + road_point_count: 4, + ..IdmController::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 } + ); + } + #[test] fn the_commands_are_what_the_laws_answer_to_the_bit() { let road = Waypoints::build_open(vec![(0.0, 0.0), (60.0, 0.0), (110.0, 40.0)]); From 2fcac624909788a1ff85409bae999fea6596a00b Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Thu, 13 Aug 2026 16:45:58 -1000 Subject: [PATCH 19/70] Refuse parameters the laws divide by A doc comment protects whoever builds `IdmParams` in Rust. It does nothing for whoever loads the packaged FMU in another tool, sets `v0_speed_tgt` to zero, and gets a car commanding NaN at every step afterward. That reader never sees this source, so the FMU has to say it back. The declaration cannot say it: `fmi-export` 0.3.0 has no `min` or `max` key, so the model description states no bounds and a host's own checker has nothing to check against. What is left is refusing at run time, which is what the four divisors do now, each refusal quoting the value that arrived. Zero, negative, infinity and NaN go the same way, since what they have in common is that no answer computed from them means anything. Packing each law's parameters where they are checked gives the two a place to live, and it is what turns commanding into something that can fail. `commands` says `calculate_commands` now, since it answers with a result rather than simply holding one. Co-authored-by: Claude Opus 5 (1M context) --- crates/continuo-fmu-controller-idm/src/lib.rs | 137 ++++++++++++++---- 1 file changed, 109 insertions(+), 28 deletions(-) diff --git a/crates/continuo-fmu-controller-idm/src/lib.rs b/crates/continuo-fmu-controller-idm/src/lib.rs index 2a0421b..bf7998c 100644 --- a/crates/continuo-fmu-controller-idm/src/lib.rs +++ b/crates/continuo-fmu-controller-idm/src/lib.rs @@ -228,6 +228,22 @@ enum BadInput { #[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 }, +} + +/// 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 }) + } } impl IdmController { @@ -289,12 +305,38 @@ impl IdmController { Fmi3Error::Error } + /// The steering law's parameters, checked. + 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. + 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 commands(&self, road: &Waypoints) -> (f64, f64) { + 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 { @@ -304,16 +346,7 @@ impl IdmController { z: self.orientation_z, }, }; - let yaw_rate_cmd = pure_pursuit_yaw_rate( - road, - pose, - PurePursuitParams { - lateral_tgt: self.lateral_tgt, - lookahead: self.lookahead, - gain_yaw_rate: self.gain_yaw_rate, - max_yaw_rate: self.max_yaw_rate, - }, - ); + 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 @@ -325,21 +358,10 @@ impl IdmController { 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, - IdmParams { - v0_speed_tgt: self.v0_speed_tgt, - t_headway: self.t_headway, - s0_gap_min: self.s0_gap_min, - a_accel_max: self.a_accel_max, - b_decel_comfort: self.b_decel_comfort, - }, - ); + 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. - (accel_cmd, yaw_rate_cmd) + Ok((accel_cmd, yaw_rate_cmd)) } } @@ -372,11 +394,13 @@ impl UserModel for IdmController { /// 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 { + 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.commands(road); + 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; @@ -556,6 +580,59 @@ mod tests { ); } + /// Asserts a controller commands nothing, naming the parameter it + /// refused and quoting the value back. + fn check_refusal(controller: &IdmController, 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 = carrying(&road); + target_speed.v0_speed_tgt = given; + check_refusal(&target_speed, &road, "v0_speed_tgt", given); + + let mut acceleration = carrying(&road); + acceleration.a_accel_max = given; + check_refusal(&acceleration, &road, "a_accel_max", given); + + let mut braking = carrying(&road); + braking.b_decel_comfort = given; + check_refusal(&braking, &road, "b_decel_comfort", given); + + let mut aim_point = carrying(&road); + aim_point.lookahead = given; + check_refusal(&aim_point, &road, "lookahead", given); + } + } + + #[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) = carrying(&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() { let road = Waypoints::build_open(vec![(0.0, 0.0), (60.0, 0.0), (110.0, 40.0)]); @@ -571,7 +648,9 @@ mod tests { controller.range[37] = 40.0; controller.range_rate[37] = -4.0; - let (accel_cmd, yaw_rate_cmd) = controller.commands(&road); + 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(25.0, 1.5, 0.0), @@ -610,7 +689,9 @@ mod tests { let mut controller = carrying(&road); controller.speed = 10.0; - let (accel_cmd, _) = controller.commands(&road); + 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. From f574a4c4c253e0fa7f18c6d214e73180346c9982 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Thu, 13 Aug 2026 16:48:53 -1000 Subject: [PATCH 20/70] Let a host retune a car between steps Declaring the law parameters fixed said a car's driving could never change once it started, which is not what a scenario wants: a car that picks up speed, or moves to another lane, is asking for exactly this. Tunable is FMI's way to say so, and it costs nothing to allow. It also settles where the parameters are read. A step reads them afresh and checks them there, which under `fixed` would have been work for a value that could not have changed, and under `tunable` is what makes a change take effect at all and keeps a new zero from getting through. The road stays fixed, and the split is real rather than cautious. Each instance builds its `Waypoints` once and keeps it, so a road set again later would be taken and then ignored, and `fixed` is what tells a host that before it tries. Driving the bundled FMU through fmpy shows it working: a car at 20 m/s asked for 30 accelerates, asked for 20 commands nothing, asked for 40 accelerates harder, and asked for 0 is refused with the reason in the log. That was a throwaway script rather than a check anyone can rerun, so the FMI-level half stays unguarded until the golden tests load the bundle through `continuo-fmi`. Co-authored-by: Claude Opus 5 (1M context) --- crates/continuo-fmu-controller-idm/src/lib.rs | 56 +++++++++++++++---- 1 file changed, 46 insertions(+), 10 deletions(-) diff --git a/crates/continuo-fmu-controller-idm/src/lib.rs b/crates/continuo-fmu-controller-idm/src/lib.rs index bf7998c..5f6a567 100644 --- a/crates/continuo-fmu-controller-idm/src/lib.rs +++ b/crates/continuo-fmu-controller-idm/src/lib.rs @@ -107,6 +107,11 @@ pub struct IdmController { #[variable(causality = Input, start = [FREE_ROAD.range_rate; MAX_DETECTIONS])] 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 = [0.0; MAX_WAYPOINTS])] road_x: [f64; MAX_WAYPOINTS], @@ -123,33 +128,37 @@ pub struct IdmController { #[variable(causality = Parameter, variability = Fixed, start = false)] 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. /// Meters left of the centerline to hold, which is what makes a lane. - #[variable(causality = Parameter, variability = Fixed, start = DEFAULT_PURSUIT.lateral_tgt)] + #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_PURSUIT.lateral_tgt)] lateral_tgt: f64, /// How far along the road to aim, in meters. - #[variable(causality = Parameter, variability = Fixed, start = DEFAULT_PURSUIT.lookahead)] + #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_PURSUIT.lookahead)] lookahead: f64, /// Yaw rate per radian of heading error, in 1/s. - #[variable(causality = Parameter, variability = Fixed, start = DEFAULT_PURSUIT.gain_yaw_rate)] + #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_PURSUIT.gain_yaw_rate)] gain_yaw_rate: f64, /// The most yaw rate to command either way, rad/s. - #[variable(causality = Parameter, variability = Fixed, start = DEFAULT_PURSUIT.max_yaw_rate)] + #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_PURSUIT.max_yaw_rate)] max_yaw_rate: f64, /// Speed held on an open road, m/s. - #[variable(causality = Parameter, variability = Fixed, start = DEFAULT_IDM.v0_speed_tgt)] + #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_IDM.v0_speed_tgt)] v0_speed_tgt: f64, /// Seconds of gap wanted at whatever speed is held. - #[variable(causality = Parameter, variability = Fixed, start = DEFAULT_IDM.t_headway)] + #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_IDM.t_headway)] t_headway: f64, /// Meters of gap wanted at a standstill. - #[variable(causality = Parameter, variability = Fixed, start = DEFAULT_IDM.s0_gap_min)] + #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_IDM.s0_gap_min)] s0_gap_min: f64, /// The most acceleration commanded, m/s^2. - #[variable(causality = Parameter, variability = Fixed, start = DEFAULT_IDM.a_accel_max)] + #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_IDM.a_accel_max)] a_accel_max: f64, /// Comfortable braking, m/s^2 and positive, the hardest commanded. - #[variable(causality = Parameter, variability = Fixed, start = DEFAULT_IDM.b_decel_comfort)] + #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_IDM.b_decel_comfort)] b_decel_comfort: f64, /// Acceleration to hold, m/s^2. @@ -306,6 +315,11 @@ impl IdmController { } /// 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 @@ -318,7 +332,8 @@ impl IdmController { }) } - /// The following law's parameters, checked. + /// 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. @@ -622,6 +637,27 @@ mod tests { } } + #[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. + let road = Waypoints::build_straight((0.0, 0.0), (1000.0, 0.0)); + let mut controller = carrying(&road); + controller.speed = 20.0; + + let (holding_20, _) = controller.calculate_commands(&road).expect("a sound set"); + controller.v0_speed_tgt = 20.0; + let (wanting_20, _) = controller.calculate_commands(&road).expect("a sound set"); + controller.v0_speed_tgt = 40.0; + let (wanting_40, _) = controller.calculate_commands(&road).expect("a sound set"); + + // Wanting exactly the speed it holds asks for nothing; wanting + // more asks for more than the default target did. + assert!(wanting_20.abs() < 1e-12, "at its target: {wanting_20}"); + assert!(wanting_40 > holding_20); + } + #[test] fn every_parameter_the_laws_divide_by_is_checked() { // The commands come out finite for the defaults, so the check From 4cfd8c752d24beaad9227c1de0d8a24a23083a48 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Thu, 13 Aug 2026 17:12:02 -1000 Subject: [PATCH 21/70] Move the errors and the bundle lookup out of lib.rs Two things in that file answered questions the rest of it does not ask. `error.rs` now holds both refusals, following the convention `continuo-core` and `continuo-fmi` already set, even though the two face opposite ways: `BadInput` is what the FMU tells a host at run time in the one channel FMI gives it, and `BundleError` is what a Rust caller gets when a build step has not been run. `require_positive` stays with the model, being a check the controller runs rather than a description of a failure. `bundle.rs` gains a doc saying what uses any of it, since nothing does yet: the golden tests that run the bundled FMU against the laws it was built from, and the demo scenario in a later PR. Its failure now names the directory the search started in and says it walked upward, rather than leaving "above" to be guessed at, and it no longer calls whoever asked a test. Co-authored-by: Claude Opus 5 (1M context) --- .../continuo-fmu-controller-idm/src/bundle.rs | 42 ++++++++++++ .../continuo-fmu-controller-idm/src/error.rs | 49 ++++++++++++++ crates/continuo-fmu-controller-idm/src/lib.rs | 65 ++----------------- 3 files changed, 97 insertions(+), 59 deletions(-) create mode 100644 crates/continuo-fmu-controller-idm/src/bundle.rs create mode 100644 crates/continuo-fmu-controller-idm/src/error.rs diff --git a/crates/continuo-fmu-controller-idm/src/bundle.rs b/crates/continuo-fmu-controller-idm/src/bundle.rs new file mode 100644 index 0000000..e875cf1 --- /dev/null +++ b/crates/continuo-fmu-controller-idm/src/bundle.rs @@ -0,0 +1,42 @@ +//! Finding the bundled `.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 golden tests that run the bundled 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::BundleError; + +/// The file `cargo xtask bundle-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 bundled FMU is written to, under `target`. +pub(crate) const FMU_DIRECTORY: &str = "fmu"; + +/// Where `cargo xtask bundle-fmus` left the bundled 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 bundle +/// means a step was not run rather than anything being broken. +pub fn bundled_fmu_path() -> Result { + let exe = std::env::current_exe().map_err(BundleError::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(BundleError::NotBundled { searched_from: exe }) +} 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..f0b8287 --- /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. [`BundleError`] is +//! what a Rust caller gets when the bundled `.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::bundle::{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 bundled FMU could not be found. +#[derive(Debug, thiserror::Error)] +pub enum BundleError { + #[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 bundle-fmus`", + searched_from.display() + )] + NotBundled { searched_from: PathBuf }, +} diff --git a/crates/continuo-fmu-controller-idm/src/lib.rs b/crates/continuo-fmu-controller-idm/src/lib.rs index 5f6a567..5d35615 100644 --- a/crates/continuo-fmu-controller-idm/src/lib.rs +++ b/crates/continuo-fmu-controller-idm/src/lib.rs @@ -19,7 +19,8 @@ //! leaves this copy behind, and `cargo xtask bundle-fmus` is what puts it //! back. -use std::path::PathBuf; +mod bundle; +mod error; use continuo_actors::control_laws::{ FREE_ROAD, IdmParams, PurePursuitParams, idm_accel, nearest_detection, pure_pursuit_yaw_rate, @@ -30,9 +31,10 @@ use fmi::fmi3::{Fmi3Error, Fmi3Res}; use fmi_export::fmi3::{Context, DefaultLoggingCategory, UserModel}; use fmi_export::{FmuModel, export_fmu}; -/// The file `cargo xtask bundle-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"; +use error::BadInput; + +pub use bundle::{FMU_FILE_NAME, bundled_fmu_path}; +pub use error::BundleError; /// How many points of road this FMU can be handed. /// @@ -220,28 +222,6 @@ impl Default for IdmController { } } -/// 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: FMI carries a status and a log -/// line, not a structured error. -#[derive(Debug, Clone, Copy, PartialEq, thiserror::Error)] -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 }, -} - /// A parameter the laws divide by, checked before they see it. /// /// Rejects a zero, a negative, an infinity and a NaN alike, since what @@ -426,39 +406,6 @@ impl UserModel for IdmController { export_fmu!(IdmController); -/// Why the bundled FMU could not be found. -#[derive(Debug, thiserror::Error)] -pub enum BundleError { - #[error("cannot find this test's own executable: {0}")] - CurrentExe(#[source] std::io::Error), - - #[error( - "no fmu/{FMU_FILE_NAME} above {}: run `cargo install cargo-fmi` once, \ - then `cargo xtask bundle-fmus`", - searched_from.display() - )] - NotBundled { searched_from: PathBuf }, -} - -/// Where `cargo xtask bundle-fmus` left the packaged FMU. -/// -/// Found by walking up from the running executable rather than from a -/// build-time path, so it works the same from a test binary, an example -/// and a tool, whichever profile built them. -pub fn bundled_fmu_path() -> Result { - let exe = std::env::current_exe().map_err(BundleError::CurrentExe)?; - for directory in exe.ancestors() { - let candidate = directory.join("fmu").join(FMU_FILE_NAME); - if candidate.is_file() { - return Ok(candidate); - } - } - - // Return the failure carrying the command that fixes it, since a - // missing bundle is a step not run rather than anything broken. - Err(BundleError::NotBundled { searched_from: exe }) -} - #[cfg(test)] mod tests { use super::*; From 2ec7fa94635c37a9525b92c5af448878a33cadb9 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Thu, 13 Aug 2026 17:13:07 -1000 Subject: [PATCH 22/70] Say package rather than bundle An FMU is a package, and this crate says so in three places: the file it looks for, the error when it is missing, and the module holding both. Two words for one thing was one too many. `cargo fmi bundle` keeps its name, being cargo-fmi's rather than ours, and it is quoted as a command rather than used as a verb. The command this project owns becomes `cargo xtask package-fmus`, renamed while it is still only a plan and nothing has to be rewritten to follow. DECISIONS keeps the one remaining "bundled", which is about a Visual Studio toolchain and has nothing to do with any of this. That is the reason renames here go site by site: an ordinary English word does not belong to whoever used it last. Co-authored-by: Claude Opus 5 (1M context) --- M6-PLAN.md | 40 +++++++++---------- crates/continuo-fmu-controller-idm/Cargo.toml | 8 ++-- .../continuo-fmu-controller-idm/src/error.rs | 14 +++---- crates/continuo-fmu-controller-idm/src/lib.rs | 14 +++---- .../src/{bundle.rs => package_fmu.rs} | 20 +++++----- 5 files changed, 48 insertions(+), 48 deletions(-) rename crates/continuo-fmu-controller-idm/src/{bundle.rs => package_fmu.rs} (69%) diff --git a/M6-PLAN.md b/M6-PLAN.md index 4f05068..73c251b 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` @@ -259,8 +259,8 @@ so nothing in M6 re-tests it per component. - **`canSerializeFMUstate` is false**, so the adapter runs this FMU in output-hash mode. Nothing in `fmi-export` implements state serialization. -- Bundling: `cargo install cargo-fmi`, then `cargo fmi --package - bundle` (wrapped by `cargo xtask bundle-fmus`) builds the cdylib +- Packaging: `cargo install cargo-fmi`, then `cargo fmi --package + bundle` (wrapped by `cargo xtask package-fmus`) 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 @@ -942,21 +942,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 @@ -964,25 +964,25 @@ 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`. 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 @@ -1006,16 +1006,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 @@ -1156,7 +1156,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 @@ -1475,7 +1475,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/crates/continuo-fmu-controller-idm/Cargo.toml b/crates/continuo-fmu-controller-idm/Cargo.toml index 69a9378..acdb15b 100644 --- a/crates/continuo-fmu-controller-idm/Cargo.toml +++ b/crates/continuo-fmu-controller-idm/Cargo.toml @@ -6,12 +6,12 @@ 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 bundled .fmu rather than the crate. +# matters is the packaged .fmu rather than the crate. publish = false -# cdylib is what FMI loads; rlib is what `bundled_fmu_path` and the golden -# tests link against. Neither can be feature-gated, since `cargo fmi bundle` -# builds with default features. +# cdylib is what FMI loads; rlib is what `packaged_fmu_path` and the golden +# 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"] diff --git a/crates/continuo-fmu-controller-idm/src/error.rs b/crates/continuo-fmu-controller-idm/src/error.rs index f0b8287..2cfe264 100644 --- a/crates/continuo-fmu-controller-idm/src/error.rs +++ b/crates/continuo-fmu-controller-idm/src/error.rs @@ -1,8 +1,8 @@ //! 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. [`BundleError`] is -//! what a Rust caller gets when the bundled `.fmu` has not been built. +//! 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. @@ -10,7 +10,7 @@ use std::path::PathBuf; use crate::MAX_WAYPOINTS; -use crate::bundle::{FMU_DIRECTORY, FMU_FILE_NAME}; +use crate::package_fmu::{FMU_DIRECTORY, FMU_FILE_NAME}; /// Something the FMU was handed that no controller could run on. /// @@ -34,16 +34,16 @@ pub(crate) enum BadInput { NotPositive { name: &'static str, given: f64 }, } -/// Why the bundled FMU could not be found. +/// Why the packaged FMU could not be found. #[derive(Debug, thiserror::Error)] -pub enum BundleError { +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 bundle-fmus`", + run `cargo install cargo-fmi` once, then `cargo xtask package-fmus`", searched_from.display() )] - NotBundled { searched_from: PathBuf }, + NotPackaged { searched_from: PathBuf }, } diff --git a/crates/continuo-fmu-controller-idm/src/lib.rs b/crates/continuo-fmu-controller-idm/src/lib.rs index 5d35615..ed0cfcd 100644 --- a/crates/continuo-fmu-controller-idm/src/lib.rs +++ b/crates/continuo-fmu-controller-idm/src/lib.rs @@ -15,12 +15,12 @@ //! the packaging lives here. //! //! The `.fmu` carries a compiled snapshot of `continuo-actors`, since the -//! cdylib links it statically. Editing a law without rebundling therefore -//! leaves this copy behind, and `cargo xtask bundle-fmus` is what puts it -//! back. +//! 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 bundle; mod error; +mod package_fmu; use continuo_actors::control_laws::{ FREE_ROAD, IdmParams, PurePursuitParams, idm_accel, nearest_detection, pure_pursuit_yaw_rate, @@ -33,8 +33,8 @@ use fmi_export::{FmuModel, export_fmu}; use error::BadInput; -pub use bundle::{FMU_FILE_NAME, bundled_fmu_path}; -pub use error::BundleError; +pub use error::PackageError; +pub use package_fmu::{FMU_FILE_NAME, packaged_fmu_path}; /// How many points of road this FMU can be handed. /// @@ -68,7 +68,7 @@ const DEFAULT_PURSUIT: PurePursuitParams = PurePursuitParams::highway_car(0.0); /// point, from one pose and one radar scan. /// /// Each variable's **first doc line is its FMI description**, and the -/// bundler takes that line alone. So a first line has to be a whole +/// 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. #[derive(FmuModel, Debug)] diff --git a/crates/continuo-fmu-controller-idm/src/bundle.rs b/crates/continuo-fmu-controller-idm/src/package_fmu.rs similarity index 69% rename from crates/continuo-fmu-controller-idm/src/bundle.rs rename to crates/continuo-fmu-controller-idm/src/package_fmu.rs index e875cf1..c8eba5d 100644 --- a/crates/continuo-fmu-controller-idm/src/bundle.rs +++ b/crates/continuo-fmu-controller-idm/src/package_fmu.rs @@ -1,23 +1,23 @@ -//! Finding the bundled `.fmu` that this crate is built into. +//! 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 golden tests that run the bundled FMU against the +//! the boundary: the golden 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::BundleError; +use crate::error::PackageError; -/// The file `cargo xtask bundle-fmus` writes, named after the cdylib +/// 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 bundled FMU is written to, under `target`. +/// The directory the packaged FMU is written to, under `target`. pub(crate) const FMU_DIRECTORY: &str = "fmu"; -/// Where `cargo xtask bundle-fmus` left the bundled 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 @@ -25,10 +25,10 @@ pub(crate) const FMU_DIRECTORY: &str = "fmu"; /// 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 bundle +/// Failing carries the command that fixes it, because a missing package /// means a step was not run rather than anything being broken. -pub fn bundled_fmu_path() -> Result { - let exe = std::env::current_exe().map_err(BundleError::ExeNotFound)?; +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() { @@ -38,5 +38,5 @@ pub fn bundled_fmu_path() -> Result { // Return the failure naming where the search began, which is the // only part of it a caller cannot work out. - Err(BundleError::NotBundled { searched_from: exe }) + Err(PackageError::NotPackaged { searched_from: exe }) } From 06004cade7b777aa53d8d866ab62e5a70df2d383 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Thu, 13 Aug 2026 17:14:12 -1000 Subject: [PATCH 23/70] Leave lib.rs as the crate's front door The model and its tests were most of that file, so a reader met six hundred lines of interface declaration before learning what the crate contains. `lib.rs` now holds the module list, the re-exports and the macro generating the C entry points, which is all anyone needs before deciding where to look next. The macro stays there because it exports by name rather than by module path, so it reads as the front door whichever file the model lives in. The model becomes `FmuController`, since what the struct is is the FMI interface, and the laws behind it are what the crate name distinguishes. A sibling crate carrying a learned model would declare the same variables and want the same name for the same reason. Nothing moves in the package: FMI takes its model name from the crate and its identifier from the cdylib, so both are unchanged, which the repackaged modelDescription confirms. Co-authored-by: Claude Opus 5 (1M context) --- .../src/fmu_controller.rs | 667 +++++++++++++++++ crates/continuo-fmu-controller-idm/src/lib.rs | 672 +----------------- 2 files changed, 676 insertions(+), 663 deletions(-) create mode 100644 crates/continuo-fmu-controller-idm/src/fmu_controller.rs 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..f498657 --- /dev/null +++ b/crates/continuo-fmu-controller-idm/src/fmu_controller.rs @@ -0,0 +1,667 @@ +//! 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 car controller: IDM for how fast to go, pure pursuit for where to +/// point, 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. +#[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 = 0.0)] + position_x: f64, + /// Where the car is, meters north. + #[variable(causality = Input, name = "position.y", start = 0.0)] + position_y: f64, + /// Which way the car points: the quaternion's scalar part. + #[variable(causality = Input, name = "orientation.w", start = 1.0)] + orientation_w: f64, + /// Which way the car points: the quaternion's x part. + #[variable(causality = Input, name = "orientation.x", start = 0.0)] + orientation_x: f64, + /// Which way the car points: the quaternion's y part. + #[variable(causality = Input, name = "orientation.y", start = 0.0)] + orientation_y: f64, + /// Which way the car points: the quaternion's z part. + #[variable(causality = Input, name = "orientation.z", start = 0.0)] + 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 = 0.0)] + 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 = [FREE_ROAD.range; MAX_DETECTIONS])] + range: [f64; MAX_DETECTIONS], + /// How fast each range is changing, m/s, negative while closing. + #[variable(causality = Input, start = [FREE_ROAD.range_rate; MAX_DETECTIONS])] + 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 = [0.0; MAX_WAYPOINTS])] + road_x: [f64; MAX_WAYPOINTS], + /// The road's points, meters north, the first road_point_count real. + #[variable(causality = Parameter, variability = Fixed, start = [0.0; MAX_WAYPOINTS])] + road_y: [f64; MAX_WAYPOINTS], + /// How many of those points are the road, the rest being padding. + #[variable(causality = Parameter, variability = Fixed, start = 2u32)] + 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 = false)] + 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. + /// Meters left of the centerline to hold, which is what makes a lane. + #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_PURSUIT.lateral_tgt)] + lateral_tgt: f64, + /// How far along the road to aim, in meters. + #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_PURSUIT.lookahead)] + lookahead: f64, + /// Yaw rate per radian of heading error, in 1/s. + #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_PURSUIT.gain_yaw_rate)] + gain_yaw_rate: f64, + /// The most yaw rate to command either way, rad/s. + #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_PURSUIT.max_yaw_rate)] + max_yaw_rate: f64, + + /// Speed held on an open road, m/s. + #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_IDM.v0_speed_tgt)] + v0_speed_tgt: f64, + /// Seconds of gap wanted at whatever speed is held. + #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_IDM.t_headway)] + t_headway: f64, + /// Meters of gap wanted at a standstill. + #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_IDM.s0_gap_min)] + s0_gap_min: f64, + /// The most acceleration commanded, m/s^2. + #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_IDM.a_accel_max)] + a_accel_max: f64, + /// Comfortable braking, m/s^2 and positive, the hardest commanded. + #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_IDM.b_decel_comfort)] + b_decel_comfort: f64, + + /// Acceleration to hold, m/s^2. + /// + /// Calculated rather than started, since it is worked out from the + /// inputs. Saying so is what lists it among the initial unknowns, and + /// a start value beside it would be a second answer to the same + /// question, which 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 { + /// The same values the variables declare as their starts. + /// + /// Both are needed and neither is the other's copy: the declared + /// starts are what a host reads out of the model description, and + /// these are what the struct holds before anything sets it. Naming + /// the constants twice is what keeps them one set of numbers. + 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::default(), + 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 { + 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 carrying `road` as its parameters, the way a host + /// hands one across. + fn carrying(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 + } + + /// Every answer a road gives, as the bits it gives them in. + fn answers(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 arrived = carrying(&sent) + .road_from_parameters() + .expect("the count is the road's own"); + assert_eq!(arrived.is_closed(), sent.is_closed()); + assert_eq!(answers(&arrived), answers(&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 = carrying(&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 arrived = controller + .road_from_parameters() + .expect("two points are enough for an open road"); + assert_eq!(arrived.total_length(), road.total_length()); + assert_eq!(answers(&arrived), answers(&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 = carrying(&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 = carrying(&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 = carrying(&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 = carrying(&road); + target_speed.v0_speed_tgt = given; + check_refusal(&target_speed, &road, "v0_speed_tgt", given); + + let mut acceleration = carrying(&road); + acceleration.a_accel_max = given; + check_refusal(&acceleration, &road, "a_accel_max", given); + + let mut braking = carrying(&road); + braking.b_decel_comfort = given; + check_refusal(&braking, &road, "b_decel_comfort", given); + + let mut aim_point = carrying(&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. + let road = Waypoints::build_straight((0.0, 0.0), (1000.0, 0.0)); + let mut controller = carrying(&road); + controller.speed = 20.0; + + let (holding_20, _) = controller.calculate_commands(&road).expect("a sound set"); + controller.v0_speed_tgt = 20.0; + let (wanting_20, _) = controller.calculate_commands(&road).expect("a sound set"); + controller.v0_speed_tgt = 40.0; + let (wanting_40, _) = controller.calculate_commands(&road).expect("a sound set"); + + // Wanting exactly the speed it holds asks for nothing; wanting + // more asks for more than the default target did. + assert!(wanting_20.abs() < 1e-12, "at its target: {wanting_20}"); + assert!(wanting_40 > holding_20); + } + + #[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) = carrying(&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() { + let road = Waypoints::build_open(vec![(0.0, 0.0), (60.0, 0.0), (110.0, 40.0)]); + let mut controller = carrying(&road); + controller.position_x = 25.0; + controller.position_y = 1.5; + controller.orientation_z = 0.1; + controller.orientation_w = 0.9; + controller.speed = 22.0; + controller.lateral_tgt = 3.5; + // A lead 40 m off and closing, in a slot nowhere near the front, + // so picking it out is part of what is being checked. + controller.range[37] = 40.0; + controller.range_rate[37] = -4.0; + + 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(25.0, 1.5, 0.0), + orientation: Quat { + w: 0.9, + x: 0.0, + y: 0.0, + z: 0.1, + }, + }; + let expected_yaw_rate = pure_pursuit_yaw_rate( + &road, + pose, + PurePursuitParams { + lateral_tgt: 3.5, + lookahead: DEFAULT_PURSUIT.lookahead, + gain_yaw_rate: DEFAULT_PURSUIT.gain_yaw_rate, + max_yaw_rate: DEFAULT_PURSUIT.max_yaw_rate, + }, + ); + let expected_accel = idm_accel(22.0, 40.0, 4.0, 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 at 4 m/s must not read as one pulling away. + assert!( + accel_cmd < idm_accel(22.0, 40.0, -4.0, DEFAULT_IDM), + "a closing lead should be braked for harder than a receding one" + ); + } + + #[test] + fn an_empty_scan_drives_the_open_road() { + let road = Waypoints::build_straight((0.0, 0.0), (500.0, 0.0)); + let mut controller = carrying(&road); + controller.speed = 10.0; + + 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(10.0, FREE_ROAD.range, 0.0, DEFAULT_IDM).to_bits() + ); + assert!(accel_cmd > 0.0, "an open road below v0 should accelerate"); + } +} diff --git a/crates/continuo-fmu-controller-idm/src/lib.rs b/crates/continuo-fmu-controller-idm/src/lib.rs index ed0cfcd..1999d5c 100644 --- a/crates/continuo-fmu-controller-idm/src/lib.rs +++ b/crates/continuo-fmu-controller-idm/src/lib.rs @@ -1,13 +1,13 @@ //! The demo's car controller, exported as an FMI 3.0 Co-Simulation FMU. //! -//! Nothing here decides anything. The interface declaration below says -//! what crosses the boundary, and every answer comes from +//! 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 +//! 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 @@ -20,668 +20,14 @@ //! what puts it back. mod error; +mod fmu_controller; mod package_fmu; -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::fmi3::{Context, DefaultLoggingCategory, UserModel}; -use fmi_export::{FmuModel, export_fmu}; - -use error::BadInput; - pub use error::PackageError; +pub use fmu_controller::{FmuController, MAX_WAYPOINTS}; pub use package_fmu::{FMU_FILE_NAME, packaged_fmu_path}; -/// 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 car controller: IDM for how fast to go, pure pursuit for where to -/// point, 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. -#[derive(FmuModel, Debug)] -#[model(co_simulation = true, model_exchange = false, user_model = false)] -pub struct IdmController { - /// Where the car is, meters east. - #[variable(causality = Input, name = "position.x", start = 0.0)] - position_x: f64, - /// Where the car is, meters north. - #[variable(causality = Input, name = "position.y", start = 0.0)] - position_y: f64, - /// Which way the car points: the quaternion's scalar part. - #[variable(causality = Input, name = "orientation.w", start = 1.0)] - orientation_w: f64, - /// Which way the car points: the quaternion's x part. - #[variable(causality = Input, name = "orientation.x", start = 0.0)] - orientation_x: f64, - /// Which way the car points: the quaternion's y part. - #[variable(causality = Input, name = "orientation.y", start = 0.0)] - orientation_y: f64, - /// Which way the car points: the quaternion's z part. - #[variable(causality = Input, name = "orientation.z", start = 0.0)] - 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 = 0.0)] - 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 = [FREE_ROAD.range; MAX_DETECTIONS])] - range: [f64; MAX_DETECTIONS], - /// How fast each range is changing, m/s, negative while closing. - #[variable(causality = Input, start = [FREE_ROAD.range_rate; MAX_DETECTIONS])] - 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 = [0.0; MAX_WAYPOINTS])] - road_x: [f64; MAX_WAYPOINTS], - /// The road's points, meters north, the first road_point_count real. - #[variable(causality = Parameter, variability = Fixed, start = [0.0; MAX_WAYPOINTS])] - road_y: [f64; MAX_WAYPOINTS], - /// How many of those points are the road, the rest being padding. - #[variable(causality = Parameter, variability = Fixed, start = 2u32)] - 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 = false)] - 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. - /// Meters left of the centerline to hold, which is what makes a lane. - #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_PURSUIT.lateral_tgt)] - lateral_tgt: f64, - /// How far along the road to aim, in meters. - #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_PURSUIT.lookahead)] - lookahead: f64, - /// Yaw rate per radian of heading error, in 1/s. - #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_PURSUIT.gain_yaw_rate)] - gain_yaw_rate: f64, - /// The most yaw rate to command either way, rad/s. - #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_PURSUIT.max_yaw_rate)] - max_yaw_rate: f64, - - /// Speed held on an open road, m/s. - #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_IDM.v0_speed_tgt)] - v0_speed_tgt: f64, - /// Seconds of gap wanted at whatever speed is held. - #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_IDM.t_headway)] - t_headway: f64, - /// Meters of gap wanted at a standstill. - #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_IDM.s0_gap_min)] - s0_gap_min: f64, - /// The most acceleration commanded, m/s^2. - #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_IDM.a_accel_max)] - a_accel_max: f64, - /// Comfortable braking, m/s^2 and positive, the hardest commanded. - #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_IDM.b_decel_comfort)] - b_decel_comfort: f64, - - /// Acceleration to hold, m/s^2. - /// - /// Calculated rather than started, since it is worked out from the - /// inputs. Saying so is what lists it among the initial unknowns, and - /// a start value beside it would be a second answer to the same - /// question, which 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 IdmController { - /// The same values the variables declare as their starts. - /// - /// Both are needed and neither is the other's copy: the declared - /// starts are what a host reads out of the model description, and - /// these are what the struct holds before anything sets it. Naming - /// the constants twice is what keeps them one set of numbers. - fn default() -> Self { - IdmController { - 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, - } - } -} - -/// 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 }) - } -} - -impl IdmController { - /// 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::default(), - 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 IdmController { - 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) - } -} - -export_fmu!(IdmController); - -#[cfg(test)] -mod tests { - use super::*; - - /// A controller carrying `road` as its parameters, the way a host - /// hands one across. - fn carrying(road: &Waypoints) -> IdmController { - let mut controller = IdmController { - road_point_count: road.points().len() as u32, - road_closed: road.is_closed(), - ..IdmController::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 - } - - /// Every answer a road gives, as the bits it gives them in. - fn answers(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 arrived = carrying(&sent) - .road_from_parameters() - .expect("the count is the road's own"); - assert_eq!(arrived.is_closed(), sent.is_closed()); - assert_eq!(answers(&arrived), answers(&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 = carrying(&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 arrived = controller - .road_from_parameters() - .expect("two points are enough for an open road"); - assert_eq!(arrived.total_length(), road.total_length()); - assert_eq!(answers(&arrived), answers(&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 = carrying(&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 = carrying(&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 = carrying(&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 = IdmController { - road_point_count: 4, - ..IdmController::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: &IdmController, 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 = carrying(&road); - target_speed.v0_speed_tgt = given; - check_refusal(&target_speed, &road, "v0_speed_tgt", given); - - let mut acceleration = carrying(&road); - acceleration.a_accel_max = given; - check_refusal(&acceleration, &road, "a_accel_max", given); - - let mut braking = carrying(&road); - braking.b_decel_comfort = given; - check_refusal(&braking, &road, "b_decel_comfort", given); - - let mut aim_point = carrying(&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. - let road = Waypoints::build_straight((0.0, 0.0), (1000.0, 0.0)); - let mut controller = carrying(&road); - controller.speed = 20.0; - - let (holding_20, _) = controller.calculate_commands(&road).expect("a sound set"); - controller.v0_speed_tgt = 20.0; - let (wanting_20, _) = controller.calculate_commands(&road).expect("a sound set"); - controller.v0_speed_tgt = 40.0; - let (wanting_40, _) = controller.calculate_commands(&road).expect("a sound set"); - - // Wanting exactly the speed it holds asks for nothing; wanting - // more asks for more than the default target did. - assert!(wanting_20.abs() < 1e-12, "at its target: {wanting_20}"); - assert!(wanting_40 > holding_20); - } - - #[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) = carrying(&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() { - let road = Waypoints::build_open(vec![(0.0, 0.0), (60.0, 0.0), (110.0, 40.0)]); - let mut controller = carrying(&road); - controller.position_x = 25.0; - controller.position_y = 1.5; - controller.orientation_z = 0.1; - controller.orientation_w = 0.9; - controller.speed = 22.0; - controller.lateral_tgt = 3.5; - // A lead 40 m off and closing, in a slot nowhere near the front, - // so picking it out is part of what is being checked. - controller.range[37] = 40.0; - controller.range_rate[37] = -4.0; - - 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(25.0, 1.5, 0.0), - orientation: Quat { - w: 0.9, - x: 0.0, - y: 0.0, - z: 0.1, - }, - }; - let expected_yaw_rate = pure_pursuit_yaw_rate( - &road, - pose, - PurePursuitParams { - lateral_tgt: 3.5, - lookahead: DEFAULT_PURSUIT.lookahead, - gain_yaw_rate: DEFAULT_PURSUIT.gain_yaw_rate, - max_yaw_rate: DEFAULT_PURSUIT.max_yaw_rate, - }, - ); - let expected_accel = idm_accel(22.0, 40.0, 4.0, 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 at 4 m/s must not read as one pulling away. - assert!( - accel_cmd < idm_accel(22.0, 40.0, -4.0, DEFAULT_IDM), - "a closing lead should be braked for harder than a receding one" - ); - } - - #[test] - fn an_empty_scan_drives_the_open_road() { - let road = Waypoints::build_straight((0.0, 0.0), (500.0, 0.0)); - let mut controller = carrying(&road); - controller.speed = 10.0; - - 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(10.0, FREE_ROAD.range, 0.0, DEFAULT_IDM).to_bits() - ); - assert!(accel_cmd > 0.0, "an open road below v0 should accelerate"); - } -} +// 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); From e4728b7e20058da9e639e860a920725aedaccd67 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Thu, 13 Aug 2026 17:20:39 -1000 Subject: [PATCH 24/70] Declare every start value from one place The starts and the `Default` impl were two lists of the same twenty-four values, kept in step by hand and by nothing else. Each declaration now reads its own field out of `Self::default()`, so there is one list, and a value can only be changed where it is written. The repackaged model description carries the numbers it did before, which is what says this moved where they live rather than what they are. `set_start_values` expands to one assignment per variable, so the expression is evaluated twenty-four times, and it runs when an instance is made and when one is reset, never inside a step. Measured rather than assumed: 23 ns in release, where the optimizer folds the constructions down to less than a single `default()` costs, and 3.8 us in the dev profile the packager builds by default. A car joining a world pays that once, against an FMU load and a zip extraction in the same breath, so it does not register. Co-authored-by: Claude Opus 5 (1M context) --- .../src/fmu_controller.rs | 55 ++++++++++--------- 1 file changed, 28 insertions(+), 27 deletions(-) diff --git a/crates/continuo-fmu-controller-idm/src/fmu_controller.rs b/crates/continuo-fmu-controller-idm/src/fmu_controller.rs index f498657..6e8f49f 100644 --- a/crates/continuo-fmu-controller-idm/src/fmu_controller.rs +++ b/crates/continuo-fmu-controller-idm/src/fmu_controller.rs @@ -70,38 +70,38 @@ fn require_positive(name: &'static str, given: f64) -> Result { #[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 = 0.0)] + #[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 = 0.0)] + #[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 = 1.0)] + #[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 = 0.0)] + #[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 = 0.0)] + #[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 = 0.0)] + #[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 = 0.0)] + #[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 = [FREE_ROAD.range; MAX_DETECTIONS])] + #[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 = [FREE_ROAD.range_rate; MAX_DETECTIONS])] + #[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 @@ -110,19 +110,19 @@ pub struct FmuController { // 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 = [0.0; MAX_WAYPOINTS])] + #[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 = [0.0; MAX_WAYPOINTS])] + #[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 = 2u32)] + #[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 = false)] + #[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 @@ -130,32 +130,32 @@ pub struct FmuController { // is why a step reads them rather than keeping a copy taken at // initialization, and why the check on them runs there too. /// Meters left of the centerline to hold, which is what makes a lane. - #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_PURSUIT.lateral_tgt)] + #[variable(causality = Parameter, variability = Tunable, start = Self::default().lateral_tgt)] lateral_tgt: f64, /// How far along the road to aim, in meters. - #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_PURSUIT.lookahead)] + #[variable(causality = Parameter, variability = Tunable, start = Self::default().lookahead)] lookahead: f64, /// Yaw rate per radian of heading error, in 1/s. - #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_PURSUIT.gain_yaw_rate)] + #[variable(causality = Parameter, variability = Tunable, start = Self::default().gain_yaw_rate)] gain_yaw_rate: f64, /// The most yaw rate to command either way, rad/s. - #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_PURSUIT.max_yaw_rate)] + #[variable(causality = Parameter, variability = Tunable, start = Self::default().max_yaw_rate)] max_yaw_rate: f64, /// Speed held on an open road, m/s. - #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_IDM.v0_speed_tgt)] + #[variable(causality = Parameter, variability = Tunable, start = Self::default().v0_speed_tgt)] v0_speed_tgt: f64, /// Seconds of gap wanted at whatever speed is held. - #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_IDM.t_headway)] + #[variable(causality = Parameter, variability = Tunable, start = Self::default().t_headway)] t_headway: f64, /// Meters of gap wanted at a standstill. - #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_IDM.s0_gap_min)] + #[variable(causality = Parameter, variability = Tunable, start = Self::default().s0_gap_min)] s0_gap_min: f64, /// The most acceleration commanded, m/s^2. - #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_IDM.a_accel_max)] + #[variable(causality = Parameter, variability = Tunable, start = Self::default().a_accel_max)] a_accel_max: f64, /// Comfortable braking, m/s^2 and positive, the hardest commanded. - #[variable(causality = Parameter, variability = Tunable, start = DEFAULT_IDM.b_decel_comfort)] + #[variable(causality = Parameter, variability = Tunable, start = Self::default().b_decel_comfort)] b_decel_comfort: f64, /// Acceleration to hold, m/s^2. @@ -180,12 +180,13 @@ pub struct FmuController { } impl Default for FmuController { - /// The same values the variables declare as their starts. + /// Every start value this FMU has, written once. /// - /// Both are needed and neither is the other's copy: the declared - /// starts are what a host reads out of the model description, and - /// these are what the struct holds before anything sets it. Naming - /// the constants twice is what keeps them one set of numbers. + /// 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, From e54928008f20b0224cb9d70f6650b5cb567f56a2 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Thu, 13 Aug 2026 17:23:13 -1000 Subject: [PATCH 25/70] Settle that the packaged FMU is built optimized `cargo fmi bundle` builds the dev profile unless told otherwise, so the `.fmu` packaged so far is unoptimized. That does not matter for making an instance, which happens once per car, but `calculate_values` runs for every car at every control instant, and PR E puts an FMU in each of them. So the xtask passes `--release`, recorded here because that is where it will be written. Whether it may is the part worth checking rather than assuming, since the world hash is pinned across four platforms and the golden tests compare a packaged FMU against natively built laws. Driving both profiles through fmpy gives bit-identical answers: a following command, an open-road command, a steering command that goes through `atan2`, and a near-cancellation landing at -1.5e-15. rustc does not reassociate float arithmetic at any optimization level, which is why, and the golden tests are what would notice if that ever changed. Co-authored-by: Claude Opus 5 (1M context) --- M6-PLAN.md | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/M6-PLAN.md b/M6-PLAN.md index 73c251b..68d9f49 100644 --- a/M6-PLAN.md +++ b/M6-PLAN.md @@ -260,7 +260,8 @@ so nothing in M6 re-tests it per component. output-hash mode. Nothing in `fmi-export` implements state serialization. - Packaging: `cargo install cargo-fmi`, then `cargo fmi --package - bundle` (wrapped by `cargo xtask package-fmus`) builds the cdylib + 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 @@ -969,7 +970,21 @@ PLAN.md's "native arrays, float64 value references" that survives. 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 `packaged_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 From 2367314b1cc4015fbb2f9d7a5a161baf6904216c Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Thu, 13 Aug 2026 17:25:54 -1000 Subject: [PATCH 26/70] Say which law each parameter belongs to A host reading the model description meets thirteen parameters with no hint of which of the two laws owns them, and `lookahead` beside `t_headway` says nothing about that. Each description now opens with the law it tunes, which is the one thing a name like `gain_yaw_rate` cannot carry on its own. The crate and the model open by saying what they are rather than which demo they came from: a controller for traffic cars, following with IDM and turning with pure pursuit. Two questions the code did not answer now appear where they were asked. `user_model = false` is because the `UserModel` impl is written by hand, and leaving it true would export an FMU whose `calculate_values` does nothing and which commands zero forever. The logging categories are `logAll` and `trace`, being what `fmi-export` offers a model that declares no enum of its own, and the one message raised here is named rather than defaulted, since a refusal is not tracing. The output doc drops "started" for the schema's own words, `initial = Calculated`, `start` and `InitialUnknowns`, since that sentence is about what FMI forbids rather than about English. Co-authored-by: Claude Opus 5 (1M context) --- .../src/fmu_controller.rs | 40 +++++++++++-------- crates/continuo-fmu-controller-idm/src/lib.rs | 3 +- 2 files changed, 26 insertions(+), 17 deletions(-) diff --git a/crates/continuo-fmu-controller-idm/src/fmu_controller.rs b/crates/continuo-fmu-controller-idm/src/fmu_controller.rs index 6e8f49f..f1c93d5 100644 --- a/crates/continuo-fmu-controller-idm/src/fmu_controller.rs +++ b/crates/continuo-fmu-controller-idm/src/fmu_controller.rs @@ -59,13 +59,16 @@ fn require_positive(name: &'static str, given: f64) -> Result { } } -/// A car controller: IDM for how fast to go, pure pursuit for where to -/// point, from one pose and one radar scan. +/// 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 { @@ -129,41 +132,42 @@ pub struct FmuController { // 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. - /// Meters left of the centerline to hold, which is what makes a lane. + /// 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, - /// How far along the road to aim, in meters. + /// Pure pursuit: how far along the road to aim, in meters. #[variable(causality = Parameter, variability = Tunable, start = Self::default().lookahead)] lookahead: f64, - /// Yaw rate per radian of heading error, in 1/s. + /// 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, - /// The most yaw rate to command either way, rad/s. + /// 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, - /// Speed held on an open road, m/s. + /// IDM: speed held on an open road, m/s. #[variable(causality = Parameter, variability = Tunable, start = Self::default().v0_speed_tgt)] v0_speed_tgt: f64, - /// Seconds of gap wanted at whatever speed is held. + /// IDM: seconds of gap wanted at whatever speed is held. #[variable(causality = Parameter, variability = Tunable, start = Self::default().t_headway)] t_headway: f64, - /// Meters of gap wanted at a standstill. + /// IDM: meters of gap wanted at a standstill. #[variable(causality = Parameter, variability = Tunable, start = Self::default().s0_gap_min)] s0_gap_min: f64, - /// The most acceleration commanded, m/s^2. + /// IDM: the most acceleration commanded, m/s^2. #[variable(causality = Parameter, variability = Tunable, start = Self::default().a_accel_max)] a_accel_max: f64, - /// Comfortable braking, m/s^2 and positive, the hardest commanded. + /// 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. /// - /// Calculated rather than started, since it is worked out from the - /// inputs. Saying so is what lists it among the initial unknowns, and - /// a start value beside it would be a second answer to the same - /// question, which a checking importer refuses to load. + /// `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. @@ -270,7 +274,7 @@ impl FmuController { fn refuse(&self, context: &dyn Context, bad: BadInput) -> Fmi3Error { context.log( Fmi3Error::Error.into(), - DefaultLoggingCategory::default(), + DefaultLoggingCategory::LogAll, format_args!("{bad}"), ); @@ -344,6 +348,10 @@ impl FmuController { } 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 diff --git a/crates/continuo-fmu-controller-idm/src/lib.rs b/crates/continuo-fmu-controller-idm/src/lib.rs index 1999d5c..99b849a 100644 --- a/crates/continuo-fmu-controller-idm/src/lib.rs +++ b/crates/continuo-fmu-controller-idm/src/lib.rs @@ -1,4 +1,5 @@ -//! The demo's car controller, exported as an FMI 3.0 Co-Simulation FMU. +//! 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 From 6e1c297253692db2c8fa80ed42d70fb57addbfd8 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Thu, 13 Aug 2026 17:28:34 -1000 Subject: [PATCH 27/70] Name what the tests are holding `carrying` and `arrived` said what was happening to a thing without saying which thing, so a reader met `carrying(&sent)` and had to go looking. They are `controller_given` and `rebuilt` now, and `answers` becomes `sampled_along`, which says what it samples rather than that it returns something. The values in the two behavior tests get names as well. A reader met `22.0` three times and `40.0` twice and had to match them up by eye to see that the expected answer was computed from the same numbers the controller was given. Named once, a mismatch is visible instead. The open-road test assumed a target speed above 10 without saying so. It now takes a third of whatever the default target is, so it means what its name says at any target rather than only at 30. Co-authored-by: Claude Opus 5 (1M context) --- .../src/fmu_controller.rs | 130 ++++++++++-------- 1 file changed, 73 insertions(+), 57 deletions(-) diff --git a/crates/continuo-fmu-controller-idm/src/fmu_controller.rs b/crates/continuo-fmu-controller-idm/src/fmu_controller.rs index f1c93d5..7788bc4 100644 --- a/crates/continuo-fmu-controller-idm/src/fmu_controller.rs +++ b/crates/continuo-fmu-controller-idm/src/fmu_controller.rs @@ -399,9 +399,9 @@ impl UserModel for FmuController { mod tests { use super::*; - /// A controller carrying `road` as its parameters, the way a host + /// A controller holding `road` in its parameters, the way a host /// hands one across. - fn carrying(road: &Waypoints) -> FmuController { + fn controller_given(road: &Waypoints) -> FmuController { let mut controller = FmuController { road_point_count: road.points().len() as u32, road_closed: road.is_closed(), @@ -417,8 +417,9 @@ mod tests { controller } - /// Every answer a road gives, as the bits it gives them in. - fn answers(road: &Waypoints) -> Vec<(u64, u64, u64)> { + /// 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; @@ -440,18 +441,18 @@ mod tests { 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 arrived = carrying(&sent) + let rebuilt = controller_given(&sent) .road_from_parameters() .expect("the count is the road's own"); - assert_eq!(arrived.is_closed(), sent.is_closed()); - assert_eq!(answers(&arrived), answers(&sent)); + 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 = carrying(&road); + 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. @@ -460,11 +461,11 @@ mod tests { controller.road_y[i] = -500.0; } - let arrived = controller + let rebuilt = controller .road_from_parameters() .expect("two points are enough for an open road"); - assert_eq!(arrived.total_length(), road.total_length()); - assert_eq!(answers(&arrived), answers(&road)); + assert_eq!(rebuilt.total_length(), road.total_length()); + assert_eq!(sampled_along(&rebuilt), sampled_along(&road)); } #[test] @@ -472,7 +473,7 @@ mod tests { 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 = carrying(&straight); + let mut too_few = controller_given(&straight); too_few.road_point_count = 1; assert_eq!( too_few.road_from_parameters().unwrap_err(), @@ -484,7 +485,7 @@ mod tests { ); // Two points close into nothing, so a loop needs three. - let mut not_a_loop = carrying(&straight); + let mut not_a_loop = controller_given(&straight); not_a_loop.road_closed = true; assert_eq!( not_a_loop.road_from_parameters().unwrap_err(), @@ -496,7 +497,7 @@ mod tests { ); // More points than arrived would read past what the host sent. - let mut too_many = carrying(&straight); + 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(), @@ -555,19 +556,19 @@ mod tests { // 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 = carrying(&road); + 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 = carrying(&road); + let mut acceleration = controller_given(&road); acceleration.a_accel_max = given; check_refusal(&acceleration, &road, "a_accel_max", given); - let mut braking = carrying(&road); + let mut braking = controller_given(&road); braking.b_decel_comfort = given; check_refusal(&braking, &road, "b_decel_comfort", given); - let mut aim_point = carrying(&road); + let mut aim_point = controller_given(&road); aim_point.lookahead = given; check_refusal(&aim_point, &road, "lookahead", given); } @@ -578,20 +579,25 @@ mod tests { // 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 = carrying(&road); - controller.speed = 20.0; - - let (holding_20, _) = controller.calculate_commands(&road).expect("a sound set"); - controller.v0_speed_tgt = 20.0; - let (wanting_20, _) = controller.calculate_commands(&road).expect("a sound set"); - controller.v0_speed_tgt = 40.0; - let (wanting_40, _) = controller.calculate_commands(&road).expect("a sound set"); - - // Wanting exactly the speed it holds asks for nothing; wanting - // more asks for more than the default target did. - assert!(wanting_20.abs() < 1e-12, "at its target: {wanting_20}"); - assert!(wanting_40 > holding_20); + 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] @@ -599,7 +605,7 @@ mod tests { // 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) = carrying(&road) + 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()); @@ -607,59 +613,69 @@ mod tests { #[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 = carrying(&road); - controller.position_x = 25.0; - controller.position_y = 1.5; - controller.orientation_z = 0.1; - controller.orientation_w = 0.9; - controller.speed = 22.0; - controller.lateral_tgt = 3.5; - // A lead 40 m off and closing, in a slot nowhere near the front, - // so picking it out is part of what is being checked. - controller.range[37] = 40.0; - controller.range_rate[37] = -4.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(25.0, 1.5, 0.0), + position: Vec3::new(POSITION.0, POSITION.1, 0.0), orientation: Quat { - w: 0.9, + w: ORIENTATION.0, x: 0.0, y: 0.0, - z: 0.1, + z: ORIENTATION.1, }, }; let expected_yaw_rate = pure_pursuit_yaw_rate( &road, pose, PurePursuitParams { - lateral_tgt: 3.5, - lookahead: DEFAULT_PURSUIT.lookahead, - gain_yaw_rate: DEFAULT_PURSUIT.gain_yaw_rate, - max_yaw_rate: DEFAULT_PURSUIT.max_yaw_rate, + lateral_tgt: LANE, + ..DEFAULT_PURSUIT }, ); - let expected_accel = idm_accel(22.0, 40.0, 4.0, DEFAULT_IDM); + 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 at 4 m/s must not read as one pulling away. + // closing must not read as one pulling away. assert!( - accel_cmd < idm_accel(22.0, 40.0, -4.0, DEFAULT_IDM), + 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 = carrying(&road); - controller.speed = 10.0; + let mut controller = controller_given(&road); + controller.speed = speed; let (accel_cmd, _) = controller .calculate_commands(&road) @@ -669,8 +685,8 @@ mod tests { // the answer is the one an open road gives at this speed. assert_eq!( accel_cmd.to_bits(), - idm_accel(10.0, FREE_ROAD.range, 0.0, DEFAULT_IDM).to_bits() + idm_accel(speed, FREE_ROAD.range, 0.0, DEFAULT_IDM).to_bits() ); - assert!(accel_cmd > 0.0, "an open road below v0 should accelerate"); + assert!(accel_cmd > 0.0, "below its target it should accelerate"); } } From 85c87cbb1363518fb071f3a984c75819a610fa49 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Thu, 13 Aug 2026 17:32:52 -1000 Subject: [PATCH 28/70] Say why the lint table looks wrong to an editor `check-cfg` inside a lint table has been Cargo's since 1.80, and an editor calling it invalid is validating against a schema older than the feature. The comment says so, since the next reader meets the squiggle before the explanation. A build script emitting `cargo::rustc-check-cfg` silences the same three dozen warnings, tested rather than assumed, and it is what this would use if the manifest could not say it. It stays out: a build script is machinery that compiles and runs on every build, and buying it to satisfy a stale schema rather than the compiler is the wrong trade, particularly in a crate that already pulls one in through `fmi-sys`. Co-authored-by: Claude Opus 5 (1M context) --- crates/continuo-fmu-controller-idm/Cargo.toml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/crates/continuo-fmu-controller-idm/Cargo.toml b/crates/continuo-fmu-controller-idm/Cargo.toml index acdb15b..2f1b4d1 100644 --- a/crates/continuo-fmu-controller-idm/Cargo.toml +++ b/crates/continuo-fmu-controller-idm/Cargo.toml @@ -28,5 +28,10 @@ thiserror = { workspace = true } # 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)'] } From 5d504ad5d6c08c5bfd25998b75219cc07b55b677 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Fri, 14 Aug 2026 06:11:26 -1000 Subject: [PATCH 29/70] Add an xtask, so packaging an FMU is one command `cargo fmi --package continuo-fmu-controller-idm bundle --release` is what packaging has meant so far, typed by hand, with the crate named in it and the profile easy to forget. `cargo xtask package-fmus` is the whole of it now. It finds the exporters rather than being told them, by the `continuo-fmu-` crate-name prefix out of `cargo metadata`, so a second one is packaged by this and by CI with no edit anywhere. `--no-deps` keeps that to workspace members, and the names are sorted, since a run that lists its crates differently each time invites the question of what else moved. It asks whether `cargo fmi` exists before packaging anything, so a missing subcommand is reported as itself with the install command rather than as a packaging failure with a guess attached. `cargo fmi --help` exits zero when it is there and cargo exits 101 when it is not, which reading cargo's error text would only approximate. Checked by pointing the probe at a subcommand that does not exist. An xtask because cargo has no user-defined targets and its aliases cannot chain commands, and because this should be something a person types rather than a side effect of `cargo build`. It sits outside `crates/`, being tooling for this checkout rather than part of what the workspace publishes. Co-authored-by: Claude Opus 5 (1M context) --- .cargo/config.toml | 6 ++ Cargo.lock | 7 +++ Cargo.toml | 4 +- README.md | 8 +++ xtask/Cargo.toml | 13 ++++ xtask/src/main.rs | 146 +++++++++++++++++++++++++++++++++++++++++++++ 6 files changed, 183 insertions(+), 1 deletion(-) create mode 100644 .cargo/config.toml create mode 100644 xtask/Cargo.toml create mode 100644 xtask/src/main.rs 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/Cargo.lock b/Cargo.lock index becf9b0..c613eff 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -4024,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 001d6ac..e52bf95 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" diff --git a/README.md b/README.md index 1b6ae22..3ca209e 100644 --- a/README.md +++ b/README.md @@ -101,6 +101,8 @@ instant, and repeats. | [`continuo-actors`](crates/continuo-actors/) | Sample components: waypoint path, path-follow controller, unicycle physics, pose logger, traffic spawner | | [`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,12 @@ 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 + # 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/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..626deb2 --- /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 exporter carries. +/// +/// Discovery goes by name rather than by a list, so a second exporter, +/// 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 exporter in the workspace. +fn package_fmus() -> Result<(), String> { + let exporters = fmu_crates()?; + if exporters.is_empty() { + return Err(format!("no {FMU_CRATE_PREFIX}* crate in this workspace")); + } + require_cargo_fmi()?; + for name in &exporters { + 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 exporters 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 exporters, 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 exporter into `target/fmu`. +/// +/// `--release` because the default is the dev profile, and a packaged +/// controller runs its step for every actor at every control instant, +/// where the host runs it once per call. The two profiles answer bit for +/// bit alike, which is what lets the golden tests compare a release FMU +/// 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())) +} From b44cda1bda209de8609332855251b23ef4c08ed0 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Fri, 14 Aug 2026 06:11:26 -1000 Subject: [PATCH 30/70] Package and validate the FMUs in CI Packaging runs before the test steps, because the golden tests load the `.fmu` and compare it against the laws it was built from, and nothing else produces one. A job that skipped it would fail there with the laws apparently disagreeing with themselves. The step names no package, since the task finds them itself. `cargo-fmi` is pinned and cached by version. A packager that changed under us would rewrite modelDescription.xml on a commit that touched nothing, and the cache key carries the runner because the binary is native. Then fmpy validates what came out, which is the barrier the first packaged FMU earned. It declared start values on calculated outputs and listed no initial unknowns, and `continuo-fmi` loaded it without complaint while fmpy refused it outright. Our own importer accepting an FMU says nothing about whether it is valid, and a second implementation is the only thing that would have caught that. The loop guards against matching nothing, since a glob that expands to itself would run no checks and pass, which is the exact shape of failure this step exists to prevent. Co-authored-by: Claude Opus 5 (1M context) --- .github/workflows/ci.yml | 47 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 47 insertions(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index d0a89cb..c6920df 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -85,6 +85,53 @@ jobs: env: RUSTDOCFLAGS: -D warnings + # Packaging comes before the tests because the golden tests load the + # `.fmu` this writes and compare it against the laws it was built from. + # Nothing else produces it, so a job that skipped this would fail there + # with the laws apparently disagreeing with themselves. + # + # Pinned and cached by version, since a packager that changed under us + # would rewrite modelDescription.xml on an unrelated commit. The cache + # key carries the runner, because the binary is native. + - name: "Install: cargo-fmi" + uses: actions/cache@v4 + 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 + + # Names no package: the task finds every `continuo-fmu-*` crate itself, + # so a second exporter needs no edit here. + - name: "Package: FMUs" + run: cargo xtask package-fmus + + # An FMU our own importer accepts is not the same as a valid one, and + # the difference is what this catches. The first FMU this crate ever + # produced declared start values on calculated outputs and listed no + # initial unknowns; `continuo-fmi` read it happily and fmpy refused it. + # A second implementation is the only thing that would have said so. + - name: "Validate: FMUs against a second implementation" + shell: bash + run: | + python -m pip install fmpy + + # Otherwise a glob matching nothing would run no checks and pass, + # which is the shape of failure this step exists to prevent. + packaged=(target/fmu/*.fmu) + if [ ! -e "${packaged[0]}" ]; then + echo "ERROR: no .fmu in target/fmu; this check would be vacuous" + exit 1 + fi + for fmu in "${packaged[@]}"; do + 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 From 7655c7eea283a82746a0ef646261154a60118a56 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Fri, 14 Aug 2026 09:15:40 -1000 Subject: [PATCH 31/70] Say what the FMU is where a host actually reads it Rewording the crate and model docs left the one description that travels untouched. The packager takes `modelDescription.xml`'s `description` from the manifest rather than from any doc comment, so an FMU loaded in another tool still introduced itself as the demo's car controller while the source had stopped saying that. The manifest says it now, with a comment naming where it ends up, since nothing else about a `description` field suggests it leaves the crate. Co-authored-by: Claude Opus 5 (1M context) --- crates/continuo-fmu-controller-idm/Cargo.toml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/crates/continuo-fmu-controller-idm/Cargo.toml b/crates/continuo-fmu-controller-idm/Cargo.toml index 2f1b4d1..d374dac 100644 --- a/crates/continuo-fmu-controller-idm/Cargo.toml +++ b/crates/continuo-fmu-controller-idm/Cargo.toml @@ -1,6 +1,8 @@ [package] name = "continuo-fmu-controller-idm" -description = "The demo's car controller, exported as an FMI 3.0 Co-Simulation FMU" +# 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 From e7315e871dab996897970a9985a81bd5ae482a88 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Fri, 14 Aug 2026 09:16:44 -1000 Subject: [PATCH 32/70] Call them FMU crates "Exporter" was a word this project uses nowhere else, invented to name the thing being packaged. They are FMU crates, which is what the prefix they are found by already says. Co-authored-by: Claude Opus 5 (1M context) --- xtask/src/main.rs | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/xtask/src/main.rs b/xtask/src/main.rs index 626deb2..e0c0973 100644 --- a/xtask/src/main.rs +++ b/xtask/src/main.rs @@ -9,9 +9,9 @@ use std::io; use std::process::{Command, ExitCode}; -/// The crate-name prefix an FMU exporter carries. +/// The crate-name prefix an FMU crate carries. /// -/// Discovery goes by name rather than by a list, so a second exporter, +/// 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-"; @@ -40,14 +40,14 @@ fn main() -> ExitCode { } } -/// Packages every FMU exporter in the workspace. +/// Packages every FMU crate in the workspace. fn package_fmus() -> Result<(), String> { - let exporters = fmu_crates()?; - if exporters.is_empty() { + 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 &exporters { + for name in &fmu_crates { println!("packaging {name}"); package(name)?; } @@ -58,7 +58,7 @@ fn package_fmus() -> Result<(), String> { Ok(()) } -/// The FMU exporters this workspace holds, in name order. +/// 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 @@ -90,7 +90,7 @@ fn fmu_crates() -> Result, String> { .collect(); names.sort(); - // Return the exporters, in the order they will be packaged. + // Return the FMU crates, in the order they will be packaged. Ok(names) } @@ -117,7 +117,7 @@ fn require_cargo_fmi() -> Result<(), String> { } } -/// Packages one exporter into `target/fmu`. +/// Packages one FMU crate into `target/fmu`. /// /// `--release` because the default is the dev profile, and a packaged /// controller runs its step for every actor at every control instant, From 79bf108ecd070b4a32c96e73993da93ccd4c8ade Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Fri, 14 Aug 2026 09:32:50 -1000 Subject: [PATCH 33/70] Group the CI steps by what they are The order had grown by accretion: Python installed between two Rust lints, the FMU packaging wedged between the doc lint and the tests, and the viewer install sitting wherever it first became necessary. It reads as installs, lints, a build, tests, then the FMU, then the smokes now, so what a job is doing at any point is a matter of where it is. Everything a later step needs is installed first, so a missing tool is reported before any of the work rather than partway through it, and `fmpy` joins that install rather than being fetched inside the step using it. A build of each profile stands on its own, so a compile error is reported as one rather than as whichever test step reached it first. The smokes run release, which is the profile the packaged FMU will be loaded beside. That moves the world hash into a profile nothing was checking: the pin is asserted by a test that runs debug, and a release smoke only prints the hash, so a divergence would have gone through. The pinned test now runs in release as well, on all four agents. It also retires `--no-default-features` on the scaled world, which only bought a second configuration to compile once a release build existed anyway. `actions/cache` moves to v6, which is what the Node 20 deprecation notice in the logs was about. Co-authored-by: Claude Opus 5 (1M context) --- .github/workflows/ci.yml | 155 +++++++++++++++++++++------------------ 1 file changed, 83 insertions(+), 72 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index c6920df..035e46a 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -41,38 +41,53 @@ 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 + # way is what keeps that true. `fmpy` joins it for the FMU validation + # below, which is a second implementation's opinion of what we package. + - name: "Install: Python (cached)" uses: actions/setup-python@v7 with: python-version: "3.12" cache: pip cache-dependency-path: python/pyproject.toml - - name: "Install: viewer" + - name: "Install: viewer and fmpy" working-directory: python - run: python -m pip install -e . pytest ruff + run: python -m pip install -e . pytest ruff fmpy - - name: "Lint: ruff" - working-directory: python - run: | - ruff check . - ruff format --check . + # 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 - name: "Lint: clippy" run: cargo clippy --workspace --all-targets -- -D warnings @@ -85,52 +100,16 @@ jobs: env: RUSTDOCFLAGS: -D warnings - # Packaging comes before the tests because the golden tests load the - # `.fmu` this writes and compare it against the laws it was built from. - # Nothing else produces it, so a job that skipped this would fail there - # with the laws apparently disagreeing with themselves. - # - # Pinned and cached by version, since a packager that changed under us - # would rewrite modelDescription.xml on an unrelated commit. The cache - # key carries the runner, because the binary is native. - - name: "Install: cargo-fmi" - uses: actions/cache@v4 - 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 - - # Names no package: the task finds every `continuo-fmu-*` crate itself, - # so a second exporter needs no edit here. - - name: "Package: FMUs" - run: cargo xtask package-fmus - - # An FMU our own importer accepts is not the same as a valid one, and - # the difference is what this catches. The first FMU this crate ever - # produced declared start values on calculated outputs and listed no - # initial unknowns; `continuo-fmi` read it happily and fmpy refused it. - # A second implementation is the only thing that would have said so. - - name: "Validate: FMUs against a second implementation" - shell: bash + - name: "Lint: ruff" + working-directory: python run: | - python -m pip install fmpy + ruff check . + ruff format --check . - # Otherwise a glob matching nothing would run no checks and pass, - # which is the shape of failure this step exists to prevent. - packaged=(target/fmu/*.fmu) - if [ ! -e "${packaged[0]}" ]; then - echo "ERROR: no .fmu in target/fmu; this check would be vacuous" - exit 1 - fi - for fmu in "${packaged[@]}"; do - python -m fmpy validate "$fmu" - done + # A build of its own, so a compile error is reported as one rather than + # as whichever test step reached it first. + - name: "Build: debug" + run: cargo build --workspace --all-targets # `--lib` and `--test '*'` partition the suite: the first runs the unit # tests inside each lib, the second the integration targets in tests/. @@ -146,6 +125,45 @@ jobs: working-directory: python run: pytest -v + # Release from here on. The packaging step below builds each FMU + # release, since a host loads the binary it finds and cannot rebuild it, + # and the smokes then run against the profile that FMU will be loaded + # beside. + - 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 says the pin covers + # both. rustc does not reassociate float arithmetic at any optimization + # level, which is why the two agree; this is what would notice if that + # ever stopped being true, on whichever of the four agents it stopped. + - name: "Test: the world hash holds in release too" + run: > + cargo test --release -p continuo-examples --test highway + the_demo_world_hashes_to_the_same_value_on_every_platform + + # 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 + + # An FMU our own importer accepts is not the same as a valid one, so a + # second implementation is what says whether what we package is well + # formed. + - name: "FMU: validate" + shell: bash + run: | + # A glob matching nothing would expand to itself, check nothing and + # pass, which is the shape of failure this step exists to catch. + packaged=(target/fmu/*.fmu) + if [ ! -e "${packaged[0]}" ]; then + echo "ERROR: no .fmu in target/fmu; this check would be vacuous" + exit 1 + fi + for fmu in "${packaged[@]}"; do + python -m fmpy validate "$fmu" + done + # 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 @@ -155,13 +173,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 @@ -184,14 +202,14 @@ jobs: 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 @@ -235,12 +253,5 @@ 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 From 46d26c723989ed72cdd95408b3825b596ec6cb0c Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Fri, 14 Aug 2026 09:33:26 -1000 Subject: [PATCH 34/70] Say why an FMU is packaged optimized, and name the profile it is not Two things in one doc comment. The default is the debug profile, so say debug rather than dev, which is cargo's word for the manifest section rather than what anyone calls the build. And the reason for `--release` was wrong. It talked about how often a controller steps, which is beside the point and was garbled besides. What decides it is that packaging settles the question: a host loads the binary it finds inside the `.fmu` and has no way to rebuild it, so whatever optimization it was packaged with is what it runs with for good. Co-authored-by: Claude Opus 5 (1M context) --- xtask/src/main.rs | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/xtask/src/main.rs b/xtask/src/main.rs index e0c0973..06fde02 100644 --- a/xtask/src/main.rs +++ b/xtask/src/main.rs @@ -119,11 +119,11 @@ fn require_cargo_fmi() -> Result<(), String> { /// Packages one FMU crate into `target/fmu`. /// -/// `--release` because the default is the dev profile, and a packaged -/// controller runs its step for every actor at every control instant, -/// where the host runs it once per call. The two profiles answer bit for -/// bit alike, which is what lets the golden tests compare a release FMU -/// against natively built laws. +/// `--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 the golden tests compare +/// a release FMU against natively built laws. fn package(name: &str) -> Result<(), String> { let status = cargo() .args(["fmi", "--package", name, "bundle", "--release"]) From 71356336c4a1044951f49d763cb309464ed0fbf0 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Fri, 14 Aug 2026 09:34:21 -1000 Subject: [PATCH 35/70] Keep each platform's packaged FMU as an artifact Every agent packages an FMU that carries only its own platform's binary, and until now each one was thrown away with the runner. The four together cover every platform this project builds for, so keeping them is what makes a single FMU that loads anywhere possible to assemble. That assembly is sound, which is worth saying before anything depends on it. Two packagings of the same source differ in `generationDateAndTime` and in nothing else, checked rather than assumed, and the instantiation token is a UUID derived from the model's name rather than from the build, so every platform's copy carries the same one. Combining them is therefore a matter of taking one modelDescription.xml and the binaries directory from each. `if-no-files-found: error` because a step that collected nothing would otherwise report success, which is the same failure the validation loop guards against one step earlier. Co-authored-by: Claude Opus 5 (1M context) --- .github/workflows/ci.yml | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 035e46a..24977c7 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -164,6 +164,20 @@ jobs: python -m fmpy validate "$fmu" done + # Each agent packages an FMU carrying its own platform's binary, so the + # four together hold every platform this project builds for. Kept so + # they can be combined into a single FMU that loads anywhere, which + # works because everything in modelDescription.xml except its timestamp + # is identical between them: the instantiation token is a UUID derived + # from the model's name rather than from the build. + - name: "FMU: upload" + uses: actions/upload-artifact@v7 + with: + name: fmu-${{ runner.os }}-${{ runner.arch }} + path: target/fmu/*.fmu + # A step that collected nothing would otherwise pass. + if-no-files-found: error + # 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 From 70f01d962c42468f4674e986107b65a831bfa711 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Fri, 14 Aug 2026 09:47:31 -1000 Subject: [PATCH 36/70] Record the dependency that will stop compiling Every build touching the FMU crate prints a future-incompatibility warning for `proc-macro-error2`, which re-exports a private `extern crate proc_macro`. rustc will make that an error, and when it does the FMU crate stops building. Nothing here can fix it. The crate arrives three levels down through `fmi-export-derive`, 2.0.1 is the newest release, the fix is an unmerged pull request, and it sits beside an open question about whether the crate is maintained, with no commit since September 2024. So this is watched rather than patched. A `[patch]` onto a fork would pin this workspace to a git revision of a transitive proc-macro dependency to silence a warning, which is a worse place to be than the warning. The note says what the options are for whoever meets it as an error: patch it then, wait for an `fmi-export` release that drops it, or hand-write the exports the derive generates. Co-authored-by: Claude Opus 5 (1M context) --- M6-PLAN.md | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/M6-PLAN.md b/M6-PLAN.md index 68d9f49..df63730 100644 --- a/M6-PLAN.md +++ b/M6-PLAN.md @@ -259,6 +259,18 @@ so nothing in M6 re-tests it per component. - **`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 will 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, + and every build touching the FMU crate prints the warning. Nothing + here can fix it: the crate is three levels down, 2.0.1 is the newest + release, and the fix sits in an unmerged pull request beside an open + question about whether the crate is maintained at all, with no commit + since September 2024. Watching rather than patching, since pinning a + fork of a transitive proc-macro dependency is a worse position than + the warning. What breaks when rustc turns it into an error is the FMU + crate's build, so the answer then is a `[patch]`, an `fmi-export` + release that drops the dependency, or hand-writing the exports. - 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 From 6379f92c738416e2aef9c1cb4383f60793e6f1bc Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Fri, 14 Aug 2026 09:56:04 -1000 Subject: [PATCH 37/70] Start only the parts of pygame that draw `pygame.init()` starts every subsystem pygame has, and one of them is the mixer, which goes looking for a sound card. On a CI runner there is none, so ALSA reports its way through eight lines of failed lookups before the step gets on with recording a clip. Nothing in the viewer makes a sound. So the font is initialized always, and the display only for a window that will actually be shown. A recording draws to a plain surface and needs no display at all, which is what lets it run over ssh or in CI in the first place. Both paths were run to check it: a clip records frame for frame as before, a window still opens, and the mixer stays uninitialized. Silencing the driver from the workflow instead would have left every other headless run saying the same thing. Co-authored-by: Claude Opus 5 (1M context) --- python/continuo_viz/render.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) 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: From 4f2d5d3540d45031b3b03630e1b7f69aadd39a68 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Fri, 14 Aug 2026 10:00:17 -1000 Subject: [PATCH 38/70] Open each script with a line that says what it runs GitHub puts the first line of a `run` block in the step's header, so a comment wrapped across several lines showed up as a fragment: the FMU validation announced itself as "Run # A glob matching nothing would expand to itself, check nothing and". Each of the two scripts now opens with one line saying what it does, and the reasoning that was there moves up into the workflow comment, which is where somebody reading the file looks and which never reaches a log. Co-authored-by: Claude Opus 5 (1M context) --- .github/workflows/ci.yml | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 24977c7..345a4f8 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -149,12 +149,13 @@ jobs: # An FMU our own importer accepts is not the same as a valid one, so a # second implementation is what says whether what we package is well - # formed. + # formed. The glob is guarded because one matching nothing expands to + # itself, checks nothing and passes, which is the failure this step + # exists to catch. - name: "FMU: validate" shell: bash run: | - # A glob matching nothing would expand to itself, check nothing and - # pass, which is the shape of failure this step exists to catch. + # Check every packaged FMU against a second implementation. packaged=(target/fmu/*.fmu) if [ ! -e "${packaged[0]}" ]; then echo "ERROR: no .fmu in target/fmu; this check would be vacuous" @@ -203,13 +204,15 @@ 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 From a48548cc28f784339c3df566048ef81a8574e6bc Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Fri, 14 Aug 2026 10:08:56 -1000 Subject: [PATCH 39/70] Check the profiles agree without building a release test binary Asserting the pinned hash under `--release` meant compiling the highway test optimized, which `cargo build --release --examples` does not cover, and enabling dev-dependencies rebuilds a slice of the graph besides. On ubuntu-latest that was about two minutes an agent, next to a three minute release build, for one assertion. The property wanted is that the two profiles agree, and the pin itself is already asserted where the tests run debug. So this runs the demo under each profile, using binaries both build steps have already produced, and compares the hash it prints. Under a second, and it says the same thing. Two guards, because a comparison is the easiest check to make vacuous: an empty capture would match another empty one, so a demo that stopped printing its hash would pass this rather than fail it. Both were run against their failing case to see them fire. Co-authored-by: Claude Opus 5 (1M context) --- .github/workflows/ci.yml | 30 +++++++++++++++++++++++------- 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 345a4f8..e421911 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -133,14 +133,30 @@ jobs: 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 says the pin covers - # both. rustc does not reassociate float arithmetic at any optimization - # level, which is why the two agree; this is what would notice if that - # ever stopped being true, on whichever of the four agents it stopped. + # 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" - run: > - cargo test --release -p continuo-examples --test highway - the_demo_world_hashes_to_the_same_value_on_every_platform + 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 # Names no package: the task finds every `continuo-fmu-*` crate itself, # so a second FMU crate needs no edit here. From 5c673f458da148d7a1fa4dc4caf7bc1801bcffa9 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Fri, 14 Aug 2026 16:06:21 -1000 Subject: [PATCH 40/70] Note that the viewer counts components and says actors A demo run's check summary reports 32 joins and 16 leaves above 15 actors seen, which reads as a discrepancy. It is not one: membership is published per component, so each car raises two joins, and the count divides by two where the actor count does not. Deferred rather than fixed, since the label is the whole of it and the CI smoke asserts the current wording, so the two have to move together. This records what the numbers mean in the meantime. Co-authored-by: Claude Opus 5 (1M context) --- PLAN.md | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/PLAN.md b/PLAN.md index 4210ed8..56840dc 100644 --- a/PLAN.md +++ b/PLAN.md @@ -769,3 +769,13 @@ 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. From ec63f6254e5f1665bb28acc20a1992ddd2f03df5 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Fri, 14 Aug 2026 17:10:57 -1000 Subject: [PATCH 41/70] Combine FMUs carrying different platforms' binaries into one FMI 3.0 expects a single archive to hold every platform it supports. 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, so a user downloads one file rather than picking the right of four. Packaging on several machines is why it exists, and nothing in it depends on that. Any set of FMUs meeting the same description merges the same way, whether they arrived from a pipeline, a download, or four folders arranged by hand. Merging works because the archives agree on everything outside `binaries/`. Two packagings of one source differ in `generationDateAndTime` and nowhere else, and the instantiation token is a UUID derived from the model's name rather than from the build, so the description from any one of them describes them all. The first is kept and the rest checked against it with that timestamp discounted, since two archives describing different models would combine into one that misdescribes both. They are grouped by file name, which is the model's library name, so a directory holding several models' FMUs merges each set separately. That is what a workspace packaging two FMU crates produces: every platform's output holds a build of each, so the sets run across the directories rather than within them. It takes one directory and searches it rather than being handed each archive, and both that and the output directory default to the shell's working directory, so a run in a folder of downloads needs no arguments. A glob would have left this working everywhere but Windows, where neither the shell nor Python expands one, and searching makes the layout underneath irrelevant, since builds of one FMU share a name and cannot sit in one directory anyway. Merging in place is then the ordinary case, so a second run finds what the first one wrote: that is a merge of the others, would collide with all of them, and is about to be replaced, so it is passed over. An archive at that path carrying one platform is something else, an input that happens to sit where the output goes, and overwriting it would drop its platform from the result silently. It is the only file a run could destroy, so it stops instead. Python rather than an xtask because the operation is a zip rewrite rather than anything cargo does, and a job that only downloads and repacks should not need a Rust toolchain. It sits under `python/scripts` so the ruff and pytest already running there reach it, and outside `continuo_viz` so it stays out of the viewer's wheel. Checked against the four artifacts of a real CI run rather than only the archives the tests fabricate: the merged FMU carries all four platforms under one description, fmpy validates it, and stepping it through fmpy answers exactly what the locally packaged one does. Co-authored-by: Claude Opus 5 (1M context) --- python/pyproject.toml | 3 + python/scripts/merge_fmus.py | 235 ++++++++++++++++++++++++++++++++ python/tests/test_merge_fmus.py | 169 +++++++++++++++++++++++ 3 files changed, 407 insertions(+) create mode 100644 python/scripts/merge_fmus.py create mode 100644 python/tests/test_merge_fmus.py 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"} From 48ff15048220175a191812d7528ab4352a675bae Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Fri, 14 Aug 2026 17:11:39 -1000 Subject: [PATCH 42/70] Publish one FMU per model, carrying every platform CI now keeps each agent's packaged FMUs as an artifact, and a second workflow combines them. What a user wants is one file per model that loads wherever they are, and until now the four platforms only ever existed as four separate downloads. The artifacts are named for the platform rather than the model, and hold every FMU the workspace packages, because an upload step produces one artifact and a workflow cannot loop over steps. Naming them per model would mean a step per model to keep in step with the crates. The merge groups by file name instead, which needs no edit when a second FMU crate arrives. Triggered by CI finishing rather than by a clock. The event names the run whose artifacts to take, so there is no searching for the latest green build and no asking whether this commit was merged already: one merged FMU per commit that reaches main, by construction. It skips a failed run, since agents that got as far as packaging still upload, and merging those would publish an FMU from a commit that did not pass. Reading another run's artifacts needs `actions: read`, which a `workflow_run` job does not get by default, so the two permissions it uses are named and the rest dropped. The merged FMUs go through fmpy as well. Merging rewrites the archive, so validating what CI packaged says nothing about what comes out of this. Co-authored-by: Claude Opus 5 (1M context) --- .github/workflows/ci.yml | 15 +++---- .github/workflows/fmu-merge.yml | 76 +++++++++++++++++++++++++++++++++ 2 files changed, 83 insertions(+), 8 deletions(-) create mode 100644 .github/workflows/fmu-merge.yml diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index e421911..292ca65 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -171,7 +171,7 @@ jobs: - name: "FMU: validate" shell: bash run: | - # Check every packaged FMU against a second implementation. + # Validate every packaged FMU with fmpy. packaged=(target/fmu/*.fmu) if [ ! -e "${packaged[0]}" ]; then echo "ERROR: no .fmu in target/fmu; this check would be vacuous" @@ -181,16 +181,15 @@ jobs: python -m fmpy validate "$fmu" done - # Each agent packages an FMU carrying its own platform's binary, so the - # four together hold every platform this project builds for. Kept so - # they can be combined into a single FMU that loads anywhere, which - # works because everything in modelDescription.xml except its timestamp - # is identical between them: the instantiation token is a UUID derived - # from the model's name rather than from the build. + # 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: fmu-${{ runner.os }}-${{ runner.arch }} + 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..33c49a8 --- /dev/null +++ b/.github/workflows/fmu-merge.yml @@ -0,0 +1,76 @@ +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. +# +# Triggered by CI finishing rather than by a clock: the event names the run +# whose artifacts to take, so there is no searching for the latest green build +# and no asking whether this commit was merged already. One merged FMU per +# commit that reaches main, by construction. +on: + workflow_run: + workflows: ["CI"] + types: [completed] + branches: [main] + workflow_dispatch: + +# Reading another run's artifacts needs `actions: read`, which a `workflow_run` +# job does not get by default. Naming the two it uses also drops the rest. +permissions: + actions: read + contents: read + +jobs: + merge_fmus: + # A failed CI run still uploads whatever agents got that far, and merging + # those would publish an FMU from a commit that did not pass. + if: > + github.event_name == 'workflow_dispatch' || + github.event.workflow_run.conclusion == 'success' + 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: ${{ github.event.workflow_run.id || github.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 From 90ea18f618002cd3a535deb093dd0734639e01e1 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Fri, 14 Aug 2026 17:26:56 -1000 Subject: [PATCH 43/70] Build the fixed proc-macro-error2 rather than warning about it Every build touching the FMU crate printed a future-incompatibility warning: `proc-macro-error2`, three levels down through `fmi-export-derive`, re-exports a private `extern crate proc_macro`, which rustc will make an error. The earlier note said to wait for a release, and there will not be one. The crate's repository is archived, last pushed September 2024 with a dozen issues frozen, so 2.0.1 is the last version there will ever be. The workspace now patches it to the two-line change rustc's own help suggests, taken from the pull request that was open when the repository was archived, pinned by revision so what builds here is a commit that was read rather than whatever a branch points at later. Suppressing the notice was tried first and dropped. Cargo's `frequency = "never"` is workspace wide, so it would have hidden every other dependency's warnings to quiet one, and it would have left the hard error still coming. Patching removes the cause instead: the day rustc turns this into an error, nothing here breaks. The cost is a build that depends on a personal fork staying reachable, and vendoring the crate is the way to remove that if it ever matters. What ends it properly is `fmi-export` dropping the dependency, which is what to look for when it next releases. Co-authored-by: Claude Opus 5 (1M context) --- Cargo.lock | 6 ++---- Cargo.toml | 14 ++++++++++++++ M6-PLAN.md | 32 ++++++++++++++++++++------------ 3 files changed, 36 insertions(+), 16 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index c613eff..51c1480 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -2264,8 +2264,7 @@ dependencies = [ [[package]] name = "proc-macro-error-attr2" version = "2.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "96de42df36bb9bba5542fe9f1a054b8cc87e172759a1868aa05c1f3acc89dfc5" +source = "git+https://github.com/fgardt/proc-macro-error-2?rev=53ff94b61c678593158db1a3d50269c6f0de0db7#53ff94b61c678593158db1a3d50269c6f0de0db7" dependencies = [ "proc-macro2", "quote", @@ -2274,8 +2273,7 @@ dependencies = [ [[package]] name = "proc-macro-error2" version = "2.0.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "11ec05c52be0a07b08061f7dd003e7d7092e0472bc731b4af7bb1ef876109802" +source = "git+https://github.com/fgardt/proc-macro-error-2?rev=53ff94b61c678593158db1a3d50269c6f0de0db7#53ff94b61c678593158db1a3d50269c6f0de0db7" dependencies = [ "proc-macro-error-attr2", "proc-macro2", diff --git a/Cargo.toml b/Cargo.toml index e52bf95..c2033a4 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -30,3 +30,17 @@ 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 (rust-lang/rust +# issue 127909), and until then it warns on every build that touches the FMU +# crate. The crate's repository is archived, so 2.0.1 is the last release there +# will be and there is nothing to upgrade to. +# +# This is the two-line fix rustc's own help suggests, taken from the pull +# request that was open when the repository was archived. Pinned by revision, +# so what builds here is a commit that was read rather than whatever a branch +# points at 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/M6-PLAN.md b/M6-PLAN.md index df63730..367a656 100644 --- a/M6-PLAN.md +++ b/M6-PLAN.md @@ -259,18 +259,26 @@ so nothing in M6 re-tests it per component. - **`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 will 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, - and every build touching the FMU crate prints the warning. Nothing - here can fix it: the crate is three levels down, 2.0.1 is the newest - release, and the fix sits in an unmerged pull request beside an open - question about whether the crate is maintained at all, with no commit - since September 2024. Watching rather than patching, since pinning a - fork of a transitive proc-macro dependency is a worse position than - the warning. What breaks when rustc turns it into an error is the FMU - crate's build, so the answer then is a `[patch]`, an `fmi-export` - release that drops the dependency, or hand-writing the exports. + - **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, and it warned on every build touching the FMU + crate. The crate's repository is archived, last pushed September 2024 + with a dozen issues frozen, so 2.0.1 is the last release there will be + and waiting for a fix was never an option. + - The workspace patches it to the two-line change rustc's own help + suggests, taken from the pull request that was open when the + repository was archived, 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 From ded042166fbcd86d3ae1f45d673a9cbdf9d85559 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Fri, 14 Aug 2026 17:33:38 -1000 Subject: [PATCH 44/70] Link what the patch came from and why there is no upgrade The comment said the repository was archived without linking anything, so a reader had to take both halves on trust: that the fix is a known proposal rather than something invented here, and that no release will ever supersede it. Both are one click away now. The rustc issue says what the warning is, and the pull request shows the two lines being pinned, which is also where anyone checking the patch should start. Co-authored-by: Claude Opus 5 (1M context) --- Cargo.toml | 22 +++++++++++++--------- M6-PLAN.md | 15 +++++++++------ 2 files changed, 22 insertions(+), 15 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index c2033a4..e7e199c 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -32,15 +32,19 @@ 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 (rust-lang/rust -# issue 127909), and until then it warns on every build that touches the FMU -# crate. The crate's repository is archived, so 2.0.1 is the last release there -# will be and there is nothing to upgrade to. +# `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 # -# This is the two-line fix rustc's own help suggests, taken from the pull -# request that was open when the repository was archived. Pinned by revision, -# so what builds here is a commit that was read rather than whatever a branch -# points at later. It goes when `fmi-export` stops depending on the crate, -# which is the only thing that ends this properly. +# 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/M6-PLAN.md b/M6-PLAN.md index 367a656..84c4683 100644 --- a/M6-PLAN.md +++ b/M6-PLAN.md @@ -262,13 +262,16 @@ so nothing in M6 re-tests it per component. - **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, and it warned on every build touching the FMU - crate. The crate's repository is archived, last pushed September 2024 - with a dozen issues frozen, so 2.0.1 is the last release there will be - and waiting for a fix was never an option. + [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, taken from the pull request that was open when the - repository was archived, pinned by revision. Suppressing the notice + 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 From 7894efa5fde5bc58618863daca920a4821377e01 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Fri, 14 Aug 2026 17:40:38 -1000 Subject: [PATCH 45/70] Note the verify task the xtask should grow CLAUDE.md lists five commands that have to pass before a commit, in CI's order so a formatting slip is reported in seconds rather than after a compile, and they are typed by hand every time. One task would run them in that order and stop at the first failure. Recorded rather than written, with the two questions it cannot answer by guessing: whether it runs the Python half, which needs an interpreter a Rust-only change has no reason to have, and whether it packages the FMUs first, since the golden tests read what packaging last wrote and would otherwise pass against a stale artifact. Co-authored-by: Claude Opus 5 (1M context) --- PLAN.md | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/PLAN.md b/PLAN.md index 56840dc..cf0b7bc 100644 --- a/PLAN.md +++ b/PLAN.md @@ -779,3 +779,21 @@ than churning the fingerprint twice. 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. + +- **`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 golden tests compare + against what `cargo xtask package-fmus` last wrote, so a verify that + skipped it would pass against a stale artifact. + - 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. From 36d57238d652eaa1c8d7832be599b2e9fb2ecfef Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Fri, 14 Aug 2026 18:28:53 -1000 Subject: [PATCH 46/70] Compare the packaged FMU against the laws it carries An FMU crate's cdylib links continuo-actors statically, so the .fmu ships its own compiled copy of the control laws and nothing calls back into the host. Editing a law leaves that copy behind, and until now nothing said so. This adds the comparison: the packaged FMU driven across the FMI boundary against the same laws called in this process, equal to the bit. Three sweeps share one grid of situations, over the demo's straight road, a five-bend polyline and a closed ellipse. The straight road alone would prove little, since its heading is the same everywhere and a road that crossed as its first two points would pass it; the loop is what asks whether road_closed traveled, the field whose absence would have been silent. Each sweep then asserts it provoked acceleration, braking and a turn each way, because a comparison of two zeros passes whatever is wrong on either side. Beside them sit the padding past the point count, a short scan against a padded one, a tunable parameter set again between steps, and the refusals, which nothing but a person had checked before. They are behind a packaged-fmu feature rather than in the workspace suite. Everything here reads a file `cargo xtask package-fmus` writes, and nothing else in the workspace needs anything packaged, so a change that never reaches this crate should not have to build a cdylib to run the tests. `required-features` is what gates it, so cargo skips the target rather than a test skipping itself at run time. What that costs is the run where it matters most, since editing a law and running the workspace suite now says nothing about whether the FMU still agrees. CI answers that by turning the feature on in the step after packaging and ahead of the upload, so a run whose FMU disagrees with its own laws publishes nothing, and CLAUDE.md says when to turn it on by hand. Clippy and the debug build take --all-features for a plainer reason. A target held behind a feature is not passed to clippy at all, so without it this file would be the one thing in the workspace nothing lints. A mismatch names `cargo xtask package-fmus`, because a stale package presents as the IDM math disagreeing with itself, which says nothing about where to look. None of this is a golden test, whatever the milestone plan called them. A golden test compares against a stored reference, where both sides here are computed afresh and the claim is that two builds of one implementation agree. The word goes from the four other places it had reached. Nothing outside the tests changes behavior, and the demo hash holds. Co-authored-by: Claude Opus 5 (1M context) --- .github/workflows/ci.yml | 25 +- CLAUDE.md | 15 +- Cargo.lock | 2 + crates/continuo-fmu-controller-idm/Cargo.toml | 32 +- .../src/package_fmu.rs | 8 +- .../tests/packaged_fmu.rs | 652 ++++++++++++++++++ xtask/src/main.rs | 4 +- 7 files changed, 725 insertions(+), 13 deletions(-) create mode 100644 crates/continuo-fmu-controller-idm/tests/packaged_fmu.rs diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 292ca65..15f205e 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -89,8 +89,11 @@ jobs: - 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 @@ -107,9 +110,11 @@ jobs: 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. + # 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 + run: cargo build --workspace --all-targets --all-features # `--lib` and `--test '*'` partition the suite: the first runs the unit # tests inside each lib, the second the integration targets in tests/. @@ -181,6 +186,20 @@ jobs: python -m fmpy validate "$fmu" done + # The comparison packaging exists to make possible: the `.fmu` just + # built against the laws compiled into it, over the boundary rather + # than beside it. It sits behind a feature so the workspace suite + # above needs nothing packaged, and it names its package because it is + # one crate's own suite rather than a workspace-wide step. A second + # FMU crate compares different laws, so it brings its own line here. + # + # Ahead of the upload, so a run whose FMU disagrees with the laws it + # was built from publishes nothing. + - name: "FMU: test against the laws it was built from" + run: > + cargo test -p continuo-fmu-controller-idm + --features packaged-fmu --test packaged_fmu + # 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 diff --git a/CLAUDE.md b/CLAUDE.md index ac1ac83..0f7c073 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 -p continuo-fmu-controller-idm --features packaged-fmu +``` + +`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 51c1480..742e637 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -481,8 +481,10 @@ version = "0.1.0" dependencies = [ "continuo-actors", "continuo-core", + "continuo-fmi", "fmi", "fmi-export", + "serde_json", "thiserror 2.0.18", ] diff --git a/crates/continuo-fmu-controller-idm/Cargo.toml b/crates/continuo-fmu-controller-idm/Cargo.toml index d374dac..cadfde6 100644 --- a/crates/continuo-fmu-controller-idm/Cargo.toml +++ b/crates/continuo-fmu-controller-idm/Cargo.toml @@ -11,12 +11,26 @@ rust-version.workspace = true # matters is the packaged .fmu rather than the crate. publish = false -# cdylib is what FMI loads; rlib is what `packaged_fmu_path` and the golden -# tests link against. Neither can be feature-gated, since `cargo fmi bundle`, -# which packages the FMU, builds with default features. +# 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] +# Whether a packaged `.fmu` is there to test against. Off by default, so +# `cargo test --workspace` skips the target below rather than failing on a +# missing package: nothing else in the workspace needs anything packaged, and +# a Rust change that never reaches this crate should not have to build a +# cdylib to run the suite. +# +# What that costs is the run where it matters most. Editing a control law and +# running the workspace suite says nothing about whether the packaged FMU +# still agrees, because the `.fmu` carries its own compiled copy. CI turns +# this on in the step after packaging, so nothing merges unchecked, and +# CLAUDE.md says when to turn it on locally. +packaged-fmu = [] + [dependencies] continuo-actors = { workspace = true } continuo-core = { workspace = true } @@ -26,6 +40,18 @@ fmi = { workspace = true } fmi-export = { workspace = true } thiserror = { workspace = true } +# The comparison below imports the packaged FMU and drives 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 } + +# Declared so `required-features` can be, which is what gates it. Everything +# else about the target is cargo's default for a file in tests/. +[[test]] +name = "packaged_fmu" +required-features = ["packaged-fmu"] + # `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 diff --git a/crates/continuo-fmu-controller-idm/src/package_fmu.rs b/crates/continuo-fmu-controller-idm/src/package_fmu.rs index c8eba5d..4e8a981 100644 --- a/crates/continuo-fmu-controller-idm/src/package_fmu.rs +++ b/crates/continuo-fmu-controller-idm/src/package_fmu.rs @@ -1,10 +1,10 @@ //! 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 golden 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. +//! 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; 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..4010889 --- /dev/null +++ b/crates/continuo-fmu-controller-idm/tests/packaged_fmu.rs @@ -0,0 +1,652 @@ +//! 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. +//! +//! Behind the `packaged-fmu` feature, because everything here reads a file +//! `cargo xtask package-fmus` writes and no other suite in the workspace +//! does. See that feature in `Cargo.toml` for what gating it costs. +//! +//! 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. + +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, 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; +} + +/// Steps the FMU once in `situation` and returns what it commanded. +/// +/// Both outputs are bound to one key, so one message comes back carrying +/// both, which is the shape [`OutputBinding`] merges them into. +fn fmu_commands(fmu: &mut FmuComponent, tick: i64, situation: &Situation) -> (f64, 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("radar", json!({ "detections": situation.scan })), + ]; + + let mut ctx = StepCtx::new(now, None, 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. +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. The + // instance is reused across them because the laws are a map from + // inputs to commands: nothing carries from one step to the next but + // the road, which is what makes a sweep like this meaningful at all. + situations + .iter() + .enumerate() + .map(|(tick, situation)| { + let from_fmu = fmu_commands(&mut fmu, tick as i64, 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(), + ); + 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. +fn assert_both_laws_had_something_to_say(commands: &[(f64, f64)]) { + let any = |what: fn(&(f64, f64)) -> f64, sign: f64| { + commands + .iter() + .any(|command| what(command) * sign > 0.0) + .then_some(()) + }; + assert!(any(|c| c.0, 1.0).is_some(), "nothing accelerated"); + assert!(any(|c| c.0, -1.0).is_some(), "nothing braked"); + assert!(any(|c| c.1, 1.0).is_some(), "nothing turned left"); + assert!(any(|c| c.1, -1.0).is_some(), "nothing turned right"); +} + +/// Every situation worth putting a car in on `road`: along it, either side +/// of its lane, pointing off it, at four speeds, seeing four 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 situations_on(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. + one_detection(37, 40.0, -4.0), + // Close and closing hard, which is the braking end of the law. + one_detection(0, 6.0, -12.0), + // Pulling away, which is the sign the wanted gap has to be floored + // at zero for. + one_detection(63, 25.0, 9.0), + ]; + + 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 4 + // things ahead. + situations +} + +/// A scan holding one detection, in `slot`, padded out to there with +/// nothing. +/// +/// The padding is the free road 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 one_detection(slot: usize, range: f64, range_rate: f64) -> Vec { + let mut scan = vec![FREE_ROAD; slot + 1]; + scan[slot] = Detection { range, range_rate }; + + // Return the scan, which is shorter than the FMU's arrays wherever the + // slot is, so the mapping's own default fills the rest. + scan +} + +#[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 (tick, situation) in situations.iter().enumerate() { + let from_fmu = fmu_commands(&mut fmu, tick as i64, 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 })), + ]; + let mut ctx = StepCtx::new(now, None, 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 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/xtask/src/main.rs b/xtask/src/main.rs index 06fde02..7c787a2 100644 --- a/xtask/src/main.rs +++ b/xtask/src/main.rs @@ -122,8 +122,8 @@ fn require_cargo_fmi() -> Result<(), String> { /// `--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 the golden tests compare -/// a release FMU against natively built laws. +/// 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"]) From da3ca91c2c2e38b7e85e9af7817949b26ea0bc06 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Fri, 14 Aug 2026 18:48:31 -1000 Subject: [PATCH 47/70] Gate the comparison in the file rather than in the manifest `required-features` on the test target was the wrong half of the pair. CI runs its integration tests as `cargo test --workspace --test '*'`, and a glob names every target it matches, so cargo refuses the whole step over a named target whose features are off. The unqualified `cargo test --workspace` skips that target instead and passes, which is why this went green here and red on all four agents. A `#![cfg(feature = "packaged-fmu")]` on the file itself gates the same tests and leaves the target always building, empty when the feature is off, so no CI invocation has to learn about it. The `[[test]]` block goes with it, since cargo finds a file in tests/ on its own and the block carried nothing else. What each half of the gate has to say now sits beside the half it describes: the mechanical facts in the manifest, next to the feature, and what gating costs in the test file, next to the attribute. The barrier that would have caught this is the `cargo xtask verify` already deferred in PLAN.md, so that entry gains this as the case for it. The local commands and CI's are not the same commands, and only one of them was run. Co-authored-by: Claude Opus 5 (1M context) --- PLAN.md | 14 ++++++++--- crates/continuo-fmu-controller-idm/Cargo.toml | 23 ++++++------------- .../tests/packaged_fmu.rs | 11 +++++---- 3 files changed, 25 insertions(+), 23 deletions(-) diff --git a/PLAN.md b/PLAN.md index cf0b7bc..0723602 100644 --- a/PLAN.md +++ b/PLAN.md @@ -790,9 +790,17 @@ than churning the fingerprint twice. 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 golden tests compare - against what `cargo xtask package-fmus` last wrote, so a verify that - skipped it would pass against a stale artifact. + 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 diff --git a/crates/continuo-fmu-controller-idm/Cargo.toml b/crates/continuo-fmu-controller-idm/Cargo.toml index cadfde6..828bec3 100644 --- a/crates/continuo-fmu-controller-idm/Cargo.toml +++ b/crates/continuo-fmu-controller-idm/Cargo.toml @@ -18,17 +18,14 @@ publish = false crate-type = ["cdylib", "rlib"] [features] -# Whether a packaged `.fmu` is there to test against. Off by default, so -# `cargo test --workspace` skips the target below rather than failing on a -# missing package: nothing else in the workspace needs anything packaged, and -# a Rust change that never reaches this crate should not have to build a -# cdylib to run the suite. +# 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. # -# What that costs is the run where it matters most. Editing a control law and -# running the workspace suite says nothing about whether the packaged FMU -# still agrees, because the `.fmu` carries its own compiled copy. CI turns -# this on in the step after packaging, so nothing merges unchecked, and -# CLAUDE.md says when to turn it on locally. +# 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] @@ -46,12 +43,6 @@ thiserror = { workspace = true } continuo-fmi = { workspace = true } serde_json = { workspace = true } -# Declared so `required-features` can be, which is what gates it. Everything -# else about the target is cargo's default for a file in tests/. -[[test]] -name = "packaged_fmu" -required-features = ["packaged-fmu"] - # `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 diff --git a/crates/continuo-fmu-controller-idm/tests/packaged_fmu.rs b/crates/continuo-fmu-controller-idm/tests/packaged_fmu.rs index 4010889..00a8952 100644 --- a/crates/continuo-fmu-controller-idm/tests/packaged_fmu.rs +++ b/crates/continuo-fmu-controller-idm/tests/packaged_fmu.rs @@ -16,14 +16,17 @@ //! numbers it was built from all have somewhere to go wrong that no //! compiler checks. //! -//! Behind the `packaged-fmu` feature, because everything here reads a file -//! `cargo xtask package-fmus` writes and no other suite in the workspace -//! does. See that feature in `Cargo.toml` for what gating it costs. -//! //! 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::{ From 1ff3710cad1e2ed3099fdb9e776e5f1cade262d8 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Fri, 14 Aug 2026 23:19:34 -1000 Subject: [PATCH 48/70] Say what the vacuity guard checks rather than encoding it The guard took a field selector and a sign, multiplied the two and compared against zero, so `1.0` and `-1.0` sat in argument positions where they read as thresholds rather than as directions. Anyone asking whether it looked for an acceleration of 1.0 was reading it the way it was written. It then turned its answer into an `Option<()>` for each assertion to turn straight back into the bool it started as. Four direct assertions say the same thing with nothing to decode: an acceleration above zero, one below it, and a yaw rate either way. The one property worth keeping is kept, since a NaN satisfies neither comparison, so a command that diverged cannot stand as evidence that anything was commanded. That is now said rather than left to be noticed. Co-authored-by: Claude Opus 5 (1M context) --- .../tests/packaged_fmu.rs | 29 ++++++++++++------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/crates/continuo-fmu-controller-idm/tests/packaged_fmu.rs b/crates/continuo-fmu-controller-idm/tests/packaged_fmu.rs index 00a8952..563d8a4 100644 --- a/crates/continuo-fmu-controller-idm/tests/packaged_fmu.rs +++ b/crates/continuo-fmu-controller-idm/tests/packaged_fmu.rs @@ -297,17 +297,26 @@ fn commands_over( /// 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)]) { - let any = |what: fn(&(f64, f64)) -> f64, sign: f64| { - commands - .iter() - .any(|command| what(command) * sign > 0.0) - .then_some(()) - }; - assert!(any(|c| c.0, 1.0).is_some(), "nothing accelerated"); - assert!(any(|c| c.0, -1.0).is_some(), "nothing braked"); - assert!(any(|c| c.1, 1.0).is_some(), "nothing turned left"); - assert!(any(|c| c.1, -1.0).is_some(), "nothing turned right"); + 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" + ); } /// Every situation worth putting a car in on `road`: along it, either side From 8c6eb5952c7657630f34d07e4c7cb921221161f0 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Fri, 14 Aug 2026 23:25:38 -1000 Subject: [PATCH 49/70] Point the dev-dependency comment at what actually follows it It opened "the comparison below", where what is below is the dependency list and the comparison is in another file. Naming `tests/packaged_fmu.rs` says whose dependencies these are, which is the question a reader of a manifest is asking. Co-authored-by: Claude Opus 5 (1M context) --- crates/continuo-fmu-controller-idm/Cargo.toml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/crates/continuo-fmu-controller-idm/Cargo.toml b/crates/continuo-fmu-controller-idm/Cargo.toml index 828bec3..ae1284a 100644 --- a/crates/continuo-fmu-controller-idm/Cargo.toml +++ b/crates/continuo-fmu-controller-idm/Cargo.toml @@ -37,8 +37,9 @@ fmi = { workspace = true } fmi-export = { workspace = true } thiserror = { workspace = true } -# The comparison below imports the packaged FMU and drives it as a component, -# which is the only way to reach it across the boundary rather than beside it. +# 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 } From b320ca04f79a43bf25fb086c95fe107e5e10aa72 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Fri, 14 Aug 2026 23:25:48 -1000 Subject: [PATCH 50/70] Put more than one car in the scan the FMU chooses from Every scan crossing the boundary held at most one real detection, so nothing distinguished picking the nearest from picking the only one. The sweep now also sends three at once with the nearest in neither the first slot nor the last, two at the same range closing at different rates, and one filling every slot the FMU declares, where nothing is padding and the whole array is set from the message rather than from the mapping. Those alone would not be enough, because the sweep runs `nearest_detection` on both sides of the boundary and would agree on the wrong car as readily as on the right one. So a test beside them names the answer instead: given three, the command has to be the one the nearest alone would give, and it has to differ from what each of the other two would have given, or the assertion could not tell them apart. A tie is checked the same way, since ties go to the earlier slot and that is what makes a scan answer the same way every time it is read. `one_detection` becomes `scan_of`, taking as many detections as a caller wants to place. Co-authored-by: Claude Opus 5 (1M context) --- .../tests/packaged_fmu.rs | 122 ++++++++++++++++-- 1 file changed, 109 insertions(+), 13 deletions(-) diff --git a/crates/continuo-fmu-controller-idm/tests/packaged_fmu.rs b/crates/continuo-fmu-controller-idm/tests/packaged_fmu.rs index 563d8a4..294e497 100644 --- a/crates/continuo-fmu-controller-idm/tests/packaged_fmu.rs +++ b/crates/continuo-fmu-controller-idm/tests/packaged_fmu.rs @@ -320,7 +320,7 @@ fn assert_both_laws_had_something_to_say(commands: &[(f64, f64)]) { } /// Every situation worth putting a car in on `road`: along it, either side -/// of its lane, pointing off it, at four speeds, seeing four different +/// 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 @@ -346,12 +346,25 @@ fn situations_on(road: &Waypoints) -> Vec { 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. - one_detection(37, 40.0, -4.0), + scan_of(&[(37, 40.0, -4.0)]), // Close and closing hard, which is the braking end of the law. - one_detection(0, 6.0, -12.0), + scan_of(&[(0, 6.0, -12.0)]), // Pulling away, which is the sign the wanted gap has to be floored // at zero for. - one_detection(63, 25.0, 9.0), + 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(); @@ -372,27 +385,54 @@ fn situations_on(road: &Waypoints) -> Vec { } } - // Return the lot: 36 poses, at each of 4 speeds, seeing each of 4 + // Return the lot: 36 poses, at each of 4 speeds, seeing each of 7 // things ahead. situations } -/// A scan holding one detection, in `slot`, padded out to there with -/// nothing. +/// A scan holding a detection in each `(slot, range, range_rate)` given, +/// free road in the slots between them. /// -/// The padding is the free road rather than an absence, which is how a +/// 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 one_detection(slot: usize, range: f64, range_rate: f64) -> Vec { - let mut scan = vec![FREE_ROAD; slot + 1]; - scan[slot] = Detection { range, range_rate }; +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 is shorter than the FMU's arrays wherever the - // slot is, so the mapping's own default fills the rest. + // 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 @@ -623,6 +663,62 @@ fn a_tuning_parameter_changed_between_steps_changes_the_command() { 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, 0, &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, 1, &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 From dfc1af8bd46a2aa6714a266fd10ec1a4d2d9a294 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Fri, 14 Aug 2026 23:30:34 -1000 Subject: [PATCH 51/70] Put the car where nobody thought to put it Every situation the sweeps ran was one somebody chose, and a chosen case is aimed at something it already suspects. So each sweep now runs 250 more that are aimed at nothing: anywhere from a fifth of a road before its start to a fifth past its end, twelve meters off to either side, pointing any direction at all, at any speed up to 45, seeing up to five things ahead at gaps from smaller than a car to three hundred meters. None of that is a world this project runs. All of it is a value a host in another tool can set, and the FMI boundary is where such a host arrives. From a fixed seed, and from the workspace's own generator rather than a crate, which are the same decision twice. A failure nobody can reproduce is a failure nobody can fix, so the situations have to be the same on every agent and every run, and SplitMix64 is integer arithmetic that answers alike on all four platforms. The seed is arbitrary; what matters is that it never changes. Co-authored-by: Claude Opus 5 (1M context) --- .../tests/packaged_fmu.rs | 88 +++++++++++++++++-- 1 file changed, 82 insertions(+), 6 deletions(-) diff --git a/crates/continuo-fmu-controller-idm/tests/packaged_fmu.rs b/crates/continuo-fmu-controller-idm/tests/packaged_fmu.rs index 294e497..a657b0c 100644 --- a/crates/continuo-fmu-controller-idm/tests/packaged_fmu.rs +++ b/crates/continuo-fmu-controller-idm/tests/packaged_fmu.rs @@ -34,8 +34,8 @@ use continuo_actors::control_laws::{ }; use continuo_actors::{MAX_DETECTIONS, Waypoints}; use continuo_core::{ - Component, ComponentPath, CoreError, Detection, KeyExpr, Message, Pose, Quat, SimDuration, - SimTime, StepCtx, + 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}; @@ -319,14 +319,29 @@ fn assert_both_laws_had_something_to_say(commands: &[(f64, f64)]) { ); } -/// Every situation worth putting a car in on `road`: along it, either side -/// of its lane, pointing off it, at four speeds, seeing seven different -/// things ahead. +/// 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 situations_on(road: &Waypoints) -> Vec { +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 @@ -390,6 +405,67 @@ fn situations_on(road: &Waypoints) -> Vec { 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 stays under a + // second, which is what keeps it something to run while editing. + 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. /// From 6ca1509c9d6477704776c5406378bd866fc0e630 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Fri, 14 Aug 2026 23:34:09 -1000 Subject: [PATCH 52/70] Select the comparison by target name rather than by package The step named `continuo-fmu-controller-idm`, so a second FMU crate would have needed a second line beside it, which is the edit `package-fmus` exists to avoid making anywhere else. Cargo can select across a workspace by target name, so the step asks for `--test packaged_fmu` and runs whichever crates have one. That rests on every FMU crate spelling it the same way, the comparison in `tests/packaged_fmu.rs` behind a `packaged-fmu` feature, which is the convention this crate sets and the step now states. Cargo fails outright when no package has that target, listing what does exist, so the convention cannot lapse into a step that quietly checks nothing. What it does not catch is a crate spelling it differently, which would be skipped in silence. Discovering `continuo-fmu-*` crates in the xtask and running each would catch that, and is not bought here: it would be a second way to run tests where cargo already has one, for a crate that does not exist yet. CLAUDE.md's local command moves with it, so what a person is told to type is what CI types. The last failure on this branch came from those two being different commands. Co-authored-by: Claude Opus 5 (1M context) --- .github/workflows/ci.yml | 17 ++++++++--------- CLAUDE.md | 2 +- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 15f205e..f8cc8f6 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -186,19 +186,18 @@ jobs: python -m fmpy validate "$fmu" done - # The comparison packaging exists to make possible: the `.fmu` just - # built against the laws compiled into it, over the boundary rather - # than beside it. It sits behind a feature so the workspace suite - # above needs nothing packaged, and it names its package because it is - # one crate's own suite rather than a workspace-wide step. A second - # FMU crate compares different laws, so it brings its own line here. + # Checks the `.fmu` just packaged against the laws compiled into it, + # reached through FMI rather than called directly, which is what + # catches a package older than the laws. + # + # Names no package. Every FMU crate spells this the same way, a + # `tests/packaged_fmu.rs` behind a `packaged-fmu` feature, so + # selecting by target name runs all of them. # # Ahead of the upload, so a run whose FMU disagrees with the laws it # was built from publishes nothing. - name: "FMU: test against the laws it was built from" - run: > - cargo test -p continuo-fmu-controller-idm - --features packaged-fmu --test packaged_fmu + run: cargo test --workspace --all-features --test packaged_fmu # Every FMU the workspace packages, each carrying this platform's # binary alone. One artifact per platform rather than per FMU, because diff --git a/CLAUDE.md b/CLAUDE.md index 0f7c073..b04eacd 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -83,7 +83,7 @@ links and the suite above compares nothing against it: ```sh cargo xtask package-fmus -cargo test -p continuo-fmu-controller-idm --features packaged-fmu +cargo test --workspace --all-features --test packaged_fmu ``` `continuo-actors` is the one to watch, since editing a control law there From c3cffe7906ebeaa7538e3dacb7a83120dc75b43d Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Sat, 15 Aug 2026 12:06:39 -1000 Subject: [PATCH 53/70] Ask for the feature by name rather than for all of them `--all-features` reached the comparison by happening to include it, which is a weaker thing than asking. Cargo refuses a feature no selected package defines, so a rename fails this step, where the blunt flag would enable whatever the feature became and leave a target compiling to nothing that passes. Clippy catches that same rename through `unexpected_cfgs`, so this is a second guard rather than the only one, and it is the one that fails where the comparison is meant to run. It also stops the step widening on its own, since `--all-features` would take in any non-default feature a workspace crate grows later. Co-authored-by: Claude Opus 5 (1M context) --- .github/workflows/ci.yml | 6 +++--- CLAUDE.md | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index f8cc8f6..133ce4a 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -191,13 +191,13 @@ jobs: # catches a package older than the laws. # # Names no package. Every FMU crate spells this the same way, a - # `tests/packaged_fmu.rs` behind a `packaged-fmu` feature, so - # selecting by target name runs all of them. + # `tests/packaged_fmu.rs` behind a `packaged-fmu` feature, so asking + # for both names runs all of them. # # Ahead of the upload, so a run whose FMU disagrees with the laws it # was built from publishes nothing. - name: "FMU: test against the laws it was built from" - run: cargo test --workspace --all-features --test packaged_fmu + run: cargo test --workspace --features packaged-fmu --test packaged_fmu # Every FMU the workspace packages, each carrying this platform's # binary alone. One artifact per platform rather than per FMU, because diff --git a/CLAUDE.md b/CLAUDE.md index b04eacd..4de03d3 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -83,7 +83,7 @@ links and the suite above compares nothing against it: ```sh cargo xtask package-fmus -cargo test --workspace --all-features --test packaged_fmu +cargo test --workspace --features packaged-fmu --test packaged_fmu ``` `continuo-actors` is the one to watch, since editing a control law there From bccc377a45b93c415c8ebe6bed0faa103eb2de68 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Sat, 15 Aug 2026 12:20:50 -1000 Subject: [PATCH 54/70] Ask CI why the comparison rebuilds what it already has Temporary, and this commit is meant to be reverted once the run reports. The packaged-FMU step rebuilds fourteen crates every run, `fmi-sys` among them, which is 52 of its 71 seconds on Windows. Nothing about it reproduces off CI: replaying the whole sequence locally, release build and packaging included, rebuilds one crate in two seconds. The flag form is not the cause either, since the step measured 79 seconds naming the package, 70 with `--all-features` and 71 asking for the feature. So this asks rather than guesses. `-v` makes cargo print its own reason before each rebuild, which is what disproved my first theory locally. The three probes count `fmi-sys` fingerprints and rlibs after the cache restore, after the debug build, and immediately before the comparison, so the readings together separate a cache that never carried those units from a run that takes them away. Co-authored-by: Claude Opus 5 (1M context) --- .github/workflows/ci.yml | 34 +++++++++++++++++++++++++++++++++- 1 file changed, 33 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 133ce4a..2b9dd1a 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -51,6 +51,19 @@ jobs: - uses: Swatinem/rust-cache@v2 + # TEMPORARY, out again once it has reported. The packaged-FMU step + # rebuilds fourteen crates every run, `fmi-sys` among them, which is + # 52 of its 70 seconds on Windows and reproduces on no other machine. + # This probe runs again after each step that builds the same units, so + # the readings together say whether the cache never carried them or + # something in the run took them away. + - name: "Diagnose: fmi-sys after the cache restore" + shell: bash + run: | + # Count what is on disk for fmi-sys, whose rebuild costs the most. + echo "fingerprints: $(ls -d target/debug/.fingerprint/fmi-sys-* 2>/dev/null | wc -l)" + echo "rlibs: $(ls target/debug/deps/libfmi_sys*.rlib 2>/dev/null | wc -l)" + # 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 @@ -116,6 +129,14 @@ jobs: - name: "Build: debug" run: cargo build --workspace --all-targets --all-features + # TEMPORARY, with the probe above. + - name: "Diagnose: fmi-sys after the debug build" + shell: bash + run: | + # The same count, after a step that needed fmi-sys and did not rebuild it. + echo "fingerprints: $(ls -d target/debug/.fingerprint/fmi-sys-* 2>/dev/null | wc -l)" + echo "rlibs: $(ls target/debug/deps/libfmi_sys*.rlib 2>/dev/null | wc -l)" + # `--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 @@ -186,6 +207,15 @@ jobs: python -m fmpy validate "$fmu" done + # TEMPORARY, with the probes above: the last reading before the step + # that rebuilds. + - name: "Diagnose: fmi-sys before the comparison" + shell: bash + run: | + # The same count, immediately before the step that rebuilds it. + echo "fingerprints: $(ls -d target/debug/.fingerprint/fmi-sys-* 2>/dev/null | wc -l)" + echo "rlibs: $(ls target/debug/deps/libfmi_sys*.rlib 2>/dev/null | wc -l)" + # Checks the `.fmu` just packaged against the laws compiled into it, # reached through FMI rather than called directly, which is what # catches a package older than the laws. @@ -196,8 +226,10 @@ jobs: # # Ahead of the upload, so a run whose FMU disagrees with the laws it # was built from publishes nothing. + # + # TEMPORARY: `-v` so cargo names its own reason for each rebuild. - name: "FMU: test against the laws it was built from" - run: cargo test --workspace --features packaged-fmu --test packaged_fmu + run: cargo test --workspace --features packaged-fmu --test packaged_fmu -v # Every FMU the workspace packages, each carrying this platform's # binary alone. One artifact per platform rather than per FMU, because From e04c6393d92fa17d8ef6ec50b40a40ea9452e4b2 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Sat, 15 Aug 2026 12:38:07 -1000 Subject: [PATCH 55/70] Revert "Ask CI why the comparison rebuilds what it already has" This reverts commit bccc377a45b93c415c8ebe6bed0faa103eb2de68. --- .github/workflows/ci.yml | 34 +--------------------------------- 1 file changed, 1 insertion(+), 33 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 2b9dd1a..133ce4a 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -51,19 +51,6 @@ jobs: - uses: Swatinem/rust-cache@v2 - # TEMPORARY, out again once it has reported. The packaged-FMU step - # rebuilds fourteen crates every run, `fmi-sys` among them, which is - # 52 of its 70 seconds on Windows and reproduces on no other machine. - # This probe runs again after each step that builds the same units, so - # the readings together say whether the cache never carried them or - # something in the run took them away. - - name: "Diagnose: fmi-sys after the cache restore" - shell: bash - run: | - # Count what is on disk for fmi-sys, whose rebuild costs the most. - echo "fingerprints: $(ls -d target/debug/.fingerprint/fmi-sys-* 2>/dev/null | wc -l)" - echo "rlibs: $(ls target/debug/deps/libfmi_sys*.rlib 2>/dev/null | wc -l)" - # 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 @@ -129,14 +116,6 @@ jobs: - name: "Build: debug" run: cargo build --workspace --all-targets --all-features - # TEMPORARY, with the probe above. - - name: "Diagnose: fmi-sys after the debug build" - shell: bash - run: | - # The same count, after a step that needed fmi-sys and did not rebuild it. - echo "fingerprints: $(ls -d target/debug/.fingerprint/fmi-sys-* 2>/dev/null | wc -l)" - echo "rlibs: $(ls target/debug/deps/libfmi_sys*.rlib 2>/dev/null | wc -l)" - # `--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 @@ -207,15 +186,6 @@ jobs: python -m fmpy validate "$fmu" done - # TEMPORARY, with the probes above: the last reading before the step - # that rebuilds. - - name: "Diagnose: fmi-sys before the comparison" - shell: bash - run: | - # The same count, immediately before the step that rebuilds it. - echo "fingerprints: $(ls -d target/debug/.fingerprint/fmi-sys-* 2>/dev/null | wc -l)" - echo "rlibs: $(ls target/debug/deps/libfmi_sys*.rlib 2>/dev/null | wc -l)" - # Checks the `.fmu` just packaged against the laws compiled into it, # reached through FMI rather than called directly, which is what # catches a package older than the laws. @@ -226,10 +196,8 @@ jobs: # # Ahead of the upload, so a run whose FMU disagrees with the laws it # was built from publishes nothing. - # - # TEMPORARY: `-v` so cargo names its own reason for each rebuild. - name: "FMU: test against the laws it was built from" - run: cargo test --workspace --features packaged-fmu --test packaged_fmu -v + run: cargo test --workspace --features packaged-fmu --test packaged_fmu # Every FMU the workspace packages, each carrying this platform's # binary alone. One artifact per platform rather than per FMU, because From 715454688630d85db7c0440a8829f70be0a7bdc2 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Sat, 15 Aug 2026 12:38:48 -1000 Subject: [PATCH 56/70] Stop building what the next step throws away `Build: debug` took `--all-features`, so it compiled the packaged-FMU comparison and linked its cdylib, and then `Test: unit` ran without the feature, changed the crate's feature list and made cargo rebuild it. In the run measured closely that compile was 26 of the step's 31 seconds on Windows, and nothing ever read its output. The flag was there so a target behind a feature would not rot uncompiled. Clippy covers that and covers it earlier: it keeps `--all-features`, and `cargo clippy -v` confirms the file is passed to clippy-driver only when that flag is present. A type error there now fails at `Lint: clippy` rather than at a test step, which is where the build step wanted it reported anyway. This does not touch the fourteen crates the comparison step rebuilds. Those are the feature being turned on, which is a separate question. Co-authored-by: Claude Opus 5 (1M context) --- .github/workflows/ci.yml | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 133ce4a..c38310b 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -110,11 +110,15 @@ jobs: 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. + # as whichever test step reached it first. + # + # No `--all-features`, unlike clippy above. A target behind a feature + # would be built here and then thrown away, since the test steps below + # run without the feature and a changed feature list is what makes + # cargo rebuild. Clippy already compiles that target's front end, and + # earlier, so nothing goes unchecked by leaving it out. - name: "Build: debug" - run: cargo build --workspace --all-targets --all-features + run: cargo build --workspace --all-targets # `--lib` and `--test '*'` partition the suite: the first runs the unit # tests inside each lib, the second the integration targets in tests/. From 3684ad95e7722bfab335f64abd034d07dd1a3fcf Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Sat, 15 Aug 2026 12:51:18 -1000 Subject: [PATCH 57/70] Revert "Stop building what the next step throws away" This reverts commit 715454688630d85db7c0440a8829f70be0a7bdc2. --- .github/workflows/ci.yml | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index c38310b..133ce4a 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -110,15 +110,11 @@ jobs: 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. - # - # No `--all-features`, unlike clippy above. A target behind a feature - # would be built here and then thrown away, since the test steps below - # run without the feature and a changed feature list is what makes - # cargo rebuild. Clippy already compiles that target's front end, and - # earlier, so nothing goes unchecked by leaving it out. + # 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 + run: cargo build --workspace --all-targets --all-features # `--lib` and `--test '*'` partition the suite: the first runs the unit # tests inside each lib, the second the integration targets in tests/. From 41d471b5fdc3860199a25d8098faee547b9ea4f1 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Sat, 15 Aug 2026 13:19:29 -1000 Subject: [PATCH 58/70] Build the comparison in a step that says it is building The comparison step spent most of its time compiling and the rest running nine tests that take well under a second, so the CI listing read as though the tests were slow. This splits the two, and the run that follows compiles nothing. The two selections have to match exactly, and why is worth writing down. Asking for one test target rather than all of them resolves fewer packages, 142 units against 399, with the whole zenoh tree gone. Their dependencies then unify features differently, so this crate links a different `thiserror` and every crate between the two rebuilds. Cargo reports it as `info of dependency thiserror changed`, which is how the cause was found: it survives removing the feature entirely, so the feature was never what triggered it. Nothing is expected to get faster. The same work happens in a step named for what it is, which is what makes the cost measurable at all, and this step has ranged from 37 to 92 seconds across five runs of materially identical work. Co-authored-by: Claude Opus 5 (1M context) --- .github/workflows/ci.yml | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 133ce4a..1c2beb2 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -186,6 +186,19 @@ jobs: python -m fmpy validate "$fmu" done + # What the step below runs, built here, so the cost lands in a step + # named for building rather than inside one named for testing, where + # it reads as the tests being slow when they take under a second. + # + # The two commands differ only in `build` against `test`, and have to. + # Asking for one test target rather than all of them resolves fewer + # packages and unifies their dependencies' features differently, which + # is enough on its own to make cargo rebuild this crate: it reports + # the change as `info of dependency thiserror changed`. That is also + # why this cannot join the builds above, which select every target. + - name: "FMU: build the comparison" + run: cargo build --workspace --features packaged-fmu --test packaged_fmu + # Checks the `.fmu` just packaged against the laws compiled into it, # reached through FMI rather than called directly, which is what # catches a package older than the laws. From a7e43c777bef4fce738e522273d156a0a5e68a63 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Sat, 15 Aug 2026 13:19:47 -1000 Subject: [PATCH 59/70] Revert "Build the comparison in a step that says it is building" This reverts commit 41d471b5fdc3860199a25d8098faee547b9ea4f1. --- .github/workflows/ci.yml | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 1c2beb2..133ce4a 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -186,19 +186,6 @@ jobs: python -m fmpy validate "$fmu" done - # What the step below runs, built here, so the cost lands in a step - # named for building rather than inside one named for testing, where - # it reads as the tests being slow when they take under a second. - # - # The two commands differ only in `build` against `test`, and have to. - # Asking for one test target rather than all of them resolves fewer - # packages and unifies their dependencies' features differently, which - # is enough on its own to make cargo rebuild this crate: it reports - # the change as `info of dependency thiserror changed`. That is also - # why this cannot join the builds above, which select every target. - - name: "FMU: build the comparison" - run: cargo build --workspace --features packaged-fmu --test packaged_fmu - # Checks the `.fmu` just packaged against the laws compiled into it, # reached through FMI rather than called directly, which is what # catches a package older than the laws. From 0b46f11619c2bcd7ffbf66f8d79ca0de53aae27e Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Sat, 15 Aug 2026 13:34:19 -1000 Subject: [PATCH 60/70] Run the comparison where the tests already are The comparison ran as its own step asking for one test target, and asking for a single target resolves fewer packages than the steps before it: 142 units against 399, with the whole zenoh tree gone. Their dependencies then unify features differently, so the crate linked a different `thiserror` and every crate between the two rebuilt. The previous commit measured that at 12 to 31 seconds, on every agent rather than only Windows. Packaging and validation move above the tests instead, and both test steps take `--all-features`, so the comparison runs as one of the integration targets under the selection everything else already uses. Replaying the sequence locally, both test steps now compile nothing and the nine tests run inside `Test: integration`. Two steps go. The upload moves to the end, because publishing is the one thing here that leaves the job, and a step runs only when every earlier one succeeded. CLAUDE.md's local command follows, since naming the single target would tell a contributor to pay the rebuild this removes. One claim corrected while nearby. `FMU: validate` said an unmatched glob would check nothing and pass. It would not: fmpy exits nonzero on a path that does not exist, so the step failed either way. The guard is worth keeping for what it says rather than what it catches, and it now says the packaging step produced none. Co-authored-by: Claude Opus 5 (1M context) --- .github/workflows/ci.yml | 110 +++++++++++++++++++-------------------- CLAUDE.md | 2 +- 2 files changed, 56 insertions(+), 56 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 133ce4a..0eaaad7 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -116,24 +116,56 @@ jobs: - 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 + 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. The packaging step below builds each FMU - # release, since a host loads the binary it finds and cannot rebuild it, - # and the smokes then run against the profile that FMU will be loaded - # beside. + # 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 @@ -163,55 +195,6 @@ jobs: exit 1 fi - # 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 - - # An FMU our own importer accepts is not the same as a valid one, so a - # second implementation is what says whether what we package is well - # formed. The glob is guarded because one matching nothing expands to - # itself, checks nothing and passes, which is the failure this step - # exists to catch. - - 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; this check would be vacuous" - exit 1 - fi - for fmu in "${packaged[@]}"; do - python -m fmpy validate "$fmu" - done - - # Checks the `.fmu` just packaged against the laws compiled into it, - # reached through FMI rather than called directly, which is what - # catches a package older than the laws. - # - # Names no package. Every FMU crate spells this the same way, a - # `tests/packaged_fmu.rs` behind a `packaged-fmu` feature, so asking - # for both names runs all of them. - # - # Ahead of the upload, so a run whose FMU disagrees with the laws it - # was built from publishes nothing. - - name: "FMU: test against the laws it was built from" - run: cargo test --workspace --features packaged-fmu --test packaged_fmu - - # 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 - # 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 @@ -305,3 +288,20 @@ jobs: # leaves the numbers for a human reading the log. - name: "Smoke: a scaled world" 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/CLAUDE.md b/CLAUDE.md index 4de03d3..45c01ed 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -83,7 +83,7 @@ links and the suite above compares nothing against it: ```sh cargo xtask package-fmus -cargo test --workspace --features packaged-fmu --test packaged_fmu +cargo test --workspace --all-features ``` `continuo-actors` is the one to watch, since editing a control law there From 21a542b320afa6cae34f6286fceff278ef04ea2f Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Sat, 15 Aug 2026 13:48:00 -1000 Subject: [PATCH 61/70] Say in the README what the FMU crate needs from the reader Two things the crates table and the usage block had stopped covering. `continuo-actors` is described as sample components, which it still is and is no longer all it is: the control laws live there so the native controller and the exported FMU call one implementation rather than two kept in step by hand. A reader deciding where a law belongs cannot tell that from a list of components. And packaging was documented with nothing that reads what it writes. The comparison is the reason to run it, so the pair appears together, with the note that a plain `cargo test --workspace` needs nothing packaged and skips it. Co-authored-by: Claude Opus 5 (1M context) --- README.md | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 3ca209e..48c104f 100644 --- a/README.md +++ b/README.md @@ -98,7 +98,7 @@ 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` | @@ -149,6 +149,11 @@ cargo fmt --all # 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 From 9a2d4abea8044e699947365108bbd8bdc0699fd0 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Sat, 15 Aug 2026 13:50:57 -1000 Subject: [PATCH 62/70] Record what PR B settled Seven entries for the work this branch did: that the FMU is the whole controller with its laws left in `continuo-actors`, and what a compiled snapshot of those laws costs; the one place the following law departs from the published equation, and why its parameters are the published five with no emergency braking of our own; the road crossing as fixed arrays because the derive cannot size one by a parameter; the model refusing at run time what its description has no way to state; the packaging task and the merge that gives one FMU per model rather than one per platform; how the comparison is gated and where it runs; and why the archived `proc-macro-error2` is patched rather than silenced. Three of them record something that cost real time to learn and would cost it again. Asking cargo for a single test target resolves fewer packages and so rebuilds a good part of the graph. The plan's `b_max` and `GAP_FLOOR` were both answered by flooring the wanted gap instead, which also tells the ego-lane work its spawn distances need revisiting. And the CI step at issue measured 79, 70, 71, 37 and 92 seconds across five runs of identical work, so anybody optimizing here should distrust a single run, including their own. Co-authored-by: Claude Opus 5 (1M context) --- DECISIONS.md | 121 +++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 121 insertions(+) diff --git a/DECISIONS.md b/DECISIONS.md index 7458fc8..1e318b6 100644 --- a/DECISIONS.md +++ b/DECISIONS.md @@ -864,3 +864,124 @@ 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`, which the native + controller calls too. The two agree because they are one implementation, + not because somebody keeps them in step. 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. + - 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. + +- **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 a scheduled job then needs no Rust toolchain. + +- **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. From 28e9b02dbae151ab11e68745d87ad76d3bd6d4ef Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Sat, 15 Aug 2026 13:54:19 -1000 Subject: [PATCH 63/70] Check the equation against somebody else's implementation The plan asked for the IDM spot values to be cross-checked once against highway-env, and this is the result rather than the intention. Its `IDMVehicle` is the unclamped form: `desired_gap` floors nothing and `acceleration` limits nothing, which is what makes it the useful comparison rather than a mirror. Fed our parameters it agrees to the last bit or two at every sampled point where the wanted gap stays positive, and where it disagrees it disagrees by exactly the two adaptations this branch documents: it commands -1.280 m/s^2 at 20 m/s behind a lead pulling away, where flooring the wanted gap gives +1.204, and it runs to -9.977 where the command is held at the comfortable braking rate. Nothing changed in the laws, which is the outcome worth having. The numbers quoted in the entry above were checked against it too, so the figures this branch argues from are its arithmetic rather than mine. Co-authored-by: Claude Opus 5 (1M context) --- DECISIONS.md | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/DECISIONS.md b/DECISIONS.md index 1e318b6..94129fd 100644 --- a/DECISIONS.md +++ b/DECISIONS.md @@ -908,6 +908,17 @@ it got there, including the roads not taken. 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. Its own numbers differ from ours and are not the point: it wants + 10 m at a standstill against 2, since it folds a car length in, and + brakes to 5. - **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 From f0160fbe0ba7d85b5775ccae08edd1b3211a70d2 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Sat, 15 Aug 2026 14:15:54 -1000 Subject: [PATCH 64/70] Say which of the laws the native side actually calls The entry claimed all three are called by the native controller too. Only one is. `PathFollowController` calls `pure_pursuit_yaw_rate`, and nothing native calls `idm_accel` or `nearest_detection`, because M6 gives traffic its longitudinal behavior through the FMU and leaves that controller purely lateral. That matters because the shared call is the whole argument for keeping the laws in `continuo-actors`, and it is currently half demonstrated. So the entry says which half, and PLAN.md defers the component that would finish it, with the shape left open: a second component keeps the existing one lateral, which M6 made it deliberately so a learned longitudinal FMU can run beside native steering, while teaching the existing one to follow is less code and gives that up. It would also buy a native baseline, since nothing but the FMU runs IDM in a world today and no scenario can be run both ways to see what the boundary costs. Co-authored-by: Claude Opus 5 (1M context) --- DECISIONS.md | 12 +++++++++--- PLAN.md | 19 +++++++++++++++++++ 2 files changed, 28 insertions(+), 3 deletions(-) diff --git a/DECISIONS.md b/DECISIONS.md index 94129fd..b96691f 100644 --- a/DECISIONS.md +++ b/DECISIONS.md @@ -868,13 +868,19 @@ it got there, including the roads not taken. - **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`, which the native - controller calls too. The two agree because they are one implementation, - not because somebody keeps them in step. A planner publishing an + `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. diff --git a/PLAN.md b/PLAN.md index 0723602..9a6d717 100644 --- a/PLAN.md +++ b/PLAN.md @@ -780,6 +780,25 @@ than churning the fingerprint twice. 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, From 591fbb39245dbb4fd30eca4d334fea359d7af8a8 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Sat, 15 Aug 2026 14:16:16 -1000 Subject: [PATCH 65/70] Name whose parameters the cross-check used "Its own numbers" left the reader to work out whose, and the sentence sat after a run of figures that were ours. What was compared is the equation rather than the tuning, so highway-env was fed our parameters throughout, and its own are named as the ones not used: a standstill gap of 10 m against our 2, since it folds a car length in, an acceleration of 3.0 against 1.5, and comfortable braking of 5.0 against 2.0. Co-authored-by: Claude Opus 5 (1M context) --- DECISIONS.md | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/DECISIONS.md b/DECISIONS.md index b96691f..49eeaa0 100644 --- a/DECISIONS.md +++ b/DECISIONS.md @@ -922,9 +922,11 @@ it got there, including the roads not taken. 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. Its own numbers differ from ours and are not the point: it wants - 10 m at a standstill against 2, since it folds a car length in, and - brakes to 5. + 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 From ea16628927e1a00b14f3966932b92dfd44280898 Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Sun, 16 Aug 2026 09:08:08 -1000 Subject: [PATCH 66/70] Land the FMU merge workflow with a manual trigger alone A workflow cannot be dispatched until it sits on the default branch, so landing the `workflow_run` trigger beside the workflow would make the first run of it an automatic one that nothing had tried. Taking the CI run to read artifacts from as an input instead leaves that first run a chosen one, against a named run, and the trigger follows once a dispatch has shown the cross-run download works. The trigger took two things with it. The conclusion guard existed to keep a failed CI run's partial artifacts out of a merged FMU, which is a question only the automatic path asks, since a dispatch names its run deliberately. And the run id fell back to `github.run_id` when no `workflow_run` event supplied one, which is the merge run's own id and holds no artifacts, so the manual path could never have worked. Co-authored-by: Claude Opus 5 (1M context) --- .github/workflows/fmu-merge.yml | 28 ++++++++++++---------------- DECISIONS.md | 8 +++++++- 2 files changed, 19 insertions(+), 17 deletions(-) diff --git a/.github/workflows/fmu-merge.yml b/.github/workflows/fmu-merge.yml index 33c49a8..4a8f28e 100644 --- a/.github/workflows/fmu-merge.yml +++ b/.github/workflows/fmu-merge.yml @@ -6,30 +6,26 @@ name: Merge FMUs # single file per FMU that loads wherever they are rather than picking the # right one of four. # -# Triggered by CI finishing rather than by a clock: the event names the run -# whose artifacts to take, so there is no searching for the latest green build -# and no asking whether this commit was merged already. One merged FMU per -# commit that reaches main, by construction. +# 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_run: - workflows: ["CI"] - types: [completed] - branches: [main] 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 a `workflow_run` -# job does not get by default. Naming the two it uses also drops the rest. +# 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: - # A failed CI run still uploads whatever agents got that far, and merging - # those would publish an FMU from a commit that did not pass. - if: > - github.event_name == 'workflow_dispatch' || - github.event.workflow_run.conclusion == 'success' runs-on: ubuntu-latest steps: - uses: actions/checkout@v7 @@ -48,7 +44,7 @@ jobs: with: pattern: fmus-* path: artifacts - run-id: ${{ github.event.workflow_run.id || github.run_id }} + run-id: ${{ inputs.run_id }} github-token: ${{ secrets.GITHUB_TOKEN }} # Given the download directory rather than each artifact inside it, so diff --git a/DECISIONS.md b/DECISIONS.md index 49eeaa0..7411301 100644 --- a/DECISIONS.md +++ b/DECISIONS.md @@ -970,7 +970,13 @@ it got there, including the roads not taken. 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 a scheduled job then needs no Rust toolchain. + 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 From 4f30e89e599123f5c048de205a446b480ccdd30e Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Sun, 16 Aug 2026 09:15:17 -1000 Subject: [PATCH 67/70] Install fmpy in an environment of its own fmpy validates what the FMU steps package and has nothing to do with the viewer, but one environment put its tree, numpy and lxml among others, on the path where the viewer's tests run. Nothing in the viewer imports any of it today, so the hazard is prospective: an accidental import would pass here and fail for everyone who installed the viewer alone. A virtual environment rather than a second pip invocation into the same one, because a single resolution either finds a consistent set or refuses, where two sequential ones can each succeed and leave the environment inconsistent. The interpreter sits in Scripts on Windows and bin everywhere else, so the install picks it and names it for the validate step rather than leaving that step to repeat the choice. It stays with the other installs, ahead of anything that compiles. fmpy carries no version bound, so it is the step likeliest to fail on a day nothing changed, and that is worth hearing in seconds rather than after the workspace has built. Co-authored-by: Claude Opus 5 (1M context) --- .github/workflows/ci.yml | 27 ++++++++++++++++++++++----- .gitignore | 2 +- 2 files changed, 23 insertions(+), 6 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 0eaaad7..fef9088 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -70,8 +70,7 @@ jobs: # 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. `fmpy` joins it for the FMU validation - # below, which is a second implementation's opinion of what we package. + # way is what keeps that true. - name: "Install: Python (cached)" uses: actions/setup-python@v7 with: @@ -79,9 +78,27 @@ jobs: cache: pip cache-dependency-path: python/pyproject.toml - - name: "Install: viewer and fmpy" + - name: "Install: viewer" working-directory: python - run: python -m pip install -e . pytest ruff fmpy + run: python -m pip install -e . pytest ruff + + # 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: | + # 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. @@ -140,7 +157,7 @@ jobs: exit 1 fi for fmu in "${packaged[@]}"; do - python -m fmpy validate "$fmu" + "$FMPY_PYTHON" -m fmpy validate "$fmu" done # `--lib` and `--test '*'` partition the suite: the first runs the unit 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/ From 88bfea377b4cd7f79da82a561d17c2a2e3cf609b Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Sun, 16 Aug 2026 10:26:44 -1000 Subject: [PATCH 68/70] Run each packaged-FMU situation as a run of its own The sweeps drive situations picked to reach corners of the input space, and no car could drive from one to the next, so running them as consecutive steps 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 something the test established. Resetting before each one takes the assumption out. `fmi3Reset` is what the standard offers for this: it returns an instance to Instantiated without paying to load the library again, which measures around a hundred times what a reset costs, and initialization has to happen before it can step. Instantiated is also where an FMU is before it has been sized, and Configuration Mode closes before Initialization Mode opens, so the structural parameters go back in during the reset rather than at that next step. Without it a reset StateSpace would come back at the size its description declares while the bindings went on addressing the size the mapping asked for. Both sets of values are kept now rather than spent on first use, and BouncingBall and StateSpace each gained a test, checked by breaking the thing it covers. Each situation is then 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. The same inputs and the same road are behind both, so both must command the same thing, and holding the elaborate path to what a plain step answers is what `fmu_commands` now does. A single step apiece would have quietly removed `fmi3DoStep` from the sweeps altogether. Doing the whole of one situation there is also what retires the tick every caller used to invent: a reset FMU starts over, so each situation runs at the same pair of instants. The two tests beside it that were stepping independent situations in sequence are reset now as well, and the contexts this file builds carry the `dt` a conductor would supply rather than always claiming a first step. The comparison costs about 1.2 s where it cost 0.77 s. Co-authored-by: Claude Opus 5 (1M context) --- crates/continuo-fmi/src/fmu_component.rs | 101 +++++++++++++----- crates/continuo-fmi/tests/reference_fmus.rs | 62 +++++++++++ .../tests/packaged_fmu.rs | 88 +++++++++++---- 3 files changed, 205 insertions(+), 46 deletions(-) diff --git a/crates/continuo-fmi/src/fmu_component.rs b/crates/continuo-fmi/src/fmu_component.rs index 5f2e0e4..b95fc29 100644 --- a/crates/continuo-fmi/src/fmu_component.rs +++ b/crates/continuo-fmi/src/fmu_component.rs @@ -38,8 +38,14 @@ 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 @@ -174,37 +180,72 @@ impl FmuComponent { outputs, subscriptions, period, + structural_vars, vars_to_initialize, last_step: None, }; - - // 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.last_step = None; + + Ok(()) + } + /// Writes values the mapping wrote out straight into the FMU. /// /// The mapping supplies these itself rather than binding them to a @@ -214,8 +255,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 +274,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(), @@ -405,8 +451,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(), })?; 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/tests/packaged_fmu.rs b/crates/continuo-fmu-controller-idm/tests/packaged_fmu.rs index a657b0c..2cf43ef 100644 --- a/crates/continuo-fmu-controller-idm/tests/packaged_fmu.rs +++ b/crates/continuo-fmu-controller-idm/tests/packaged_fmu.rs @@ -197,12 +197,57 @@ fn set_parameter(mapping: &mut FmuMapping, name: &str, value: Value) { slot.1 = value; } -/// Steps the FMU once in `situation` and returns what it commanded. +/// 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 fmu_commands(fmu: &mut FmuComponent, tick: i64, situation: &Situation) -> (f64, f64) { - let now = SimTime::from_millis(tick * PERIOD_MS); +fn step_once( + fmu: &mut FmuComponent, + now: SimTime, + dt: Option, + situation: &Situation, +) -> (f64, f64) { let inbox = vec![ message( "pose", @@ -215,7 +260,7 @@ fn fmu_commands(fmu: &mut FmuComponent, tick: i64, situation: &Situation) -> (f6 message("radar", json!({ "detections": situation.scan })), ]; - let mut ctx = StepCtx::new(now, None, WORLD, 0, inbox); + 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"); @@ -252,6 +297,11 @@ fn native_commands( /// 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, @@ -261,15 +311,11 @@ fn commands_over( 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. The - // instance is reused across them because the laws are a map from - // inputs to commands: nothing carries from one step to the next but - // the road, which is what makes a sweep like this meaningful at all. + // Return each situation's commands, having agreed at every one. situations .iter() - .enumerate() - .map(|(tick, situation)| { - let from_fmu = fmu_commands(&mut fmu, tick as i64, situation); + .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(), @@ -287,6 +333,8 @@ fn commands_over( from_laws.1, situation.summary(), ); + + // Return what the laws answered, which the FMU has agreed with. from_laws }) .collect() @@ -420,8 +468,9 @@ fn chosen_situations(road: &Waypoints) -> Vec { /// 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 stays under a - // second, which is what keeps it something to run while editing. + // 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 @@ -575,8 +624,8 @@ fn the_padding_past_the_count_is_never_read() { set_parameter(&mut mapping, "road_y", json!(road_y)); let mut fmu = FmuComponent::new("controller", packaged(), mapping).expect("the FMU builds"); - for (tick, situation) in situations.iter().enumerate() { - let from_fmu = fmu_commands(&mut fmu, tick as i64, situation); + 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()), @@ -705,7 +754,10 @@ fn a_tuning_parameter_changed_between_steps_changes_the_command() { ), message("tuning", json!({ "v0_speed_tgt": v0_speed_tgt })), ]; - let mut ctx = StepCtx::new(now, None, WORLD, 0, inbox); + // 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"); @@ -764,7 +816,7 @@ fn the_nearest_of_several_detections_is_the_one_followed() { // 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, 0, &situation(scan)); + 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(), @@ -782,7 +834,7 @@ fn the_nearest_of_several_detections_is_the_one_followed() { // 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, 1, &situation(tied)); + 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(), From 0ea30863b868358ea4cdcebc5a2b96d7a15d662d Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Sun, 16 Aug 2026 10:30:01 -1000 Subject: [PATCH 69/70] Name the two FMI states an imported FMU rests in `step` does two different things depending on whether the instance has been initialized, and an `Option` made that read as a null check when what it models is FMI's own state machine. A named enum says so: the match arms are Uninitialized and Stepping rather than None and Some, and `reset` assigns a state rather than clearing a time. The two are the states an instance rests in between calls. The standard's machine has more, but with event mode off the rest open and close inside a single one: Configuration Mode within construction, Initialization Mode within the step that performs it. That is the distinction a reader needs to tell the arms apart, and it is worth saying where the field is declared. Behavior is unchanged, and the demo's world hash not moving is the proof. Co-authored-by: Claude Opus 5 (1M context) --- crates/continuo-fmi/src/fmu_component.rs | 37 +++++++++++++++++------- 1 file changed, 26 insertions(+), 11 deletions(-) diff --git a/crates/continuo-fmi/src/fmu_component.rs b/crates/continuo-fmi/src/fmu_component.rs index b95fc29..9cc6721 100644 --- a/crates/continuo-fmi/src/fmu_component.rs +++ b/crates/continuo-fmi/src/fmu_component.rs @@ -47,10 +47,25 @@ pub struct FmuComponent { /// 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 { @@ -182,7 +197,7 @@ impl FmuComponent { period, structural_vars, vars_to_initialize, - last_step: None, + phase: Phase::Uninitialized, }; component.size_by_structural_parameters()?; @@ -241,7 +256,7 @@ impl FmuComponent { .map_err(|error| CoreError::ComponentFailure { reason: error.to_string(), })?; - self.last_step = None; + self.phase = Phase::Uninitialized; Ok(()) } @@ -436,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| { @@ -460,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; @@ -533,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) From b87dddf281ae7b4be2a7ee91be6798e46385e6fa Mon Sep 17 00:00:00 2001 From: Effendi Dufford Date: Sun, 16 Aug 2026 10:32:08 -1000 Subject: [PATCH 70/70] Record why the packaged-FMU comparison resets between situations Two facts in this are the kind that cost a day to rediscover. A reset does not restore an FMU's size, because it lands in Instantiated and Configuration Mode has already closed by the time the next step runs. And resetting rather than rebuilding is what the standard offers for running a model repeatedly, which measures at about a hundredth of the cost, since construction extracts the archive and loads the library. The third is a shape worth recognizing rather than a fact: resetting and stepping once would have cost no measurable time and would have removed `fmi3DoStep` from the sweeps entirely. A change that buys safety for free is worth looking at twice. Co-authored-by: Claude Opus 5 (1M context) --- DECISIONS.md | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/DECISIONS.md b/DECISIONS.md index 7411301..e441fd5 100644 --- a/DECISIONS.md +++ b/DECISIONS.md @@ -1010,3 +1010,28 @@ it got there, including the roads not taken. `[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.