From d58e6a379ff89d556370dd778f28c4ccaffff0d7 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Mon, 27 Apr 2026 21:08:20 +0900 Subject: [PATCH 01/20] add: imu_corrector with test case Signed-off-by: nokosaaan --- .../test_autoware/imu_corrector/Cargo.toml | 11 + .../test_autoware/imu_corrector/src/lib.rs | 478 ++++++++++++++++++ 2 files changed, 489 insertions(+) create mode 100644 applications/tests/test_autoware/imu_corrector/Cargo.toml create mode 100644 applications/tests/test_autoware/imu_corrector/src/lib.rs diff --git a/applications/tests/test_autoware/imu_corrector/Cargo.toml b/applications/tests/test_autoware/imu_corrector/Cargo.toml new file mode 100644 index 000000000..30b5ce849 --- /dev/null +++ b/applications/tests/test_autoware/imu_corrector/Cargo.toml @@ -0,0 +1,11 @@ +[package] +name = "imu_corrector" +version = "0.1.0" +edition = "2021" + +[dependencies] +log = "0.4" +nalgebra = { version = "0.32", default-features = false, features = ["libm"] } +awkernel_async_lib = { path = "../../../../awkernel_async_lib", default-features = false } +awkernel_lib = { path = "../../../../awkernel_lib", default-features = false } +imu_driver = { path = "../imu_driver", default-features = false } \ No newline at end of file diff --git a/applications/tests/test_autoware/imu_corrector/src/lib.rs b/applications/tests/test_autoware/imu_corrector/src/lib.rs new file mode 100644 index 000000000..2752b33f9 --- /dev/null +++ b/applications/tests/test_autoware/imu_corrector/src/lib.rs @@ -0,0 +1,478 @@ +#![no_std] +extern crate alloc; + +use alloc::{format, string::String}; +use imu_driver::{Header, ImuMsg, Quaternion, Vector3}; +use nalgebra::{Quaternion as NQuaternion, UnitQuaternion, Vector3 as NVector3}; + +#[derive(Clone, Debug)] +pub struct Transform { + pub translation: Vector3, + pub rotation: Quaternion, +} + +impl Transform { + pub fn identity() -> Self { + Self { + translation: imu_driver::Vector3::new(5.22444, 0.07615, 2.72762), // use aip_x2_gen2_description. + rotation: imu_driver::Quaternion { + x: 1.0, + y: 0.0, + z: 0.0, + w: 0.0, + }, + } + } + + fn to_nalgebra_vector3(&self, vec: &Vector3) -> NVector3 { + NVector3::new(vec.x, vec.y, vec.z) + } + + fn to_imu_vector3(&self, vec: &NVector3) -> Vector3 { + Vector3::new(vec.x, vec.y, vec.z) + } + + fn to_nalgebra_quaternion(&self, quat: &Quaternion) -> UnitQuaternion { + let n_quat = NQuaternion::new(quat.w, quat.x, quat.y, quat.z); + UnitQuaternion::from_quaternion(n_quat) + } + + pub fn apply_to_vector(&self, vec: Vector3) -> Vector3 { + let nalgebra_vec = self.to_nalgebra_vector3(&vec); + let nalgebra_quat = self.to_nalgebra_quaternion(&self.rotation); + let nalgebra_trans = self.to_nalgebra_vector3(&self.translation); + let rotated = nalgebra_quat * nalgebra_vec; + let result = rotated + nalgebra_trans; + self.to_imu_vector3(&result) + } +} + +pub trait TransformListener { + fn get_latest_transform(&self, from_frame: &str, to_frame: &str) -> Option; + fn get_transform_at_time( + &self, + from_frame: &str, + to_frame: &str, + timestamp: u64, + ) -> Option; +} + +pub struct MockTransformListener { + transforms: alloc::collections::BTreeMap, +} + +impl MockTransformListener { + pub fn new() -> Self { + Self { + transforms: alloc::collections::BTreeMap::new(), + } + } + + pub fn add_transform(&mut self, from_frame: &str, to_frame: &str, transform: Transform) { + let key = format!("{}_to_{}", from_frame, to_frame); + self.transforms.insert(key, transform); + } +} + +impl TransformListener for MockTransformListener { + fn get_latest_transform(&self, from_frame: &str, to_frame: &str) -> Option { + let key = format!("{}_to_{}", from_frame, to_frame); + self.transforms.get(&key).cloned() + } + + fn get_transform_at_time( + &self, + from_frame: &str, + to_frame: &str, + _timestamp: u64, + ) -> Option { + self.get_latest_transform(from_frame, to_frame) + } +} + +#[derive(Clone, Debug)] +pub struct ImuWithCovariance { + pub header: Header, + pub orientation: Quaternion, + pub angular_velocity: Vector3, + pub angular_velocity_covariance: [f64; 9], + pub linear_acceleration: Vector3, + pub linear_acceleration_covariance: [f64; 9], +} + +impl ImuWithCovariance { + pub fn from_imu_msg(imu_msg: &ImuMsg) -> Self { + Self { + header: imu_msg.header.clone(), + orientation: imu_msg.orientation.clone(), + angular_velocity: imu_msg.angular_velocity.clone(), + angular_velocity_covariance: [0.0; 9], + linear_acceleration: imu_msg.linear_acceleration.clone(), + linear_acceleration_covariance: [0.0; 9], + } + } + + pub fn to_imu_msg(&self) -> ImuMsg { + ImuMsg { + header: self.header.clone(), + orientation: self.orientation.clone(), + angular_velocity: self.angular_velocity.clone(), + linear_acceleration: self.linear_acceleration.clone(), + } + } +} + +pub struct ImuCorrectorConfig { + pub angular_velocity_offset_x: f64, + pub angular_velocity_offset_y: f64, + pub angular_velocity_offset_z: f64, + pub angular_velocity_stddev_xx: f64, + pub angular_velocity_stddev_yy: f64, + pub angular_velocity_stddev_zz: f64, + pub acceleration_stddev: f64, + pub output_frame: &'static str, +} + +impl Default for ImuCorrectorConfig { + fn default() -> Self { + Self { + angular_velocity_offset_x: 0.0, + angular_velocity_offset_y: 0.0, + angular_velocity_offset_z: 0.0, + angular_velocity_stddev_xx: 0.03, + angular_velocity_stddev_yy: 0.03, + angular_velocity_stddev_zz: 0.03, + acceleration_stddev: 10000.0, + output_frame: "base_link", + } + } +} + +pub struct ImuCorrector { + config: ImuCorrectorConfig, + transform_listener: T, +} + +impl ImuCorrector { + pub fn new() -> Self { + Self { + config: ImuCorrectorConfig::default(), + transform_listener: MockTransformListener::new(), + } + } + + pub fn with_transform_listener(transform_listener: MockTransformListener) -> Self { + Self { + config: ImuCorrectorConfig::default(), + transform_listener, + } + } +} + +impl ImuCorrector { + pub fn set_config(&mut self, config: ImuCorrectorConfig) { + self.config = config; + } + + fn to_nalgebra_vector3(&self, vec: &Vector3) -> NVector3 { + NVector3::new(vec.x, vec.y, vec.z) + } + + fn to_imu_vector3(&self, vec: &NVector3) -> Vector3 { + Vector3::new(vec.x, vec.y, vec.z) + } + + fn to_nalgebra_quaternion(&self, quat: &Quaternion) -> UnitQuaternion { + let n_quat = NQuaternion::new(quat.w, quat.x, quat.y, quat.z); + UnitQuaternion::from_quaternion(n_quat) + } + + fn transform_vector3(&self, vec: &Vector3, transform: &Transform) -> Vector3 { + let nalgebra_vec = self.to_nalgebra_vector3(vec); + let nalgebra_quat = self.to_nalgebra_quaternion(&transform.rotation); + let nalgebra_trans = self.to_nalgebra_vector3(&transform.translation); + let rotated = nalgebra_quat * nalgebra_vec; + let result = rotated + nalgebra_trans; + self.to_imu_vector3(&result) + } + + fn transform_covariance(&self, cov: &[f64; 9]) -> [f64; 9] { + let max_cov = cov[0].max(cov[4]).max(cov[8]); + let mut cov_transformed = [0.0; 9]; + cov_transformed[0] = max_cov; + cov_transformed[4] = max_cov; + cov_transformed[8] = max_cov; + + cov_transformed + } + + pub fn correct_imu_with_covariance( + &self, + imu_msg: &ImuMsg, + transform: Option<&Transform>, + ) -> ImuWithCovariance { + let mut corrected_imu = ImuWithCovariance::from_imu_msg(imu_msg); + corrected_imu.angular_velocity.x -= self.config.angular_velocity_offset_x; + corrected_imu.angular_velocity.y -= self.config.angular_velocity_offset_y; + corrected_imu.angular_velocity.z -= self.config.angular_velocity_offset_z; + corrected_imu.angular_velocity_covariance[0] = + self.config.angular_velocity_stddev_xx * self.config.angular_velocity_stddev_xx; + corrected_imu.angular_velocity_covariance[4] = + self.config.angular_velocity_stddev_yy * self.config.angular_velocity_stddev_yy; + corrected_imu.angular_velocity_covariance[8] = + self.config.angular_velocity_stddev_zz * self.config.angular_velocity_stddev_zz; + let accel_var = self.config.acceleration_stddev * self.config.acceleration_stddev; + corrected_imu.linear_acceleration_covariance[0] = accel_var; + corrected_imu.linear_acceleration_covariance[4] = accel_var; + corrected_imu.linear_acceleration_covariance[8] = accel_var; + + if let Some(tf) = transform { + corrected_imu.linear_acceleration = + self.transform_vector3(&corrected_imu.linear_acceleration, tf); + corrected_imu.linear_acceleration_covariance = + self.transform_covariance(&corrected_imu.linear_acceleration_covariance); + corrected_imu.angular_velocity = + self.transform_vector3(&corrected_imu.angular_velocity, tf); + corrected_imu.angular_velocity_covariance = + self.transform_covariance(&corrected_imu.angular_velocity_covariance); + corrected_imu.header.frame_id = self.config.output_frame; + } + + corrected_imu + } + + pub fn correct_imu(&self, imu_msg: &ImuMsg, transform: Option<&Transform>) -> ImuMsg { + let corrected_with_cov = self.correct_imu_with_covariance(imu_msg, transform); + corrected_with_cov.to_imu_msg() + } + + pub fn correct_imu_with_dynamic_tf(&self, imu_msg: &ImuMsg) -> Option { + let transform = self + .transform_listener + .get_latest_transform(&imu_msg.header.frame_id, self.config.output_frame)?; + + let corrected_with_cov = self.correct_imu_with_covariance(imu_msg, Some(&transform)); + Some(corrected_with_cov.to_imu_msg()) + } + + pub fn correct_imu_with_dynamic_tf_covariance( + &self, + imu_msg: &ImuMsg, + ) -> Option { + let transform = self + .transform_listener + .get_latest_transform(&imu_msg.header.frame_id, self.config.output_frame)?; + + Some(self.correct_imu_with_covariance(imu_msg, Some(&transform))) + } + + pub fn correct_imu_simple(&self, imu_msg: &ImuMsg) -> ImuMsg { + let mut corrected_msg = imu_msg.clone(); + + corrected_msg.angular_velocity.x -= self.config.angular_velocity_offset_x; + corrected_msg.angular_velocity.y -= self.config.angular_velocity_offset_y; + corrected_msg.angular_velocity.z -= self.config.angular_velocity_offset_z; + + corrected_msg.header.frame_id = self.config.output_frame; + + corrected_msg + } + + pub fn get_covariance_config(&self) -> (f64, f64, f64, f64) { + ( + self.config.angular_velocity_stddev_xx, + self.config.angular_velocity_stddev_yy, + self.config.angular_velocity_stddev_zz, + self.config.acceleration_stddev, + ) + } +} + +pub fn transform_covariance(cov: &[f64; 9]) -> [f64; 9] { + let max_cov = cov[0].max(cov[4]).max(cov[8]); + let mut cov_transformed = [0.0; 9]; + cov_transformed[0] = max_cov; + cov_transformed[4] = max_cov; + cov_transformed[8] = max_cov; + cov_transformed +} + +#[cfg(test)] +mod tests { + use super::*; + + fn sample_imu_msg() -> ImuMsg { + ImuMsg { + header: Header { + frame_id: "imu_link", + timestamp: 0, + }, + orientation: Quaternion { + x: 0.0, + y: 0.0, + z: 0.0, + w: 1.0, + }, + angular_velocity: Vector3::new(0.1, 0.2, 0.3), + linear_acceleration: Vector3::new(9.8, 0.0, 0.0), + } + } + + #[test] + fn static_bias_correction_updates_angular_velocity() { + let mut config = ImuCorrectorConfig::default(); + config.angular_velocity_offset_x = 0.05; + config.angular_velocity_offset_y = 0.1; + config.angular_velocity_offset_z = 0.15; + + let mut corrector = ImuCorrector::new(); + corrector.set_config(config); + + let imu_msg = sample_imu_msg(); + let corrected = corrector.correct_imu(&imu_msg, None); + + assert_eq!(corrected.angular_velocity.x, 0.05); + assert_eq!(corrected.angular_velocity.y, 0.1); + assert_eq!(corrected.angular_velocity.z, 0.15); + assert_eq!(corrected.header.frame_id, "imu_link"); + } + + #[test] + fn simple_path_sets_output_frame() { + let corrector = ImuCorrector::new(); + let imu_msg = sample_imu_msg(); + + let corrected_msg = corrector.correct_imu_simple(&imu_msg); + + assert_eq!(corrected_msg.header.frame_id, "base_link"); + assert_eq!(corrected_msg.angular_velocity.x, 0.1); + assert_eq!(corrected_msg.angular_velocity.y, 0.2); + assert_eq!(corrected_msg.angular_velocity.z, 0.3); + } + + #[test] + fn covariance_defaults_are_set_from_config() { + let corrector = ImuCorrector::new(); + let imu_msg = sample_imu_msg(); + + let corrected_with_cov = corrector.correct_imu_with_covariance(&imu_msg, None); + let expected_angular_var = 0.03 * 0.03; + assert_eq!( + corrected_with_cov.angular_velocity_covariance[0], + expected_angular_var + ); + assert_eq!( + corrected_with_cov.angular_velocity_covariance[4], + expected_angular_var + ); + assert_eq!( + corrected_with_cov.angular_velocity_covariance[8], + expected_angular_var + ); + let expected_accel_var = 10000.0 * 10000.0; + assert_eq!( + corrected_with_cov.linear_acceleration_covariance[0], + expected_accel_var + ); + assert_eq!( + corrected_with_cov.linear_acceleration_covariance[4], + expected_accel_var + ); + assert_eq!( + corrected_with_cov.linear_acceleration_covariance[8], + expected_accel_var + ); + } + + #[test] + fn covariance_transform_uses_max_diagonal() { + let corrector = ImuCorrector::new(); + let input_cov = [1.0, 0.5, 0.3, 0.5, 2.0, 0.4, 0.3, 0.4, 3.0]; + let transformed_cov = corrector.transform_covariance(&input_cov); + + assert_eq!(transformed_cov[0], 3.0); + assert_eq!(transformed_cov[4], 3.0); + assert_eq!(transformed_cov[8], 3.0); + assert_eq!(transformed_cov[1], 0.0); + assert_eq!(transformed_cov[2], 0.0); + assert_eq!(transformed_cov[3], 0.0); + assert_eq!(transformed_cov[5], 0.0); + assert_eq!(transformed_cov[6], 0.0); + assert_eq!(transformed_cov[7], 0.0); + } + + #[test] + fn public_covariance_transform_uses_max_diagonal() { + let input_cov = [1.0, 0.5, 0.3, 0.5, 2.0, 0.4, 0.3, 0.4, 3.0]; + let transformed_cov = transform_covariance(&input_cov); + assert_eq!(transformed_cov[0], 3.0); + assert_eq!(transformed_cov[4], 3.0); + assert_eq!(transformed_cov[8], 3.0); + assert_eq!(transformed_cov[1], 0.0); + assert_eq!(transformed_cov[2], 0.0); + assert_eq!(transformed_cov[3], 0.0); + assert_eq!(transformed_cov[5], 0.0); + assert_eq!(transformed_cov[6], 0.0); + assert_eq!(transformed_cov[7], 0.0); + } + + #[test] + fn transform_applies_to_vectors_and_sets_output_frame() { + let corrector = ImuCorrector::new(); + let imu_msg = sample_imu_msg(); + + let transform = Transform { + translation: Vector3::new(1.0, 2.0, 3.0), + rotation: Quaternion { + x: 0.0, + y: 0.0, + z: 0.0, + w: 1.0, + }, + }; + + let corrected_with_cov = corrector.correct_imu_with_covariance(&imu_msg, Some(&transform)); + assert_eq!(corrected_with_cov.header.frame_id, "base_link"); + assert_eq!(corrected_with_cov.linear_acceleration.x, 10.8); + assert_eq!(corrected_with_cov.linear_acceleration.y, 2.0); + assert_eq!(corrected_with_cov.linear_acceleration.z, 3.0); + assert_eq!(corrected_with_cov.angular_velocity.x, 1.1); + assert_eq!(corrected_with_cov.angular_velocity.y, 2.2); + assert_eq!(corrected_with_cov.angular_velocity.z, 3.3); + } + + #[test] + fn dynamic_tf_path_applies_transform() { + let mut transform_listener = MockTransformListener::new(); + let transform = Transform { + translation: Vector3::new(1.0, 0.0, 0.0), + rotation: Quaternion { + x: 0.0, + y: 0.0, + z: 0.0, + w: 1.0, + }, + }; + transform_listener.add_transform("imu_link", "base_link", transform); + + let corrector = ImuCorrector::with_transform_listener(transform_listener); + let imu_msg = sample_imu_msg(); + + let corrected_msg = corrector.correct_imu_with_dynamic_tf(&imu_msg); + assert!(corrected_msg.is_some()); + + let corrected_msg = corrected_msg.unwrap(); + assert_eq!(corrected_msg.header.frame_id, "base_link"); + assert_eq!(corrected_msg.linear_acceleration.x, 10.8); + } + + #[test] + fn dynamic_tf_path_returns_none_when_transform_is_missing() { + let transform_listener = MockTransformListener::new(); + let corrector = ImuCorrector::with_transform_listener(transform_listener); + let imu_msg = sample_imu_msg(); + + let corrected_msg = corrector.correct_imu_with_dynamic_tf(&imu_msg); + assert!(corrected_msg.is_none()); + } +} \ No newline at end of file From 1b97cf05267eb98d33efa3e3442279cb273ac5d4 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Mon, 27 Apr 2026 21:39:49 +0900 Subject: [PATCH 02/20] fix: delete dependics on imu_corrector to satisfy simple test Signed-off-by: nokosaaan --- applications/tests/test_autoware/imu_corrector/Cargo.toml | 3 --- 1 file changed, 3 deletions(-) diff --git a/applications/tests/test_autoware/imu_corrector/Cargo.toml b/applications/tests/test_autoware/imu_corrector/Cargo.toml index 30b5ce849..0397eed62 100644 --- a/applications/tests/test_autoware/imu_corrector/Cargo.toml +++ b/applications/tests/test_autoware/imu_corrector/Cargo.toml @@ -4,8 +4,5 @@ version = "0.1.0" edition = "2021" [dependencies] -log = "0.4" nalgebra = { version = "0.32", default-features = false, features = ["libm"] } -awkernel_async_lib = { path = "../../../../awkernel_async_lib", default-features = false } -awkernel_lib = { path = "../../../../awkernel_lib", default-features = false } imu_driver = { path = "../imu_driver", default-features = false } \ No newline at end of file From 11537c0a275ec3335c35ad8192c0407f8c9cf955 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Mon, 27 Apr 2026 21:54:38 +0900 Subject: [PATCH 03/20] fix: apply copilot comment 1 Signed-off-by: nokosaaan --- .../test_autoware/imu_corrector/src/lib.rs | 28 +++++++++++-------- 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/applications/tests/test_autoware/imu_corrector/src/lib.rs b/applications/tests/test_autoware/imu_corrector/src/lib.rs index 2752b33f9..c2ec69b30 100644 --- a/applications/tests/test_autoware/imu_corrector/src/lib.rs +++ b/applications/tests/test_autoware/imu_corrector/src/lib.rs @@ -190,10 +190,8 @@ impl ImuCorrector { fn transform_vector3(&self, vec: &Vector3, transform: &Transform) -> Vector3 { let nalgebra_vec = self.to_nalgebra_vector3(vec); let nalgebra_quat = self.to_nalgebra_quaternion(&transform.rotation); - let nalgebra_trans = self.to_nalgebra_vector3(&transform.translation); let rotated = nalgebra_quat * nalgebra_vec; - let result = rotated + nalgebra_trans; - self.to_imu_vector3(&result) + self.to_imu_vector3(&rotated) } fn transform_covariance(&self, cov: &[f64; 9]) -> [f64; 9] { @@ -301,6 +299,10 @@ pub fn transform_covariance(cov: &[f64; 9]) -> [f64; 9] { mod tests { use super::*; + fn assert_almost_eq(actual: f64, expected: f64) { + assert!((actual - expected).abs() < 1e-10); + } + fn sample_imu_msg() -> ImuMsg { ImuMsg { header: Header { @@ -426,19 +428,19 @@ mod tests { rotation: Quaternion { x: 0.0, y: 0.0, - z: 0.0, - w: 1.0, + z: 0.7071067811865475, + w: 0.7071067811865476, }, }; let corrected_with_cov = corrector.correct_imu_with_covariance(&imu_msg, Some(&transform)); assert_eq!(corrected_with_cov.header.frame_id, "base_link"); - assert_eq!(corrected_with_cov.linear_acceleration.x, 10.8); - assert_eq!(corrected_with_cov.linear_acceleration.y, 2.0); - assert_eq!(corrected_with_cov.linear_acceleration.z, 3.0); - assert_eq!(corrected_with_cov.angular_velocity.x, 1.1); - assert_eq!(corrected_with_cov.angular_velocity.y, 2.2); - assert_eq!(corrected_with_cov.angular_velocity.z, 3.3); + assert_almost_eq(corrected_with_cov.linear_acceleration.x, 0.0); + assert_almost_eq(corrected_with_cov.linear_acceleration.y, 9.8); + assert_almost_eq(corrected_with_cov.linear_acceleration.z, 0.0); + assert_almost_eq(corrected_with_cov.angular_velocity.x, -0.2); + assert_almost_eq(corrected_with_cov.angular_velocity.y, 0.1); + assert_almost_eq(corrected_with_cov.angular_velocity.z, 0.3); } #[test] @@ -463,7 +465,9 @@ mod tests { let corrected_msg = corrected_msg.unwrap(); assert_eq!(corrected_msg.header.frame_id, "base_link"); - assert_eq!(corrected_msg.linear_acceleration.x, 10.8); + assert_eq!(corrected_msg.linear_acceleration.x, 9.8); + assert_eq!(corrected_msg.linear_acceleration.y, 0.0); + assert_eq!(corrected_msg.linear_acceleration.z, 0.0); } #[test] From 6d82977a28a4d6f8d15a820efe51f67fbc3a0b65 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Mon, 27 Apr 2026 22:00:47 +0900 Subject: [PATCH 04/20] fix: apply copilot comment 2 Signed-off-by: nokosaaan --- applications/tests/test_autoware/Cargo.toml | 1 + 1 file changed, 1 insertion(+) diff --git a/applications/tests/test_autoware/Cargo.toml b/applications/tests/test_autoware/Cargo.toml index bb1aab646..f745c6851 100644 --- a/applications/tests/test_autoware/Cargo.toml +++ b/applications/tests/test_autoware/Cargo.toml @@ -14,4 +14,5 @@ csv-core = "0.1" awkernel_async_lib = { path = "../../../awkernel_async_lib", default-features = false } awkernel_lib = { path = "../../../awkernel_lib", default-features = false } imu_driver = { path = "./imu_driver", default-features = false } +imu_corrector = { path = "./imu_corrector", default-features = false } vehicle_velocity_converter = { path = "./vehicle_velocity_converter", default-features = false } From 3c2f6c09193abbec9863b671109bddf94350705a Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Mon, 27 Apr 2026 22:03:13 +0900 Subject: [PATCH 05/20] fix: apply cargo fmt 4 Signed-off-by: nokosaaan --- applications/tests/test_autoware/imu_corrector/src/lib.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/applications/tests/test_autoware/imu_corrector/src/lib.rs b/applications/tests/test_autoware/imu_corrector/src/lib.rs index c2ec69b30..002e3748c 100644 --- a/applications/tests/test_autoware/imu_corrector/src/lib.rs +++ b/applications/tests/test_autoware/imu_corrector/src/lib.rs @@ -479,4 +479,4 @@ mod tests { let corrected_msg = corrector.correct_imu_with_dynamic_tf(&imu_msg); assert!(corrected_msg.is_none()); } -} \ No newline at end of file +} From 85f26ac5a9b6206b10537776d3669b087c6872d0 Mon Sep 17 00:00:00 2001 From: nokosaaan <106376734+nokosaaan@users.noreply.github.com> Date: Mon, 27 Apr 2026 22:04:02 +0900 Subject: [PATCH 06/20] Update applications/tests/test_autoware/imu_corrector/src/lib.rs Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- applications/tests/test_autoware/imu_corrector/src/lib.rs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/applications/tests/test_autoware/imu_corrector/src/lib.rs b/applications/tests/test_autoware/imu_corrector/src/lib.rs index 002e3748c..abf265a33 100644 --- a/applications/tests/test_autoware/imu_corrector/src/lib.rs +++ b/applications/tests/test_autoware/imu_corrector/src/lib.rs @@ -14,12 +14,12 @@ pub struct Transform { impl Transform { pub fn identity() -> Self { Self { - translation: imu_driver::Vector3::new(5.22444, 0.07615, 2.72762), // use aip_x2_gen2_description. + translation: imu_driver::Vector3::new(0.0, 0.0, 0.0), rotation: imu_driver::Quaternion { - x: 1.0, + x: 0.0, y: 0.0, z: 0.0, - w: 0.0, + w: 1.0, }, } } From f8042fca5d8c7409ce84798eeab7ae721332bd06 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Mon, 11 May 2026 09:56:14 +0900 Subject: [PATCH 07/20] fix: move autoware position Signed-off-by: nokosaaan --- Cargo.toml | 1 + .../{tests/test_autoware => autoware}/Cargo.toml | 6 +++--- .../imu_corrector/Cargo.toml | 0 .../imu_corrector/src/lib.rs | 0 .../test_autoware => autoware}/imu_driver/Cargo.toml | 0 .../test_autoware => autoware}/imu_driver/src/lib.rs | 0 .../{tests/test_autoware => autoware}/src/lib.rs | 0 .../vehicle_velocity_converter/Cargo.toml | 0 .../vehicle_velocity_converter/src/lib.rs | 0 userland/Cargo.toml | 10 +++++----- userland/src/lib.rs | 6 +++--- 11 files changed, 12 insertions(+), 11 deletions(-) rename applications/{tests/test_autoware => autoware}/Cargo.toml (67%) rename applications/{tests/test_autoware => autoware}/imu_corrector/Cargo.toml (100%) rename applications/{tests/test_autoware => autoware}/imu_corrector/src/lib.rs (100%) rename applications/{tests/test_autoware => autoware}/imu_driver/Cargo.toml (100%) rename applications/{tests/test_autoware => autoware}/imu_driver/src/lib.rs (100%) rename applications/{tests/test_autoware => autoware}/src/lib.rs (100%) rename applications/{tests/test_autoware => autoware}/vehicle_velocity_converter/Cargo.toml (100%) rename applications/{tests/test_autoware => autoware}/vehicle_velocity_converter/src/lib.rs (100%) diff --git a/Cargo.toml b/Cargo.toml index 6f4af7aab..3e46c056e 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -13,6 +13,7 @@ members = [ "applications/awkernel_services", "applications/awkernel_shell", "applications/awkernel_display", + "applications/autoware", "applications/rd_gen_to_dags", "applications/tests/*", "smoltcp", diff --git a/applications/tests/test_autoware/Cargo.toml b/applications/autoware/Cargo.toml similarity index 67% rename from applications/tests/test_autoware/Cargo.toml rename to applications/autoware/Cargo.toml index f745c6851..96d8a9c7f 100644 --- a/applications/tests/test_autoware/Cargo.toml +++ b/applications/autoware/Cargo.toml @@ -1,5 +1,5 @@ [package] -name = "test_autoware" +name = "autoware" version = "0.1.0" edition = "2021" @@ -11,8 +11,8 @@ crate-type = ["rlib"] log = "0.4" libm = "0.2" csv-core = "0.1" -awkernel_async_lib = { path = "../../../awkernel_async_lib", default-features = false } -awkernel_lib = { path = "../../../awkernel_lib", default-features = false } +awkernel_async_lib = { path = "../../awkernel_async_lib", default-features = false } +awkernel_lib = { path = "../../awkernel_lib", default-features = false } imu_driver = { path = "./imu_driver", default-features = false } imu_corrector = { path = "./imu_corrector", default-features = false } vehicle_velocity_converter = { path = "./vehicle_velocity_converter", default-features = false } diff --git a/applications/tests/test_autoware/imu_corrector/Cargo.toml b/applications/autoware/imu_corrector/Cargo.toml similarity index 100% rename from applications/tests/test_autoware/imu_corrector/Cargo.toml rename to applications/autoware/imu_corrector/Cargo.toml diff --git a/applications/tests/test_autoware/imu_corrector/src/lib.rs b/applications/autoware/imu_corrector/src/lib.rs similarity index 100% rename from applications/tests/test_autoware/imu_corrector/src/lib.rs rename to applications/autoware/imu_corrector/src/lib.rs diff --git a/applications/tests/test_autoware/imu_driver/Cargo.toml b/applications/autoware/imu_driver/Cargo.toml similarity index 100% rename from applications/tests/test_autoware/imu_driver/Cargo.toml rename to applications/autoware/imu_driver/Cargo.toml diff --git a/applications/tests/test_autoware/imu_driver/src/lib.rs b/applications/autoware/imu_driver/src/lib.rs similarity index 100% rename from applications/tests/test_autoware/imu_driver/src/lib.rs rename to applications/autoware/imu_driver/src/lib.rs diff --git a/applications/tests/test_autoware/src/lib.rs b/applications/autoware/src/lib.rs similarity index 100% rename from applications/tests/test_autoware/src/lib.rs rename to applications/autoware/src/lib.rs diff --git a/applications/tests/test_autoware/vehicle_velocity_converter/Cargo.toml b/applications/autoware/vehicle_velocity_converter/Cargo.toml similarity index 100% rename from applications/tests/test_autoware/vehicle_velocity_converter/Cargo.toml rename to applications/autoware/vehicle_velocity_converter/Cargo.toml diff --git a/applications/tests/test_autoware/vehicle_velocity_converter/src/lib.rs b/applications/autoware/vehicle_velocity_converter/src/lib.rs similarity index 100% rename from applications/tests/test_autoware/vehicle_velocity_converter/src/lib.rs rename to applications/autoware/vehicle_velocity_converter/src/lib.rs diff --git a/userland/Cargo.toml b/userland/Cargo.toml index 222256a19..6b0d5ed84 100644 --- a/userland/Cargo.toml +++ b/userland/Cargo.toml @@ -14,6 +14,10 @@ path = "../awkernel_async_lib" [dependencies.awkernel_services] path = "../applications/awkernel_services" +[dependencies.autoware] +path = "../applications/autoware" +optional = true + [dependencies.rd_gen_to_dags] path = "../applications/rd_gen_to_dags" optional = true @@ -66,10 +70,6 @@ optional = true path = "../applications/tests/test_dag" optional = true -[dependencies.test_autoware] -path = "../applications/tests/test_autoware" -optional = true - [dependencies.test_dvfs] path = "../applications/tests/test_dvfs" optional = true @@ -84,6 +84,7 @@ perf = ["awkernel_services/perf"] # Evaluation applications rd_gen_to_dags = ["dep:rd_gen_to_dags"] +autoware = ["dep:autoware"] # Test applications test_network = ["dep:test_network"] @@ -97,5 +98,4 @@ test_gedf = ["dep:test_gedf"] test_measure_channel = ["dep:test_measure_channel"] test_measure_channel_heavy = ["dep:test_measure_channel_heavy"] test_dag = ["dep:test_dag"] -test_autoware = ["dep:test_autoware"] test_voluntary_preemption = ["dep:test_voluntary_preemption"] diff --git a/userland/src/lib.rs b/userland/src/lib.rs index 7082e1eba..bdad77b5b 100644 --- a/userland/src/lib.rs +++ b/userland/src/lib.rs @@ -7,6 +7,9 @@ use alloc::borrow::Cow; pub async fn main() -> Result<(), Cow<'static, str>> { awkernel_services::run().await; + #[cfg(feature = "autoware")] + autoware::run().await; // run the autoware application + #[cfg(feature = "rd_gen_to_dags")] rd_gen_to_dags::run().await; // run the rd_gen_to_dags application @@ -46,9 +49,6 @@ pub async fn main() -> Result<(), Cow<'static, str>> { #[cfg(feature = "test_dag")] test_dag::run().await; // test for DAG - #[cfg(feature = "test_autoware")] - test_autoware::run().await; // test for Autoware - #[cfg(feature = "test_dvfs")] test_dvfs::run().await; // test for DVFS From 85d68c5f69c3f1bad86986ffea25e1698302c0eb Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Mon, 11 May 2026 10:01:40 +0900 Subject: [PATCH 08/20] fix: delete vehicle Signed-off-by: nokosaaan --- applications/autoware/Cargo.toml | 1 - .../vehicle_velocity_converter/Cargo.toml | 6 - .../vehicle_velocity_converter/src/lib.rs | 321 ------------------ 3 files changed, 328 deletions(-) delete mode 100644 applications/autoware/vehicle_velocity_converter/Cargo.toml delete mode 100644 applications/autoware/vehicle_velocity_converter/src/lib.rs diff --git a/applications/autoware/Cargo.toml b/applications/autoware/Cargo.toml index 96d8a9c7f..23be165ec 100644 --- a/applications/autoware/Cargo.toml +++ b/applications/autoware/Cargo.toml @@ -15,4 +15,3 @@ awkernel_async_lib = { path = "../../awkernel_async_lib", default-features = fal awkernel_lib = { path = "../../awkernel_lib", default-features = false } imu_driver = { path = "./imu_driver", default-features = false } imu_corrector = { path = "./imu_corrector", default-features = false } -vehicle_velocity_converter = { path = "./vehicle_velocity_converter", default-features = false } diff --git a/applications/autoware/vehicle_velocity_converter/Cargo.toml b/applications/autoware/vehicle_velocity_converter/Cargo.toml deleted file mode 100644 index 24f988e6e..000000000 --- a/applications/autoware/vehicle_velocity_converter/Cargo.toml +++ /dev/null @@ -1,6 +0,0 @@ -[package] -name = "vehicle_velocity_converter" -version = "0.1.0" -edition = "2021" - -[dependencies] diff --git a/applications/autoware/vehicle_velocity_converter/src/lib.rs b/applications/autoware/vehicle_velocity_converter/src/lib.rs deleted file mode 100644 index f7a96dc43..000000000 --- a/applications/autoware/vehicle_velocity_converter/src/lib.rs +++ /dev/null @@ -1,321 +0,0 @@ -// Copyright 2021 TierIV -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#![no_std] - -#[derive(Debug, Clone)] -pub struct Header { - pub frame_id: &'static str, - pub timestamp: u64, -} - -#[derive(Debug, Clone)] -pub struct Vector3 { - pub x: f64, - pub y: f64, - pub z: f64, -} - -#[derive(Debug, Clone)] -pub struct VelocityReport { - pub header: Header, - pub longitudinal_velocity: f64, - pub lateral_velocity: f64, - pub heading_rate: f64, -} - -#[derive(Debug, Clone)] -pub struct VelocityCsvRow { - pub timestamp: u64, - pub longitudinal_velocity: f64, - pub lateral_velocity: f64, - pub heading_rate: f64, -} - -pub fn build_velocity_report_from_csv_row( - row: &VelocityCsvRow, - frame_id: &'static str, - timestamp: u64, -) -> VelocityReport { - VelocityReport { - header: Header { - frame_id, - timestamp, - }, - longitudinal_velocity: row.longitudinal_velocity, - lateral_velocity: row.lateral_velocity, - heading_rate: row.heading_rate, - } -} - -#[derive(Debug, Clone)] -pub struct TwistWithCovarianceStamped { - pub header: Header, - pub twist: TwistWithCovariance, -} - -#[derive(Debug, Clone)] -pub struct TwistWithCovariance { - pub twist: Twist, - pub covariance: [f64; 36], -} - -#[derive(Debug, Clone)] -pub struct Twist { - pub linear: Vector3, - pub angular: Vector3, -} - -#[repr(C)] -pub struct Odometry { - pub velocity: f64, -} - -pub struct VehicleVelocityConverter { - frame_id: &'static str, - stddev_vx: f64, - stddev_wz: f64, - speed_scale_factor: f64, -} - -impl VehicleVelocityConverter { - pub fn new( - frame_id: &'static str, - stddev_vx: f64, - stddev_wz: f64, - speed_scale_factor: f64, - ) -> Self { - Self { - frame_id, - stddev_vx, - stddev_wz, - speed_scale_factor, - } - } - - pub fn from_params_array( - velocity_stddev_xx: Option, - angular_velocity_stddev_zz: Option, - speed_scale_factor: Option, - frame_id: &'static str, - ) -> Self { - let stddev_vx = velocity_stddev_xx.unwrap_or(0.2); - let stddev_wz = angular_velocity_stddev_zz.unwrap_or(0.1); - let speed_scale_factor = speed_scale_factor.unwrap_or(1.0); - - Self::new(frame_id, stddev_vx, stddev_wz, speed_scale_factor) - } - - pub fn default() -> Self { - Self::new("base_link", 0.2, 0.1, 1.0) - } - - pub fn convert_velocity_report(&self, msg: &VelocityReport) -> TwistWithCovarianceStamped { - TwistWithCovarianceStamped { - header: msg.header.clone(), - twist: TwistWithCovariance { - twist: Twist { - linear: Vector3 { - x: msg.longitudinal_velocity * self.speed_scale_factor, - y: msg.lateral_velocity, - z: 0.0, - }, - angular: Vector3 { - x: 0.0, - y: 0.0, - z: msg.heading_rate, - }, - }, - covariance: self.create_covariance_matrix(), - }, - } - } - - fn create_covariance_matrix(&self) -> [f64; 36] { - let mut covariance = [0.0; 36]; - covariance[0 + 0 * 6] = self.stddev_vx * self.stddev_vx; - covariance[1 + 1 * 6] = 10000.0; - covariance[2 + 2 * 6] = 10000.0; - covariance[3 + 3 * 6] = 10000.0; - covariance[4 + 4 * 6] = 10000.0; - covariance[5 + 5 * 6] = self.stddev_wz * self.stddev_wz; - - covariance - } - - pub fn get_frame_id(&self) -> &'static str { - self.frame_id - } - - pub fn get_stddev_vx(&self) -> f64 { - self.stddev_vx - } - - pub fn get_stddev_wz(&self) -> f64 { - self.stddev_wz - } - - pub fn get_speed_scale_factor(&self) -> f64 { - self.speed_scale_factor - } -} - -pub mod reactor_helpers { - use super::*; - - pub fn create_empty_twist(timestamp: u64) -> TwistWithCovarianceStamped { - TwistWithCovarianceStamped { - header: Header { - frame_id: "base_link", - timestamp, - }, - twist: TwistWithCovariance { - twist: Twist { - linear: Vector3 { - x: 0.0, - y: 0.0, - z: 0.0, - }, - angular: Vector3 { - x: 0.0, - y: 0.0, - z: 0.0, - }, - }, - covariance: [0.0; 36], - }, - } - } -} - -#[cfg(test)] -mod tests { - use super::*; - - fn assert_approx_eq(actual: f64, expected: f64, eps: f64) { - assert!( - (actual - expected).abs() <= eps, - "left: {actual}, right: {expected}" - ); - } - - #[test] - fn node_instantiation() { - let converter = VehicleVelocityConverter::from_params_array( - Some(0.2), - Some(0.1), - Some(1.0), - "base_link", - ); - - assert_eq!(converter.get_frame_id(), "base_link"); - assert_eq!(converter.get_stddev_vx(), 0.2); - assert_eq!(converter.get_stddev_wz(), 0.1); - assert_eq!(converter.get_speed_scale_factor(), 1.0); - } - - #[test] - fn message_conversion() { - let converter = VehicleVelocityConverter::from_params_array( - Some(0.2), - Some(0.1), - Some(1.5), - "base_link", - ); - let velocity_report = VelocityReport { - header: Header { - frame_id: "base_link", - timestamp: 1234567890, - }, - longitudinal_velocity: 2.0, - lateral_velocity: 0.1, - heading_rate: 0.3, - }; - - let twist_msg = converter.convert_velocity_report(&velocity_report); - - assert_eq!(twist_msg.header.frame_id, velocity_report.header.frame_id); - assert_eq!( - twist_msg.twist.twist.linear.x, - velocity_report.longitudinal_velocity * 1.5 - ); - assert_eq!( - twist_msg.twist.twist.linear.y, - velocity_report.lateral_velocity - ); - assert_eq!( - twist_msg.twist.twist.angular.z, - velocity_report.heading_rate - ); - assert_approx_eq(twist_msg.twist.covariance[0], 0.2 * 0.2, 1e-12); - assert_eq!(twist_msg.twist.covariance[7], 10000.0); - assert_eq!(twist_msg.twist.covariance[14], 10000.0); - assert_eq!(twist_msg.twist.covariance[21], 10000.0); - assert_eq!(twist_msg.twist.covariance[28], 10000.0); - assert_approx_eq(twist_msg.twist.covariance[35], 0.1 * 0.1, 1e-12); - } - - #[test] - fn different_frame_id() { - let converter = VehicleVelocityConverter::from_params_array( - Some(0.2), - Some(0.1), - Some(1.0), - "base_link", - ); - - let velocity_report = VelocityReport { - header: Header { - frame_id: "different_frame", - timestamp: 1234567890, - }, - longitudinal_velocity: 2.0, - lateral_velocity: 0.1, - heading_rate: 0.3, - }; - - let twist_msg = converter.convert_velocity_report(&velocity_report); - - // As in the original C++ test, conversion still succeeds even with a different frame_id. - assert_eq!(twist_msg.header.frame_id, velocity_report.header.frame_id); - assert_eq!( - twist_msg.twist.twist.linear.x, - velocity_report.longitudinal_velocity - ); - assert_eq!( - twist_msg.twist.twist.linear.y, - velocity_report.lateral_velocity - ); - assert_eq!( - twist_msg.twist.twist.angular.z, - velocity_report.heading_rate - ); - } - - #[test] - fn from_params_array_with_defaults() { - let converter = VehicleVelocityConverter::from_params_array(None, None, None, "base_link"); - - assert_eq!(converter.get_stddev_vx(), 0.2); - assert_eq!(converter.get_stddev_wz(), 0.1); - assert_eq!(converter.get_speed_scale_factor(), 1.0); - } - - #[test] - fn reactor_helpers() { - let empty_twist = reactor_helpers::create_empty_twist(1234567890); - assert_eq!(empty_twist.header.frame_id, "base_link"); - assert_eq!(empty_twist.twist.twist.linear.x, 0.0); - } -} From 1b8071303a95e36f35c5e3fada40da8f2198815d Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Mon, 11 May 2026 10:48:48 +0900 Subject: [PATCH 09/20] fix: comment 2 Signed-off-by: nokosaaan --- applications/autoware/imu_corrector/src/lib.rs | 6 ++++++ applications/autoware/imu_driver/src/lib.rs | 6 ++++++ 2 files changed, 12 insertions(+) diff --git a/applications/autoware/imu_corrector/src/lib.rs b/applications/autoware/imu_corrector/src/lib.rs index abf265a33..ff61b769b 100644 --- a/applications/autoware/imu_corrector/src/lib.rs +++ b/applications/autoware/imu_corrector/src/lib.rs @@ -1,3 +1,9 @@ +// Ported from the following versions of the original C++ code: +// universe/autoware_universe: +// type: git +// url: https://github.com/autowarefoundation/autoware_universe.git +// version: 0.51.0 + #![no_std] extern crate alloc; diff --git a/applications/autoware/imu_driver/src/lib.rs b/applications/autoware/imu_driver/src/lib.rs index a7abc330f..36cf39679 100644 --- a/applications/autoware/imu_driver/src/lib.rs +++ b/applications/autoware/imu_driver/src/lib.rs @@ -1,3 +1,9 @@ +// Ported from the following versions of the original C++ code: +// tamagawa_imu_driver +// type: git +// url: https://github.com/tier4/tamagawa_imu_driver +// version: 0.1.0 + #![no_std] extern crate alloc; From 37f842e89b6d24dd67ce44c15412a0f2b1568b06 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Thu, 21 May 2026 02:30:18 +0900 Subject: [PATCH 10/20] fix: add lisence Signed-off-by: nokosaaan --- applications/autoware/imu_corrector/Cargo.toml | 2 +- applications/autoware/imu_corrector/src/lib.rs | 16 ++++++++++++++++ applications/autoware/imu_driver/src/lib.rs | 1 - 3 files changed, 17 insertions(+), 2 deletions(-) diff --git a/applications/autoware/imu_corrector/Cargo.toml b/applications/autoware/imu_corrector/Cargo.toml index 0397eed62..931c78678 100644 --- a/applications/autoware/imu_corrector/Cargo.toml +++ b/applications/autoware/imu_corrector/Cargo.toml @@ -5,4 +5,4 @@ edition = "2021" [dependencies] nalgebra = { version = "0.32", default-features = false, features = ["libm"] } -imu_driver = { path = "../imu_driver", default-features = false } \ No newline at end of file +imu_driver = { path = "../imu_driver", default-features = false } diff --git a/applications/autoware/imu_corrector/src/lib.rs b/applications/autoware/imu_corrector/src/lib.rs index ff61b769b..aca4f1184 100644 --- a/applications/autoware/imu_corrector/src/lib.rs +++ b/applications/autoware/imu_corrector/src/lib.rs @@ -1,7 +1,23 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// // Ported from the following versions of the original C++ code: // universe/autoware_universe: // type: git // url: https://github.com/autowarefoundation/autoware_universe.git +// original file path: sensing/autoware_imu_corrector/src/imu_corrector_core.cpp +// test code: Created in-house for I/O verification // version: 0.51.0 #![no_std] diff --git a/applications/autoware/imu_driver/src/lib.rs b/applications/autoware/imu_driver/src/lib.rs index 972661e34..6ff1a99ae 100644 --- a/applications/autoware/imu_driver/src/lib.rs +++ b/applications/autoware/imu_driver/src/lib.rs @@ -53,7 +53,6 @@ // original file path: src/tag_serial_driver.cpp // test code: Created in-house for I/O verification // version: 0.1.0 -// version: 0.1.0 #![no_std] extern crate alloc; From ab204ce79b35d8e1f55a0b2d2e8cc6fb01e11544 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Thu, 21 May 2026 02:46:45 +0900 Subject: [PATCH 11/20] fix: add TODO comment Signed-off-by: nokosaaan --- applications/autoware/imu_corrector/src/lib.rs | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/applications/autoware/imu_corrector/src/lib.rs b/applications/autoware/imu_corrector/src/lib.rs index aca4f1184..25bc7e08a 100644 --- a/applications/autoware/imu_corrector/src/lib.rs +++ b/applications/autoware/imu_corrector/src/lib.rs @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. // -// Ported from the following versions of the original C++ code: +// Partially ported from the following versions of the original C++ code (See the TODO comment in ImuCorrectorConfig): // universe/autoware_universe: // type: git // url: https://github.com/autowarefoundation/autoware_universe.git @@ -144,6 +144,11 @@ impl ImuWithCovariance { } } +// TODO: The `gyro_bias` and `gyro_scale` functions have not been implemented. +// Since this application is currently running on the MRM redundant system and for evaluation purposes, +// the absence of these functions does not cause any apparent issues at this time. +// However, since their behavior may change when processing data from +// “long-duration operations” or “high-precision real-world driving,” they will need to be added in the future. pub struct ImuCorrectorConfig { pub angular_velocity_offset_x: f64, pub angular_velocity_offset_y: f64, From b3bd7aca1af143204432b9a54d1e6e91b4d4ae13 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Thu, 21 May 2026 20:28:48 +0900 Subject: [PATCH 12/20] fix: add comment for correct_imu_with_covariance Signed-off-by: nokosaaan --- applications/autoware/imu_corrector/src/lib.rs | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/applications/autoware/imu_corrector/src/lib.rs b/applications/autoware/imu_corrector/src/lib.rs index 25bc7e08a..c971a205d 100644 --- a/applications/autoware/imu_corrector/src/lib.rs +++ b/applications/autoware/imu_corrector/src/lib.rs @@ -231,6 +231,22 @@ impl ImuCorrector { cov_transformed } + // NOTE: Unlike the original C++ implementation which returns early (without publishing) + // when TF lookup fails, this implementation cannot do the same due to Awkernel DAG's + // type constraints: all reactors must return their OutputTuple, and omitting output + // would cause downstream reactors (e.g. gyro_odometer) to block indefinitely. + // + // Currently, the process continues even if `None` is passed, and the value does not + // change after the transform. However, This was not caught during evaluation + // because the TF used in testing had an identity rotation (translation only, no effect on vector values). + // However, the actual vehicle TF would produce incorrect results if skipped. + // TODO: To be implemented in a subsequent PR: + // Fix by hardcoding a fixed tf value to prevent None from being passed by the caller. + // + // If dynamic TF support is added in the future, the recommended fallback is to + // return an empty/zeroed ImuWithCovariance (analogous to `create_empty_twist` in + // gyro_odometer) rather than uncorrected data, to avoid silently publishing + // imu_link-frame data labelled as base_link. pub fn correct_imu_with_covariance( &self, imu_msg: &ImuMsg, @@ -251,6 +267,7 @@ impl ImuCorrector { corrected_imu.linear_acceleration_covariance[4] = accel_var; corrected_imu.linear_acceleration_covariance[8] = accel_var; + if let Some(tf) = transform { corrected_imu.linear_acceleration = self.transform_vector3(&corrected_imu.linear_acceleration, tf); From 4e75c825fd1cf00c8f51e70b25c20c1dce264ffa Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Thu, 21 May 2026 20:30:59 +0900 Subject: [PATCH 13/20] fix: apply cargo fmt 5 Signed-off-by: nokosaaan --- applications/autoware/imu_corrector/src/lib.rs | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/applications/autoware/imu_corrector/src/lib.rs b/applications/autoware/imu_corrector/src/lib.rs index c971a205d..fe84de715 100644 --- a/applications/autoware/imu_corrector/src/lib.rs +++ b/applications/autoware/imu_corrector/src/lib.rs @@ -236,11 +236,11 @@ impl ImuCorrector { // type constraints: all reactors must return their OutputTuple, and omitting output // would cause downstream reactors (e.g. gyro_odometer) to block indefinitely. // - // Currently, the process continues even if `None` is passed, and the value does not + // Currently, the process continues even if `None` is passed, and the value does not // change after the transform. However, This was not caught during evaluation // because the TF used in testing had an identity rotation (translation only, no effect on vector values). // However, the actual vehicle TF would produce incorrect results if skipped. - // TODO: To be implemented in a subsequent PR: + // TODO: To be implemented in a subsequent PR: // Fix by hardcoding a fixed tf value to prevent None from being passed by the caller. // // If dynamic TF support is added in the future, the recommended fallback is to @@ -267,7 +267,6 @@ impl ImuCorrector { corrected_imu.linear_acceleration_covariance[4] = accel_var; corrected_imu.linear_acceleration_covariance[8] = accel_var; - if let Some(tf) = transform { corrected_imu.linear_acceleration = self.transform_vector3(&corrected_imu.linear_acceleration, tf); From 4cd97583f691d9178899c67a7d2300c2e2294263 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Thu, 21 May 2026 21:11:50 +0900 Subject: [PATCH 14/20] fix: add comment on to_imu_msg orientation Signed-off-by: nokosaaan --- applications/autoware/imu_corrector/src/lib.rs | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/applications/autoware/imu_corrector/src/lib.rs b/applications/autoware/imu_corrector/src/lib.rs index fe84de715..67a0927fd 100644 --- a/applications/autoware/imu_corrector/src/lib.rs +++ b/applications/autoware/imu_corrector/src/lib.rs @@ -137,6 +137,16 @@ impl ImuWithCovariance { pub fn to_imu_msg(&self) -> ImuMsg { ImuMsg { header: self.header.clone(), + // NOTE: Unlike the original C++ implementation, which intentionally omits + // orientation from the output message (imu_msg_base_link has no orientation + // assignment), this Rust implementation copies orientation from the input. + // This is a side effect of the clone-based approach rather than an intentional + // design choice. + // + // In the current MRM pipeline, downstream nodes (gyro_odometer, ekf_localizer) + // do not use orientation, so this difference has no functional impact. + // If a consumer of orientation is added in the future, this should be revisited + // to align with the C++ behavior (i.e., leave orientation as default/zero). orientation: self.orientation.clone(), angular_velocity: self.angular_velocity.clone(), linear_acceleration: self.linear_acceleration.clone(), From 4b122cf3c201c27f2cb0a60b76c45942d19c0b1d Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Thu, 21 May 2026 21:20:50 +0900 Subject: [PATCH 15/20] fix: add comment on apply_to_vector Signed-off-by: nokosaaan --- applications/autoware/imu_corrector/src/lib.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/applications/autoware/imu_corrector/src/lib.rs b/applications/autoware/imu_corrector/src/lib.rs index 67a0927fd..70165f355 100644 --- a/applications/autoware/imu_corrector/src/lib.rs +++ b/applications/autoware/imu_corrector/src/lib.rs @@ -59,6 +59,7 @@ impl Transform { UnitQuaternion::from_quaternion(n_quat) } + // This code is used as part of the `gyro_odometer` function processing. pub fn apply_to_vector(&self, vec: Vector3) -> Vector3 { let nalgebra_vec = self.to_nalgebra_vector3(&vec); let nalgebra_quat = self.to_nalgebra_quaternion(&self.rotation); From 53d74726b3b4485e33b97abf7b5ff7e8d70570f9 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Thu, 21 May 2026 21:37:54 +0900 Subject: [PATCH 16/20] refactor: public and private function transform_covariance Signed-off-by: nokosaaan --- applications/autoware/imu_corrector/src/lib.rs | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/applications/autoware/imu_corrector/src/lib.rs b/applications/autoware/imu_corrector/src/lib.rs index 70165f355..579a54e17 100644 --- a/applications/autoware/imu_corrector/src/lib.rs +++ b/applications/autoware/imu_corrector/src/lib.rs @@ -232,7 +232,7 @@ impl ImuCorrector { self.to_imu_vector3(&rotated) } - fn transform_covariance(&self, cov: &[f64; 9]) -> [f64; 9] { + fn transform_covariance_impl(cov: &[f64; 9]) -> [f64; 9] { let max_cov = cov[0].max(cov[4]).max(cov[8]); let mut cov_transformed = [0.0; 9]; cov_transformed[0] = max_cov; @@ -282,11 +282,11 @@ impl ImuCorrector { corrected_imu.linear_acceleration = self.transform_vector3(&corrected_imu.linear_acceleration, tf); corrected_imu.linear_acceleration_covariance = - self.transform_covariance(&corrected_imu.linear_acceleration_covariance); + Self::transform_covariance_impl(&corrected_imu.linear_acceleration_covariance); corrected_imu.angular_velocity = self.transform_vector3(&corrected_imu.angular_velocity, tf); corrected_imu.angular_velocity_covariance = - self.transform_covariance(&corrected_imu.angular_velocity_covariance); + Self::transform_covariance_impl(&corrected_imu.angular_velocity_covariance); corrected_imu.header.frame_id = self.config.output_frame; } @@ -341,12 +341,7 @@ impl ImuCorrector { } pub fn transform_covariance(cov: &[f64; 9]) -> [f64; 9] { - let max_cov = cov[0].max(cov[4]).max(cov[8]); - let mut cov_transformed = [0.0; 9]; - cov_transformed[0] = max_cov; - cov_transformed[4] = max_cov; - cov_transformed[8] = max_cov; - cov_transformed + ImuCorrector::::transform_covariance_impl(cov) } #[cfg(test)] @@ -442,9 +437,9 @@ mod tests { #[test] fn covariance_transform_uses_max_diagonal() { - let corrector = ImuCorrector::new(); let input_cov = [1.0, 0.5, 0.3, 0.5, 2.0, 0.4, 0.3, 0.4, 3.0]; - let transformed_cov = corrector.transform_covariance(&input_cov); + let transformed_cov = + ImuCorrector::::transform_covariance_impl(&input_cov); assert_eq!(transformed_cov[0], 3.0); assert_eq!(transformed_cov[4], 3.0); From 78752fa3b1d79dbb4a985420084c6190c6d1e8a6 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Thu, 21 May 2026 21:41:06 +0900 Subject: [PATCH 17/20] fix: delete duplicate test function of transform_covariance Signed-off-by: nokosaaan --- applications/autoware/imu_corrector/src/lib.rs | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/applications/autoware/imu_corrector/src/lib.rs b/applications/autoware/imu_corrector/src/lib.rs index 579a54e17..e75b3cbed 100644 --- a/applications/autoware/imu_corrector/src/lib.rs +++ b/applications/autoware/imu_corrector/src/lib.rs @@ -437,23 +437,6 @@ mod tests { #[test] fn covariance_transform_uses_max_diagonal() { - let input_cov = [1.0, 0.5, 0.3, 0.5, 2.0, 0.4, 0.3, 0.4, 3.0]; - let transformed_cov = - ImuCorrector::::transform_covariance_impl(&input_cov); - - assert_eq!(transformed_cov[0], 3.0); - assert_eq!(transformed_cov[4], 3.0); - assert_eq!(transformed_cov[8], 3.0); - assert_eq!(transformed_cov[1], 0.0); - assert_eq!(transformed_cov[2], 0.0); - assert_eq!(transformed_cov[3], 0.0); - assert_eq!(transformed_cov[5], 0.0); - assert_eq!(transformed_cov[6], 0.0); - assert_eq!(transformed_cov[7], 0.0); - } - - #[test] - fn public_covariance_transform_uses_max_diagonal() { let input_cov = [1.0, 0.5, 0.3, 0.5, 2.0, 0.4, 0.3, 0.4, 3.0]; let transformed_cov = transform_covariance(&input_cov); assert_eq!(transformed_cov[0], 3.0); From e5b9b74b659ed5a473e79879c887a72bf52a2a81 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Fri, 22 May 2026 17:18:49 +0900 Subject: [PATCH 18/20] fix: delete orientation from ImuMsg Signed-off-by: nokosaaan --- .../autoware/imu_corrector/src/lib.rs | 19 ------------------- applications/autoware/imu_driver/src/lib.rs | 9 --------- 2 files changed, 28 deletions(-) diff --git a/applications/autoware/imu_corrector/src/lib.rs b/applications/autoware/imu_corrector/src/lib.rs index e75b3cbed..097532d74 100644 --- a/applications/autoware/imu_corrector/src/lib.rs +++ b/applications/autoware/imu_corrector/src/lib.rs @@ -116,7 +116,6 @@ impl TransformListener for MockTransformListener { #[derive(Clone, Debug)] pub struct ImuWithCovariance { pub header: Header, - pub orientation: Quaternion, pub angular_velocity: Vector3, pub angular_velocity_covariance: [f64; 9], pub linear_acceleration: Vector3, @@ -127,7 +126,6 @@ impl ImuWithCovariance { pub fn from_imu_msg(imu_msg: &ImuMsg) -> Self { Self { header: imu_msg.header.clone(), - orientation: imu_msg.orientation.clone(), angular_velocity: imu_msg.angular_velocity.clone(), angular_velocity_covariance: [0.0; 9], linear_acceleration: imu_msg.linear_acceleration.clone(), @@ -138,17 +136,6 @@ impl ImuWithCovariance { pub fn to_imu_msg(&self) -> ImuMsg { ImuMsg { header: self.header.clone(), - // NOTE: Unlike the original C++ implementation, which intentionally omits - // orientation from the output message (imu_msg_base_link has no orientation - // assignment), this Rust implementation copies orientation from the input. - // This is a side effect of the clone-based approach rather than an intentional - // design choice. - // - // In the current MRM pipeline, downstream nodes (gyro_odometer, ekf_localizer) - // do not use orientation, so this difference has no functional impact. - // If a consumer of orientation is added in the future, this should be revisited - // to align with the C++ behavior (i.e., leave orientation as default/zero). - orientation: self.orientation.clone(), angular_velocity: self.angular_velocity.clone(), linear_acceleration: self.linear_acceleration.clone(), } @@ -358,12 +345,6 @@ mod tests { frame_id: "imu_link", timestamp: 0, }, - orientation: Quaternion { - x: 0.0, - y: 0.0, - z: 0.0, - w: 1.0, - }, angular_velocity: Vector3::new(0.1, 0.2, 0.3), linear_acceleration: Vector3::new(9.8, 0.0, 0.0), } diff --git a/applications/autoware/imu_driver/src/lib.rs b/applications/autoware/imu_driver/src/lib.rs index 6ff1a99ae..857886eac 100644 --- a/applications/autoware/imu_driver/src/lib.rs +++ b/applications/autoware/imu_driver/src/lib.rs @@ -63,7 +63,6 @@ use core::f64::consts::PI; #[derive(Clone, Debug)] pub struct ImuMsg { pub header: Header, - pub orientation: Quaternion, pub angular_velocity: Vector3, pub linear_acceleration: Vector3, } @@ -71,7 +70,6 @@ pub struct ImuMsg { #[derive(Clone, Debug)] pub struct ImuCsvRow { pub timestamp: u64, - pub orientation: Quaternion, pub angular_velocity: Vector3, pub linear_acceleration: Vector3, } @@ -110,12 +108,6 @@ impl Default for ImuMsg { frame_id: "imu", timestamp: 0, }, - orientation: Quaternion { - x: 0.0, - y: 0.0, - z: 0.0, - w: 1.0, - }, angular_velocity: Vector3::new(0.0, 0.0, 0.0), linear_acceleration: Vector3::new(0.0, 0.0, 0.0), } @@ -132,7 +124,6 @@ pub fn build_imu_msg_from_csv_row( frame_id, timestamp, }, - orientation: row.orientation.clone(), angular_velocity: row.angular_velocity.clone(), linear_acceleration: row.linear_acceleration.clone(), } From ae1d934b4496f263f54e15b3e7dcbfdc2925aec3 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Fri, 22 May 2026 17:40:50 +0900 Subject: [PATCH 19/20] fix: extract functions from Transform and ImuCorrector Signed-off-by: nokosaaan --- .../autoware/imu_corrector/src/lib.rs | 53 +++++++------------ 1 file changed, 20 insertions(+), 33 deletions(-) diff --git a/applications/autoware/imu_corrector/src/lib.rs b/applications/autoware/imu_corrector/src/lib.rs index 097532d74..e058e33a5 100644 --- a/applications/autoware/imu_corrector/src/lib.rs +++ b/applications/autoware/imu_corrector/src/lib.rs @@ -46,27 +46,14 @@ impl Transform { } } - fn to_nalgebra_vector3(&self, vec: &Vector3) -> NVector3 { - NVector3::new(vec.x, vec.y, vec.z) - } - - fn to_imu_vector3(&self, vec: &NVector3) -> Vector3 { - Vector3::new(vec.x, vec.y, vec.z) - } - - fn to_nalgebra_quaternion(&self, quat: &Quaternion) -> UnitQuaternion { - let n_quat = NQuaternion::new(quat.w, quat.x, quat.y, quat.z); - UnitQuaternion::from_quaternion(n_quat) - } - // This code is used as part of the `gyro_odometer` function processing. pub fn apply_to_vector(&self, vec: Vector3) -> Vector3 { - let nalgebra_vec = self.to_nalgebra_vector3(&vec); - let nalgebra_quat = self.to_nalgebra_quaternion(&self.rotation); - let nalgebra_trans = self.to_nalgebra_vector3(&self.translation); + let nalgebra_vec = to_nalgebra_vector3(&vec); + let nalgebra_quat = to_nalgebra_quaternion(&self.rotation); + let nalgebra_trans = to_nalgebra_vector3(&self.translation); let rotated = nalgebra_quat * nalgebra_vec; let result = rotated + nalgebra_trans; - self.to_imu_vector3(&result) + to_imu_vector3(&result) } } @@ -199,24 +186,11 @@ impl ImuCorrector { self.config = config; } - fn to_nalgebra_vector3(&self, vec: &Vector3) -> NVector3 { - NVector3::new(vec.x, vec.y, vec.z) - } - - fn to_imu_vector3(&self, vec: &NVector3) -> Vector3 { - Vector3::new(vec.x, vec.y, vec.z) - } - - fn to_nalgebra_quaternion(&self, quat: &Quaternion) -> UnitQuaternion { - let n_quat = NQuaternion::new(quat.w, quat.x, quat.y, quat.z); - UnitQuaternion::from_quaternion(n_quat) - } - fn transform_vector3(&self, vec: &Vector3, transform: &Transform) -> Vector3 { - let nalgebra_vec = self.to_nalgebra_vector3(vec); - let nalgebra_quat = self.to_nalgebra_quaternion(&transform.rotation); + let nalgebra_vec = to_nalgebra_vector3(vec); + let nalgebra_quat = to_nalgebra_quaternion(&transform.rotation); let rotated = nalgebra_quat * nalgebra_vec; - self.to_imu_vector3(&rotated) + to_imu_vector3(&rotated) } fn transform_covariance_impl(cov: &[f64; 9]) -> [f64; 9] { @@ -327,6 +301,19 @@ impl ImuCorrector { } } +fn to_nalgebra_vector3(vec: &Vector3) -> NVector3 { + NVector3::new(vec.x, vec.y, vec.z) +} + +fn to_imu_vector3(vec: &NVector3) -> Vector3 { + Vector3::new(vec.x, vec.y, vec.z) +} + +fn to_nalgebra_quaternion(quat: &Quaternion) -> UnitQuaternion { + let n_quat = NQuaternion::new(quat.w, quat.x, quat.y, quat.z); + UnitQuaternion::from_quaternion(n_quat) +} + pub fn transform_covariance(cov: &[f64; 9]) -> [f64; 9] { ImuCorrector::::transform_covariance_impl(cov) } From 9d6a46ddadfe26e81db379afb690d9e9c152f01d Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Fri, 22 May 2026 17:52:55 +0900 Subject: [PATCH 20/20] fix: add Default trait Signed-off-by: nokosaaan --- applications/autoware/imu_corrector/src/lib.rs | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/applications/autoware/imu_corrector/src/lib.rs b/applications/autoware/imu_corrector/src/lib.rs index e058e33a5..7e573e213 100644 --- a/applications/autoware/imu_corrector/src/lib.rs +++ b/applications/autoware/imu_corrector/src/lib.rs @@ -84,6 +84,12 @@ impl MockTransformListener { } } +impl Default for MockTransformListener { + fn default() -> Self { + Self::new() + } +} + impl TransformListener for MockTransformListener { fn get_latest_transform(&self, from_frame: &str, to_frame: &str) -> Option { let key = format!("{}_to_{}", from_frame, to_frame); @@ -181,6 +187,12 @@ impl ImuCorrector { } } +impl Default for ImuCorrector { + fn default() -> Self { + Self::new() + } +} + impl ImuCorrector { pub fn set_config(&mut self, config: ImuCorrectorConfig) { self.config = config;