diff --git a/applications/autoware/common_types/Cargo.toml b/applications/autoware/common_types/Cargo.toml index c54b7769b..be0a5baf1 100644 --- a/applications/autoware/common_types/Cargo.toml +++ b/applications/autoware/common_types/Cargo.toml @@ -4,3 +4,4 @@ version = "0.1.0" edition = "2021" [dependencies] +nalgebra = { version = "0.35", default-features = false, features = ["libm"] } diff --git a/applications/autoware/common_types/src/lib.rs b/applications/autoware/common_types/src/lib.rs index 2ca030990..2f5859597 100644 --- a/applications/autoware/common_types/src/lib.rs +++ b/applications/autoware/common_types/src/lib.rs @@ -1,21 +1,27 @@ +//! This crate defines data types that are common to the Autoware project. +//! +//! For algebraic operations and transformations, this crate relies on nalgebra. +//! This implies some differences from the conventions used in ROS. +//! +//! For example, quaternions are represented using nalgebra's convention rather than ROS's. +//! `nalgebra`'s quaternion convention is `Quaternion::new(w, i, j, k)`. +//! On the other hand, ROS uses the `(x, y, z, w)` convention for quaternions. +//! +//! We will use nalgebra's convention throughout this crate, +//! and only convert to/from ROS's convention at I/O boundaries. +//! ROS compatibility functions are marked as deprecated, and should only be used for interfacing with ROS +//! or for maintaining compatibility with existing ROS-based code. + #![no_std] + extern crate alloc; +pub mod transform; + #[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 } - } -} +pub use nalgebra::{Quaternion, UnitQuaternion, Vector3}; diff --git a/applications/autoware/common_types/src/transform.rs b/applications/autoware/common_types/src/transform.rs new file mode 100644 index 000000000..9f2136dc7 --- /dev/null +++ b/applications/autoware/common_types/src/transform.rs @@ -0,0 +1,427 @@ +use crate::{Header, Quaternion, UnitQuaternion, Vector3}; +use alloc::{format, string::String}; +use nalgebra::Vector4; + +#[derive(Clone, Debug)] +#[non_exhaustive] +pub enum Error { + DenormalizedQuaternion, + NonFinite, +} + +impl core::fmt::Display for Error { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + match self { + Error::DenormalizedQuaternion => write!(f, "Denormalized quaternion"), + Error::NonFinite => write!(f, "Non-finite translation or quaternion"), + } + } +} + +impl core::error::Error for Error {} + +pub type Result = core::result::Result; + +#[derive(Clone, Debug)] +pub struct Transform { + translation: Vector3, + rotation: UnitQuaternion, +} + +impl Transform { + /// Creates a new `Transform` with the given translation and rotation. + #[inline] + pub const fn new(translation: Vector3, rotation: UnitQuaternion) -> Self { + Self { + translation, + rotation, + } + } + + /// Tries to create a new `Transform` with the given translation and rotation. + /// + /// Rejects non-finite translation or rotation components and quaternions whose squared norm differs + /// from one by at least 0.01. Accepted near-unit quaternions are normalized to preserve invariants. + /// + /// # Note + /// + /// Nalgebra's `Quaternion::new` takes `(w, i, j, k)`, not ROS's `(x, y, z, w)`. + /// For ROS-order components, construct `Quaternion::new(w, x, y, z)` explicitly. + #[inline] + pub fn try_new(translation: Vector3, rotation: Quaternion) -> Result { + if translation + .iter() + .chain(rotation.coords.iter()) + .any(|value| !value.is_finite()) + { + return Err(Error::NonFinite); + } + let norm_squared = rotation.norm_squared(); + if !norm_squared.is_finite() || (norm_squared - 1.0).abs() >= 0.01 { + return Err(Error::DenormalizedQuaternion); + } + Ok(Self::new( + translation, + UnitQuaternion::from_quaternion(rotation), + )) + } + + /// Legacy ROS2-porting helper for quaternion `[x, y, z, w]` components. + /// + /// This compatibility helper may be removed in the future. Prefer [`Self::try_new`] + /// with an explicitly constructed `Quaternion::new(w, x, y, z)`. + /// Uses the same finite-input and near-unit rotation checks as [`Self::try_new`]. + #[deprecated( + note = "Legacy helper for porting from ROS2; may be removed in the future. Prefer Transform::try_new with Quaternion::new(w, x, y, z)." + )] + #[inline] + pub fn try_from_ros_xyzw(translation: Vector3, rotation: [f64; 4]) -> Result { + let [x, y, z, w] = rotation; + Self::try_new( + translation, + Quaternion::from_vector(Vector4::new(x, y, z, w)), + ) + } + + /// Returns the identity transform (no translation, no rotation). + #[inline] + pub fn identity() -> Self { + Self::new(Vector3::new(0.0, 0.0, 0.0), UnitQuaternion::identity()) + } + + /// Applies the transform to the given vector. + #[inline] + pub fn apply(&self, vec: &Vector3) -> Vector3 { + self.rotation * vec + self.translation + } + + #[inline] + pub fn rotate(&self, vec: &Vector3) -> Vector3 { + self.rotation * vec + } + + #[inline] + pub fn translate(&self, vec: &Vector3) -> Vector3 { + vec + self.translation + } +} + +impl Default for Transform { + fn default() -> Self { + Self::identity() + } +} + +#[derive(Clone, Debug)] +pub struct TransformStamped { + pub header: Header, + pub child_frame_id: &'static str, + pub transform: Transform, +} + +impl TransformStamped { + #[inline] + pub const fn new(header: Header, child_frame_id: &'static str, transform: Transform) -> Self { + Self { + header, + child_frame_id, + transform, + } + } +} + +impl core::ops::Deref for TransformStamped { + type Target = Transform; + + fn deref(&self) -> &Self::Target { + &self.transform + } +} + +pub trait TransformListener { + fn get_latest_transform(&self, from_frame: &str, to_frame: &str) -> Option; + fn get_transform_at_time( + &self, + from_frame: &str, + to_frame: &str, + timestamp: u64, + ) -> Option; +} + +pub struct MockTransformListener { + transforms: alloc::collections::BTreeMap, +} + +impl MockTransformListener { + pub fn new() -> Self { + Self { + transforms: alloc::collections::BTreeMap::new(), + } + } + + pub fn add_transform(&mut self, from_frame: &str, to_frame: &str, transform: Transform) { + let key = format!("{}_to_{}", from_frame, to_frame); + self.transforms.insert(key, transform); + } +} + +impl Default for MockTransformListener { + fn default() -> Self { + Self::new() + } +} + +impl TransformListener for MockTransformListener { + fn get_latest_transform(&self, from_frame: &str, to_frame: &str) -> Option { + let key = format!("{}_to_{}", from_frame, to_frame); + 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) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn assert_almost_eq(actual: f64, expected: f64) { + assert!((actual - expected).abs() < 1e-10); + } + + #[test] + fn try_new_rejects_zero_length_quaternion() { + let result = Transform::try_new(Vector3::zeros(), Quaternion::new(0.0, 0.0, 0.0, 0.0)); + assert!(matches!(result, Err(Error::DenormalizedQuaternion))); + } + + #[test] + fn try_new_rejects_non_finite_quaternion_components() { + for value in [f64::NAN, f64::INFINITY, f64::NEG_INFINITY] { + for rotation in [ + Quaternion::new(value, 0.0, 0.0, 0.0), + Quaternion::new(1.0, value, 0.0, 0.0), + Quaternion::new(1.0, 0.0, value, 0.0), + Quaternion::new(1.0, 0.0, 0.0, value), + ] { + let result = Transform::try_new(Vector3::zeros(), rotation); + + assert!( + matches!(result, Err(Error::NonFinite)), + "expected NonFinite for {rotation:?}" + ); + } + } + } + + #[test] + fn try_new_rejects_quaternion_with_underflowing_squared_norm() { + let rotation = Quaternion::new(f64::MIN_POSITIVE, 0.0, 0.0, 0.0); + let result = Transform::try_new(Vector3::zeros(), rotation); + + assert!(matches!(result, Err(Error::DenormalizedQuaternion))); + } + + #[test] + fn try_new_rejects_non_finite_translation_components() { + for value in [f64::NAN, f64::INFINITY, f64::NEG_INFINITY] { + for translation in [ + Vector3::new(value, 0.0, 0.0), + Vector3::new(0.0, value, 0.0), + Vector3::new(0.0, 0.0, value), + ] { + // Non-finite inputs take precedence over a denormalized rotation. + for rotation in [Quaternion::identity(), Quaternion::new(0.0, 0.0, 0.0, 0.0)] { + assert!(matches!( + Transform::try_new(translation, rotation), + Err(Error::NonFinite) + )); + } + } + } + } + + #[test] + fn try_new_rejects_overflowing_subnormal_and_arbitrary_length_quaternions() { + for rotation in [ + Quaternion::new(1e155, 0.0, 0.0, 1e155), + Quaternion::new(1e-160, 0.0, 0.0, 1e-160), + Quaternion::new(2.0, 0.0, 0.0, 2.0), + ] { + assert!(matches!( + Transform::try_new(Vector3::zeros(), rotation), + Err(Error::DenormalizedQuaternion) + )); + } + } + + #[test] + fn try_new_enforces_squared_norm_tolerance_on_both_sides() { + for norm_squared in [0.990001_f64, 1.009999] { + let rotation = Quaternion::new(norm_squared.sqrt(), 0.0, 0.0, 0.0); + assert!((rotation.norm_squared() - 1.0).abs() < 0.01); + let transform = Transform::try_new(Vector3::zeros(), rotation).unwrap(); + assert_almost_eq(transform.rotation.norm(), 1.0); + } + for norm_squared in [0.989999_f64, 1.010001] { + let rotation = Quaternion::new(norm_squared.sqrt(), 0.0, 0.0, 0.0); + assert!((rotation.norm_squared() - 1.0).abs() >= 0.01); + assert!(matches!( + Transform::try_new(Vector3::zeros(), rotation), + Err(Error::DenormalizedQuaternion) + )); + } + } + + #[test] + fn try_new_normalizes_valid_quaternion_and_preserves_translation() { + let translation = Vector3::new(1.0, 2.0, 3.0); + let component = core::f64::consts::FRAC_1_SQRT_2 * 1.002; + let rotation = Quaternion::new(component, 0.0, 0.0, component); + let transform = Transform::try_new(translation, rotation).unwrap(); + + assert_eq!(&transform.translation, &translation); + assert_almost_eq(transform.rotation.norm(), 1.0); + + let rotated = transform.rotate(&Vector3::new(1.0, 0.0, 0.0)); + assert_almost_eq(rotated.x, 0.0); + assert_almost_eq(rotated.y, 1.0); + assert_almost_eq(rotated.norm(), 1.0); + + let transformed = transform.apply(&Vector3::new(1.0, 0.0, 0.0)); + assert_almost_eq(transformed.x, 1.0); + assert_almost_eq(transformed.y, 3.0); + assert_almost_eq(transformed.z, 3.0); + } + + #[test] + fn quaternion_component_order_matches_nalgebra_convention() { + let half_sqrt_two = core::f64::consts::FRAC_1_SQRT_2; + let yaw = Transform::try_new( + Vector3::zeros(), + Quaternion::new(half_sqrt_two, 0.0, 0.0, half_sqrt_two), + ) + .unwrap(); + let rotated = yaw.rotate(&Vector3::new(1.0, 0.0, 0.0)); + + assert_almost_eq(rotated.x, 0.0); + assert_almost_eq(rotated.y, 1.0); + assert_almost_eq(rotated.z, 0.0); + + let roll = + Transform::try_new(Vector3::zeros(), Quaternion::new(0.0, 1.0, 0.0, 0.0)).unwrap(); + let angular_velocity = Vector3::new(1.0, 2.0, 3.0); + let rotated = roll.rotate(&angular_velocity); + + assert_almost_eq(rotated.x, angular_velocity.x); + assert_almost_eq(rotated.y, -angular_velocity.y); + assert_almost_eq(rotated.z, -angular_velocity.z); + } + + #[test] + #[expect( + deprecated, + reason = "regression test for the legacy ROS2-porting helper" + )] + fn from_ros_xyzw_preserves_identity_and_translation() { + let transform = + Transform::try_from_ros_xyzw(Vector3::new(1.0, 2.0, 3.0), [0.0, 0.0, 0.0, 1.0]) + .unwrap(); + let point = Vector3::new(4.0, 5.0, 6.0); + let rotated = transform.rotate(&point); + let transformed = transform.apply(&point); + + for (actual, expected) in rotated.iter().zip(point.iter()) { + assert_almost_eq(*actual, *expected); + } + for (actual, expected) in transformed.iter().zip([5.0, 7.0, 9.0]) { + assert_almost_eq(*actual, expected); + } + } + + #[test] + #[expect( + deprecated, + reason = "regression test for the legacy ROS2-porting helper" + )] + fn from_ros_xyzw_maps_each_rotation_axis_correctly() { + let c = core::f64::consts::FRAC_1_SQRT_2; + for (quaternion, input, expected) in [ + ([c, 0.0, 0.0, c], Vector3::y(), Vector3::z()), + ([0.0, c, 0.0, c], Vector3::z(), Vector3::x()), + ([0.0, 0.0, c, c], Vector3::x(), Vector3::y()), + ] { + let transform = Transform::try_from_ros_xyzw(Vector3::zeros(), quaternion).unwrap(); + let rotated = transform.rotate(&input); + for (actual, expected) in rotated.iter().zip(expected.iter()) { + assert_almost_eq(*actual, *expected); + } + assert_almost_eq(rotated.norm(), 1.0); + } + } + + #[test] + #[expect( + deprecated, + reason = "regression test for the legacy ROS2-porting helper" + )] + fn from_ros_xyzw_reuses_input_validation() { + for value in [f64::NAN, f64::INFINITY, f64::NEG_INFINITY] { + for index in 0..3 { + let mut translation = [0.0; 3]; + translation[index] = value; + assert!(matches!( + Transform::try_from_ros_xyzw(translation.into(), [0.0, 0.0, 0.0, 1.0]), + Err(Error::NonFinite) + )); + } + for index in 0..4 { + let mut rotation = [0.0, 0.0, 0.0, 1.0]; + rotation[index] = value; + assert!(matches!( + Transform::try_from_ros_xyzw(Vector3::zeros(), rotation), + Err(Error::NonFinite) + )); + } + } + for rotation in [[0.0; 4], [0.0, 0.0, 0.0, 2.0]] { + assert!(matches!( + Transform::try_from_ros_xyzw(Vector3::zeros(), rotation), + Err(Error::DenormalizedQuaternion) + )); + } + } + + #[test] + fn identity_transform_preserves_vector() { + let vector = Vector3::new(1.0, -2.0, 3.0); + let transformed = Transform::identity().apply(&vector); + + assert_almost_eq(transformed.x, vector.x); + assert_almost_eq(transformed.y, vector.y); + assert_almost_eq(transformed.z, vector.z); + } + + #[test] + fn transform_rotates_then_translates_vector() { + let half_sqrt_two = core::f64::consts::FRAC_1_SQRT_2; + let translation = Vector3::new(1.0, 2.0, 3.0); + let rotation = Quaternion::new(half_sqrt_two, 0.0, 0.0, half_sqrt_two); + + let transform = Transform::try_new(translation, rotation); + assert!(transform.is_ok()); + let transform = transform.unwrap(); + + let transformed = transform.apply(&Vector3::new(1.0, 0.0, 0.0)); + + assert_almost_eq(transformed.x, 1.0); + assert_almost_eq(transformed.y, 3.0); + assert_almost_eq(transformed.z, 3.0); + } +} diff --git a/applications/autoware/gyro_odometer/Cargo.toml b/applications/autoware/gyro_odometer/Cargo.toml index 332559fa2..e1b88ce75 100644 --- a/applications/autoware/gyro_odometer/Cargo.toml +++ b/applications/autoware/gyro_odometer/Cargo.toml @@ -6,4 +6,5 @@ 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} +vehicle_velocity_converter = { path = "../vehicle_velocity_converter", default-features = false } +common_types = { path = "../common_types", default-features = false } diff --git a/applications/autoware/gyro_odometer/src/lib.rs b/applications/autoware/gyro_odometer/src/lib.rs index cd3d57068..2c86ebe3a 100644 --- a/applications/autoware/gyro_odometer/src/lib.rs +++ b/applications/autoware/gyro_odometer/src/lib.rs @@ -28,8 +28,9 @@ 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, Vector3}; +pub use common_types::{transform::Transform, Header, Vector3}; +pub use imu_corrector::{transform_covariance, ImuWithCovariance}; +pub use imu_driver::ImuMsg; pub use vehicle_velocity_converter::{Twist, TwistWithCovariance, TwistWithCovarianceStamped}; static GYRO_ODOMETER_INSTANCE: AtomicPtr = AtomicPtr::new(null_mut()); @@ -126,7 +127,7 @@ impl GyroOdometerCore { // via transform_covariance() after transforming angular_velocity. This is omitted here // because the TF used in evaluation is a fixed identity transform, so the result is the same. for gyro in &mut self.gyro_queue { - let transformed_angular_velocity = tf.apply_to_vector(gyro.angular_velocity.clone()); + let transformed_angular_velocity = tf.rotate(&gyro.angular_velocity); gyro.angular_velocity = transformed_angular_velocity; } diff --git a/applications/autoware/imu_corrector/Cargo.toml b/applications/autoware/imu_corrector/Cargo.toml index 931c78678..a4a9753f8 100644 --- a/applications/autoware/imu_corrector/Cargo.toml +++ b/applications/autoware/imu_corrector/Cargo.toml @@ -4,5 +4,5 @@ version = "0.1.0" edition = "2021" [dependencies] -nalgebra = { version = "0.32", default-features = false, features = ["libm"] } +common_types = { path = "../common_types", default-features = false } imu_driver = { path = "../imu_driver", default-features = false } diff --git a/applications/autoware/imu_corrector/src/lib.rs b/applications/autoware/imu_corrector/src/lib.rs index 7e573e213..97058fc79 100644 --- a/applications/autoware/imu_corrector/src/lib.rs +++ b/applications/autoware/imu_corrector/src/lib.rs @@ -23,95 +23,18 @@ #![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, - }, - } - } - - // This code is used as part of the `gyro_odometer` function processing. - pub fn apply_to_vector(&self, vec: Vector3) -> Vector3 { - let nalgebra_vec = to_nalgebra_vector3(&vec); - let nalgebra_quat = to_nalgebra_quaternion(&self.rotation); - let nalgebra_trans = to_nalgebra_vector3(&self.translation); - let rotated = nalgebra_quat * nalgebra_vec; - let result = rotated + nalgebra_trans; - to_imu_vector3(&result) - } -} - -pub trait TransformListener { - fn get_latest_transform(&self, from_frame: &str, to_frame: &str) -> Option; - fn get_transform_at_time( - &self, - from_frame: &str, - to_frame: &str, - timestamp: u64, - ) -> Option; -} - -pub struct MockTransformListener { - transforms: alloc::collections::BTreeMap, -} - -impl MockTransformListener { - pub fn new() -> Self { - Self { - transforms: alloc::collections::BTreeMap::new(), - } - } - - pub fn add_transform(&mut self, from_frame: &str, to_frame: &str, transform: Transform) { - let key = format!("{}_to_{}", from_frame, to_frame); - self.transforms.insert(key, transform); - } -} - -impl Default for MockTransformListener { - fn default() -> Self { - Self::new() - } -} - -impl TransformListener for MockTransformListener { - fn get_latest_transform(&self, from_frame: &str, to_frame: &str) -> Option { - let key = format!("{}_to_{}", from_frame, to_frame); - 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) - } -} +use common_types::{ + transform::{MockTransformListener, Transform, TransformListener}, + Header, Vector3, +}; +use imu_driver::ImuMsg; #[derive(Clone, Debug)] pub struct ImuWithCovariance { pub header: Header, - pub angular_velocity: Vector3, + pub angular_velocity: Vector3, pub angular_velocity_covariance: [f64; 9], - pub linear_acceleration: Vector3, + pub linear_acceleration: Vector3, pub linear_acceleration_covariance: [f64; 9], } @@ -119,9 +42,9 @@ impl ImuWithCovariance { pub fn from_imu_msg(imu_msg: &ImuMsg) -> Self { Self { header: imu_msg.header.clone(), - angular_velocity: imu_msg.angular_velocity.clone(), + angular_velocity: imu_msg.angular_velocity, angular_velocity_covariance: [0.0; 9], - linear_acceleration: imu_msg.linear_acceleration.clone(), + linear_acceleration: imu_msg.linear_acceleration, linear_acceleration_covariance: [0.0; 9], } } @@ -129,8 +52,8 @@ impl ImuWithCovariance { pub fn to_imu_msg(&self) -> ImuMsg { ImuMsg { header: self.header.clone(), - angular_velocity: self.angular_velocity.clone(), - linear_acceleration: self.linear_acceleration.clone(), + angular_velocity: self.angular_velocity, + linear_acceleration: self.linear_acceleration, } } } @@ -198,11 +121,8 @@ impl ImuCorrector { self.config = config; } - fn transform_vector3(&self, vec: &Vector3, transform: &Transform) -> Vector3 { - let nalgebra_vec = to_nalgebra_vector3(vec); - let nalgebra_quat = to_nalgebra_quaternion(&transform.rotation); - let rotated = nalgebra_quat * nalgebra_vec; - to_imu_vector3(&rotated) + fn transform_vector3(&self, vec: &Vector3, transform: &Transform) -> Vector3 { + transform.rotate(vec) } fn transform_covariance_impl(cov: &[f64; 9]) -> [f64; 9] { @@ -226,6 +146,7 @@ impl ImuCorrector { // However, the actual vehicle TF would produce incorrect results if skipped. // TODO: To be implemented in a subsequent PR: // Fix by hardcoding a fixed tf value to prevent None from being passed by the caller. + // Prefer Transform::try_new with Quaternion::new(w, x, y, z), not ROS order [x, y, z, w]. // // If dynamic TF support is added in the future, the recommended fallback is to // return an empty/zeroed ImuWithCovariance (analogous to `create_empty_twist` in @@ -313,19 +234,6 @@ impl ImuCorrector { } } -fn to_nalgebra_vector3(vec: &Vector3) -> NVector3 { - NVector3::new(vec.x, vec.y, vec.z) -} - -fn to_imu_vector3(vec: &NVector3) -> Vector3 { - Vector3::new(vec.x, vec.y, vec.z) -} - -fn to_nalgebra_quaternion(quat: &Quaternion) -> UnitQuaternion { - let n_quat = NQuaternion::new(quat.w, quat.x, quat.y, quat.z); - UnitQuaternion::from_quaternion(n_quat) -} - pub fn transform_covariance(cov: &[f64; 9]) -> [f64; 9] { ImuCorrector::::transform_covariance_impl(cov) } @@ -333,6 +241,7 @@ pub fn transform_covariance(cov: &[f64; 9]) -> [f64; 9] { #[cfg(test)] mod tests { use super::*; + use common_types::{Quaternion, UnitQuaternion}; fn assert_almost_eq(actual: f64, expected: f64) { assert!((actual - expected).abs() < 1e-10); @@ -435,15 +344,13 @@ mod tests { 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 half_sqrt_two = core::f64::consts::FRAC_1_SQRT_2; + let translation = Vector3::new(1.0, 2.0, 3.0); + let rotation = Quaternion::new(half_sqrt_two, 0.0, 0.0, half_sqrt_two); + + let transform = Transform::try_new(translation, rotation); + assert!(transform.is_ok()); + let transform = transform.unwrap(); let corrected_with_cov = corrector.correct_imu_with_covariance(&imu_msg, Some(&transform)); assert_eq!(corrected_with_cov.header.frame_id, "base_link"); @@ -458,15 +365,7 @@ mod tests { #[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, - }, - }; + let transform = Transform::new(Vector3::new(1.0, 0.0, 0.0), UnitQuaternion::identity()); transform_listener.add_transform("imu_link", "base_link", transform); let corrector = ImuCorrector::with_transform_listener(transform_listener); diff --git a/applications/autoware/imu_driver/src/lib.rs b/applications/autoware/imu_driver/src/lib.rs index c04edb768..7eb4350df 100644 --- a/applications/autoware/imu_driver/src/lib.rs +++ b/applications/autoware/imu_driver/src/lib.rs @@ -60,28 +60,20 @@ extern crate alloc; use alloc::{format, string::String, vec, vec::Vec}; use core::f64::consts::PI; -pub use common_types::{Header, Vector3}; +pub use common_types::{Header, Quaternion, Vector3}; #[derive(Clone, Debug)] pub struct ImuMsg { pub header: Header, - pub angular_velocity: Vector3, - pub linear_acceleration: Vector3, + pub angular_velocity: Vector3, + pub linear_acceleration: Vector3, } #[derive(Clone, Debug)] pub struct ImuCsvRow { pub timestamp: u64, - 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, + pub angular_velocity: Vector3, + pub linear_acceleration: Vector3, } impl Default for ImuMsg { @@ -107,8 +99,8 @@ pub fn build_imu_msg_from_csv_row( frame_id, timestamp, }, - angular_velocity: row.angular_velocity.clone(), - linear_acceleration: row.linear_acceleration.clone(), + angular_velocity: row.angular_velocity, + linear_acceleration: row.linear_acceleration, } } @@ -154,8 +146,8 @@ impl TamagawaImuParser { pub fn generate_dummy_binary_data( &mut self, _timestamp: u64, - angular_velocity: Vector3, - linear_acceleration: Vector3, + angular_velocity: Vector3, + linear_acceleration: Vector3, ) -> Vec { let mut data = vec![0u8; 58]; diff --git a/applications/autoware/vehicle_velocity_converter/src/lib.rs b/applications/autoware/vehicle_velocity_converter/src/lib.rs index 19f9a36de..84caca1a1 100644 --- a/applications/autoware/vehicle_velocity_converter/src/lib.rs +++ b/applications/autoware/vehicle_velocity_converter/src/lib.rs @@ -70,8 +70,8 @@ pub struct TwistWithCovariance { #[derive(Debug, Clone)] pub struct Twist { - pub linear: Vector3, - pub angular: Vector3, + pub linear: Vector3, + pub angular: Vector3, } pub struct VehicleVelocityConverter { @@ -121,16 +121,12 @@ impl VehicleVelocityConverter { 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, - }, + linear: Vector3::new( + msg.longitudinal_velocity * self.speed_scale_factor, + msg.lateral_velocity, + 0.0, + ), + angular: Vector3::new(0.0, 0.0, msg.heading_rate), }, covariance: self.create_covariance_matrix(), }, @@ -183,16 +179,8 @@ pub mod reactor_helpers { }, 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, - }, + linear: Vector3::new(0.0, 0.0, 0.0), + angular: Vector3::new(0.0, 0.0, 0.0), }, covariance: [0.0; 36], },