From cf69d09cb02e7368d754c37f119da9bab334f23f Mon Sep 17 00:00:00 2001 From: jbernavaprah Date: Sun, 28 Jun 2026 20:49:56 +0200 Subject: [PATCH] feat: port bno085/vl53l1x/zed_f9p/oak_d_lite to phoxal 0.16 Port the four remaining sensor component drivers from the old phoxal 0.9 Runtime trait onto the new #[derive(phoxal::Runtime)] pattern (matching the already-migrated ddsm115), publishing the recovered y2026_1 component-capability contracts. - bno085: imu + accelerometer + gyroscope - vl53l1x: range (distance from the Range capability's max_range_m) - zed_f9p: gnss (was previously unimplemented; now a working stub publisher) - oak_d_lite: camera (per distinct capability id: left_mono/rgb/right_mono) + depth + imu + accelerometer + gyroscope Each driver discovers its capabilities from the resolved component model (ctx.robot().component_for_instance) rather than hardcoding ids, so a component with multiple same-kind capabilities (oak_d_lite's three cameras) is handled. A fixed #[step] clock plus a per-capability divisor (round(step_hz / publish_rate_hz)) reproduces the old per-capability publish rates. Backends are simulation stubs (as the old bno085/vl53l1x were; zed_f9p/oak_d_lite were unimplemented) - real hardware backends are a follow-up. Workspace moves to phoxal 0.16 (incl. ddsm115); clap/tokio/async-trait/tracing dropped per crate (the framework owns the runtime + CLI). Old src/runtime.rs modules removed; each driver is a single src/main.rs. Co-Authored-By: Claude Opus 4.8 --- Cargo.lock | 76 ++----- Cargo.toml | 2 +- bno085/Cargo.toml | 7 +- bno085/src/main.rs | 258 +++++++++++++++++++++-- bno085/src/runtime.rs | 226 --------------------- ddsm115/Cargo.toml | 6 +- oak_d_lite/Cargo.toml | 6 +- oak_d_lite/src/main.rs | 451 +++++++++++++++++++++++++++++++++++++++-- vl53l1x/Cargo.toml | 7 +- vl53l1x/src/main.rs | 185 ++++++++++++++--- vl53l1x/src/runtime.rs | 143 ------------- zed_f9p/Cargo.toml | 6 +- zed_f9p/src/main.rs | 164 +++++++++++++-- 13 files changed, 997 insertions(+), 540 deletions(-) delete mode 100644 bno085/src/runtime.rs delete mode 100644 vl53l1x/src/runtime.rs diff --git a/Cargo.lock b/Cargo.lock index 5bc1364..f9d4e83 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -581,18 +581,6 @@ dependencies = [ "syn 2.0.117", ] -[[package]] -name = "derive_setters" -version = "0.1.9" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b7e6f6fa1f03c14ae082120b84b3c7fbd7b8588d924cf2d7c3daf9afd49df8b9" -dependencies = [ - "darling", - "proc-macro2", - "quote", - "syn 2.0.117", -] - [[package]] name = "digest" version = "0.10.7" @@ -1925,37 +1913,9 @@ dependencies = [ [[package]] name = "phoxal" -version = "0.9.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c53144d1bcfbfcef30ee5f915f5376b4513199d4ff67b68f31a322661fde0b57" -dependencies = [ - "anyhow", - "arc-swap", - "async-trait", - "clap", - "const_format", - "derive-new", - "derive_setters", - "dotenvy", - "nalgebra", - "rmp-serde", - "serde", - "serde_bytes", - "serde_json", - "serde_yaml", - "thiserror 2.0.18", - "tokio", - "tracing", - "tracing-subscriber", - "urdf-rs", - "zenoh", -] - -[[package]] -name = "phoxal" -version = "0.15.0" +version = "0.16.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0ca6758cb378ea4682e8eec317b0e6fc52e50c2a46ef8e35b6236d35279604f7" +checksum = "5510ff7df9f5ca50a51632cb7dde67dbc64f06c141cdf975e3a4516d1a3263c3" dependencies = [ "anyhow", "arc-swap", @@ -1982,11 +1942,8 @@ name = "phoxal-component-bno085" version = "0.1.0" dependencies = [ "anyhow", - "async-trait", - "clap", - "phoxal 0.9.0", - "tokio", - "tracing", + "phoxal", + "serde_json", ] [[package]] @@ -1994,7 +1951,7 @@ name = "phoxal-component-ddsm115" version = "0.1.0" dependencies = [ "anyhow", - "phoxal 0.15.0", + "phoxal", "serde_json", ] @@ -2003,10 +1960,8 @@ name = "phoxal-component-oak_d_lite" version = "0.1.0" dependencies = [ "anyhow", - "clap", - "phoxal 0.9.0", - "tokio", - "tracing", + "phoxal", + "serde_json", ] [[package]] @@ -2014,11 +1969,8 @@ name = "phoxal-component-vl53l1x" version = "0.1.0" dependencies = [ "anyhow", - "async-trait", - "clap", - "phoxal 0.9.0", - "tokio", - "tracing", + "phoxal", + "serde_json", ] [[package]] @@ -2026,17 +1978,15 @@ name = "phoxal-component-zed_f9p" version = "0.1.0" dependencies = [ "anyhow", - "clap", - "phoxal 0.9.0", - "tokio", - "tracing", + "phoxal", + "serde_json", ] [[package]] name = "phoxal-macros" -version = "0.15.0" +version = "0.16.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cd3296316af6a96165976e24f0f2763fc05d691be2b8ba79fec32115d27b9d84" +checksum = "3f7a910c9e9e43b91ae51ee28634a16a9adadb38c28ba489487590b86ca65a27" dependencies = [ "heck", "proc-macro2", diff --git a/Cargo.toml b/Cargo.toml index 600dbf9..53d1645 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -22,7 +22,7 @@ async-trait = "0.1.89" clap = { version = "4.6.1", features = ["derive"] } tokio = { version = "1", features = ["macros", "rt", "rt-multi-thread", "time", "signal", "sync", "fs"] } tracing = "0.1.44" -phoxal = { version = "0.9.0" } +phoxal = { version = "0.16" } [workspace.lints.rust] unused = "warn" diff --git a/bno085/Cargo.toml b/bno085/Cargo.toml index aa0b9a0..e092a91 100644 --- a/bno085/Cargo.toml +++ b/bno085/Cargo.toml @@ -15,8 +15,7 @@ workspace = true [dependencies] anyhow = { workspace = true } -async-trait = { workspace = true } -clap = { workspace = true, features = ["derive", "env"] } -tokio = { workspace = true, features = ["macros", "rt", "rt-multi-thread", "signal", "sync", "time"] } -tracing = { workspace = true } phoxal = { workspace = true } + +[dev-dependencies] +serde_json = "1" diff --git a/bno085/src/main.rs b/bno085/src/main.rs index fbbed7c..141075e 100644 --- a/bno085/src/main.rs +++ b/bno085/src/main.rs @@ -1,25 +1,239 @@ -mod runtime; - -use anyhow::Result; -use clap::Parser; -use phoxal::runtime::{DriverRuntimeArgs, RuntimeProcess}; -use phoxal::util::init_tracing; -use tracing::info; - -use crate::runtime::Config; - -#[tokio::main] -async fn main() -> Result<()> { - init_tracing()?; - let args = DriverRuntimeArgs::parse(); - let robot = args.runtime.robot()?; - let bus = args.runtime.connect_bus().await?; - let binding = robot.driver_binding(&args.component_id)?; - let config = Config::new(&binding.component_id, binding.component)?; - info!("BNO085 runtime ready"); - RuntimeProcess::new(&bus, args.simulation(), config.clock_period()) - .run::(config) - .await?; +//! `bno085` — BNO085 IMU component driver stub. +use anyhow::{Result, bail}; +use phoxal::api::y2026_1 as api; +use phoxal::model::component::v1::capability::Capability; +use phoxal::prelude::*; + +const STEP_HZ: f64 = 100.0; + +#[derive(phoxal::Runtime)] +#[phoxal(id = "bno085", api = y2026_1)] +struct Bno085 { + imu: Vec>, + imu_divisors: Vec, + accelerometer: Vec>, + accelerometer_divisors: Vec, + gyroscope: Vec>, + gyroscope_divisors: Vec, +} + +#[derive(Debug, Clone)] +struct CapabilitySchedule { + capability_id: String, + divisor: u64, +} + +#[phoxal::runtime] +impl Bno085 { + #[setup] + async fn setup(ctx: &mut SetupContext) -> Result { + let instance = ctx.component()?.to_string(); + let (imu_slots, accelerometer_slots, gyroscope_slots) = { + let robot = ctx.robot()?; + let spec = robot.component_for_instance(&instance)?; + let mut imu = Vec::new(); + let mut accelerometer = Vec::new(); + let mut gyroscope = Vec::new(); + + for (capability_id, capability) in &spec.capabilities { + match capability { + Capability::Imu(config) => { + imu.push(schedule(capability_id, config.publish_rate_hz)?); + } + Capability::Accelerometer(config) => { + accelerometer.push(schedule(capability_id, config.publish_rate_hz)?); + } + Capability::Gyroscope(config) => { + gyroscope.push(schedule(capability_id, config.publish_rate_hz)?); + } + _ => {} + } + } + + (imu, accelerometer, gyroscope) + }; + + if imu_slots.is_empty() { + bail!("bno085 requires at least one imu capability"); + } + if accelerometer_slots.is_empty() { + bail!("bno085 requires at least one accelerometer capability"); + } + if gyroscope_slots.is_empty() { + bail!("bno085 requires at least one gyroscope capability"); + } + + let mut imu = Vec::new(); + let mut imu_divisors = Vec::new(); + for slot in imu_slots { + imu.push( + ctx.publisher( + api::topic::new() + .component() + .imu_sample(&instance, &slot.capability_id), + ) + .await?, + ); + imu_divisors.push(slot.divisor); + } + + let mut accelerometer = Vec::new(); + let mut accelerometer_divisors = Vec::new(); + for slot in accelerometer_slots { + accelerometer.push( + ctx.publisher( + api::topic::new() + .component() + .accelerometer_sample(&instance, &slot.capability_id), + ) + .await?, + ); + accelerometer_divisors.push(slot.divisor); + } + + let mut gyroscope = Vec::new(); + let mut gyroscope_divisors = Vec::new(); + for slot in gyroscope_slots { + gyroscope.push( + ctx.publisher( + api::topic::new() + .component() + .gyroscope_sample(&instance, &slot.capability_id), + ) + .await?, + ); + gyroscope_divisors.push(slot.divisor); + } + + Ok(Self { + imu, + imu_divisors, + accelerometer, + accelerometer_divisors, + gyroscope, + gyroscope_divisors, + }) + } + + #[step(hz = 100)] + async fn step(&mut self, step: StepContext) -> Result<()> { + let at = step.time(); + let step_index = step.step_index(); + + for (publisher, divisor) in self.imu.iter().zip(&self.imu_divisors) { + if is_due(step_index, *divisor) { + publisher.publish_at(at, imu_sample()).await?; + } + } + + for (publisher, divisor) in self.accelerometer.iter().zip(&self.accelerometer_divisors) { + if is_due(step_index, *divisor) { + publisher.publish_at(at, accelerometer_sample()).await?; + } + } + + for (publisher, divisor) in self.gyroscope.iter().zip(&self.gyroscope_divisors) { + if is_due(step_index, *divisor) { + publisher.publish_at(at, gyroscope_sample()).await?; + } + } + + Ok(()) + } + + #[shutdown] + async fn shutdown(&mut self, _ctx: ShutdownContext) -> Result<()> { + Ok(()) + } +} + +fn schedule(capability_id: &str, publish_rate_hz: f64) -> Result { + validate_publish_rate(capability_id, publish_rate_hz)?; + Ok(CapabilitySchedule { + capability_id: capability_id.to_string(), + divisor: divisor_for_rate(STEP_HZ, publish_rate_hz), + }) +} + +fn validate_publish_rate(capability_id: &str, publish_rate_hz: f64) -> Result<()> { + if !publish_rate_hz.is_finite() || publish_rate_hz <= 0.0 { + bail!("capability '{capability_id}' publish_rate_hz must be > 0"); + } Ok(()) } + +fn divisor_for_rate(step_hz: f64, publish_rate_hz: f64) -> u64 { + (step_hz / publish_rate_hz).round().max(1.0) as u64 +} + +fn is_due(step_index: u64, divisor: u64) -> bool { + divisor <= 1 || step_index % divisor == 0 +} + +fn imu_sample() -> api::component::ImuSample { + api::component::ImuSample { + orientation: Some([1.0, 0.0, 0.0, 0.0]), + angular_velocity_radps: [0.0; 3], + linear_acceleration_mps2: [0.0; 3], + covariance: None, + noise_density: None, + sensor_frame_id: None, + measured_at_ns: None, + health: api::component::SensorHealth::Nominal, + bias: None, + } +} + +fn accelerometer_sample() -> api::component::AccelerometerSample { + api::component::AccelerometerSample { + linear_acceleration: [0.0; 3], + } +} + +fn gyroscope_sample() -> api::component::GyroscopeSample { + api::component::GyroscopeSample { + angular_velocity: [0.0; 3], + } +} + +fn main() -> phoxal::Result<()> { + phoxal::run::() +} + +#[cfg(test)] +mod tests { + use super::{Bno085, divisor_for_rate, is_due}; + use phoxal::api::ContractBody; + use phoxal::api::y2026_1 as api; + + #[test] + fn divisor_rounds_to_fixed_step_clock() { + assert_eq!(divisor_for_rate(100.0, 100.0), 1); + assert_eq!(divisor_for_rate(100.0, 50.0), 2); + assert_eq!(divisor_for_rate(100.0, 20.0), 5); + assert_eq!(divisor_for_rate(100.0, 1000.0), 1); + assert!(is_due(10, 5)); + assert!(!is_due(11, 5)); + } + + #[test] + fn emit_apis_reports_per_component_contracts() { + let json = phoxal::runtime::emit_apis_json::(); + let value: serde_json::Value = serde_json::from_str(&json).unwrap(); + assert_eq!(value["artifact"]["id"], "bno085"); + let contracts = value["required_contracts"].as_array().unwrap(); + for family in [ + ::FAMILY, + ::FAMILY, + ::FAMILY, + ] { + assert!( + contracts + .iter() + .any(|c| c["family"] == family && c["direction"] == "publish"), + "missing publish contract for {family}" + ); + } + } +} diff --git a/bno085/src/runtime.rs b/bno085/src/runtime.rs deleted file mode 100644 index 7dfe4c0..0000000 --- a/bno085/src/runtime.rs +++ /dev/null @@ -1,226 +0,0 @@ -use std::time::Duration; - -use anyhow::{Result, bail}; -use phoxal::api::component::capability::accelerometer::{self, v1 as accelerometer_v1}; -use phoxal::api::component::capability::gyroscope::{self, v1 as gyroscope_v1}; -use phoxal::api::component::capability::imu::{self, v1 as imu_v1}; -use phoxal::api::topic; -use phoxal::model::component::v1::CapabilityRef; -use phoxal::model::component::v1::capability::Capability; -use phoxal::runtime::clock::{Schedule, SchedulePolicy, Step}; -use phoxal::runtime::{EmptyArgs, Io, RobotRuntimeArgs, Runtime, RuntimeInputs, TopicPublisher}; - -#[derive(Clone)] -pub struct Config { - imu: SampledSensor, - accelerometer: SampledSensor, - gyroscope: SampledSensor, -} - -impl Config { - pub fn new( - component_id: &str, - component: &phoxal::model::component::v1::Component, - ) -> Result { - Ok(Self { - imu: sensor(component_id, component, CapabilityKind::Imu)?, - accelerometer: sensor(component_id, component, CapabilityKind::Accelerometer)?, - gyroscope: sensor(component_id, component, CapabilityKind::Gyroscope)?, - }) - } - - pub fn clock_period(&self) -> Duration { - let highest_publish_rate_hz = self - .imu - .publish_rate_hz() - .max(self.accelerometer.publish_rate_hz()) - .max(self.gyroscope.publish_rate_hz()); - Duration::from_secs_f64(1.0 / highest_publish_rate_hz) - } -} - -#[derive(Clone, Copy)] -enum CapabilityKind { - Imu, - Accelerometer, - Gyroscope, -} - -fn sensor( - component_id: &str, - component: &phoxal::model::component::v1::Component, - kind: CapabilityKind, -) -> Result { - let mut sensor = None; - - for (local_capability_id, capability) in &component.capabilities { - let capability_ref = CapabilityRef::new(component_id, local_capability_id); - match (kind, capability) { - (CapabilityKind::Imu, Capability::Imu(config)) => { - if sensor.is_some() { - bail!("bno085 requires exactly one imu capability"); - } - sensor = Some(SampledSensor::new(capability_ref, config.publish_rate_hz)?); - } - (CapabilityKind::Accelerometer, Capability::Accelerometer(config)) => { - if sensor.is_some() { - bail!("bno085 requires exactly one accelerometer capability"); - } - sensor = Some(SampledSensor::new(capability_ref, config.publish_rate_hz)?); - } - (CapabilityKind::Gyroscope, Capability::Gyroscope(config)) => { - if sensor.is_some() { - bail!("bno085 requires exactly one gyroscope capability"); - } - sensor = Some(SampledSensor::new(capability_ref, config.publish_rate_hz)?); - } - _ => {} - } - } - - sensor.ok_or_else(|| match kind { - CapabilityKind::Imu => anyhow::anyhow!("bno085 requires exactly one imu capability"), - CapabilityKind::Accelerometer => { - anyhow::anyhow!("bno085 requires exactly one accelerometer capability") - } - CapabilityKind::Gyroscope => { - anyhow::anyhow!("bno085 requires exactly one gyroscope capability") - } - }) -} - -#[derive(Debug, Clone)] -struct SampledSensor { - capability: CapabilityRef, - publish_rate_hz: f64, -} - -impl SampledSensor { - fn new(capability: CapabilityRef, publish_rate_hz: f64) -> Result { - if !publish_rate_hz.is_finite() || publish_rate_hz <= 0.0 { - bail!("component '{}' publish_rate_hz must be > 0", capability); - } - - Ok(Self { - capability, - publish_rate_hz, - }) - } - - fn publish_rate_hz(&self) -> f64 { - self.publish_rate_hz - } -} - -pub enum Input {} - -#[derive(Debug, Default)] -struct StubBackend; - -impl StubBackend { - async fn orientation(&mut self) -> Result<[f32; 4]> { - Ok([1.0, 0.0, 0.0, 0.0]) - } - - async fn accelerometer(&mut self) -> Result<[f32; 3]> { - Ok([0.0; 3]) - } - - async fn gyroscope(&mut self) -> Result<[f32; 3]> { - Ok([0.0; 3]) - } -} - -pub struct Bno085Runtime { - backend: StubBackend, - imu: Schedule, - accelerometer: Schedule, - gyroscope: Schedule, - imu_pub: TopicPublisher, - accelerometer_pub: TopicPublisher, - gyroscope_pub: TopicPublisher, -} - -#[async_trait::async_trait] -impl Runtime for Bno085Runtime { - const RUNTIME_ID: &'static str = "bno085"; - - type Args = EmptyArgs; - type Config = Config; - type Input = Input; - - fn config(_args: &Self::Args, _common: &RobotRuntimeArgs) -> Result { - bail!("component drivers resolve config from DriverRuntimeArgs") - } - - fn clock_period(config: &Self::Config) -> Duration { - config.clock_period() - } - - async fn new(io: &mut Io, config: Self::Config) -> Result { - let imu_pub = io - .publisher_topic( - topic::new() - .component(&config.imu.capability.component_id) - .imu(&config.imu.capability.capability_id) - .data(), - ) - .await?; - let accelerometer_pub = io - .publisher_topic( - topic::new() - .component(&config.accelerometer.capability.component_id) - .accelerometer(&config.accelerometer.capability.capability_id) - .data(), - ) - .await?; - let gyroscope_pub = io - .publisher_topic( - topic::new() - .component(&config.gyroscope.capability.component_id) - .gyroscope(&config.gyroscope.capability.capability_id) - .data(), - ) - .await?; - - Ok(Self { - backend: StubBackend, - imu: Schedule::from_publish_hz(config.imu.publish_rate_hz, SchedulePolicy::Skip), - accelerometer: Schedule::from_publish_hz( - config.accelerometer.publish_rate_hz, - SchedulePolicy::Skip, - ), - gyroscope: Schedule::from_publish_hz( - config.gyroscope.publish_rate_hz, - SchedulePolicy::Skip, - ), - imu_pub, - accelerometer_pub, - gyroscope_pub, - }) - } - - async fn step(&mut self, step: Step, _inputs: RuntimeInputs) -> Result<()> { - let at_ns = step.tick.time_ns(); - if self.imu.due_steps(step.tick.time_ns()) > 0 { - let sample = imu_v1::Sample::new(self.backend.orientation().await?); - self.imu_pub.put(at_ns, &imu::Sample::V1(sample)).await?; - } - - if self.accelerometer.due_steps(step.tick.time_ns()) > 0 { - let sample = accelerometer_v1::Sample::new(self.backend.accelerometer().await?); - self.accelerometer_pub - .put(at_ns, &accelerometer::Sample::V1(sample)) - .await?; - } - - if self.gyroscope.due_steps(step.tick.time_ns()) > 0 { - let sample = gyroscope_v1::Sample::new(self.backend.gyroscope().await?); - self.gyroscope_pub - .put(at_ns, &gyroscope::Sample::V1(sample)) - .await?; - } - - Ok(()) - } -} diff --git a/ddsm115/Cargo.toml b/ddsm115/Cargo.toml index 75c6986..a4330e3 100644 --- a/ddsm115/Cargo.toml +++ b/ddsm115/Cargo.toml @@ -13,13 +13,9 @@ repository.workspace = true [lints] workspace = true -# Migrated to the new framework authoring surface: the driver is `#[derive(Runtime)]` -# + `phoxal::run`, so it needs only `phoxal` + `anyhow` (no clap/tokio/async-trait in -# user code). Pinned directly to 0.15 — the other catalog drivers are still on the -# workspace's 0.9 until they migrate; cargo keeps both in the lock. [dependencies] anyhow = { workspace = true } -phoxal = "0.15" +phoxal = { workspace = true } [dev-dependencies] serde_json = "1" diff --git a/oak_d_lite/Cargo.toml b/oak_d_lite/Cargo.toml index 3003de2..0d0820e 100644 --- a/oak_d_lite/Cargo.toml +++ b/oak_d_lite/Cargo.toml @@ -15,7 +15,7 @@ workspace = true [dependencies] anyhow = { workspace = true } -clap = { workspace = true, features = ["derive", "env"] } -tokio = { workspace = true, features = ["macros", "rt", "rt-multi-thread"] } -tracing = { workspace = true } phoxal = { workspace = true } + +[dev-dependencies] +serde_json = "1" diff --git a/oak_d_lite/src/main.rs b/oak_d_lite/src/main.rs index 1ebdbf5..6aa90a9 100644 --- a/oak_d_lite/src/main.rs +++ b/oak_d_lite/src/main.rs @@ -1,22 +1,433 @@ +//! `oak_d_lite` — OAK-D Lite camera/depth/IMU component driver stub. + use anyhow::{Result, bail}; -use clap::Parser; -use phoxal::runtime::DriverRuntimeArgs; -use phoxal::util::init_tracing; -use tracing::error; - -#[tokio::main] -async fn main() -> Result<()> { - init_tracing()?; - let args = DriverRuntimeArgs::parse(); - let robot = args.runtime.robot()?; - let binding = robot.driver_binding(&args.component_id)?; - - error!( - component_id = %binding.component_id, - "OAK-D Lite physical component driver is not implemented" - ); - bail!( - "OAK-D Lite physical component driver is not implemented for component '{}'", - binding.component_id - ) +use phoxal::api::y2026_1 as api; +use phoxal::model::component::v1::capability::{CameraMode, Capability}; +use phoxal::prelude::*; + +const STEP_HZ: f64 = 100.0; + +#[derive(phoxal::Runtime)] +#[phoxal(id = "oak_d_lite", api = y2026_1)] +struct OakDLite { + camera: Vec>, + camera_specs: Vec, + depth: Vec>, + depth_specs: Vec, + imu: Vec>, + imu_divisors: Vec, + accelerometer: Vec>, + accelerometer_divisors: Vec, + gyroscope: Vec>, + gyroscope_divisors: Vec, +} + +#[derive(Debug, Clone)] +struct CameraSlot { + capability_id: String, + spec: CameraSpec, +} + +#[derive(Debug, Clone)] +struct CameraSpec { + divisor: u64, + width: u32, + height: u32, + encoding: api::component::CameraEncoding, + data_len: usize, +} + +#[derive(Debug, Clone)] +struct DepthSlot { + capability_id: String, + spec: DepthSpec, +} + +#[derive(Debug, Clone)] +struct DepthSpec { + divisor: u64, + width: u32, + height: u32, + sample_len: usize, +} + +#[derive(Debug, Clone)] +struct SensorSlot { + capability_id: String, + divisor: u64, +} + +#[phoxal::runtime] +impl OakDLite { + #[setup] + async fn setup(ctx: &mut SetupContext) -> Result { + let instance = ctx.component()?.to_string(); + let (camera_slots, depth_slots, imu_slots, accelerometer_slots, gyroscope_slots) = { + let robot = ctx.robot()?; + let spec = robot.component_for_instance(&instance)?; + let mut camera = Vec::new(); + let mut depth = Vec::new(); + let mut imu = Vec::new(); + let mut accelerometer = Vec::new(); + let mut gyroscope = Vec::new(); + + for (capability_id, capability) in &spec.capabilities { + match capability { + Capability::Camera(config) => { + validate_publish_rate(capability_id, config.publish_rate_hz)?; + let encoding = encoding_for_mode(config.mode); + let data_len = frame_byte_len( + config.width_px, + config.height_px, + channels_for_encoding(encoding), + ) + .ok_or_else(|| { + anyhow::anyhow!( + "camera capability '{capability_id}' frame dimensions are too large" + ) + })?; + camera.push(CameraSlot { + capability_id: capability_id.to_string(), + spec: CameraSpec { + divisor: divisor_for_rate(STEP_HZ, config.publish_rate_hz), + width: config.width_px, + height: config.height_px, + encoding, + data_len, + }, + }); + } + Capability::Depth(config) => { + validate_publish_rate(capability_id, config.publish_rate_hz)?; + let sample_len = + depth_sample_len(config.width_px, config.height_px).ok_or_else( + || { + anyhow::anyhow!( + "depth capability '{capability_id}' frame dimensions are too large" + ) + }, + )?; + depth.push(DepthSlot { + capability_id: capability_id.to_string(), + spec: DepthSpec { + divisor: divisor_for_rate(STEP_HZ, config.publish_rate_hz), + width: config.width_px, + height: config.height_px, + sample_len, + }, + }); + } + Capability::Imu(config) => { + imu.push(sensor_slot(capability_id, config.publish_rate_hz)?); + } + Capability::Accelerometer(config) => { + accelerometer.push(sensor_slot(capability_id, config.publish_rate_hz)?); + } + Capability::Gyroscope(config) => { + gyroscope.push(sensor_slot(capability_id, config.publish_rate_hz)?); + } + _ => {} + } + } + + (camera, depth, imu, accelerometer, gyroscope) + }; + + if camera_slots.is_empty() + && depth_slots.is_empty() + && imu_slots.is_empty() + && accelerometer_slots.is_empty() + && gyroscope_slots.is_empty() + { + bail!( + "oak_d_lite requires at least one camera, depth, imu, accelerometer, or gyroscope capability" + ); + } + + let mut camera = Vec::new(); + let mut camera_specs = Vec::new(); + for slot in camera_slots { + camera.push( + ctx.publisher( + api::topic::new() + .component() + .camera_frame(&instance, &slot.capability_id), + ) + .await?, + ); + camera_specs.push(slot.spec); + } + + let mut depth = Vec::new(); + let mut depth_specs = Vec::new(); + for slot in depth_slots { + depth.push( + ctx.publisher( + api::topic::new() + .component() + .depth_frame(&instance, &slot.capability_id), + ) + .await?, + ); + depth_specs.push(slot.spec); + } + + let mut imu = Vec::new(); + let mut imu_divisors = Vec::new(); + for slot in imu_slots { + imu.push( + ctx.publisher( + api::topic::new() + .component() + .imu_sample(&instance, &slot.capability_id), + ) + .await?, + ); + imu_divisors.push(slot.divisor); + } + + let mut accelerometer = Vec::new(); + let mut accelerometer_divisors = Vec::new(); + for slot in accelerometer_slots { + accelerometer.push( + ctx.publisher( + api::topic::new() + .component() + .accelerometer_sample(&instance, &slot.capability_id), + ) + .await?, + ); + accelerometer_divisors.push(slot.divisor); + } + + let mut gyroscope = Vec::new(); + let mut gyroscope_divisors = Vec::new(); + for slot in gyroscope_slots { + gyroscope.push( + ctx.publisher( + api::topic::new() + .component() + .gyroscope_sample(&instance, &slot.capability_id), + ) + .await?, + ); + gyroscope_divisors.push(slot.divisor); + } + + Ok(Self { + camera, + camera_specs, + depth, + depth_specs, + imu, + imu_divisors, + accelerometer, + accelerometer_divisors, + gyroscope, + gyroscope_divisors, + }) + } + + #[step(hz = 100)] + async fn step(&mut self, step: StepContext) -> Result<()> { + let at = step.time(); + let step_index = step.step_index(); + + for (publisher, spec) in self.camera.iter().zip(&self.camera_specs) { + if is_due(step_index, spec.divisor) { + publisher.publish_at(at, camera_frame(spec)).await?; + } + } + + for (publisher, spec) in self.depth.iter().zip(&self.depth_specs) { + if is_due(step_index, spec.divisor) { + publisher.publish_at(at, depth_frame(spec)).await?; + } + } + + for (publisher, divisor) in self.imu.iter().zip(&self.imu_divisors) { + if is_due(step_index, *divisor) { + publisher.publish_at(at, imu_sample()).await?; + } + } + + for (publisher, divisor) in self.accelerometer.iter().zip(&self.accelerometer_divisors) { + if is_due(step_index, *divisor) { + publisher.publish_at(at, accelerometer_sample()).await?; + } + } + + for (publisher, divisor) in self.gyroscope.iter().zip(&self.gyroscope_divisors) { + if is_due(step_index, *divisor) { + publisher.publish_at(at, gyroscope_sample()).await?; + } + } + + Ok(()) + } + + #[shutdown] + async fn shutdown(&mut self, _ctx: ShutdownContext) -> Result<()> { + Ok(()) + } +} + +fn sensor_slot(capability_id: &str, publish_rate_hz: f64) -> Result { + validate_publish_rate(capability_id, publish_rate_hz)?; + Ok(SensorSlot { + capability_id: capability_id.to_string(), + divisor: divisor_for_rate(STEP_HZ, publish_rate_hz), + }) +} + +fn validate_publish_rate(capability_id: &str, publish_rate_hz: f64) -> Result<()> { + if !publish_rate_hz.is_finite() || publish_rate_hz <= 0.0 { + bail!("capability '{capability_id}' publish_rate_hz must be > 0"); + } + Ok(()) +} + +fn divisor_for_rate(step_hz: f64, publish_rate_hz: f64) -> u64 { + (step_hz / publish_rate_hz).round().max(1.0) as u64 +} + +fn is_due(step_index: u64, divisor: u64) -> bool { + divisor <= 1 || step_index % divisor == 0 +} + +fn encoding_for_mode(mode: CameraMode) -> api::component::CameraEncoding { + match mode { + CameraMode::Mono => api::component::CameraEncoding::L8, + CameraMode::Rgb => api::component::CameraEncoding::Rgb8, + } +} + +fn channels_for_encoding(encoding: api::component::CameraEncoding) -> usize { + match encoding { + api::component::CameraEncoding::L8 => 1, + api::component::CameraEncoding::Rgb8 => 3, + api::component::CameraEncoding::Rgba8 => 4, + api::component::CameraEncoding::Jpeg | api::component::CameraEncoding::Png => 1, + } +} + +fn frame_byte_len(width: u32, height: u32, channels: usize) -> Option { + let pixels = u64::from(width).checked_mul(u64::from(height))?; + let channels = u64::try_from(channels).ok()?; + let bytes = pixels.checked_mul(channels)?; + usize::try_from(bytes).ok() +} + +fn depth_sample_len(width: u32, height: u32) -> Option { + frame_byte_len(width, height, 1) +} + +fn camera_frame(spec: &CameraSpec) -> api::component::CameraFrame { + api::component::CameraFrame { + width: spec.width, + height: spec.height, + encoding: spec.encoding, + intrinsics: None, + distortion: None, + exposure: None, + measured_at_ns: None, + calibration: None, + data: vec![0u8; spec.data_len], + } +} + +fn depth_frame(spec: &DepthSpec) -> api::component::DepthFrame { + api::component::DepthFrame { + samples_mm: vec![0u16; spec.sample_len], + encoding: api::component::DepthEncoding::U16Millimeters, + invalid_sample_policy: api::component::DepthInvalidSamplePolicy::ZeroIsInvalid, + width: Some(spec.width), + height: Some(spec.height), + intrinsics: None, + distortion: None, + exposure: None, + measured_at_ns: None, + calibration: None, + } +} + +fn imu_sample() -> api::component::ImuSample { + api::component::ImuSample { + orientation: Some([1.0, 0.0, 0.0, 0.0]), + angular_velocity_radps: [0.0; 3], + linear_acceleration_mps2: [0.0; 3], + covariance: None, + noise_density: None, + sensor_frame_id: None, + measured_at_ns: None, + health: api::component::SensorHealth::Nominal, + bias: None, + } +} + +fn accelerometer_sample() -> api::component::AccelerometerSample { + api::component::AccelerometerSample { + linear_acceleration: [0.0; 3], + } +} + +fn gyroscope_sample() -> api::component::GyroscopeSample { + api::component::GyroscopeSample { + angular_velocity: [0.0; 3], + } +} + +fn main() -> phoxal::Result<()> { + phoxal::run::() +} + +#[cfg(test)] +mod tests { + use super::{ + OakDLite, channels_for_encoding, divisor_for_rate, encoding_for_mode, frame_byte_len, + }; + use phoxal::api::ContractBody; + use phoxal::api::y2026_1 as api; + use phoxal::model::component::v1::capability::CameraMode; + + #[test] + fn camera_encoding_and_frame_size_follow_mode() { + assert!(matches!( + encoding_for_mode(CameraMode::Mono), + api::component::CameraEncoding::L8 + )); + assert!(matches!( + encoding_for_mode(CameraMode::Rgb), + api::component::CameraEncoding::Rgb8 + )); + assert_eq!(channels_for_encoding(api::component::CameraEncoding::L8), 1); + assert_eq!( + channels_for_encoding(api::component::CameraEncoding::Rgb8), + 3 + ); + assert_eq!(frame_byte_len(640, 480, 3), Some(921_600)); + assert_eq!(divisor_for_rate(100.0, 25.0), 4); + } + + #[test] + fn emit_apis_reports_per_component_contracts() { + let json = phoxal::runtime::emit_apis_json::(); + let value: serde_json::Value = serde_json::from_str(&json).unwrap(); + assert_eq!(value["artifact"]["id"], "oak_d_lite"); + let contracts = value["required_contracts"].as_array().unwrap(); + for family in [ + ::FAMILY, + ::FAMILY, + ::FAMILY, + ::FAMILY, + ::FAMILY, + ] { + assert!( + contracts + .iter() + .any(|c| c["family"] == family && c["direction"] == "publish"), + "missing publish contract for {family}" + ); + } + } } diff --git a/vl53l1x/Cargo.toml b/vl53l1x/Cargo.toml index 4291363..ef1e0e0 100644 --- a/vl53l1x/Cargo.toml +++ b/vl53l1x/Cargo.toml @@ -15,8 +15,7 @@ workspace = true [dependencies] anyhow = { workspace = true } -async-trait = { workspace = true } -clap = { workspace = true, features = ["derive", "env"] } -tokio = { workspace = true, features = ["macros", "rt", "rt-multi-thread", "signal", "sync", "time"] } -tracing = { workspace = true } phoxal = { workspace = true } + +[dev-dependencies] +serde_json = "1" diff --git a/vl53l1x/src/main.rs b/vl53l1x/src/main.rs index 2ea75b0..2e8b03b 100644 --- a/vl53l1x/src/main.rs +++ b/vl53l1x/src/main.rs @@ -1,29 +1,162 @@ -mod runtime; - -use anyhow::Result; -use clap::Parser; -use phoxal::runtime::{DriverRuntimeArgs, RuntimeProcess}; -use phoxal::util::init_tracing; -use tracing::info; - -use crate::runtime::Config; - -#[tokio::main] -async fn main() -> Result<()> { - init_tracing()?; - let args = DriverRuntimeArgs::parse(); - let robot = args.runtime.robot()?; - let bus = args.runtime.connect_bus().await?; - let binding = robot.driver_binding(&args.component_id)?; - let config = Config::new(&binding.component_id, binding.component)?; - - info!( - publish_rate_hz = config.publish_rate_hz(), - "VL53L1X runtime ready" - ); - RuntimeProcess::new(&bus, args.simulation(), config.clock_period()) - .run::(config) - .await?; +//! `vl53l1x` — VL53L1X range component driver stub. +use anyhow::{Result, bail}; +use phoxal::api::y2026_1 as api; +use phoxal::model::component::v1::capability::Capability; +use phoxal::prelude::*; + +const STEP_HZ: f64 = 20.0; + +#[derive(phoxal::Runtime)] +#[phoxal(id = "vl53l1x", api = y2026_1)] +struct Vl53l1x { + range: Vec>, + range_specs: Vec, +} + +#[derive(Debug, Clone)] +struct RangeSlot { + capability_id: String, + spec: RangeSpec, +} + +#[derive(Debug, Clone)] +struct RangeSpec { + divisor: u64, + min_range_m: f32, + max_range_m: f32, +} + +#[phoxal::runtime] +impl Vl53l1x { + #[setup] + async fn setup(ctx: &mut SetupContext) -> Result { + let instance = ctx.component()?.to_string(); + let slots = { + let robot = ctx.robot()?; + let spec = robot.component_for_instance(&instance)?; + let mut slots = Vec::new(); + + for (capability_id, capability) in &spec.capabilities { + if let Capability::Range(config) = capability { + validate_publish_rate(capability_id, config.publish_rate_hz)?; + slots.push(RangeSlot { + capability_id: capability_id.to_string(), + spec: RangeSpec { + divisor: divisor_for_rate(STEP_HZ, config.publish_rate_hz), + min_range_m: config.min_range_m as f32, + max_range_m: config.max_range_m as f32, + }, + }); + } + } + + slots + }; + + if slots.is_empty() { + bail!("vl53l1x requires at least one range capability"); + } + + let mut range = Vec::new(); + let mut range_specs = Vec::new(); + for slot in slots { + range.push( + ctx.publisher( + api::topic::new() + .component() + .range_sample(&instance, &slot.capability_id), + ) + .await?, + ); + range_specs.push(slot.spec); + } + + Ok(Self { range, range_specs }) + } + + #[step(hz = 20)] + async fn step(&mut self, step: StepContext) -> Result<()> { + let at = step.time(); + let step_index = step.step_index(); + + for (publisher, spec) in self.range.iter().zip(&self.range_specs) { + if is_due(step_index, spec.divisor) { + publisher.publish_at(at, range_sample(spec)).await?; + } + } + + Ok(()) + } + + #[shutdown] + async fn shutdown(&mut self, _ctx: ShutdownContext) -> Result<()> { + Ok(()) + } +} + +fn validate_publish_rate(capability_id: &str, publish_rate_hz: f64) -> Result<()> { + if !publish_rate_hz.is_finite() || publish_rate_hz <= 0.0 { + bail!("capability '{capability_id}' publish_rate_hz must be > 0"); + } Ok(()) } + +fn divisor_for_rate(step_hz: f64, publish_rate_hz: f64) -> u64 { + (step_hz / publish_rate_hz).round().max(1.0) as u64 +} + +fn is_due(step_index: u64, divisor: u64) -> bool { + divisor <= 1 || step_index % divisor == 0 +} + +fn range_sample(spec: &RangeSpec) -> api::component::RangeSample { + api::component::RangeSample { + distance_m: spec.max_range_m, + limits: Some(api::component::Limits { + min_m: spec.min_range_m, + max_m: spec.max_range_m, + }), + measured_at_ns: None, + quality: None, + health: api::component::SensorHealth::Nominal, + } +} + +fn main() -> phoxal::Result<()> { + phoxal::run::() +} + +#[cfg(test)] +mod tests { + use super::{Vl53l1x, divisor_for_rate, range_sample}; + use phoxal::api::ContractBody; + use phoxal::api::y2026_1 as api; + + #[test] + fn divisor_and_stub_distance_follow_range_config() { + assert_eq!(divisor_for_rate(20.0, 20.0), 1); + assert_eq!(divisor_for_rate(20.0, 10.0), 2); + + let spec = super::RangeSpec { + divisor: 1, + min_range_m: 0.1, + max_range_m: 4.0, + }; + let sample = range_sample(&spec); + assert_eq!(sample.distance_m, 4.0); + assert_eq!(sample.limits.unwrap().min_m, 0.1); + } + + #[test] + fn emit_apis_reports_per_component_contracts() { + let json = phoxal::runtime::emit_apis_json::(); + let value: serde_json::Value = serde_json::from_str(&json).unwrap(); + assert_eq!(value["artifact"]["id"], "vl53l1x"); + let contracts = value["required_contracts"].as_array().unwrap(); + assert!(contracts.iter().any(|c| { + c["family"] == ::FAMILY + && c["direction"] == "publish" + })); + } +} diff --git a/vl53l1x/src/runtime.rs b/vl53l1x/src/runtime.rs deleted file mode 100644 index 6f2d308..0000000 --- a/vl53l1x/src/runtime.rs +++ /dev/null @@ -1,143 +0,0 @@ -use anyhow::{Result, bail}; -use phoxal::api::component::capability::range::{self, v1 as range_v1}; -use phoxal::api::topic; -use phoxal::model::component::v1::CapabilityRef; -use phoxal::model::component::v1::capability::{Capability, Range as RangeConfig}; -use phoxal::runtime::clock::{Schedule, SchedulePolicy, Step}; -use phoxal::runtime::{EmptyArgs, Io, RobotRuntimeArgs, Runtime, RuntimeInputs, TopicPublisher}; - -#[derive(Clone)] -pub struct Config { - range: SampledSensor, -} - -impl Config { - pub fn new( - component_id: &str, - component: &phoxal::model::component::v1::Component, - ) -> Result { - Ok(Self { - range: Self::inspect(component_id, component)?, - }) - } - - fn inspect( - component_id: &str, - component: &phoxal::model::component::v1::Component, - ) -> Result { - let mut range = None; - - for (local_capability_id, capability) in &component.capabilities { - let capability_ref = CapabilityRef::new(component_id, local_capability_id); - if let Capability::Range(range_config) = capability { - if range.is_some() { - bail!("vl53l1x supports at most one range capability"); - } - range = Some(SampledSensor::new(capability_ref, range_config)?); - } - } - - let range = - range.ok_or_else(|| anyhow::anyhow!("vl53l1x requires one range capability"))?; - Ok(range) - } - - pub fn clock_period(&self) -> std::time::Duration { - std::time::Duration::from_secs_f64(1.0 / self.range.publish_rate_hz()) - } - - pub fn publish_rate_hz(&self) -> f64 { - self.range.publish_rate_hz() - } -} - -#[derive(Debug, Clone)] -struct SampledSensor { - capability: CapabilityRef, - publish_rate_hz: f64, - default_distance_m: f32, -} - -impl SampledSensor { - fn new(capability: CapabilityRef, config: &RangeConfig) -> Result { - if !config.publish_rate_hz.is_finite() || config.publish_rate_hz <= 0.0 { - bail!("component '{}' publish_rate_hz must be > 0", capability); - } - - Ok(Self { - capability, - publish_rate_hz: config.publish_rate_hz, - default_distance_m: config.max_range_m as f32, - }) - } - - fn publish_rate_hz(&self) -> f64 { - self.publish_rate_hz - } -} - -pub enum Input {} - -#[derive(Debug, Default)] -struct StubBackend; - -impl StubBackend { - async fn distance_m(&mut self, default_distance_m: f32) -> Result { - Ok(default_distance_m) - } -} - -pub struct Vl53l1xRuntime { - backend: StubBackend, - default_distance_m: f32, - schedule: Schedule, - range_pub: TopicPublisher, -} - -#[async_trait::async_trait] -impl Runtime for Vl53l1xRuntime { - const RUNTIME_ID: &'static str = "vl53l1x"; - - type Args = EmptyArgs; - type Config = Config; - type Input = Input; - - fn config(_args: &Self::Args, _common: &RobotRuntimeArgs) -> Result { - bail!("component drivers resolve config from DriverRuntimeArgs") - } - - fn clock_period(config: &Self::Config) -> std::time::Duration { - config.clock_period() - } - - async fn new(io: &mut Io, config: Self::Config) -> Result { - let range_pub = io - .publisher_topic( - topic::new() - .component(&config.range.capability.component_id) - .range(&config.range.capability.capability_id) - .data(), - ) - .await?; - - Ok(Self { - backend: StubBackend, - default_distance_m: config.range.default_distance_m, - schedule: Schedule::from_publish_hz(config.range.publish_rate_hz, SchedulePolicy::Skip), - range_pub, - }) - } - - async fn step(&mut self, step: Step, _inputs: RuntimeInputs) -> Result<()> { - if self.schedule.due_steps(step.tick.time_ns()) == 0 { - return Ok(()); - } - - let sample = range_v1::Sample::new(self.backend.distance_m(self.default_distance_m).await?); - self.range_pub - .put(step.tick.time_ns(), &range::Sample::V1(sample)) - .await?; - - Ok(()) - } -} diff --git a/zed_f9p/Cargo.toml b/zed_f9p/Cargo.toml index dee36e1..41e705d 100644 --- a/zed_f9p/Cargo.toml +++ b/zed_f9p/Cargo.toml @@ -15,7 +15,7 @@ workspace = true [dependencies] anyhow = { workspace = true } -clap = { workspace = true, features = ["derive", "env"] } -tokio = { workspace = true, features = ["macros", "rt", "rt-multi-thread"] } -tracing = { workspace = true } phoxal = { workspace = true } + +[dev-dependencies] +serde_json = "1" diff --git a/zed_f9p/src/main.rs b/zed_f9p/src/main.rs index fb23264..41635f4 100644 --- a/zed_f9p/src/main.rs +++ b/zed_f9p/src/main.rs @@ -1,22 +1,146 @@ +//! `zed_f9p` — u-blox ZED-F9P GNSS component driver stub. + use anyhow::{Result, bail}; -use clap::Parser; -use phoxal::runtime::DriverRuntimeArgs; -use phoxal::util::init_tracing; -use tracing::error; - -#[tokio::main] -async fn main() -> Result<()> { - init_tracing()?; - let args = DriverRuntimeArgs::parse(); - let robot = args.runtime.robot()?; - let binding = robot.driver_binding(&args.component_id)?; - - error!( - component_id = %binding.component_id, - "ZED-F9P physical component driver is not implemented" - ); - bail!( - "ZED-F9P physical component driver is not implemented for component '{}'", - binding.component_id - ) +use phoxal::api::y2026_1 as api; +use phoxal::model::component::v1::capability::Capability; +use phoxal::prelude::*; + +const STEP_HZ: f64 = 10.0; + +#[derive(phoxal::Runtime)] +#[phoxal(id = "zed_f9p", api = y2026_1)] +struct ZedF9p { + gnss: Vec>, + gnss_divisors: Vec, +} + +#[derive(Debug, Clone)] +struct GnssSlot { + capability_id: String, + divisor: u64, +} + +#[phoxal::runtime] +impl ZedF9p { + #[setup] + async fn setup(ctx: &mut SetupContext) -> Result { + let instance = ctx.component()?.to_string(); + let slots = { + let robot = ctx.robot()?; + let spec = robot.component_for_instance(&instance)?; + let mut slots = Vec::new(); + + for (capability_id, capability) in &spec.capabilities { + if let Capability::Gnss(config) = capability { + validate_publish_rate(capability_id, config.publish_rate_hz)?; + let _coordinate_system = config.coordinate_system; + slots.push(GnssSlot { + capability_id: capability_id.to_string(), + divisor: divisor_for_rate(STEP_HZ, config.publish_rate_hz), + }); + } + } + + slots + }; + + if slots.is_empty() { + bail!("zed_f9p requires at least one gnss capability"); + } + + let mut gnss = Vec::new(); + let mut gnss_divisors = Vec::new(); + for slot in slots { + gnss.push( + ctx.publisher( + api::topic::new() + .component() + .gnss_sample(&instance, &slot.capability_id), + ) + .await?, + ); + gnss_divisors.push(slot.divisor); + } + + Ok(Self { + gnss, + gnss_divisors, + }) + } + + #[step(hz = 10)] + async fn step(&mut self, step: StepContext) -> Result<()> { + let at = step.time(); + let step_index = step.step_index(); + + for (publisher, divisor) in self.gnss.iter().zip(&self.gnss_divisors) { + if is_due(step_index, *divisor) { + publisher.publish_at(at, gnss_sample()).await?; + } + } + + Ok(()) + } + + #[shutdown] + async fn shutdown(&mut self, _ctx: ShutdownContext) -> Result<()> { + Ok(()) + } +} + +fn validate_publish_rate(capability_id: &str, publish_rate_hz: f64) -> Result<()> { + if !publish_rate_hz.is_finite() || publish_rate_hz <= 0.0 { + bail!("capability '{capability_id}' publish_rate_hz must be > 0"); + } + Ok(()) +} + +fn divisor_for_rate(step_hz: f64, publish_rate_hz: f64) -> u64 { + (step_hz / publish_rate_hz).round().max(1.0) as u64 +} + +fn is_due(step_index: u64, divisor: u64) -> bool { + divisor <= 1 || step_index % divisor == 0 +} + +fn gnss_sample() -> api::component::GnssSample { + api::component::GnssSample { + latitude: 0.0, + longitude: 0.0, + altitude: 0.0, + position_covariance: [0.0; 9], + } +} + +fn main() -> phoxal::Result<()> { + phoxal::run::() +} + +#[cfg(test)] +mod tests { + use super::{ZedF9p, divisor_for_rate, gnss_sample}; + use phoxal::api::ContractBody; + use phoxal::api::y2026_1 as api; + + #[test] + fn divisor_and_stub_sample_are_stable() { + assert_eq!(divisor_for_rate(10.0, 10.0), 1); + assert_eq!(divisor_for_rate(10.0, 5.0), 2); + + let sample = gnss_sample(); + assert_eq!(sample.latitude, 0.0); + assert_eq!(sample.position_covariance, [0.0; 9]); + } + + #[test] + fn emit_apis_reports_per_component_contracts() { + let json = phoxal::runtime::emit_apis_json::(); + let value: serde_json::Value = serde_json::from_str(&json).unwrap(); + assert_eq!(value["artifact"]["id"], "zed_f9p"); + let contracts = value["required_contracts"].as_array().unwrap(); + assert!(contracts.iter().any(|c| { + c["family"] == ::FAMILY + && c["direction"] == "publish" + })); + } }