Repository navigation
feat(autoware): use nalgebra for common types - #775
romancardenas wants to merge 2 commits into
Conversation
| // 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.apply(&gyro.angular_velocity); |
There was a problem hiding this comment.
tf.apply() adds the translation to the angular velocity. The original C++ (autoware_core 1.8.0, gyro_odometer_core.cpp L155) uses tf2::doTransform(Vector3Stamped), which applies only the rotation (gmTransformToKDL(t).M * v). Translation must not be added to a direction vector.
This is pre-existing (inherited from apply_to_vector) and is hidden today because get_transform() always returns identity. However, imu_corrector already uses rotate() in this PR, and once a real IMU→base_link TF is hardcoded (e.g. translation (5.22, 0.08, 2.73) for the aip_launcher IMU), this line would add ~5 rad/s to the angular velocity.
Suggestion:
let transformed_angular_velocity = tf.rotate(&gyro.angular_velocity);
Fine to do it here or in a follow-up PR.
| pub fn try_new(translation: Vector3<f64>, rotation: Quaternion<f64>) -> Result<Self> { | ||
| if rotation.coords.iter().any(|value| !value.is_finite()) { | ||
| Err(Error::NonFiniteQuaternion) | ||
| } else if rotation.norm_squared() == 0.0 { | ||
| Err(Error::ZeroLengthQuaternion) | ||
| } else { | ||
| Ok(Self::new( | ||
| translation, | ||
| UnitQuaternion::from_quaternion(rotation), | ||
| )) | ||
| } | ||
| } |
There was a problem hiding this comment.
try_new can still return Ok with a broken rotation:
- Large components overflow
norm_squared()toinf, which passes the== 0.0check. E.g.Quaternion::new(1e155, 0.0, 0.0, 1e155)(should be a 90° yaw) returns Ok, androtate((1,0,0))gives (1,0,0). - Subnormal norms lose precision. With w=k=1e-160,
rotate((1,0,0))gives (-1.1e-5, 1.000011, 0), which is not unit length. translationis not checked for NaN/inf.
tf2 (BufferCore::setTransform) rejects NaN in translation/rotation and rejects quaternions with |‖q‖² − 1| ≥ 0.01 (TF_DENORMALIZED_QUATERNION) instead of normalizing arbitrary lengths. Suggest matching that:
if translation.iter().chain(rotation.coords.iter()).any(|v| !v.is_finite()) {
return Err(Error::NonFinite);
}
let n2 = rotation.norm_squared();
if !n2.is_finite() || (n2 - 1.0).abs() >= 0.01 {
return Err(Error::DenormalizedQuaternion);
}
Ok(Self::new(translation, UnitQuaternion::from_quaternion(rotation)))
The test try_new_rejects_quaternion_with_underflowing_squared_norm would then map to the new error.
| Self { x, y, z } | ||
| } | ||
| } | ||
| pub use nalgebra::{Quaternion, UnitQuaternion, Vector3}; |
There was a problem hiding this comment.
Re-exporting nalgebra's Quaternion changes the constructor order from the old field-named {x, y, z, w} (ROS order) to Quaternion::new(w, i, j, k). The TODO in imu_corrector/src/lib.rs L147-148 plans to hardcode a fixed TF next, and copying ROS values into new() would silently give a wrong rotation. Could we add a ROS-order constructor (e.g. Transform::from_ros_xyzw(t: [f64; 3], q: [f64; 4]), via Quaternion::from_vector(Vector4::new(x, y, z, w))) or at least document the order?
Description
Instead of defining custom types for
Vector3,Quaternion, etc., this PR usesnalgebranative types.The main benefits is that we can use a lot of methods and functionalities provided by this package.
IMPORTANT: packages do not depend directly on
nalgebra. Instead,common_typesserves as a common framework where we re-export the functionalities we consider necessary.I adapted the current autoware packages to follow this approach. The most important changes can be found in
imu_corrector.I think this change will allow us to better scale as we include functionalities to autoware. I am about to start addressing #748 and detected that this restructuring would be beneficial.
How was this PR tested?
scripts/ci.shpasses on my machine.