use std::collections::BTreeSet;
use std::path::{Path, PathBuf};
use crate::api::v1::frame::{FrameId, FrameLookupResponse, FrameTransform, Source};
use crate::api::v1::localize::{
AffectedKeyframeSummary, Keyframe, KeyframeId, LocalizationRevision, LocalizationRevisionCause,
LocalizationRevisionId, PoseEstimate as LocalizePoseEstimate, Region,
};
use crate::api::v1::odometry::{
OdometryEstimate, PoseEstimate, Status, StatusMode, VelocityEstimate,
};
use crate::api::v1::presence::{Heartbeat, Readiness, RuntimeId, RuntimeReadiness, Summary};
use crate::api::v1::simulation::pose::Pose;
use crate::bus::topic::Topic;
use crate::model::v1;
use anyhow::{Context, Result, bail, ensure};
use nalgebra::{Isometry3, Quaternion, Translation3, UnitQuaternion};
const TRACK_WIDTH_M: f64 = 0.40;
pub fn assert_topic_schema<Kind>(
topic: Topic<Kind>,
schema_name: &str,
schema_version: u32,
label: &str,
) -> Result<()> {
ensure!(
topic.schema() == schema_name,
"{label} schema name drifted: expected {schema_name}, got {}",
topic.schema()
);
ensure!(
topic.version() == schema_version,
"{label} schema version drifted: expected {schema_version}, got {}",
topic.version()
);
Ok(())
}
pub fn assert_close(name: &str, actual: f64, expected: f64, tolerance: f64) -> Result<()> {
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}"
)
}
}
pub fn heartbeat(runtime_id: &str, readiness: Readiness) -> Heartbeat {
Heartbeat {
runtime_id: RuntimeId::new(runtime_id),
readiness,
}
}
pub fn readiness_summary(mut heartbeats: Vec<Heartbeat>) -> Summary {
heartbeats.sort_by(|left, right| left.runtime_id.0.cmp(&right.runtime_id.0));
Summary {
autonomy_ready: false,
runtimes: heartbeats
.into_iter()
.map(|heartbeat| RuntimeReadiness {
runtime_id: heartbeat.runtime_id,
readiness: heartbeat.readiness,
})
.collect(),
}
}
pub fn assert_ready_summary(summary: &Summary, expected_runtime_ids: &[&str]) -> Result<()> {
let actual = summary
.runtimes
.iter()
.map(|runtime| runtime.runtime_id.0.as_str())
.collect::<BTreeSet<_>>();
let expected = expected_runtime_ids
.iter()
.copied()
.collect::<BTreeSet<_>>();
ensure!(
actual == expected,
"presence summary runtimes differ: actual={actual:?}, expected={expected:?}"
);
ensure!(
summary
.runtimes
.iter()
.all(|runtime| runtime.readiness == Readiness::Ready),
"presence summary includes a runtime that is not Ready"
);
Ok(())
}
pub fn readiness_for(summary: &Summary, runtime_id: &str) -> Option<Readiness> {
summary
.runtimes
.iter()
.find(|runtime| runtime.runtime_id.0 == runtime_id)
.map(|runtime| runtime.readiness)
}
pub fn compose(
parent_to_mid: &FrameTransform,
mid_to_child: &FrameTransform,
parent: &FrameId,
child: &FrameId,
) -> FrameTransform {
let composed = isometry(parent_to_mid) * isometry(mid_to_child);
let rotation = composed.rotation.quaternion();
FrameTransform {
parent_frame_id: Some(parent.clone()),
child_frame_id: child.clone(),
translation_m: [
composed.translation.vector.x,
composed.translation.vector.y,
composed.translation.vector.z,
],
rotation_xyzw: [rotation.i, rotation.j, rotation.k, rotation.w],
source: Source::Lookup,
}
}
pub fn isometry(transform: &FrameTransform) -> Isometry3<f64> {
Isometry3::from_parts(
Translation3::new(
transform.translation_m[0],
transform.translation_m[1],
transform.translation_m[2],
),
UnitQuaternion::from_quaternion(Quaternion::new(
transform.rotation_xyzw[3],
transform.rotation_xyzw[0],
transform.rotation_xyzw[1],
transform.rotation_xyzw[2],
)),
)
}
pub fn ok_transform(response: FrameLookupResponse) -> Result<FrameTransform> {
match response {
FrameLookupResponse::Ok { transform, .. } => Ok(transform),
FrameLookupResponse::UnknownFrame { frame_id } => {
bail!("lookup reported unknown frame {frame_id}")
}
FrameLookupResponse::DisconnectedTree {
parent_frame_id,
child_frame_id,
} => bail!("lookup reported disconnected tree {parent_frame_id}->{child_frame_id}"),
FrameLookupResponse::ExtrapolationTooOld {
oldest_available_ns,
} => bail!("lookup reported data too old; oldest available {oldest_available_ns}"),
FrameLookupResponse::ExtrapolationTooNew {
newest_available_ns,
} => bail!("lookup reported data too new; newest available {newest_available_ns}"),
FrameLookupResponse::Busy => bail!("lookup reported busy"),
}
}
pub fn fixture_robot(fixture_bundle: &str) -> Result<v1::Robot> {
let workspace_root = workspace_root()?;
let bundle_path = fixture_bundle_path(&workspace_root, fixture_bundle);
v1::Robot::read_from_dir(&bundle_path).context("failed to load scenario fixture robot")
}
pub fn workspace_root() -> Result<PathBuf> {
let manifest_dir = PathBuf::from(
std::env::var("CARGO_MANIFEST_DIR")
.context("CARGO_MANIFEST_DIR not set; run via a runtime cargo package")?,
);
let Some(workspace_root) = manifest_dir.parent().and_then(|path| path.parent()) else {
bail!(
"runtime CARGO_MANIFEST_DIR must live two levels below the workspace root: {}",
manifest_dir.display()
);
};
let workspace_toml = workspace_root.join("Cargo.toml");
debug_assert!(
std::fs::read_to_string(&workspace_toml)
.map(|contents| contents.contains("[workspace]"))
.unwrap_or(false),
"{} must be the workspace Cargo.toml",
workspace_toml.display()
);
Ok(workspace_root.to_path_buf())
}
pub fn fixture_bundle_path(workspace_root: &Path, fixture_bundle: &str) -> PathBuf {
workspace_root
.join("fixture")
.join("robot")
.join(fixture_bundle)
}
pub fn localization_revision(
epoch: u64,
sequence: u64,
previous_revision_id: Option<LocalizationRevisionId>,
) -> LocalizationRevision {
let keyframe_id = KeyframeId::new(format!("kf-{sequence}"));
LocalizationRevision {
revision_id: LocalizationRevisionId { epoch, sequence },
previous_revision_id,
cause: LocalizationRevisionCause::SensorIntegration,
affected_keyframes: AffectedKeyframeSummary {
keyframe_ids: vec![keyframe_id],
region: Some(Region {
frame_id: FrameId::new("map"),
min_xyz_m: [-1.0, -1.0, 0.0],
max_xyz_m: [1.0, 1.0, 1.0],
}),
},
inline_correction_available: false,
correction_fetch_required: false,
}
}
pub fn keyframe(keyframe_id: &str, revision: LocalizationRevisionId) -> Keyframe {
Keyframe {
keyframe_id: KeyframeId::new(keyframe_id),
revision,
pose: LocalizePoseEstimate {
frame_id: FrameId::new("map"),
child_frame_id: FrameId::new("base_link"),
translation_m: [1.0, 0.0, 0.0],
rotation_xyzw: yaw_quaternion(0.0),
},
descriptors: Vec::new(),
}
}
pub fn estimate_from_wheel_delta(left_delta_m: f64, right_delta_m: f64) -> OdometryEstimate {
let delta_center_m = (left_delta_m + right_delta_m) / 2.0;
let delta_yaw_rad = (right_delta_m - left_delta_m) / TRACK_WIDTH_M;
let (x, y) = if delta_yaw_rad.abs() <= f64::EPSILON {
(delta_center_m, 0.0)
} else {
let radius = delta_center_m / delta_yaw_rad;
(
radius * delta_yaw_rad.sin(),
radius * (1.0 - delta_yaw_rad.cos()),
)
};
OdometryEstimate {
pose: PoseEstimate {
frame_id: FrameId::new("odom"),
child_frame_id: FrameId::new("base_footprint"),
translation_m: [x, y, 0.0],
rotation_xyzw: yaw_quaternion(delta_yaw_rad),
},
velocity: VelocityEstimate {
frame_id: FrameId::new("base_footprint"),
linear_mps: [delta_center_m, 0.0, 0.0],
angular_radps: [0.0, 0.0, delta_yaw_rad],
},
covariance: Some(crate::api::v1::odometry::Covariance {
values: vec![0.0; 36],
}),
status: Status {
mode: StatusMode::Tracking,
reasons: Vec::new(),
},
}
}
pub fn pose_from_estimate(estimate: &OdometryEstimate) -> Pose {
Pose {
frame_id: estimate.pose.frame_id.0.clone(),
translation_m: estimate.pose.translation_m,
rotation_xyzw: estimate.pose.rotation_xyzw,
}
}
pub fn origin_pose() -> Pose {
Pose {
frame_id: "odom".to_string(),
translation_m: [0.0, 0.0, 0.0],
rotation_xyzw: yaw_quaternion(0.0),
}
}
pub fn yaw_quaternion(yaw_rad: f64) -> [f64; 4] {
[0.0, 0.0, (yaw_rad / 2.0).sin(), (yaw_rad / 2.0).cos()]
}
pub 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)
}