diff --git a/applications/tests/test_autoware/Cargo.toml b/applications/tests/test_autoware/Cargo.toml new file mode 100644 index 000000000..048eec8d5 --- /dev/null +++ b/applications/tests/test_autoware/Cargo.toml @@ -0,0 +1,21 @@ +[package] +name = "test_autoware" +version = "0.1.0" +edition = "2021" + +[lib] +path = "src/lib.rs" +crate-type = ["rlib"] + +[dependencies] +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 } +common_types = { path = "./common_types", 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} +ekf_localizer = { path = "./ekf_localizer", default-features = false} diff --git a/applications/tests/test_autoware/build.rs b/applications/tests/test_autoware/build.rs new file mode 100644 index 000000000..7269fdc9f --- /dev/null +++ b/applications/tests/test_autoware/build.rs @@ -0,0 +1,31 @@ +use std::{env, fs, path::PathBuf}; + +fn main() { + let out_dir = PathBuf::from(env::var("OUT_DIR").expect("OUT_DIR is not set")); + let csv_data_path = out_dir.join("csv_data.rs"); + + let imu_csv = read_csv_from_env("IMU_CSV_PATH"); + let velocity_csv = read_csv_from_env("VELOCITY_CSV_PATH"); + + let generated = format!( + "pub const IMU_CSV_DATA_STR: &str = {imu:?};\npub const VELOCITY_CSV_DATA_STR: &str = {velocity:?};\n", + imu = imu_csv, + velocity = velocity_csv, + ); + + fs::write(&csv_data_path, generated).expect("failed to write generated csv constants"); +} + +fn read_csv_from_env(var_name: &str) -> String { + let Ok(path) = env::var(var_name) else { + return String::new(); + }; + + match fs::read_to_string(&path) { + Ok(contents) => contents, + Err(err) => { + println!("cargo:warning=failed to read {var_name} from {path}: {err}"); + String::new() + } + } +} 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/ekf_localizer/Cargo.toml b/applications/tests/test_autoware/ekf_localizer/Cargo.toml new file mode 100644 index 000000000..e2b8866b6 --- /dev/null +++ b/applications/tests/test_autoware/ekf_localizer/Cargo.toml @@ -0,0 +1,11 @@ +[package] +name = "ekf_localizer" +version = "0.1.0" +edition = "2021" + +[dependencies] +libm = "0.2" +nalgebra = { version = "0.32", default-features = false} +approx = "0.5" +common_types = { path = "../common_types", 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/ekf_localizer/src/lib.rs b/applications/tests/test_autoware/ekf_localizer/src/lib.rs new file mode 100644 index 000000000..e07f1223d --- /dev/null +++ b/applications/tests/test_autoware/ekf_localizer/src/lib.rs @@ -0,0 +1,646 @@ +#![no_std] +#![allow(non_snake_case)] + +extern crate alloc; + +use alloc::{vec, vec::Vec}; +pub use common_types::Header; +use core::ptr::null_mut; +use core::sync::atomic::{AtomicPtr, Ordering as AtomicOrdering}; +use libm::{atan2, cos, sin}; +use nalgebra::{Matrix6, Vector3, Vector6}; +pub use vehicle_velocity_converter::TwistWithCovariance; + +static EKF_MODULE_INSTANCE: AtomicPtr = AtomicPtr::new(null_mut()); + +#[derive(Debug, Clone, Copy, PartialEq)] +pub enum StateIndex { + X = 0, + Y = 1, + Yaw = 2, + YawBias = 3, + Vx = 4, + Wz = 5, +} + +pub type StateVector = Vector6; +pub type StateCovariance = Matrix6; + +#[derive(Debug, Clone, Copy)] +pub struct Point3D { + pub x: f64, + pub y: f64, + pub z: f64, +} + +#[derive(Debug, Clone, Copy)] +pub struct Quaternion { + pub x: f64, + pub y: f64, + pub z: f64, + pub w: f64, +} + +#[derive(Debug, Clone, Copy)] +pub struct Pose { + pub position: Point3D, + pub orientation: Quaternion, +} + +#[derive(Debug, Clone, Copy)] +pub struct Twist { + pub linear: Vector3, + pub angular: Vector3, +} + +#[derive(Debug, Clone, Copy)] +pub struct PoseWithCovariance { + pub pose: Pose, + pub covariance: [f64; 36], +} + +#[derive(Debug, Clone)] +pub struct EKFOdometry { + pub header: common_types::Header, + pub child_frame_id: &'static str, + pub pose: PoseWithCovariance, + pub twist: TwistWithCovariance, +} + +#[derive(Debug, Clone)] +pub struct EKFParameters { + pub enable_yaw_bias_estimation: bool, + pub extend_state_step: usize, + pub proc_stddev_vx_c: f64, + pub proc_stddev_wz_c: f64, + pub proc_stddev_yaw_c: f64, + pub z_filter_proc_dev: f64, + pub roll_filter_proc_dev: f64, + pub pitch_filter_proc_dev: f64, +} + +impl Default for EKFParameters { + fn default() -> Self { + Self { + enable_yaw_bias_estimation: true, + extend_state_step: 50, + proc_stddev_vx_c: 2.0, + proc_stddev_wz_c: 1.0, + proc_stddev_yaw_c: 0.005, + z_filter_proc_dev: 1.0, + roll_filter_proc_dev: 0.1, + pitch_filter_proc_dev: 0.1, + } + } +} + +#[derive(Debug, Clone)] +pub struct Simple1DFilter { + initialized: bool, + x: f64, + var: f64, + proc_var_x_c: f64, +} + +impl Default for Simple1DFilter { + fn default() -> Self { + Self::new() + } +} + +impl Simple1DFilter { + pub fn new() -> Self { + Self { + initialized: false, + x: 0.0, + var: 1e9, + proc_var_x_c: 0.0, + } + } + + pub fn init(&mut self, init_obs: f64, obs_var: f64) { + self.x = init_obs; + self.var = obs_var; + self.initialized = true; + } + + pub fn update(&mut self, obs: f64, obs_var: f64, dt: f64) { + if !self.initialized { + self.init(obs, obs_var); + return; + } + + let proc_var_x_d = self.proc_var_x_c * dt * dt; + self.var += proc_var_x_d; + + let kalman_gain = self.var / (self.var + obs_var); + self.x += kalman_gain * (obs - self.x); + self.var *= 1.0 - kalman_gain; + } + + pub fn set_proc_var(&mut self, proc_var: f64) { + self.proc_var_x_c = proc_var; + } + + pub fn get_x(&self) -> f64 { + self.x + } + + pub fn get_var(&self) -> f64 { + self.var + } +} + +#[derive(Debug, Clone)] +pub struct EKFModule { + params: EKFParameters, + state: StateVector, + covariance: StateCovariance, + z_filter: Simple1DFilter, + roll_filter: Simple1DFilter, + pitch_filter: Simple1DFilter, + accumulated_delay_times: Vec, + // When true, only prediction is performed (no measurement updates) + is_mrm_mode: bool, +} + +impl EKFModule { + pub fn new(params: EKFParameters) -> Self { + let state = StateVector::zeros(); + let mut covariance = StateCovariance::identity() * 1e15; + + covariance[(StateIndex::Yaw as usize, StateIndex::Yaw as usize)] = 50.0; + if params.enable_yaw_bias_estimation { + covariance[(StateIndex::YawBias as usize, StateIndex::YawBias as usize)] = 50.0; + } + covariance[(StateIndex::Vx as usize, StateIndex::Vx as usize)] = 1000.0; + covariance[(StateIndex::Wz as usize, StateIndex::Wz as usize)] = 50.0; + + let mut z_filter = Simple1DFilter::new(); + let mut roll_filter = Simple1DFilter::new(); + let mut pitch_filter = Simple1DFilter::new(); + + z_filter.set_proc_var(params.z_filter_proc_dev * params.z_filter_proc_dev); + roll_filter.set_proc_var(params.roll_filter_proc_dev * params.roll_filter_proc_dev); + pitch_filter.set_proc_var(params.pitch_filter_proc_dev * params.pitch_filter_proc_dev); + + let accumulated_delay_times = vec![1e15; params.extend_state_step]; + + Self { + params, + state, + covariance, + z_filter, + roll_filter, + pitch_filter, + accumulated_delay_times, + is_mrm_mode: false, + } + } + + pub fn initialize(&mut self, initial_pose: Pose) { + self.state[StateIndex::X as usize] = initial_pose.position.x; + self.state[StateIndex::Y as usize] = initial_pose.position.y; + self.state[StateIndex::Yaw as usize] = self.quaternion_to_yaw(initial_pose.orientation); + self.state[StateIndex::YawBias as usize] = 0.0; + self.state[StateIndex::Vx as usize] = 0.0; + self.state[StateIndex::Wz as usize] = 0.0; + + self.covariance = StateCovariance::identity() * 0.01; + if self.params.enable_yaw_bias_estimation { + self.covariance[(StateIndex::YawBias as usize, StateIndex::YawBias as usize)] = 0.0001; + } + + self.z_filter.init(initial_pose.position.z, 0.01); + self.roll_filter.init(0.0, 0.01); + self.pitch_filter.init(0.0, 0.01); + } + + fn predict_next_state(&self, dt: f64) -> StateVector { + let mut x_next = self.state; + let x = self.state[StateIndex::X as usize]; + let y = self.state[StateIndex::Y as usize]; + let yaw = self.state[StateIndex::Yaw as usize]; + let yaw_bias = self.state[StateIndex::YawBias as usize]; + let vx = self.state[StateIndex::Vx as usize]; + let wz = self.state[StateIndex::Wz as usize]; + + x_next[StateIndex::X as usize] = x + vx * cos(yaw + yaw_bias) * dt; + x_next[StateIndex::Y as usize] = y + vx * sin(yaw + yaw_bias) * dt; + let yaw_next = yaw + wz * dt; + x_next[StateIndex::Yaw as usize] = atan2(sin(yaw_next), cos(yaw_next)); + x_next[StateIndex::YawBias as usize] = yaw_bias; + x_next[StateIndex::Vx as usize] = vx; + x_next[StateIndex::Wz as usize] = wz; + + x_next + } + + fn create_state_transition_matrix(&self, dt: f64) -> Matrix6 { + let mut F = Matrix6::identity(); + let yaw = self.state[StateIndex::Yaw as usize]; + let yaw_bias = self.state[StateIndex::YawBias as usize]; + let vx = self.state[StateIndex::Vx as usize]; + + F[(StateIndex::X as usize, StateIndex::Yaw as usize)] = -vx * sin(yaw + yaw_bias) * dt; + F[(StateIndex::X as usize, StateIndex::YawBias as usize)] = -vx * sin(yaw + yaw_bias) * dt; + F[(StateIndex::X as usize, StateIndex::Vx as usize)] = cos(yaw + yaw_bias) * dt; + + F[(StateIndex::Y as usize, StateIndex::Yaw as usize)] = vx * cos(yaw + yaw_bias) * dt; + F[(StateIndex::Y as usize, StateIndex::YawBias as usize)] = vx * cos(yaw + yaw_bias) * dt; + F[(StateIndex::Y as usize, StateIndex::Vx as usize)] = sin(yaw + yaw_bias) * dt; + + F[(StateIndex::Yaw as usize, StateIndex::Wz as usize)] = dt; + + F + } + + fn process_noise_covariance(&self, dt: f64) -> Matrix6 { + let mut Q = Matrix6::zeros(); + + Q[(StateIndex::Vx as usize, StateIndex::Vx as usize)] = + self.params.proc_stddev_vx_c * self.params.proc_stddev_vx_c * dt * dt; + Q[(StateIndex::Wz as usize, StateIndex::Wz as usize)] = + self.params.proc_stddev_wz_c * self.params.proc_stddev_wz_c * dt * dt; + Q[(StateIndex::Yaw as usize, StateIndex::Yaw as usize)] = + self.params.proc_stddev_yaw_c * self.params.proc_stddev_yaw_c * dt * dt; + + Q[(StateIndex::X as usize, StateIndex::X as usize)] = 0.0; + Q[(StateIndex::Y as usize, StateIndex::Y as usize)] = 0.0; + Q[(StateIndex::YawBias as usize, StateIndex::YawBias as usize)] = 0.0; + + Q + } + + pub fn predict(&mut self, dt: f64) { + self.state = self.predict_next_state(dt); + let F = self.create_state_transition_matrix(dt); + let Q = self.process_noise_covariance(dt); + self.covariance = F * self.covariance * F.transpose() + Q; + self.accumulate_delay_time(dt); + } + + pub fn predict_with_delay(&mut self, dt: f64) { + self.predict(dt); + } + + pub fn predict_only(&mut self, dt: f64) { + self.predict(dt); + } + + pub fn get_current_pose(&self, get_biased_yaw: bool) -> Pose { + let z = self.z_filter.get_x(); + let roll = self.roll_filter.get_x(); + let pitch = self.pitch_filter.get_x(); + + let x = self.state[StateIndex::X as usize]; + let y = self.state[StateIndex::Y as usize]; + let biased_yaw = self.state[StateIndex::Yaw as usize]; + let yaw_bias = self.state[StateIndex::YawBias as usize]; + + let yaw = if get_biased_yaw { + biased_yaw + } else { + biased_yaw + yaw_bias + }; + + Pose { + position: Point3D { x, y, z }, + orientation: self.rpy_to_quaternion(roll, pitch, yaw), + } + } + + pub fn get_current_twist(&self) -> Twist { + let vx = self.state[StateIndex::Vx as usize]; + let wz = self.state[StateIndex::Wz as usize]; + + Twist { + linear: Vector3::new(vx, 0.0, 0.0), + angular: Vector3::new(0.0, 0.0, wz), + } + } + + pub fn get_yaw_bias(&self) -> f64 { + self.state[StateIndex::YawBias as usize] + } + + pub fn get_current_pose_with_covariance(&self) -> PoseWithCovariance { + let pose = self.get_current_pose(false); + let pose_covariance = self.get_current_pose_covariance(); + PoseWithCovariance { + pose, + covariance: pose_covariance, + } + } + + pub fn get_current_pose_covariance(&self) -> [f64; 36] { + let mut cov = [0.0; 36]; + + for i in 0..6 { + for j in 0..6 { + cov[i * 6 + j] = self.covariance[(i, j)]; + } + } + + cov[14] = self.z_filter.get_var(); + cov[21] = self.roll_filter.get_var(); + cov[28] = self.pitch_filter.get_var(); + + cov + } + + pub fn get_current_twist_covariance(&self) -> [f64; 36] { + let mut cov = [0.0; 36]; + + cov[0] = self.covariance[(StateIndex::Vx as usize, StateIndex::Vx as usize)]; + cov[35] = self.covariance[(StateIndex::Wz as usize, StateIndex::Wz as usize)]; + + cov + } + + pub fn update_velocity(&mut self, vx_measurement: f64, wz_measurement: f64) { + if self.is_mrm_mode { + return; + } + + let vx_obs_var = 1.0; + let wz_obs_var = 0.1; + + let vx_var = self.covariance[(StateIndex::Vx as usize, StateIndex::Vx as usize)]; + let vx_gain = vx_var / (vx_var + vx_obs_var); + self.state[StateIndex::Vx as usize] = self.state[StateIndex::Vx as usize] + + vx_gain * (vx_measurement - self.state[StateIndex::Vx as usize]); + self.covariance[(StateIndex::Vx as usize, StateIndex::Vx as usize)] = + (1.0 - vx_gain) * vx_var; + + let wz_var = self.covariance[(StateIndex::Wz as usize, StateIndex::Wz as usize)]; + let wz_gain = wz_var / (wz_var + wz_obs_var); + self.state[StateIndex::Wz as usize] = self.state[StateIndex::Wz as usize] + + wz_gain * (wz_measurement - self.state[StateIndex::Wz as usize]); + self.covariance[(StateIndex::Wz as usize, StateIndex::Wz as usize)] = + (1.0 - wz_gain) * wz_var; + } + + pub fn set_mrm_mode(&mut self, is_mrm: bool) { + self.is_mrm_mode = is_mrm; + } + + pub fn is_mrm(&self) -> bool { + self.is_mrm_mode + } + + pub fn accumulate_delay_time(&mut self, dt: f64) { + let len = self.accumulated_delay_times.len(); + if len == 0 { + return; + } + + let last_time = self.accumulated_delay_times[len - 1]; + let new_time = last_time + dt; + + for i in 0..len - 1 { + self.accumulated_delay_times[i] = self.accumulated_delay_times[i + 1]; + } + self.accumulated_delay_times[len - 1] = new_time; + } + + pub fn find_closest_delay_time_index(&self, target_value: f64) -> usize { + let len = self.accumulated_delay_times.len(); + if len == 0 { + return 0; + } + + if target_value > self.accumulated_delay_times[len - 1] { + return len; + } + + let mut closest_index = 0; + let mut min_diff = f64::MAX; + + for i in 0..len { + let time = self.accumulated_delay_times[i]; + let diff = (target_value - time).abs(); + if diff < min_diff { + min_diff = diff; + closest_index = i; + } + } + + closest_index + } + + fn quaternion_to_yaw(&self, q: Quaternion) -> f64 { + atan2( + 2.0 * (q.w * q.z + q.x * q.y), + 1.0 - 2.0 * (q.y * q.y + q.z * q.z), + ) + } + + fn rpy_to_quaternion(&self, roll: f64, pitch: f64, yaw: f64) -> Quaternion { + let cy = cos(yaw * 0.5); + let sy = sin(yaw * 0.5); + let cp = cos(pitch * 0.5); + let sp = sin(pitch * 0.5); + let cr = cos(roll * 0.5); + let sr = sin(roll * 0.5); + + Quaternion { + w: cr * cp * cy + sr * sp * sy, + x: sr * cp * cy - cr * sp * sy, + y: cr * sp * cy + sr * cp * sy, + z: cr * cp * sy - sr * sp * cy, + } + } +} + +pub fn get_or_initialize_default_module() -> &'static mut EKFModule { + let existing = EKF_MODULE_INSTANCE.load(AtomicOrdering::Acquire); + if !existing.is_null() { + return unsafe { &mut *existing }; + } + + let boxed = alloc::boxed::Box::new(EKFModule::new(EKFParameters::default())); + let ptr = alloc::boxed::Box::into_raw(boxed); + + match EKF_MODULE_INSTANCE.compare_exchange( + null_mut(), + ptr, + AtomicOrdering::AcqRel, + AtomicOrdering::Acquire, + ) { + Ok(_) => unsafe { &mut *ptr }, + Err(existing_ptr) => unsafe { + let _ = alloc::boxed::Box::from_raw(ptr); + &mut *existing_ptr + }, + } +} + +#[cfg(test)] +mod tests { + use super::*; + use core::f64::consts::PI; + use nalgebra::{Matrix6, Vector6}; + + #[test] + fn predict_next_state_matches_formula() { + let params = EKFParameters::default(); + let mut ekf = EKFModule::new(params); + + let x_curr = Vector6::new(2.0, 3.0, PI / 2.0, PI / 4.0, 10.0, 2.0 * PI / 3.0); + + ekf.state = x_curr.clone(); + + let dt = 0.5; + let x_next = ekf.predict_next_state(dt); + + let tol = 1e-10; + assert!((x_next[0] - (2.0 + 10.0 * (PI / 2.0 + PI / 4.0).cos() * dt)).abs() < tol); + assert!((x_next[1] - (3.0 + 10.0 * (PI / 2.0 + PI / 4.0).sin() * dt)).abs() < tol); + let yaw_next = PI / 2.0 + (2.0 * PI / 3.0) * dt; + let expected_yaw = yaw_next.sin().atan2(yaw_next.cos()); + assert!((x_next[2] - expected_yaw).abs() < 1e-6); + assert!((x_next[3] - x_curr[3]).abs() < tol); + assert!((x_next[4] - x_curr[4]).abs() < tol); + assert!((x_next[5] - x_curr[5]).abs() < tol); + } + + #[test] + fn create_state_transition_matrix_numeric_approximation() { + let params = EKFParameters::default(); + let mut ekf = EKFModule::new(params); + + // check around zero + let dt = 0.1; + let dx = Vector6::from_element(0.1); + let x = Vector6::zeros(); + + ekf.state = x.clone(); + let a = ekf.create_state_transition_matrix(dt); + + ekf.state = x.clone() + dx.clone(); + let x1 = ekf.predict_next_state(dt); + ekf.state = x.clone(); + let x0 = ekf.predict_next_state(dt); + let df = x1 - x0; + + { + let mut s = 0.0; + let v = df - a * dx; + for i in 0..6 { + let val = v[i]; + s += val * val; + } + assert!(s.sqrt() < 2e-3); + } + + // check around a non-zero state + let dx = Vector6::from_element(0.1); + let x = Vector6::new(0.1, 0.2, 0.1, 0.4, 0.1, 0.3); + + ekf.state = x.clone(); + let a = ekf.create_state_transition_matrix(dt); + + ekf.state = x.clone() + dx.clone(); + let x1 = ekf.predict_next_state(dt); + ekf.state = x.clone(); + let x0 = ekf.predict_next_state(dt); + let df = x1 - x0; + + { + let mut s = 0.0; + let v = df - a * dx; + for i in 0..6 { + let val = v[i]; + s += val * val; + } + assert!(s.sqrt() < 5e-3); + } + } + + #[test] + fn process_noise_covariance_values() { + let mut params = EKFParameters::default(); + params.proc_stddev_yaw_c = 1.0; + params.proc_stddev_vx_c = 2.0; + params.proc_stddev_wz_c = 3.0; + + let ekf = EKFModule::new(params); + + let q = ekf.process_noise_covariance(1.0); + + // indices: yaw = 2, vx = 4, wz = 5 + assert!((q[(2, 2)] - 1.0_f64.powi(2)).abs() < 1e-12); + assert!((q[(4, 4)] - 2.0_f64.powi(2)).abs() < 1e-12); + assert!((q[(5, 5)] - 3.0_f64.powi(2)).abs() < 1e-12); + + // zero case + let mut params = EKFParameters::default(); + params.proc_stddev_yaw_c = 0.0; + params.proc_stddev_vx_c = 0.0; + params.proc_stddev_wz_c = 0.0; + let ekf2 = EKFModule::new(params); + let q2 = ekf2.process_noise_covariance(1.0); + { + let mut s = 0.0; + for i in 0..6 { + for j in 0..6 { + let val = q2[(i, j)]; + s += val * val; + } + } + assert!(s == 0.0); + } + } + + #[test] + fn pose_and_twist_covariance_mapping() { + let params = EKFParameters::default(); + let mut ekf = EKFModule::new(params); + + // prepare a covariance matrix with top-left 3x3 = 1..9 + let mut p = Matrix6::::zeros(); + p[(0, 0)] = 1.0; + p[(0, 1)] = 2.0; + p[(0, 2)] = 3.0; + p[(1, 0)] = 4.0; + p[(1, 1)] = 5.0; + p[(1, 2)] = 6.0; + p[(2, 0)] = 7.0; + p[(2, 1)] = 8.0; + p[(2, 2)] = 9.0; + + ekf.covariance = p; + + // override the filter variances so those indices are replaced + ekf.z_filter.var = 100.0; + ekf.roll_filter.var = 200.0; + ekf.pitch_filter.var = 300.0; + + let cov = ekf.get_current_pose_covariance(); + + // check a few mapped entries according to get_current_pose_covariance implementation + assert_eq!(cov[0], 1.0); + assert_eq!(cov[1], 2.0); + assert_eq!(cov[2], 3.0); + assert_eq!(cov[6], 4.0); + assert_eq!(cov[7], 5.0); + assert_eq!(cov[8], 6.0); + assert_eq!(cov[12], 7.0); + assert_eq!(cov[13], 8.0); + // index 14 is overwritten by z_filter.var + assert_eq!(cov[14], 100.0); + + // twist covariance mapping (Vx -> index 0, Wz -> index 35) + let mut p2 = Matrix6::::zeros(); + p2[(4, 4)] = 1.0; + p2[(4, 5)] = 2.0; + p2[(5, 4)] = 3.0; + p2[(5, 5)] = 4.0; + ekf.covariance = p2; + + let tcov = ekf.get_current_twist_covariance(); + assert_eq!(tcov[0], 1.0); + assert_eq!(tcov[35], 4.0); + } +} 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..6916a16ae --- /dev/null +++ b/applications/tests/test_autoware/gyro_odometer/src/lib.rs @@ -0,0 +1,464 @@ +#![no_std] +extern crate alloc; + +use alloc::{collections::VecDeque, string::String}; +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, +}; + +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]; + } + 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 { + let _ = (from_frame, to_frame); + 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 { + self.concat_gyro_and_odometer(current_time) + .unwrap_or_default() + } + + 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 }) + } + } +} 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..0397eed62 --- /dev/null +++ b/applications/tests/test_autoware/imu_corrector/Cargo.toml @@ -0,0 +1,8 @@ +[package] +name = "imu_corrector" +version = "0.1.0" +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 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..edeb4c134 --- /dev/null +++ b/applications/tests/test_autoware/imu_corrector/src/lib.rs @@ -0,0 +1,494 @@ +#![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(0.0, 0.0, 0.0), + rotation: imu_driver::Quaternion { + x: 0.0, + y: 0.0, + z: 0.0, + w: 1.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 Default for MockTransformListener { + fn default() -> Self { + Self::new() + } +} + +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 Default for ImuCorrector { + fn default() -> Self { + Self::new() + } +} + +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 rotated = nalgebra_quat * nalgebra_vec; + self.to_imu_vector3(&rotated) + } + + 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 assert_almost_eq(actual: f64, expected: f64) { + assert!((actual - expected).abs() < 1e-10); + } + + 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.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_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] + 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, 9.8); + assert_eq!(corrected_msg.linear_acceleration.y, 0.0); + assert_eq!(corrected_msg.linear_acceleration.z, 0.0); + } + + #[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()); + } +} diff --git a/applications/tests/test_autoware/imu_driver/Cargo.toml b/applications/tests/test_autoware/imu_driver/Cargo.toml new file mode 100644 index 000000000..ceec72d7a --- /dev/null +++ b/applications/tests/test_autoware/imu_driver/Cargo.toml @@ -0,0 +1,7 @@ +[package] +name = "imu_driver" +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 new file mode 100644 index 000000000..15ef58e24 --- /dev/null +++ b/applications/tests/test_autoware/imu_driver/src/lib.rs @@ -0,0 +1,339 @@ +#![no_std] +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, + pub orientation: Quaternion, + pub angular_velocity: Vector3, + pub linear_acceleration: Vector3, +} + +#[derive(Clone, Debug)] +pub struct ImuCsvRow { + pub timestamp: u64, + pub orientation: Quaternion, + pub angular_velocity: Vector3, + pub linear_acceleration: Vector3, +} + +#[derive(Debug, Clone)] +pub struct Quaternion { + pub x: f64, + pub y: f64, + pub z: f64, + pub w: f64, +} + +impl Default for ImuMsg { + fn default() -> Self { + Self { + header: Header { + 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), + } + } +} + +pub fn build_imu_msg_from_csv_row( + row: &ImuCsvRow, + frame_id: &'static str, + timestamp: u64, +) -> ImuMsg { + ImuMsg { + header: Header { + frame_id, + timestamp, + }, + orientation: row.orientation.clone(), + angular_velocity: row.angular_velocity.clone(), + linear_acceleration: row.linear_acceleration.clone(), + } +} + +pub struct TamagawaImuParser { + frame_id: &'static str, + dummy_counter: u16, +} + +impl TamagawaImuParser { + pub fn new(frame_id: &'static str) -> Self { + Self { + frame_id, + dummy_counter: 0, + } + } + + pub fn parse_binary_data(&self, data: &[u8], timestamp: u64) -> Option { + if data.len() != 58 || data[0..9] != *b"$TSC,BIN," { + return None; + } + + let mut imu_msg = ImuMsg::default(); + imu_msg.header.frame_id = self.frame_id; + imu_msg.header.timestamp = timestamp; + + let _counter = ((data[11] as u16) << 8) | (data[12] as u16); + let raw_data = self.parse_signed_16bit(&data[15..17]); + imu_msg.angular_velocity.x = self.convert_angular_velocity(raw_data); + let raw_data = self.parse_signed_16bit(&data[17..19]); + imu_msg.angular_velocity.y = self.convert_angular_velocity(raw_data); + let raw_data = self.parse_signed_16bit(&data[19..21]); + imu_msg.angular_velocity.z = self.convert_angular_velocity(raw_data); + let raw_data = self.parse_signed_16bit(&data[21..23]); + imu_msg.linear_acceleration.x = self.convert_acceleration(raw_data); + let raw_data = self.parse_signed_16bit(&data[23..25]); + imu_msg.linear_acceleration.y = self.convert_acceleration(raw_data); + let raw_data = self.parse_signed_16bit(&data[25..27]); + imu_msg.linear_acceleration.z = self.convert_acceleration(raw_data); + + Some(imu_msg) + } + + pub fn generate_dummy_binary_data( + &mut self, + _timestamp: u64, + angular_velocity: Vector3, + linear_acceleration: Vector3, + ) -> Vec { + let mut data = vec![0u8; 58]; + + data[0..5].copy_from_slice(b"$TSC,"); + data[5] = b'B'; + data[6] = b'I'; + data[7] = b'N'; + data[8] = b','; + data[11] = (self.dummy_counter >> 8) as u8; + data[12] = (self.dummy_counter & 0xFF) as u8; + self.dummy_counter = self.dummy_counter.wrapping_add(1); + + let angular_vel_x_lsb = self.convert_angular_velocity_to_lsb(angular_velocity.x); + let angular_vel_y_lsb = self.convert_angular_velocity_to_lsb(angular_velocity.y); + let angular_vel_z_lsb = self.convert_angular_velocity_to_lsb(angular_velocity.z); + let accel_x_lsb = self.convert_acceleration_to_lsb(linear_acceleration.x); + let accel_y_lsb = self.convert_acceleration_to_lsb(linear_acceleration.y); + let accel_z_lsb = self.convert_acceleration_to_lsb(linear_acceleration.z); + + data[15] = (angular_vel_x_lsb >> 8) as u8; + data[16] = (angular_vel_x_lsb & 0xFF) as u8; + data[17] = (angular_vel_y_lsb >> 8) as u8; + data[18] = (angular_vel_y_lsb & 0xFF) as u8; + data[19] = (angular_vel_z_lsb >> 8) as u8; + data[20] = (angular_vel_z_lsb & 0xFF) as u8; + data[21] = (accel_x_lsb >> 8) as u8; + data[22] = (accel_x_lsb & 0xFF) as u8; + data[23] = (accel_y_lsb >> 8) as u8; + data[24] = (accel_y_lsb & 0xFF) as u8; + data[25] = (accel_z_lsb >> 8) as u8; + data[26] = (accel_z_lsb & 0xFF) as u8; + + data + } + + pub fn generate_static_dummy_data(&mut self, timestamp: u64) -> Vec { + let angular_velocity = Vector3::new(0.1, 0.2, 0.01); + let linear_acceleration = Vector3::new(0.0, 0.0, 9.80665); + + self.generate_dummy_binary_data(timestamp, angular_velocity, linear_acceleration) + } + + fn convert_angular_velocity_to_lsb(&self, rad_per_sec: f64) -> i16 { + let deg_per_sec = rad_per_sec * 180.0 / PI; + let lsb = deg_per_sec / (200.0 / (1 << 15) as f64); + lsb as i16 + } + + fn convert_acceleration_to_lsb(&self, m_per_sec_squared: f64) -> i16 { + let lsb = m_per_sec_squared / (100.0 / (1 << 15) as f64); + lsb as i16 + } + + fn parse_signed_16bit(&self, data: &[u8]) -> i16 { + if data.len() != 2 { + return 0; + } + let high_byte = (data[0] as i32) << 8; + let low_byte = data[1] as i32; + let result = (high_byte & 0xFFFFFF00u32 as i32) | (low_byte & 0x000000FF); + result as i16 + } + + fn convert_angular_velocity(&self, raw_data: i16) -> f64 { + let lsb_to_deg_per_sec = 200.0 / (1 << 15) as f64; + let deg_to_rad = PI / 180.0; + (raw_data as f64) * lsb_to_deg_per_sec * deg_to_rad + } + + fn convert_acceleration(&self, raw_data: i16) -> f64 { + let lsb_to_m_per_sec_squared = 100.0 / (1 << 15) as f64; + (raw_data as f64) * lsb_to_m_per_sec_squared + } + + pub fn generate_version_request() -> &'static [u8] { + b"$TSC,VER*29\r\n" + } + + pub fn generate_offset_cancel_request(offset_value: i32) -> String { + format!("$TSC,OFC,{}\r\n", offset_value) + } + + pub fn generate_heading_reset_request() -> &'static [u8] { + b"$TSC,HRST*29\r\n" + } + + pub fn generate_binary_request(rate_hz: u32) -> String { + format!("$TSC,BIN,{}\r\n", rate_hz) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use core::f64::consts::PI; + + fn assert_approx_eq(actual: f64, expected: f64, eps: f64) { + assert!( + (actual - expected).abs() <= eps, + "left: {actual}, right: {expected}" + ); + } + + fn expected_ang_vel(raw: i16) -> f64 { + let lsb_to_deg_per_sec = 200.0 / (1 << 15) as f64; + let deg_to_rad = PI / 180.0; + (raw as f64) * lsb_to_deg_per_sec * deg_to_rad + } + + fn expected_acc(raw: i16) -> f64 { + let lsb_to_m_per_sec_squared = 100.0 / (1 << 15) as f64; + (raw as f64) * lsb_to_m_per_sec_squared + } + + fn put_i16_be(buf: &mut [u8], value: i16) { + let raw = value as u16; + buf[0] = (raw >> 8) as u8; + buf[1] = (raw & 0xFF) as u8; + } + + #[test] + fn parse_rejects_invalid_length() { + let parser = TamagawaImuParser::new("imu_link"); + let data = [0u8; 57]; + + assert!(parser.parse_binary_data(&data, 1).is_none()); + } + + #[test] + fn parse_rejects_non_bin_header() { + let parser = TamagawaImuParser::new("imu_link"); + let mut data = [0u8; 58]; + data[0..5].copy_from_slice(b"$TSC,"); + data[5..9].copy_from_slice(b"XIN,"); + + assert!(parser.parse_binary_data(&data, 1).is_none()); + } + + #[test] + fn parse_rejects_invalid_tsc_prefix() { + let parser = TamagawaImuParser::new("imu_link"); + let mut data = [0u8; 58]; + data[0..9].copy_from_slice(b"@TSC,BIN,"); + + assert!(parser.parse_binary_data(&data, 1).is_none()); + } + + #[test] + fn parse_extracts_fields_and_converts_units() { + let parser = TamagawaImuParser::new("imu_link"); + let timestamp = 123456789u64; + + let mut data = [0u8; 58]; + data[0..5].copy_from_slice(b"$TSC,"); + data[5..9].copy_from_slice(b"BIN,"); + + let gx: i16 = 1; + let gy: i16 = -2; + let gz: i16 = 1234; + let ax: i16 = -300; + let ay: i16 = 0; + let az: i16 = 32767; + + put_i16_be(&mut data[15..17], gx); + put_i16_be(&mut data[17..19], gy); + put_i16_be(&mut data[19..21], gz); + put_i16_be(&mut data[21..23], ax); + put_i16_be(&mut data[23..25], ay); + put_i16_be(&mut data[25..27], az); + + let imu = parser + .parse_binary_data(&data, timestamp) + .expect("should parse"); + + assert_eq!(imu.header.frame_id, "imu_link"); + assert_eq!(imu.header.timestamp, timestamp); + + let eps = 1e-12; + assert_approx_eq(imu.angular_velocity.x, expected_ang_vel(gx), eps); + assert_approx_eq(imu.angular_velocity.y, expected_ang_vel(gy), eps); + assert_approx_eq(imu.angular_velocity.z, expected_ang_vel(gz), eps); + + assert_approx_eq(imu.linear_acceleration.x, expected_acc(ax), eps); + assert_approx_eq(imu.linear_acceleration.y, expected_acc(ay), eps); + assert_approx_eq(imu.linear_acceleration.z, expected_acc(az), eps); + } + + #[test] + fn generate_dummy_data_roundtrip_is_close() { + let mut parser = TamagawaImuParser::new("imu_link"); + let timestamp = 42u64; + + let input_angular_velocity = Vector3::new(0.5, -0.3, 1.2); + let input_linear_acceleration = Vector3::new(8.5, 2.1, 10.2); + + let data = parser.generate_dummy_binary_data( + timestamp, + input_angular_velocity.clone(), + input_linear_acceleration.clone(), + ); + let imu = parser + .parse_binary_data(&data, timestamp) + .expect("should parse"); + + let ang_eps = (200.0 / (1 << 15) as f64) * (PI / 180.0); + let acc_eps = 100.0 / (1 << 15) as f64; + + assert_approx_eq(imu.angular_velocity.x, input_angular_velocity.x, ang_eps); + assert_approx_eq(imu.angular_velocity.y, input_angular_velocity.y, ang_eps); + assert_approx_eq(imu.angular_velocity.z, input_angular_velocity.z, ang_eps); + + assert_approx_eq( + imu.linear_acceleration.x, + input_linear_acceleration.x, + acc_eps, + ); + assert_approx_eq( + imu.linear_acceleration.y, + input_linear_acceleration.y, + acc_eps, + ); + assert_approx_eq( + imu.linear_acceleration.z, + input_linear_acceleration.z, + acc_eps, + ); + } +} diff --git a/applications/tests/test_autoware/src/lib.rs b/applications/tests/test_autoware/src/lib.rs new file mode 100644 index 000000000..37881b3fb --- /dev/null +++ b/applications/tests/test_autoware/src/lib.rs @@ -0,0 +1,667 @@ +#![no_std] +#![allow(static_mut_refs)] +extern crate alloc; + +use alloc::{borrow::Cow, format, string::String, vec, vec::Vec}; +use awkernel_async_lib::dag::{create_dag, finish_create_dags}; +use awkernel_async_lib::net::IpAddr; +use awkernel_async_lib::scheduler::SchedulerType; +use awkernel_lib::delay::wait_microsec; +use awkernel_lib::sync::mutex::{MCSNode, Mutex}; +use core::net::Ipv4Addr; +use core::sync::atomic::{AtomicBool, AtomicUsize, Ordering}; +use core::time::Duration; +use csv_core::{ReadRecordResult, Reader}; + +pub use common_types::Header; +use core::slice; +use ekf_localizer::{ + get_or_initialize_default_module, EKFOdometry, Point3D, Pose, PoseWithCovariance, Quaternion, +}; +use imu_corrector::{ImuCorrector, ImuWithCovariance}; +use imu_driver::{build_imu_msg_from_csv_row, ImuCsvRow, ImuMsg, TamagawaImuParser}; +use vehicle_velocity_converter::{ + build_velocity_report_from_csv_row, reactor_helpers, Twist, TwistWithCovariance, + TwistWithCovarianceStamped, VehicleVelocityConverter, VelocityCsvRow, +}; + +const LOG_ENABLE: bool = false; + +const INTERFACE_ID: u64 = 0; +const INTERFACE_ADDR: Ipv4Addr = Ipv4Addr::new(10, 0, 2, 64); +const UDP_TCP_DST_ADDR: Ipv4Addr = Ipv4Addr::new(10, 0, 2, 2); +const UDP_DST_PORT: u16 = 26099; + +static mut LATEST_JSON_DATA: Option = None; +static JSON_DATA_READY: AtomicBool = AtomicBool::new(false); +static JSON_DATA_LENGTH: AtomicUsize = AtomicUsize::new(0); + +include!(concat!(env!("OUT_DIR"), "/csv_data.rs")); + +static mut IMU_CSV_DATA: Option> = None; +static mut VELOCITY_CSV_DATA: Option> = None; +static IMU_CSV_COUNT: Mutex = Mutex::new(0); +static VELOCITY_CSV_COUNT: Mutex = Mutex::new(0); + +pub async fn run() { + wait_microsec(1000000); + + if let Err(e) = initialize_csv_data() { + log::warn!("Failed to initialize CSV data: {}", e); + } + + log::info!("Starting Autoware test application with simplified TCP networking"); + + let dag = create_dag(); + let _dag_id = dag.get_id(); + + dag.register_periodic_reactor::<_, (i32, i32, i32)>( + "start_dummy_data".into(), + move || -> (i32, i32, i32) { (1, 2, 3) }, + vec![ + Cow::from("start_imu"), + Cow::from("start_vel"), + Cow::from("start_pose"), + ], + SchedulerType::GEDF(5), + Duration::from_millis(50), + ) + .await; + + dag.register_reactor::<_, (i32,), (ImuMsg,)>( + "imu_driver".into(), + move |(_start_msg,): (i32,)| -> (ImuMsg,) { + let mut node = MCSNode::new(); + let mut count_guard = IMU_CSV_COUNT.lock(&mut node); + let count = *count_guard; + let data = unsafe { IMU_CSV_DATA.as_ref() }; + let awkernel_timestamp = get_awkernel_uptime_timestamp(); + + let imu_msg = if let Some(csv_data) = data { + if csv_data.is_empty() { + // Fallback: generate dummy IMU data + let mut parser = TamagawaImuParser::new("imu_link"); + let static_dummy_data = parser.generate_static_dummy_data(awkernel_timestamp); + parser + .parse_binary_data(&static_dummy_data, awkernel_timestamp) + .unwrap_or_default() + } else { + let idx = count % csv_data.len(); + let row = &csv_data[idx]; + build_imu_msg_from_csv_row(row, "imu_link", awkernel_timestamp) + } + } else { + // Fallback: generate dummy IMU data if data is not initialized + let mut parser = TamagawaImuParser::new("imu_link"); + let static_dummy_data = parser.generate_static_dummy_data(awkernel_timestamp); + parser + .parse_binary_data(&static_dummy_data, awkernel_timestamp) + .unwrap_or_default() + }; + + *count_guard += 1; + if *count_guard >= 5700 { + *count_guard = 0; + log::info!("rust_e2e_app: finish csv for IMU"); + loop { + wait_microsec(1000); + } + } + + if LOG_ENABLE { + log::debug!( + "IMU data in imu_driver_node,num={}, timestamp={}", + count, + imu_msg.header.timestamp + ); + } + + (imu_msg,) + }, + vec![Cow::from("start_imu")], + vec![Cow::from("imu_data")], + SchedulerType::GEDF(5), + ) + .await; + + dag.register_reactor::<_, (i32,), (TwistWithCovarianceStamped,)>( + "vehicle_velocity_converter".into(), + move |(_start_msg,): (i32,)| -> (TwistWithCovarianceStamped,) { + let converter = VehicleVelocityConverter::default(); + + let mut node = MCSNode::new(); + let mut count_guard = VELOCITY_CSV_COUNT.lock(&mut node); + let count = *count_guard; + let data = unsafe { VELOCITY_CSV_DATA.as_ref() }; + let awkernel_timestamp = get_awkernel_uptime_timestamp(); + + let twist_msg = if let Some(csv_data) = data { + if csv_data.is_empty() { + // Fallback: generate dummy velocity report + let dummy_report = vehicle_velocity_converter::VelocityReport { + header: common_types::Header { + frame_id: "base_link", + timestamp: awkernel_timestamp, + }, + longitudinal_velocity: 1.0, + lateral_velocity: 0.0, + heading_rate: 0.0, + }; + converter.convert_velocity_report(&dummy_report) + } else { + let idx = count % csv_data.len(); + let row = &csv_data[idx]; + let velocity_report = + build_velocity_report_from_csv_row(row, "base_link", awkernel_timestamp); + converter.convert_velocity_report(&velocity_report) + } + } else { + // Fallback: generate dummy velocity report if data is not initialized + let dummy_report = vehicle_velocity_converter::VelocityReport { + header: common_types::Header { + frame_id: "base_link", + timestamp: awkernel_timestamp, + }, + longitudinal_velocity: 1.0, + lateral_velocity: 0.0, + heading_rate: 0.0, + }; + converter.convert_velocity_report(&dummy_report) + }; + + *count_guard += 1; + if *count_guard >= 5700 { + *count_guard = 0; + log::info!("rust_e2e_app: finish csv for Velocity"); + loop { + wait_microsec(1000); + } + } + + if LOG_ENABLE { + log::debug!("Vehicle velocity converter: Converted velocity report to twist - linear.x={:.3}, angular.z={:.3}, awkernel_timestamp={}", + twist_msg.twist.twist.linear.x, + twist_msg.twist.twist.angular.z, + twist_msg.header.timestamp + ); + } + + (twist_msg,) + }, + vec![Cow::from("start_vel")], + vec![Cow::from("velocity_twist")], + SchedulerType::GEDF(5), + ) + .await; + + dag.register_reactor::<_, (ImuMsg,), (ImuWithCovariance,)>( + "imu_corrector".into(), + |(imu_msg,): (ImuMsg,)| -> (ImuWithCovariance,) { + let corrector = ImuCorrector::new(); + let corrected = corrector.correct_imu_with_covariance(&imu_msg, None); + (corrected,) + }, + vec![Cow::from("imu_data")], + vec![Cow::from("corrected_imu_data")], + SchedulerType::GEDF(5), + ) + .await; + + dag.register_reactor::<_, ( + ImuWithCovariance, + TwistWithCovarianceStamped, + ), (TwistWithCovarianceStamped,)>( + "gyro_odometer".into(), + |(imu_with_cov, vehicle_twist): ( + ImuWithCovariance, + TwistWithCovarianceStamped, + )| + -> (TwistWithCovarianceStamped,) { + let current_timestamp = imu_with_cov.header.timestamp; + let current_time = get_awkernel_uptime_timestamp(); + + let gyro_odometer = match gyro_odometer::get_or_initialize() { + Ok(core) => core, + Err(_) => { + return (reactor_helpers::create_empty_twist(current_timestamp),); + } + }; + + gyro_odometer.add_vehicle_twist(vehicle_twist); + gyro_odometer.add_imu(imu_with_cov); + + match gyro_odometer.process_and_get_result(current_time) { + Some(result) => (gyro_odometer.process_result(result),), + None => (reactor_helpers::create_empty_twist(current_timestamp),), + } + }, + vec![Cow::from("corrected_imu_data"), Cow::from("velocity_twist")], + vec![Cow::from("twist_with_covariance")], + SchedulerType::GEDF(5), + ) + .await; + + dag.register_reactor::<_, (i32,), (Pose,)>( + "pose_dummy_generator".into(), + move |(_start_msg,): (i32,)| -> (Pose,) { + let x = 0.0; + let y = 0.0; + let z = 0.0; + + let pose = Pose { + position: Point3D { x, y, z }, + orientation: Quaternion { + x: 0.0, + y: 0.0, + z: 0.0, + w: 1.0, + }, + }; + + (pose,) + }, + vec![Cow::from("start_pose")], + vec![Cow::from("dummy_pose")], + SchedulerType::GEDF(5), + ) + .await; + + dag.register_reactor::<_, (Pose, TwistWithCovarianceStamped), (Pose, EKFOdometry)>( + "ekf_localizer".into(), + |(pose, twist): (Pose, TwistWithCovarianceStamped)| -> (Pose, EKFOdometry) { + let ekf = get_or_initialize_default_module(); + + static mut INITIALIZED: bool = false; + unsafe { + if !INITIALIZED { + ekf.initialize(pose); + INITIALIZED = true; + } + } + + // Use a fixed 50ms timestep to match the Autoware publisher cadence. + const FIXED_DT: f64 = 0.05; + let dt = FIXED_DT; + + if dt > 0.0 { + ekf.predict(dt); + } + + let vx = twist.twist.twist.linear.x; + let wz = twist.twist.twist.angular.z; + ekf.update_velocity(vx, wz); + + let ekf_pose = ekf.get_current_pose(false); + + let pose_covariance = ekf.get_current_pose_covariance(); + let twist_covariance = ekf.get_current_twist_covariance(); + + let ekf_twist = ekf.get_current_twist(); + + let odometry = EKFOdometry { + header: common_types::Header { + frame_id: "map", + timestamp: twist.header.timestamp, + }, + child_frame_id: "base_link", + pose: PoseWithCovariance { + pose: ekf_pose, + covariance: pose_covariance, + }, + twist: TwistWithCovariance { + twist: Twist { + linear: common_types::Vector3::new( + ekf_twist.linear.x, + ekf_twist.linear.y, + ekf_twist.linear.z, + ), + angular: common_types::Vector3::new( + ekf_twist.angular.x, + ekf_twist.angular.y, + ekf_twist.angular.z, + ), + }, + covariance: twist_covariance, + }, + }; + + (ekf_pose, odometry) + }, + vec![Cow::from("dummy_pose"), Cow::from("twist_with_covariance")], + vec![Cow::from("estimated_pose"), Cow::from("ekf_odometry")], + SchedulerType::GEDF(5), + ) + .await; + + dag.register_sink_reactor::<_, (Pose, EKFOdometry)>( + "ekf_sink".into(), + move |(_pose, ekf_odom): (Pose, EKFOdometry)| { + + let json_data = format!( + r#"{{"header":{{"frame_id":"{}","timestamp":{}}},"child_frame_id":"{}","pose":{{"pose":{{"position":{{"x":{:.6},"y":{:.6},"z":{:.6}}},"orientation":{{"x":{:.6},"y":{:.6},"z":{:.6},"w":{:.6}}}}},"covariance":[{}]}},"twist":{{"twist":{{"linear":{{"x":{:.6},"y":{:.6},"z":{:.6}}},"angular":{{"x":{:.6},"y":{:.6},"z":{:.6}}}}},"covariance":[{}]}}}}"#, + ekf_odom.header.frame_id, + ekf_odom.header.timestamp, + ekf_odom.child_frame_id, + ekf_odom.pose.pose.position.x, + ekf_odom.pose.pose.position.y, + ekf_odom.pose.pose.position.z, + ekf_odom.pose.pose.orientation.x, + ekf_odom.pose.pose.orientation.y, + ekf_odom.pose.pose.orientation.z, + ekf_odom.pose.pose.orientation.w, + ekf_odom.pose.covariance.iter().map(|&x| format!("{:.6}", x)).collect::>().join(","), + ekf_odom.twist.twist.linear.x, + ekf_odom.twist.twist.linear.y, + ekf_odom.twist.twist.linear.z, + ekf_odom.twist.twist.angular.x, + ekf_odom.twist.twist.angular.y, + ekf_odom.twist.twist.angular.z, + ekf_odom.twist.covariance.iter().map(|&x| format!("{:.6}", x)).collect::>().join(",") + ); + + save_json_data_to_global(json_data); + }, + vec![Cow::from("estimated_pose"), Cow::from("ekf_odometry")], + SchedulerType::GEDF(5), + Duration::from_millis(50), + ) + .await; + + let result = finish_create_dags(slice::from_ref(&dag)).await; + + match result { + Ok(_) => { + log::info!("Autoware test application DAGs created successfully"); + } + Err(errors) => { + log::error!("Failed to create Autoware test application DAGs"); + for error in errors { + log::error!("- {error}"); + } + } + } + + log::info!("Autoware test application DAG completed"); + + log::info!("=== Network test start ==="); + log::info!("Interface ID: {}", INTERFACE_ID); + log::info!("Interface IP: {}", INTERFACE_ADDR); + log::info!("Destination IP: {}", UDP_TCP_DST_ADDR); + awkernel_lib::net::add_ipv4_addr(INTERFACE_ID, INTERFACE_ADDR, 24); + log::info!( + "Configured IPv4 address {} on interface {}", + INTERFACE_ADDR, + INTERFACE_ID + ); + + log::info!("Waiting for network stack initialization..."); + awkernel_async_lib::sleep(Duration::from_secs(2)).await; + + log::info!("Starting periodic UDP sender task"); + start_periodic_udp_sender().await; + + log::info!("Waiting for JSON data to become ready..."); + let mut wait_count = 0; + const MAX_WAIT_COUNT: u32 = 3; + + while !JSON_DATA_READY.load(Ordering::Relaxed) && wait_count < MAX_WAIT_COUNT { + awkernel_async_lib::sleep(Duration::from_secs(1)).await; + wait_count += 1; + } + + if JSON_DATA_READY.load(Ordering::Relaxed) { + } else { + log::warn!("JSON data was not ready. Periodic UDP sender task remains waiting"); + } + + log::info!("Autoware test application completed"); +} + +fn initialize_csv_data() -> Result<(), &'static str> { + unsafe { + if IMU_CSV_DATA.is_none() { + let imu_data = parse_imu_csv(IMU_CSV_DATA_STR)?; + log::info!("Loaded IMU CSV data: {} rows", imu_data.len()); + IMU_CSV_DATA = Some(imu_data); + } + + if VELOCITY_CSV_DATA.is_none() { + let velocity_data = parse_velocity_csv(VELOCITY_CSV_DATA_STR)?; + log::info!("Loaded velocity CSV data: {} rows", velocity_data.len()); + VELOCITY_CSV_DATA = Some(velocity_data); + } + } + + Ok(()) +} + +fn parse_imu_csv(csv: &str) -> Result, &'static str> { + let mut rows = Vec::new(); + + parse_csv_records(csv, |fields| { + if fields.len() < 12 { + return Err("IMU CSV has insufficient columns"); + } + + let timestamp = parse_timestamp(fields[0], fields[1])?; + let orientation = imu_driver::Quaternion { + x: parse_f64(fields[2])?, + y: parse_f64(fields[3])?, + z: parse_f64(fields[4])?, + w: parse_f64(fields[5])?, + }; + let angular_velocity = common_types::Vector3::new( + parse_f64(fields[6])?, + parse_f64(fields[7])?, + parse_f64(fields[8])?, + ); + let linear_acceleration = common_types::Vector3::new( + parse_f64(fields[9])?, + parse_f64(fields[10])?, + parse_f64(fields[11])?, + ); + + rows.push(ImuCsvRow { + timestamp, + orientation, + angular_velocity, + linear_acceleration, + }); + Ok(()) + })?; + + Ok(rows) +} + +fn parse_velocity_csv(csv: &str) -> Result, &'static str> { + let mut rows = Vec::new(); + + parse_csv_records(csv, |fields| { + if fields.len() < 5 { + return Err("Velocity CSV has insufficient columns"); + } + + let timestamp = parse_timestamp(fields[0], fields[1])?; + let longitudinal_velocity = parse_f64(fields[2])?; + let lateral_velocity = parse_f64(fields[3])?; + let heading_rate = parse_f64(fields[4])?; + + rows.push(VelocityCsvRow { + timestamp, + longitudinal_velocity, + lateral_velocity, + heading_rate, + }); + Ok(()) + })?; + + Ok(rows) +} + +fn parse_csv_records(csv: &str, mut on_record: F) -> Result<(), &'static str> +where + F: FnMut(&[&str]) -> Result<(), &'static str>, +{ + let mut reader = Reader::new(); + let mut input = csv.as_bytes(); + let mut output = vec![0u8; 4096]; + let mut ends = vec![0usize; 32]; + let mut header_skipped = false; + + loop { + let (result, in_read, _out_written, num_fields) = + reader.read_record(input, &mut output, &mut ends); + input = &input[in_read..]; + + if matches!(result, ReadRecordResult::OutputFull) { + return Err("CSV output buffer is too small"); + } + + if num_fields == 0 { + if matches!(result, ReadRecordResult::InputEmpty | ReadRecordResult::End) { + break; + } + continue; + } + + let mut fields: Vec<&str> = Vec::with_capacity(num_fields); + let mut start = 0usize; + for &end in ends.iter().take(num_fields) { + let slice = &output[start..end]; + let field = core::str::from_utf8(slice).map_err(|_| "Failed to decode CSV UTF-8")?; + fields.push(field); + start = end; + } + + if !header_skipped { + header_skipped = true; + } else { + on_record(&fields)?; + } + + if matches!(result, ReadRecordResult::End) { + break; + } + } + + Ok(()) +} + +fn parse_timestamp(sec: &str, nsec: &str) -> Result { + let sec_val = parse_u64(sec)?; + let nsec_val = parse_u64(nsec)?; + let ts = sec_val + .checked_mul(1_000_000_000) + .and_then(|v| v.checked_add(nsec_val)) + .ok_or("Timestamp calculation overflowed")?; + Ok(ts) +} + +fn parse_u64(field: &str) -> Result { + let trimmed = field.trim(); + if trimmed.is_empty() { + return Ok(0); + } + trimmed.parse::().map_err(|_| "Failed to parse u64") +} + +fn parse_f64(field: &str) -> Result { + let trimmed = field.trim(); + if trimmed.is_empty() { + return Ok(0.0); + } + trimmed.parse::().map_err(|_| "Failed to parse f64") +} + +fn get_awkernel_uptime_timestamp() -> u64 { + let uptime_nanos = awkernel_lib::delay::uptime_nano(); + if uptime_nanos > u64::MAX as u128 { + u64::MAX + } else { + uptime_nanos as u64 + } +} + +pub async fn start_periodic_udp_sender() { + awkernel_async_lib::spawn( + "periodic_udp_sender".into(), + periodic_udp_sender_task(), + awkernel_async_lib::scheduler::SchedulerType::GEDF(5), + ) + .await; +} + +async fn periodic_udp_sender_task() { + let socket_result = awkernel_async_lib::net::udp::UdpSocket::bind_on_interface( + INTERFACE_ID, + &Default::default(), + ); + + let mut socket = match socket_result { + Ok(socket) => socket, + Err(e) => { + log::error!( + "Periodic UDP sender task: failed to create UDP socket: {:?}", + e + ); + return; + } + }; + + let dst_addr = IpAddr::new_v4(UDP_TCP_DST_ADDR); + let mut counter = 0; + + loop { + if !JSON_DATA_READY.load(Ordering::Relaxed) { + awkernel_async_lib::sleep(Duration::from_secs(1)).await; + continue; + } + + let json_data = unsafe { LATEST_JSON_DATA.clone() }; + + if let Some(data) = json_data { + match socket.send(data.as_bytes(), &dst_addr, UDP_DST_PORT).await { + Ok(_) => { + counter += 1; + log::info!( + "Periodic UDP sender task: send success #{} ({} bytes)", + counter, + data.len() + ); + + let mut buf = [0u8; 1024]; + if let Some(Ok((n, src_addr, src_port))) = awkernel_async_lib::timeout( + Duration::from_millis(100), + socket.recv(&mut buf), + ) + .await + { + if let Ok(response) = core::str::from_utf8(&buf[..n]) { + log::debug!( + "Periodic UDP sender task: response received: {}:{} - {}", + src_addr.get_addr(), + src_port, + response + ); + } + } + } + Err(e) => { + log::warn!("Periodic UDP sender task: send failed: {:?}", e); + } + } + } else { + log::warn!("Periodic UDP sender task: failed to get JSON data"); + } + + awkernel_async_lib::sleep(Duration::from_millis(5)).await; + } +} + +fn save_json_data_to_global(json_data: String) { + unsafe { + LATEST_JSON_DATA = Some(json_data.clone()); + } + JSON_DATA_READY.store(true, Ordering::Relaxed); + JSON_DATA_LENGTH.store(json_data.len(), Ordering::Relaxed); +} diff --git a/applications/tests/test_autoware/vehicle_velocity_converter/Cargo.toml b/applications/tests/test_autoware/vehicle_velocity_converter/Cargo.toml new file mode 100644 index 000000000..9d3d8b790 --- /dev/null +++ b/applications/tests/test_autoware/vehicle_velocity_converter/Cargo.toml @@ -0,0 +1,7 @@ +[package] +name = "vehicle_velocity_converter" +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 new file mode 100644 index 000000000..1c6b864d1 --- /dev/null +++ b/applications/tests/test_autoware/vehicle_velocity_converter/src/lib.rs @@ -0,0 +1,312 @@ +// 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] + +pub use common_types::{Header, Vector3}; + +#[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 Default for VehicleVelocityConverter { + fn default() -> Self { + Self::new("base_link", 0.2, 0.1, 1.0) + } +} + +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 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] = self.stddev_vx * self.stddev_vx; + covariance[7] = 10000.0; + covariance[14] = 10000.0; + covariance[21] = 10000.0; + covariance[28] = 10000.0; + covariance[35] = 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); + } +} diff --git a/userland/Cargo.toml b/userland/Cargo.toml index 664ec35f3..c6fac4eaa 100644 --- a/userland/Cargo.toml +++ b/userland/Cargo.toml @@ -66,6 +66,10 @@ 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 @@ -75,7 +79,7 @@ path = "../applications/tests/test_voluntary_preemption" optional = true [features] -default = [] +default = ["test_autoware"] perf = ["awkernel_services/perf"] # Evaluation applications @@ -93,4 +97,5 @@ 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 01a66fe18..7082e1eba 100644 --- a/userland/src/lib.rs +++ b/userland/src/lib.rs @@ -46,6 +46,9 @@ 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