From 20f95f400908af0105b16cbee3d814fe9d26b926 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Fri, 8 May 2026 13:19:26 +0900 Subject: [PATCH 1/8] add: gyro_odometer with test case Signed-off-by: nokosaaan --- .../test_autoware/common_types/Cargo.toml | 6 + .../test_autoware/common_types/src/lib.rs | 21 + .../test_autoware/gyro_odometer/Cargo.toml | 9 + .../test_autoware/gyro_odometer/src/lib.rs | 461 ++++++++++++++++++ .../tests/test_autoware/imu_driver/Cargo.toml | 1 + .../tests/test_autoware/imu_driver/src/lib.rs | 21 +- .../vehicle_velocity_converter/Cargo.toml | 1 + .../vehicle_velocity_converter/src/lib.rs | 13 +- 8 files changed, 502 insertions(+), 31 deletions(-) create mode 100644 applications/tests/test_autoware/common_types/Cargo.toml create mode 100644 applications/tests/test_autoware/common_types/src/lib.rs create mode 100644 applications/tests/test_autoware/gyro_odometer/Cargo.toml create mode 100644 applications/tests/test_autoware/gyro_odometer/src/lib.rs diff --git a/applications/tests/test_autoware/common_types/Cargo.toml b/applications/tests/test_autoware/common_types/Cargo.toml new file mode 100644 index 000000000..c54b7769b --- /dev/null +++ b/applications/tests/test_autoware/common_types/Cargo.toml @@ -0,0 +1,6 @@ +[package] +name = "common_types" +version = "0.1.0" +edition = "2021" + +[dependencies] diff --git a/applications/tests/test_autoware/common_types/src/lib.rs b/applications/tests/test_autoware/common_types/src/lib.rs new file mode 100644 index 000000000..2ca030990 --- /dev/null +++ b/applications/tests/test_autoware/common_types/src/lib.rs @@ -0,0 +1,21 @@ +#![no_std] +extern crate alloc; + +#[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, +} + +impl Vector3 { + pub fn new(x: f64, y: f64, z: f64) -> Self { + Self { x, y, z } + } +} diff --git a/applications/tests/test_autoware/gyro_odometer/Cargo.toml b/applications/tests/test_autoware/gyro_odometer/Cargo.toml new file mode 100644 index 000000000..be11aaefb --- /dev/null +++ b/applications/tests/test_autoware/gyro_odometer/Cargo.toml @@ -0,0 +1,9 @@ +[package] +name = "gyro_odometer" +version = "0.1.0" +edition = "2021" + +[dependencies] +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} \ No newline at end of file diff --git a/applications/tests/test_autoware/gyro_odometer/src/lib.rs b/applications/tests/test_autoware/gyro_odometer/src/lib.rs new file mode 100644 index 000000000..834924786 --- /dev/null +++ b/applications/tests/test_autoware/gyro_odometer/src/lib.rs @@ -0,0 +1,461 @@ +#![no_std] +extern crate alloc; + +use alloc::{collections::VecDeque, string::String}; +use core::time::Duration; +use core::ptr::null_mut; +use core::sync::atomic::{AtomicPtr, Ordering as AtomicOrdering}; + +pub use imu_corrector::{transform_covariance, ImuWithCovariance, Transform}; +pub use imu_driver::{Header, ImuMsg, Quaternion, Vector3}; +pub use vehicle_velocity_converter::{ + Odometry, Twist, TwistWithCovariance, TwistWithCovarianceStamped, +}; + +static GYRO_ODOMETER_INSTANCE: AtomicPtr = AtomicPtr::new(null_mut()); + +const COV_IDX_X_X: usize = 0; +const COV_IDX_Y_Y: usize = 4; +const COV_IDX_Z_Z: usize = 8; +const COV_IDX_XYZRPY_X_X: usize = 0; +const COV_IDX_XYZRPY_Y_Y: usize = 7; +const COV_IDX_XYZRPY_Z_Z: usize = 14; +const COV_IDX_XYZRPY_ROLL_ROLL: usize = 21; +const COV_IDX_XYZRPY_PITCH_PITCH: usize = 28; +const COV_IDX_XYZRPY_YAW_YAW: usize = 35; + +pub struct GyroOdometerCore { + pub output_frame: String, + pub message_timeout_sec: f64, + pub vehicle_twist_arrived: bool, + pub imu_arrived: bool, + pub vehicle_twist_queue: VecDeque, + pub gyro_queue: VecDeque, + pub config: GyroOdometerConfig, +} + +impl GyroOdometerCore { + pub fn new(config: GyroOdometerConfig) -> Result { + let queue_size = config.queue_size; + let output_frame = config.output_frame.clone(); + let message_timeout_sec = config.message_timeout_sec; + + Ok(Self { + output_frame, + message_timeout_sec, + vehicle_twist_arrived: false, + imu_arrived: false, + vehicle_twist_queue: VecDeque::with_capacity(queue_size), + gyro_queue: VecDeque::with_capacity(queue_size), + config, + }) + } + + pub fn concat_gyro_and_odometer( + &mut self, + current_time: u64, + ) -> Result> { + if !self.vehicle_twist_arrived { + self.vehicle_twist_queue.clear(); + self.gyro_queue.clear(); + return Ok(None); + } + if !self.imu_arrived { + self.vehicle_twist_queue.clear(); + self.gyro_queue.clear(); + return Ok(None); + } + if !self.vehicle_twist_queue.is_empty() && !self.gyro_queue.is_empty() { + let latest_vehicle_twist_stamp = + self.vehicle_twist_queue.back().unwrap().header.timestamp; + let latest_imu_stamp = self.gyro_queue.back().unwrap().header.timestamp; + + if Self::check_timeout( + current_time, + latest_vehicle_twist_stamp, + self.message_timeout_sec, + ) { + self.vehicle_twist_queue.clear(); + self.gyro_queue.clear(); + return Err(GyroOdometerError::TimeoutError(String::from( + "Vehicle twist message timeout", + ))); + } + + if Self::check_timeout(current_time, latest_imu_stamp, self.message_timeout_sec) { + self.vehicle_twist_queue.clear(); + self.gyro_queue.clear(); + return Err(GyroOdometerError::TimeoutError(String::from( + "IMU message timeout", + ))); + } + } + + if self.vehicle_twist_queue.is_empty() || self.gyro_queue.is_empty() { + return Ok(None); + } + + let tf = self.get_transform( + &self.gyro_queue.front().unwrap().header.frame_id, + &self.output_frame, + )?; + + for gyro in &mut self.gyro_queue { + let transformed_angular_velocity = tf.apply_to_vector(gyro.angular_velocity.clone()); + gyro.angular_velocity = transformed_angular_velocity; + } + + let mut vx_mean = 0.0; + let mut gyro_mean = Vector3::new(0.0, 0.0, 0.0); + let mut vx_covariance_original = 0.0; + let mut gyro_covariance_original = Vector3::new(0.0, 0.0, 0.0); + + for vehicle_twist in &self.vehicle_twist_queue { + vx_mean += vehicle_twist.twist.twist.linear.x; + vx_covariance_original += vehicle_twist.twist.covariance[0 * 6 + 0]; + } + vx_mean /= self.vehicle_twist_queue.len() as f64; + vx_covariance_original /= self.vehicle_twist_queue.len() as f64; + + for gyro in &self.gyro_queue { + gyro_mean.x += gyro.angular_velocity.x; + gyro_mean.y += gyro.angular_velocity.y; + gyro_mean.z += gyro.angular_velocity.z; + gyro_covariance_original.x += gyro.angular_velocity_covariance[COV_IDX_X_X]; + gyro_covariance_original.y += gyro.angular_velocity_covariance[COV_IDX_Y_Y]; + gyro_covariance_original.z += gyro.angular_velocity_covariance[COV_IDX_Z_Z]; + } + gyro_mean.x /= self.gyro_queue.len() as f64; + gyro_mean.y /= self.gyro_queue.len() as f64; + gyro_mean.z /= self.gyro_queue.len() as f64; + gyro_covariance_original.x /= self.gyro_queue.len() as f64; + gyro_covariance_original.y /= self.gyro_queue.len() as f64; + gyro_covariance_original.z /= self.gyro_queue.len() as f64; + + let latest_vehicle_twist_stamp = self.vehicle_twist_queue.back().unwrap().header.timestamp; + let latest_imu_stamp = self.gyro_queue.back().unwrap().header.timestamp; + + let result_timestamp = if latest_vehicle_twist_stamp < latest_imu_stamp { + latest_imu_stamp + } else { + latest_vehicle_twist_stamp + }; + + let mut result = TwistWithCovarianceStamped { + header: Header { + frame_id: self.gyro_queue.front().unwrap().header.frame_id, + timestamp: result_timestamp, + }, + twist: TwistWithCovariance { + twist: Twist { + linear: Vector3::new(vx_mean, 0.0, 0.0), + angular: gyro_mean, + }, + covariance: [0.0; 36], + }, + }; + + result.twist.covariance[COV_IDX_XYZRPY_X_X] = + vx_covariance_original / self.vehicle_twist_queue.len() as f64; + result.twist.covariance[COV_IDX_XYZRPY_Y_Y] = 100000.0; + result.twist.covariance[COV_IDX_XYZRPY_Z_Z] = 100000.0; + result.twist.covariance[COV_IDX_XYZRPY_ROLL_ROLL] = + gyro_covariance_original.x / self.gyro_queue.len() as f64; + result.twist.covariance[COV_IDX_XYZRPY_PITCH_PITCH] = + gyro_covariance_original.y / self.gyro_queue.len() as f64; + result.twist.covariance[COV_IDX_XYZRPY_YAW_YAW] = + gyro_covariance_original.z / self.gyro_queue.len() as f64; + + self.vehicle_twist_queue.clear(); + self.gyro_queue.clear(); + + Ok(Some(result)) + } + + pub fn check_timeout(current_timestamp: u64, last_timestamp: u64, timeout_sec: f64) -> bool { + let dt = (current_timestamp as f64 - last_timestamp as f64) / 1_000_000_000.0; + dt.abs() > timeout_sec + } + pub fn get_transform(&self, from_frame: &str, to_frame: &str) -> Result { + if from_frame == to_frame || from_frame == "" || to_frame == "" { + Ok(Transform::identity()) + } else { + Ok(Transform::identity()) + } + } + + pub fn process_result( + &self, + twist_with_cov_raw: TwistWithCovarianceStamped, + ) -> TwistWithCovarianceStamped { + if twist_with_cov_raw.twist.twist.angular.z.abs() < 0.01 + && twist_with_cov_raw.twist.twist.linear.x.abs() < 0.01 + { + let mut twist = twist_with_cov_raw; + twist.twist.twist.angular.x = 0.0; + twist.twist.twist.angular.y = 0.0; + twist.twist.twist.angular.z = 0.0; + twist + } else { + twist_with_cov_raw + } + } + + pub fn convert_vehicle_velocity_to_twist( + &self, + odometry: &Odometry, + timestamp: u64, + ) -> TwistWithCovarianceStamped { + TwistWithCovarianceStamped { + header: Header { + frame_id: "base_link", + timestamp, + }, + twist: TwistWithCovariance { + twist: Twist { + linear: Vector3::new(odometry.velocity, 0.0, 0.0), + angular: Vector3::new(0.0, 0.0, 0.0), + }, + covariance: [0.0; 36], + }, + } + } + + pub fn add_vehicle_twist(&mut self, twist: TwistWithCovarianceStamped) { + self.vehicle_twist_arrived = true; + self.vehicle_twist_queue.push_back(twist); + } + + pub fn add_imu(&mut self, imu: ImuWithCovariance) { + self.imu_arrived = true; + self.gyro_queue.push_back(imu); + } + + pub fn process_imu_with_covariance(&mut self, imu: ImuWithCovariance) -> Result<()> { + self.add_imu(imu); + Ok(()) + } + + pub fn process_and_get_result( + &mut self, + current_time: u64, + ) -> Option { + match self.concat_gyro_and_odometer(current_time) { + Ok(result) => result, + Err(_) => None, + } + } + + pub fn get_queue_sizes(&self) -> (usize, usize) { + (self.vehicle_twist_queue.len(), self.gyro_queue.len()) + } + + pub fn clear_queues(&mut self) { + self.vehicle_twist_queue.clear(); + self.gyro_queue.clear(); + } + + pub fn reset_arrival_flags(&mut self) { + self.vehicle_twist_arrived = false; + self.imu_arrived = false; + } +} + +#[derive(Debug)] +pub enum GyroOdometerError { + TransformError(String), + TimeoutError(String), + QueueError(String), + ParameterError(String), +} + +impl core::fmt::Display for GyroOdometerError { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + match self { + GyroOdometerError::TransformError(msg) => write!(f, "Transform error: {}", msg), + GyroOdometerError::TimeoutError(msg) => write!(f, "Timeout error: {}", msg), + GyroOdometerError::QueueError(msg) => write!(f, "Queue error: {}", msg), + GyroOdometerError::ParameterError(msg) => write!(f, "Invalid parameter: {}", msg), + } + } +} + +impl core::error::Error for GyroOdometerError {} + +type Result = core::result::Result; + +#[derive(Debug, Clone)] +pub struct GyroOdometerConfig { + pub output_frame: String, + pub message_timeout_sec: f64, + pub queue_size: usize, + pub transform_timeout: Duration, + pub min_velocity_threshold: f64, + pub covariance_scale: f64, +} + +impl Default for GyroOdometerConfig { + fn default() -> Self { + Self { + output_frame: String::from("base_link"), + message_timeout_sec: 1.0, + queue_size: 100, + transform_timeout: Duration::from_secs(1), + min_velocity_threshold: 0.01, + covariance_scale: 100000.0, + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + // Equivalent helpers to Autoware C++ test helper. + fn generate_sample_imu() -> ImuMsg { + ImuMsg { + header: Header { + frame_id: "base_link", + timestamp: 123456789, + }, + 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), + } + } + + fn generate_sample_velocity() -> TwistWithCovarianceStamped { + TwistWithCovarianceStamped { + header: Header { + frame_id: "base_link", + timestamp: 123456789, + }, + twist: TwistWithCovariance { + twist: Twist { + linear: Vector3::new(1.0, 0.0, 0.0), + angular: Vector3::new(0.0, 0.0, 0.0), + }, + covariance: [0.0; 36], + }, + } + } + + fn get_config_with_default_params() -> GyroOdometerConfig { + GyroOdometerConfig { + output_frame: String::from("base_link"), + message_timeout_sec: 1e12, + ..GyroOdometerConfig::default() + } + } + + #[test] + fn test_gyro_odometer_core_creation() { + let config = get_config_with_default_params(); + let core = GyroOdometerCore::new(config); + assert!(core.is_ok()); + } + + #[test] + fn test_imu_with_covariance_conversion() { + let imu_msg = generate_sample_imu(); + + let imu_with_cov = ImuWithCovariance::from_imu_msg(&imu_msg); + let converted_back = imu_with_cov.to_imu_msg(); + + assert_eq!(imu_msg.header.frame_id, converted_back.header.frame_id); + assert_eq!(imu_msg.header.timestamp, converted_back.header.timestamp); + assert_eq!( + imu_msg.angular_velocity.x, + converted_back.angular_velocity.x + ); + assert_eq!( + imu_msg.angular_velocity.y, + converted_back.angular_velocity.y + ); + assert_eq!( + imu_msg.angular_velocity.z, + converted_back.angular_velocity.z + ); + } + + #[test] + fn test_vehicle_velocity_conversion() { + let config = get_config_with_default_params(); + let core = GyroOdometerCore::new(config).unwrap(); + + let sample_twist = generate_sample_velocity(); + assert_eq!(sample_twist.header.frame_id, "base_link"); + assert_eq!(sample_twist.twist.twist.linear.x, 1.0); + + let odometry = Odometry { + velocity: sample_twist.twist.twist.linear.x, + }; + let twist = core.convert_vehicle_velocity_to_twist(&odometry, sample_twist.header.timestamp); + + assert_eq!(twist.header.frame_id, sample_twist.header.frame_id); + assert_eq!(twist.header.timestamp, 123456789); + assert_eq!(twist.twist.twist.linear.x, 1.0); + assert_eq!(twist.twist.twist.linear.y, 0.0); + assert_eq!(twist.twist.twist.linear.z, 0.0); + } + + #[test] + fn test_imu_corrector_integration() { + let config = get_config_with_default_params(); + let mut core = GyroOdometerCore::new(config).unwrap(); + + let imu_msg = generate_sample_imu(); + let mut imu_with_cov = ImuWithCovariance::from_imu_msg(&imu_msg); + imu_with_cov.angular_velocity_covariance = + [0.0009, 0.0, 0.0, 0.0, 0.0009, 0.0, 0.0, 0.0, 0.0009]; + imu_with_cov.linear_acceleration_covariance = + [100000000.0, 0.0, 0.0, 0.0, 100000000.0, 0.0, 0.0, 0.0, 100000000.0]; + + let result = core.process_imu_with_covariance(imu_with_cov); + assert!(result.is_ok()); + } + + #[test] + fn test_transform_covariance_from_imu_corrector() { + let input = [1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0]; + let output = transform_covariance(&input); + assert_eq!(output[COV_IDX_X_X], 3.0); + assert_eq!(output[COV_IDX_Y_Y], 3.0); + assert_eq!(output[COV_IDX_Z_Z], 3.0); + } +} + +pub fn get_or_initialize() -> Result<&'static mut GyroOdometerCore> { + let ptr = GYRO_ODOMETER_INSTANCE.load(AtomicOrdering::Acquire); + + if !ptr.is_null() { + return Ok(unsafe { &mut *ptr }); + } + + let config = GyroOdometerConfig::default(); + let core = GyroOdometerCore::new(config)?; + let boxed_core = alloc::boxed::Box::new(core); + let new_ptr = alloc::boxed::Box::into_raw(boxed_core); + + match GYRO_ODOMETER_INSTANCE.compare_exchange( + null_mut(), + new_ptr, + AtomicOrdering::Acquire, + AtomicOrdering::Relaxed, + ) { + Ok(_) => { + Ok(unsafe { &mut *new_ptr }) + } + Err(existing_ptr) => { + unsafe { + let _ = alloc::boxed::Box::from_raw(new_ptr); + } + Ok(unsafe { &mut *existing_ptr }) + } + } +} \ No newline at end of file diff --git a/applications/tests/test_autoware/imu_driver/Cargo.toml b/applications/tests/test_autoware/imu_driver/Cargo.toml index da816a55f..ceec72d7a 100644 --- a/applications/tests/test_autoware/imu_driver/Cargo.toml +++ b/applications/tests/test_autoware/imu_driver/Cargo.toml @@ -4,3 +4,4 @@ version = "0.1.0" edition = "2021" [dependencies] +common_types = { path = "../common_types", default-features = false } \ No newline at end of file diff --git a/applications/tests/test_autoware/imu_driver/src/lib.rs b/applications/tests/test_autoware/imu_driver/src/lib.rs index a7abc330f..15ef58e24 100644 --- a/applications/tests/test_autoware/imu_driver/src/lib.rs +++ b/applications/tests/test_autoware/imu_driver/src/lib.rs @@ -4,6 +4,8 @@ extern crate alloc; use alloc::{format, string::String, vec, vec::Vec}; use core::f64::consts::PI; +pub use common_types::{Header, Vector3}; + #[derive(Clone, Debug)] pub struct ImuMsg { pub header: Header, @@ -20,12 +22,6 @@ pub struct ImuCsvRow { pub linear_acceleration: Vector3, } -#[derive(Clone, Debug)] -pub struct Header { - pub frame_id: &'static str, - pub timestamp: u64, -} - #[derive(Debug, Clone)] pub struct Quaternion { pub x: f64, @@ -34,19 +30,6 @@ pub struct Quaternion { pub w: f64, } -#[derive(Clone, Debug)] -pub struct Vector3 { - pub x: f64, - pub y: f64, - pub z: f64, -} - -impl Vector3 { - pub fn new(x: f64, y: f64, z: f64) -> Self { - Self { x, y, z } - } -} - impl Default for ImuMsg { fn default() -> Self { Self { diff --git a/applications/tests/test_autoware/vehicle_velocity_converter/Cargo.toml b/applications/tests/test_autoware/vehicle_velocity_converter/Cargo.toml index 24f988e6e..9d3d8b790 100644 --- a/applications/tests/test_autoware/vehicle_velocity_converter/Cargo.toml +++ b/applications/tests/test_autoware/vehicle_velocity_converter/Cargo.toml @@ -4,3 +4,4 @@ version = "0.1.0" edition = "2021" [dependencies] +common_types = { path = "../common_types", default-features = false } diff --git a/applications/tests/test_autoware/vehicle_velocity_converter/src/lib.rs b/applications/tests/test_autoware/vehicle_velocity_converter/src/lib.rs index f7a96dc43..2b523c95f 100644 --- a/applications/tests/test_autoware/vehicle_velocity_converter/src/lib.rs +++ b/applications/tests/test_autoware/vehicle_velocity_converter/src/lib.rs @@ -14,18 +14,7 @@ #![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, -} +pub use common_types::{Header, Vector3}; #[derive(Debug, Clone)] pub struct VelocityReport { From 4dabbabd7cd8f6c5abaf3a3f6ca339b57c1db3e6 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Mon, 11 May 2026 10:10:59 +0900 Subject: [PATCH 2/8] fix: move autoware position 2 Signed-off-by: nokosaaan --- applications/{tests/test_autoware => autoware}/Cargo.toml | 0 .../{tests/test_autoware => autoware}/common_types/Cargo.toml | 0 .../{tests/test_autoware => autoware}/common_types/src/lib.rs | 0 .../{tests/test_autoware => autoware}/gyro_odometer/Cargo.toml | 0 .../{tests/test_autoware => autoware}/gyro_odometer/src/lib.rs | 0 .../{tests/test_autoware => autoware}/imu_corrector/Cargo.toml | 0 .../{tests/test_autoware => autoware}/imu_corrector/src/lib.rs | 0 .../{tests/test_autoware => autoware}/imu_driver/Cargo.toml | 0 .../{tests/test_autoware => autoware}/imu_driver/src/lib.rs | 0 applications/{tests/test_autoware => autoware}/src/lib.rs | 0 .../vehicle_velocity_converter/Cargo.toml | 0 .../vehicle_velocity_converter/src/lib.rs | 0 12 files changed, 0 insertions(+), 0 deletions(-) rename applications/{tests/test_autoware => autoware}/Cargo.toml (100%) rename applications/{tests/test_autoware => autoware}/common_types/Cargo.toml (100%) rename applications/{tests/test_autoware => autoware}/common_types/src/lib.rs (100%) rename applications/{tests/test_autoware => autoware}/gyro_odometer/Cargo.toml (100%) rename applications/{tests/test_autoware => autoware}/gyro_odometer/src/lib.rs (100%) 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/applications/tests/test_autoware/Cargo.toml b/applications/autoware/Cargo.toml similarity index 100% rename from applications/tests/test_autoware/Cargo.toml rename to applications/autoware/Cargo.toml diff --git a/applications/tests/test_autoware/common_types/Cargo.toml b/applications/autoware/common_types/Cargo.toml similarity index 100% rename from applications/tests/test_autoware/common_types/Cargo.toml rename to applications/autoware/common_types/Cargo.toml diff --git a/applications/tests/test_autoware/common_types/src/lib.rs b/applications/autoware/common_types/src/lib.rs similarity index 100% rename from applications/tests/test_autoware/common_types/src/lib.rs rename to applications/autoware/common_types/src/lib.rs diff --git a/applications/tests/test_autoware/gyro_odometer/Cargo.toml b/applications/autoware/gyro_odometer/Cargo.toml similarity index 100% rename from applications/tests/test_autoware/gyro_odometer/Cargo.toml rename to applications/autoware/gyro_odometer/Cargo.toml diff --git a/applications/tests/test_autoware/gyro_odometer/src/lib.rs b/applications/autoware/gyro_odometer/src/lib.rs similarity index 100% rename from applications/tests/test_autoware/gyro_odometer/src/lib.rs rename to applications/autoware/gyro_odometer/src/lib.rs 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 From 77c07f8c412cb23bb37d03aab06a32059b9611bb Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Mon, 11 May 2026 10:15:22 +0900 Subject: [PATCH 3/8] fix: dependics path 2 Signed-off-by: nokosaaan --- Cargo.toml | 1 + applications/autoware/Cargo.toml | 6 +++--- userland/Cargo.toml | 10 +++++----- userland/src/lib.rs | 6 +++--- 4 files changed, 12 insertions(+), 11 deletions(-) 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/autoware/Cargo.toml b/applications/autoware/Cargo.toml index f745c6851..96d8a9c7f 100644 --- a/applications/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/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 b2133094436b10689753a58bd62c2f0274915e18 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Mon, 11 May 2026 10:18:25 +0900 Subject: [PATCH 4/8] fix: reset line Signed-off-by: nokosaaan --- applications/autoware/gyro_odometer/Cargo.toml | 2 +- applications/autoware/imu_corrector/Cargo.toml | 2 +- applications/autoware/imu_driver/Cargo.toml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/applications/autoware/gyro_odometer/Cargo.toml b/applications/autoware/gyro_odometer/Cargo.toml index be11aaefb..332559fa2 100644 --- a/applications/autoware/gyro_odometer/Cargo.toml +++ b/applications/autoware/gyro_odometer/Cargo.toml @@ -6,4 +6,4 @@ edition = "2021" [dependencies] 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} \ No newline at end of file +vehicle_velocity_converter = { path = "../vehicle_velocity_converter", default-features = false} 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_driver/Cargo.toml b/applications/autoware/imu_driver/Cargo.toml index ceec72d7a..16fd6f500 100644 --- a/applications/autoware/imu_driver/Cargo.toml +++ b/applications/autoware/imu_driver/Cargo.toml @@ -4,4 +4,4 @@ version = "0.1.0" edition = "2021" [dependencies] -common_types = { path = "../common_types", default-features = false } \ No newline at end of file +common_types = { path = "../common_types", default-features = false } From 94662bf4cf8a84041903f0a532148d5e88f71587 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Mon, 11 May 2026 10:51:15 +0900 Subject: [PATCH 5/8] fix: comment 3 Signed-off-by: nokosaaan --- applications/autoware/gyro_odometer/src/lib.rs | 6 ++++++ applications/autoware/imu_corrector/src/lib.rs | 6 ++++++ applications/autoware/imu_driver/src/lib.rs | 6 ++++++ .../vehicle_velocity_converter/src/lib.rs | 18 +++++------------- 4 files changed, 23 insertions(+), 13 deletions(-) diff --git a/applications/autoware/gyro_odometer/src/lib.rs b/applications/autoware/gyro_odometer/src/lib.rs index 834924786..7d6ae0d47 100644 --- a/applications/autoware/gyro_odometer/src/lib.rs +++ b/applications/autoware/gyro_odometer/src/lib.rs @@ -1,3 +1,9 @@ +// Ported from the following versions of the original C++ code: +// core/autoware_core: +// type: git +// url: https://github.com/autowarefoundation/autoware_core.git +// version: 1.8.0 + #![no_std] extern crate alloc; 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 15ef58e24..bdbf28c5a 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; diff --git a/applications/autoware/vehicle_velocity_converter/src/lib.rs b/applications/autoware/vehicle_velocity_converter/src/lib.rs index 2b523c95f..cbb2632b8 100644 --- a/applications/autoware/vehicle_velocity_converter/src/lib.rs +++ b/applications/autoware/vehicle_velocity_converter/src/lib.rs @@ -1,16 +1,8 @@ -// 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. +// Ported from the following versions of the original C++ code: +// core/autoware_core: +// type: git +// url: https://github.com/autowarefoundation/autoware_core.git +// version: 1.8.0 #![no_std] From b0a98913fba09c99d19cd004279d1a6b853fb4de Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Tue, 26 May 2026 22:28:45 +0900 Subject: [PATCH 6/8] fix: delete test/test_autoware Signed-off-by: nokosaaan --- applications/tests/test_autoware/imu_driver/Cargo.toml | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 applications/tests/test_autoware/imu_driver/Cargo.toml diff --git a/applications/tests/test_autoware/imu_driver/Cargo.toml b/applications/tests/test_autoware/imu_driver/Cargo.toml deleted file mode 100644 index e69de29bb..000000000 From 225729cc93d4f0a4c2f4587ffe90055108cdfcdc Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Tue, 26 May 2026 22:33:54 +0900 Subject: [PATCH 7/8] fix: add license comment Signed-off-by: nokosaaan --- applications/autoware/gyro_odometer/src/lib.rs | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/applications/autoware/gyro_odometer/src/lib.rs b/applications/autoware/gyro_odometer/src/lib.rs index 7d6ae0d47..cb63b85d7 100644 --- a/applications/autoware/gyro_odometer/src/lib.rs +++ b/applications/autoware/gyro_odometer/src/lib.rs @@ -1,7 +1,23 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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: // core/autoware_core: // type: git // url: https://github.com/autowarefoundation/autoware_core.git +// original file path: localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp +// test code: localization/autoware_gyro_odometer/test/test_gyro_odometer_helper.cpp // version: 1.8.0 #![no_std] @@ -464,4 +480,4 @@ pub fn get_or_initialize() -> Result<&'static mut GyroOdometerCore> { Ok(unsafe { &mut *existing_ptr }) } } -} \ No newline at end of file +} From 57cc3ff6c45d6a015ea74d7dda94199c61e418c9 Mon Sep 17 00:00:00 2001 From: nokosaaan Date: Tue, 26 May 2026 22:55:03 +0900 Subject: [PATCH 8/8] fix: delete orientation and Odometry from unsafe Signed-off-by: nokosaaan --- applications/autoware/Cargo.toml | 2 + .../autoware/gyro_odometer/src/lib.rs | 75 +++++++------------ 2 files changed, 29 insertions(+), 48 deletions(-) diff --git a/applications/autoware/Cargo.toml b/applications/autoware/Cargo.toml index 96d8a9c7f..31b7485c2 100644 --- a/applications/autoware/Cargo.toml +++ b/applications/autoware/Cargo.toml @@ -16,3 +16,5 @@ 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 } +gyro_odometer = { path = "./gyro_odometer", default-features = false} + diff --git a/applications/autoware/gyro_odometer/src/lib.rs b/applications/autoware/gyro_odometer/src/lib.rs index cb63b85d7..373e19b27 100644 --- a/applications/autoware/gyro_odometer/src/lib.rs +++ b/applications/autoware/gyro_odometer/src/lib.rs @@ -11,7 +11,7 @@ // 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: // core/autoware_core: // type: git @@ -24,15 +24,13 @@ extern crate alloc; use alloc::{collections::VecDeque, string::String}; -use core::time::Duration; use core::ptr::null_mut; use core::sync::atomic::{AtomicPtr, Ordering as AtomicOrdering}; +use core::time::Duration; pub use imu_corrector::{transform_covariance, ImuWithCovariance, Transform}; -pub use imu_driver::{Header, ImuMsg, Quaternion, Vector3}; -pub use vehicle_velocity_converter::{ - Odometry, Twist, TwistWithCovariance, TwistWithCovarianceStamped, -}; +pub use imu_driver::{Header, ImuMsg, Vector3}; +pub use vehicle_velocity_converter::{Twist, TwistWithCovariance, TwistWithCovarianceStamped}; static GYRO_ODOMETER_INSTANCE: AtomicPtr = AtomicPtr::new(null_mut()); @@ -223,26 +221,6 @@ impl GyroOdometerCore { } } - pub fn convert_vehicle_velocity_to_twist( - &self, - odometry: &Odometry, - timestamp: u64, - ) -> TwistWithCovarianceStamped { - TwistWithCovarianceStamped { - header: Header { - frame_id: "base_link", - timestamp, - }, - twist: TwistWithCovariance { - twist: Twist { - linear: Vector3::new(odometry.velocity, 0.0, 0.0), - angular: Vector3::new(0.0, 0.0, 0.0), - }, - covariance: [0.0; 36], - }, - } - } - pub fn add_vehicle_twist(&mut self, twist: TwistWithCovarianceStamped) { self.vehicle_twist_arrived = true; self.vehicle_twist_queue.push_back(twist); @@ -340,12 +318,6 @@ mod tests { frame_id: "base_link", timestamp: 123456789, }, - 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), } @@ -406,24 +378,24 @@ mod tests { } #[test] - fn test_vehicle_velocity_conversion() { + fn test_add_vehicle_twist() { let config = get_config_with_default_params(); - let core = GyroOdometerCore::new(config).unwrap(); + let mut core = GyroOdometerCore::new(config).unwrap(); let sample_twist = generate_sample_velocity(); assert_eq!(sample_twist.header.frame_id, "base_link"); assert_eq!(sample_twist.twist.twist.linear.x, 1.0); - let odometry = Odometry { - velocity: sample_twist.twist.twist.linear.x, - }; - let twist = core.convert_vehicle_velocity_to_twist(&odometry, sample_twist.header.timestamp); + core.add_vehicle_twist(sample_twist.clone()); - assert_eq!(twist.header.frame_id, sample_twist.header.frame_id); - assert_eq!(twist.header.timestamp, 123456789); - assert_eq!(twist.twist.twist.linear.x, 1.0); - assert_eq!(twist.twist.twist.linear.y, 0.0); - assert_eq!(twist.twist.twist.linear.z, 0.0); + assert_eq!(core.get_queue_sizes(), (1, 0)); + assert!(core.vehicle_twist_arrived); + let queued_twist = core.vehicle_twist_queue.front().unwrap(); + assert_eq!(queued_twist.header.frame_id, sample_twist.header.frame_id); + assert_eq!(queued_twist.header.timestamp, sample_twist.header.timestamp); + assert_eq!(queued_twist.twist.twist.linear.x, 1.0); + assert_eq!(queued_twist.twist.twist.linear.y, 0.0); + assert_eq!(queued_twist.twist.twist.linear.z, 0.0); } #[test] @@ -435,8 +407,17 @@ mod tests { let mut imu_with_cov = ImuWithCovariance::from_imu_msg(&imu_msg); imu_with_cov.angular_velocity_covariance = [0.0009, 0.0, 0.0, 0.0, 0.0009, 0.0, 0.0, 0.0, 0.0009]; - imu_with_cov.linear_acceleration_covariance = - [100000000.0, 0.0, 0.0, 0.0, 100000000.0, 0.0, 0.0, 0.0, 100000000.0]; + imu_with_cov.linear_acceleration_covariance = [ + 100000000.0, + 0.0, + 0.0, + 0.0, + 100000000.0, + 0.0, + 0.0, + 0.0, + 100000000.0, + ]; let result = core.process_imu_with_covariance(imu_with_cov); assert!(result.is_ok()); @@ -470,9 +451,7 @@ pub fn get_or_initialize() -> Result<&'static mut GyroOdometerCore> { AtomicOrdering::Acquire, AtomicOrdering::Relaxed, ) { - Ok(_) => { - Ok(unsafe { &mut *new_ptr }) - } + Ok(_) => Ok(unsafe { &mut *new_ptr }), Err(existing_ptr) => { unsafe { let _ = alloc::boxed::Box::from_raw(new_ptr);