use crate::api::v1::localize::{LocalizationRevision, LocalizationRevisionId};
use crate::api::v1::map::{MapRevision, MapRevisionId};
use crate::api::v1::simulation::pose::Pose;
use anyhow::{Result, bail};
#[derive(Debug, Clone, Copy, PartialEq, PartialOrd)]
pub struct Meters(pub f64);
#[derive(Debug, Clone, Copy, PartialEq, PartialOrd)]
pub struct Radians(pub f64);
pub trait EpochRevision {
fn epoch(&self) -> u64;
fn sequence(&self) -> u64;
}
impl EpochRevision for LocalizationRevisionId {
fn epoch(&self) -> u64 {
self.epoch
}
fn sequence(&self) -> u64 {
self.sequence
}
}
impl EpochRevision for MapRevisionId {
fn epoch(&self) -> u64 {
self.epoch
}
fn sequence(&self) -> u64 {
self.sequence
}
}
pub fn assert_forward_delta(
start: &Pose,
end: &Pose,
expected: Meters,
tolerance: Meters,
) -> Result<()> {
let actual = end.translation_m[0] - start.translation_m[0];
assert_close("forward delta", actual, expected.0, tolerance.0)
}
pub fn assert_lateral_drift(start: &Pose, end: &Pose, max_drift: Meters) -> Result<()> {
let drift = (end.translation_m[1] - start.translation_m[1]).abs();
if drift <= max_drift.0 {
Ok(())
} else {
bail!(
"lateral drift {drift:.6} m exceeds maximum {:.6} m",
max_drift.0
)
}
}
pub fn assert_yaw_drift(start: &Pose, end: &Pose, max_drift: Radians) -> Result<()> {
let drift = angle_delta(
yaw_from_xyzw(start.rotation_xyzw),
yaw_from_xyzw(end.rotation_xyzw),
);
if drift.abs() <= max_drift.0 {
Ok(())
} else {
bail!(
"yaw drift {:.6} rad exceeds maximum {:.6} rad",
drift.abs(),
max_drift.0
)
}
}
pub fn assert_revision_linked(map: &MapRevision, localize: &LocalizationRevision) -> Result<()> {
if map.built_from_localize_revision == localize.revision_id {
Ok(())
} else {
bail!(
"map revision {:?} is built from localization revision {:?}, expected {:?}",
map.map_revision_id,
map.built_from_localize_revision,
localize.revision_id
)
}
}
pub fn assert_revision_monotonic<T>(revisions: &[T]) -> Result<()>
where
T: EpochRevision + std::fmt::Debug,
{
for window in revisions.windows(2) {
let previous = &window[0];
let current = &window[1];
if current.epoch() != previous.epoch() {
bail!("revision epoch changed from {previous:?} to {current:?}");
}
if current.sequence() <= previous.sequence() {
bail!("revision sequence did not increase from {previous:?} to {current:?}");
}
}
Ok(())
}
fn assert_close(name: &str, actual: f64, expected: f64, tolerance: f64) -> Result<()> {
validate_finite(name, actual)?;
validate_finite("expected", expected)?;
validate_finite("tolerance", tolerance)?;
if tolerance < 0.0 {
bail!("tolerance must be non-negative, got {tolerance}");
}
let delta = (actual - expected).abs();
if delta <= tolerance {
Ok(())
} else {
bail!(
"{name} {actual:.6} differs from expected {expected:.6} by {delta:.6}, tolerance {tolerance:.6}"
)
}
}
fn validate_finite(name: &str, value: f64) -> Result<()> {
if value.is_finite() {
Ok(())
} else {
bail!("{name} must be finite, got {value}")
}
}
fn yaw_from_xyzw(rotation_xyzw: [f64; 4]) -> f64 {
let [x, y, z, w] = rotation_xyzw;
let siny_cosp = 2.0 * (w * z + x * y);
let cosy_cosp = 1.0 - 2.0 * (y * y + z * z);
siny_cosp.atan2(cosy_cosp)
}
fn angle_delta(start: f64, end: f64) -> f64 {
let two_pi = std::f64::consts::TAU;
(end - start + std::f64::consts::PI).rem_euclid(two_pi) - std::f64::consts::PI
}
#[cfg(test)]
mod tests {
use crate::api::v1::localize::{
AffectedKeyframeSummary, LocalizationRevision, LocalizationRevisionCause,
LocalizationRevisionId,
};
use crate::api::v1::map::{MapRevision, MapRevisionCause, MapRevisionId};
use crate::api::v1::simulation::pose::Pose;
use super::{
Meters, assert_forward_delta, assert_lateral_drift, assert_revision_linked,
assert_revision_monotonic, assert_yaw_drift,
};
#[test]
fn forward_delta_uses_x_axis() {
let start = pose([1.0, 2.0, 0.0], [0.0, 0.0, 0.0, 1.0]);
let end = pose([2.05, 2.2, 0.0], [0.0, 0.0, 0.0, 1.0]);
assert_forward_delta(&start, &end, Meters(1.0), Meters(0.1)).unwrap();
}
#[test]
fn lateral_drift_rejects_excess_y_axis_motion() {
let start = pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]);
let end = pose([0.0, 0.3, 0.0], [0.0, 0.0, 0.0, 1.0]);
assert!(assert_lateral_drift(&start, &end, Meters(0.1)).is_err());
}
#[test]
fn yaw_drift_wraps_across_pi() {
let start = pose([0.0, 0.0, 0.0], yaw_quaternion(std::f64::consts::PI - 0.01));
let end = pose(
[0.0, 0.0, 0.0],
yaw_quaternion(-std::f64::consts::PI + 0.01),
);
assert_yaw_drift(&start, &end, super::Radians(0.03)).unwrap();
}
#[test]
fn linked_revisions_must_match() {
let localize = LocalizationRevision {
revision_id: LocalizationRevisionId {
epoch: 1,
sequence: 7,
},
previous_revision_id: None,
cause: LocalizationRevisionCause::SensorIntegration,
affected_keyframes: AffectedKeyframeSummary {
keyframe_ids: Vec::new(),
region: None,
},
inline_correction_available: false,
correction_fetch_required: false,
};
let map = MapRevision {
map_revision_id: MapRevisionId {
epoch: 1,
sequence: 3,
},
previous_map_revision_id: None,
built_from_localize_revision: localize.revision_id,
cause: MapRevisionCause::SensorIntegration,
affected_region: None,
};
assert_revision_linked(&map, &localize).unwrap();
}
#[test]
fn monotonic_revision_rejects_equal_sequence() {
let revisions = [
LocalizationRevisionId {
epoch: 1,
sequence: 2,
},
LocalizationRevisionId {
epoch: 1,
sequence: 2,
},
];
assert!(assert_revision_monotonic(&revisions).is_err());
}
fn pose(translation_m: [f64; 3], rotation_xyzw: [f64; 4]) -> Pose {
Pose {
frame_id: "map".to_string(),
translation_m,
rotation_xyzw,
}
}
fn yaw_quaternion(yaw_rad: f64) -> [f64; 4] {
[0.0, 0.0, (yaw_rad / 2.0).sin(), (yaw_rad / 2.0).cos()]
}
}