1use astrs_data::{AstrsMessage, DataError, DataType, Field, RecordBatch, Result};
36
37use super::geometry::{Quaternion, Vector3};
38use super::{build, read, single_row_column};
39
40pub const COVARIANCE_3X3: usize = 9;
42
43#[derive(Debug, Clone, Default, PartialEq)]
45pub struct LaserScan {
46 pub angle_min: f32,
48 pub angle_max: f32,
50 pub angle_increment: f32,
52 pub time_increment: f32,
54 pub scan_time: f32,
56 pub range_min: f32,
58 pub range_max: f32,
60 pub ranges: Vec<f32>,
62 pub intensities: Vec<f32>,
64}
65
66impl LaserScan {
67 #[must_use]
69 pub fn beam_count(&self) -> usize {
70 self.ranges.len()
71 }
72
73 #[must_use]
75 pub fn angle_of(&self, index: usize) -> Option<f32> {
76 if index >= self.ranges.len() {
77 return None;
78 }
79 let step = u32::try_from(index).unwrap_or(u32::MAX);
82 #[expect(
83 clippy::cast_precision_loss,
84 reason = "a beam index is far below f32's 2^24 exact-integer ceiling"
85 )]
86 Some(self.angle_min + self.angle_increment * (step as f32))
87 }
88
89 #[must_use]
91 pub fn has_intensities(&self) -> bool {
92 !self.intensities.is_empty() && self.intensities.len() == self.ranges.len()
93 }
94
95 #[must_use]
97 pub fn layout() -> DataType {
98 DataType::strukt([
99 Field::required("angle_min", DataType::Float32),
100 Field::required("angle_max", DataType::Float32),
101 Field::required("angle_increment", DataType::Float32),
102 Field::required("time_increment", DataType::Float32),
103 Field::required("scan_time", DataType::Float32),
104 Field::required("range_min", DataType::Float32),
105 Field::required("range_max", DataType::Float32),
106 Field::required(
107 "ranges",
108 DataType::list(Field::required("range", DataType::Float32)),
109 ),
110 Field::required(
111 "intensities",
112 DataType::list(Field::required("intensity", DataType::Float32)),
113 ),
114 ])
115 }
116}
117
118impl AstrsMessage for LaserScan {
119 const URN: &'static str = "std/sensor/v1/LaserScan";
120
121 fn data_type() -> DataType {
122 Self::layout()
123 }
124
125 fn to_record_batch(&self) -> Result<RecordBatch> {
126 let column = build::structure(vec![
127 ("angle_min", build::primitive::<f32>(&[self.angle_min])),
128 ("angle_max", build::primitive::<f32>(&[self.angle_max])),
129 (
130 "angle_increment",
131 build::primitive::<f32>(&[self.angle_increment]),
132 ),
133 (
134 "time_increment",
135 build::primitive::<f32>(&[self.time_increment]),
136 ),
137 ("scan_time", build::primitive::<f32>(&[self.scan_time])),
138 ("range_min", build::primitive::<f32>(&[self.range_min])),
139 ("range_max", build::primitive::<f32>(&[self.range_max])),
140 (
141 "ranges",
142 build::primitive_lists::<f32>("range", &[&self.ranges])?,
143 ),
144 (
145 "intensities",
146 build::primitive_lists::<f32>("intensity", &[&self.intensities])?,
147 ),
148 ])?;
149 Ok(RecordBatch::from_payload(column))
150 }
151
152 fn from_record_batch(batch: &RecordBatch) -> Result<Self> {
153 let column = single_row_column(batch)?;
154 Ok(Self {
155 angle_min: read::f32_at(read::child(column, "angle_min")?, 0)?,
156 angle_max: read::f32_at(read::child(column, "angle_max")?, 0)?,
157 angle_increment: read::f32_at(read::child(column, "angle_increment")?, 0)?,
158 time_increment: read::f32_at(read::child(column, "time_increment")?, 0)?,
159 scan_time: read::f32_at(read::child(column, "scan_time")?, 0)?,
160 range_min: read::f32_at(read::child(column, "range_min")?, 0)?,
161 range_max: read::f32_at(read::child(column, "range_max")?, 0)?,
162 ranges: read::primitive_list_at::<f32>(read::child(column, "ranges")?, 0)?,
163 intensities: read::primitive_list_at::<f32>(read::child(column, "intensities")?, 0)?,
164 })
165 }
166}
167
168super::impl_from_payload!(LaserScan);
169
170#[derive(Debug, Clone, PartialEq)]
172pub struct Imu {
173 pub orientation: Quaternion,
175 pub orientation_covariance: [f64; COVARIANCE_3X3],
177 pub angular_velocity: Vector3,
179 pub angular_velocity_covariance: [f64; COVARIANCE_3X3],
181 pub linear_acceleration: Vector3,
183 pub linear_acceleration_covariance: [f64; COVARIANCE_3X3],
185}
186
187impl Imu {
188 #[must_use]
191 pub const fn from_readings(
192 orientation: Quaternion,
193 angular_velocity: Vector3,
194 linear_acceleration: Vector3,
195 ) -> Self {
196 Self {
197 orientation,
198 orientation_covariance: [0.0; COVARIANCE_3X3],
199 angular_velocity,
200 angular_velocity_covariance: [0.0; COVARIANCE_3X3],
201 linear_acceleration,
202 linear_acceleration_covariance: [0.0; COVARIANCE_3X3],
203 }
204 }
205
206 #[must_use]
208 pub fn layout() -> DataType {
209 DataType::strukt([
210 Field::required("orientation", Quaternion::layout()),
211 Field::required("orientation_covariance", covariance_layout(3)),
212 Field::required("angular_velocity", Vector3::layout()),
213 Field::required("angular_velocity_covariance", covariance_layout(3)),
214 Field::required("linear_acceleration", Vector3::layout()),
215 Field::required("linear_acceleration_covariance", covariance_layout(3)),
216 ])
217 }
218}
219
220impl Default for Imu {
221 fn default() -> Self {
222 Self::from_readings(Quaternion::IDENTITY, Vector3::ZERO, Vector3::ZERO)
223 }
224}
225
226impl AstrsMessage for Imu {
227 const URN: &'static str = "std/sensor/v1/Imu";
228
229 fn data_type() -> DataType {
230 Self::layout()
231 }
232
233 fn to_record_batch(&self) -> Result<RecordBatch> {
234 let column = build::structure(vec![
235 (
236 "orientation",
237 Quaternion::column(core::slice::from_ref(&self.orientation))?,
238 ),
239 (
240 "orientation_covariance",
241 build::fixed_tuples::<f64>("v", 9, &self.orientation_covariance)?,
242 ),
243 (
244 "angular_velocity",
245 Vector3::column(core::slice::from_ref(&self.angular_velocity))?,
246 ),
247 (
248 "angular_velocity_covariance",
249 build::fixed_tuples::<f64>("v", 9, &self.angular_velocity_covariance)?,
250 ),
251 (
252 "linear_acceleration",
253 Vector3::column(core::slice::from_ref(&self.linear_acceleration))?,
254 ),
255 (
256 "linear_acceleration_covariance",
257 build::fixed_tuples::<f64>("v", 9, &self.linear_acceleration_covariance)?,
258 ),
259 ])?;
260 Ok(RecordBatch::from_payload(column))
261 }
262
263 fn from_record_batch(batch: &RecordBatch) -> Result<Self> {
264 let column = single_row_column(batch)?;
265 Ok(Self {
266 orientation: Quaternion::at(read::child(column, "orientation")?, 0)?,
267 orientation_covariance: covariance_at(
268 read::child(column, "orientation_covariance")?,
269 "orientation_covariance",
270 )?,
271 angular_velocity: Vector3::at(read::child(column, "angular_velocity")?, 0)?,
272 angular_velocity_covariance: covariance_at(
273 read::child(column, "angular_velocity_covariance")?,
274 "angular_velocity_covariance",
275 )?,
276 linear_acceleration: Vector3::at(read::child(column, "linear_acceleration")?, 0)?,
277 linear_acceleration_covariance: covariance_at(
278 read::child(column, "linear_acceleration_covariance")?,
279 "linear_acceleration_covariance",
280 )?,
281 })
282 }
283}
284
285super::impl_from_payload!(Imu);
286
287#[derive(Debug, Clone, PartialEq)]
289pub struct NavSatFix {
290 pub status: i8,
292 pub service: u16,
294 pub latitude: f64,
296 pub longitude: f64,
298 pub altitude: f64,
300 pub position_covariance: [f64; COVARIANCE_3X3],
302 pub position_covariance_type: u8,
304}
305
306impl NavSatFix {
307 #[must_use]
309 pub const fn at_coordinate(latitude: f64, longitude: f64, altitude: f64) -> Self {
310 Self {
311 status: 0,
312 service: 1,
313 latitude,
314 longitude,
315 altitude,
316 position_covariance: [0.0; COVARIANCE_3X3],
317 position_covariance_type: 0,
318 }
319 }
320
321 #[must_use]
323 pub fn layout() -> DataType {
324 DataType::strukt([
325 Field::required("status", DataType::Int8),
326 Field::required("service", DataType::UInt16),
327 Field::required("latitude", DataType::Float64),
328 Field::required("longitude", DataType::Float64),
329 Field::required("altitude", DataType::Float64),
330 Field::required("position_covariance", covariance_layout(3)),
331 Field::required("position_covariance_type", DataType::UInt8),
332 ])
333 }
334}
335
336impl Default for NavSatFix {
337 fn default() -> Self {
338 Self::at_coordinate(0.0, 0.0, 0.0)
339 }
340}
341
342impl AstrsMessage for NavSatFix {
343 const URN: &'static str = "std/sensor/v1/NavSatFix";
344
345 fn data_type() -> DataType {
346 Self::layout()
347 }
348
349 fn to_record_batch(&self) -> Result<RecordBatch> {
350 let column = build::structure(vec![
351 ("status", build::primitive::<i8>(&[self.status])),
352 ("service", build::primitive::<u16>(&[self.service])),
353 ("latitude", build::primitive::<f64>(&[self.latitude])),
354 ("longitude", build::primitive::<f64>(&[self.longitude])),
355 ("altitude", build::primitive::<f64>(&[self.altitude])),
356 (
357 "position_covariance",
358 build::fixed_tuples::<f64>("v", 9, &self.position_covariance)?,
359 ),
360 (
361 "position_covariance_type",
362 build::primitive::<u8>(&[self.position_covariance_type]),
363 ),
364 ])?;
365 Ok(RecordBatch::from_payload(column))
366 }
367
368 fn from_record_batch(batch: &RecordBatch) -> Result<Self> {
369 let column = single_row_column(batch)?;
370 Ok(Self {
371 status: read::primitive_at::<i8>(read::child(column, "status")?, 0)?,
372 service: read::primitive_at::<u16>(read::child(column, "service")?, 0)?,
373 latitude: read::f64_at(read::child(column, "latitude")?, 0)?,
374 longitude: read::f64_at(read::child(column, "longitude")?, 0)?,
375 altitude: read::f64_at(read::child(column, "altitude")?, 0)?,
376 position_covariance: covariance_at(
377 read::child(column, "position_covariance")?,
378 "position_covariance",
379 )?,
380 position_covariance_type: read::primitive_at::<u8>(
381 read::child(column, "position_covariance_type")?,
382 0,
383 )?,
384 })
385 }
386}
387
388super::impl_from_payload!(NavSatFix);
389
390#[derive(Debug, Clone, Copy, Default, PartialEq)]
392pub struct Range {
393 pub radiation_type: u8,
396 pub field_of_view: f32,
398 pub min_range: f32,
400 pub max_range: f32,
402 pub range: f32,
404}
405
406impl Range {
407 #[must_use]
409 pub fn is_in_range(&self) -> bool {
410 self.range >= self.min_range && self.range <= self.max_range
411 }
412
413 #[must_use]
415 pub fn layout() -> DataType {
416 DataType::strukt([
417 Field::required("radiation_type", DataType::UInt8),
418 Field::required("field_of_view", DataType::Float32),
419 Field::required("min_range", DataType::Float32),
420 Field::required("max_range", DataType::Float32),
421 Field::required("range", DataType::Float32),
422 ])
423 }
424}
425
426impl AstrsMessage for Range {
427 const URN: &'static str = "std/sensor/v1/Range";
428
429 fn data_type() -> DataType {
430 Self::layout()
431 }
432
433 fn to_record_batch(&self) -> Result<RecordBatch> {
434 let column = build::structure(vec![
435 (
436 "radiation_type",
437 build::primitive::<u8>(&[self.radiation_type]),
438 ),
439 (
440 "field_of_view",
441 build::primitive::<f32>(&[self.field_of_view]),
442 ),
443 ("min_range", build::primitive::<f32>(&[self.min_range])),
444 ("max_range", build::primitive::<f32>(&[self.max_range])),
445 ("range", build::primitive::<f32>(&[self.range])),
446 ])?;
447 Ok(RecordBatch::from_payload(column))
448 }
449
450 fn from_record_batch(batch: &RecordBatch) -> Result<Self> {
451 let column = single_row_column(batch)?;
452 Ok(Self {
453 radiation_type: read::primitive_at::<u8>(read::child(column, "radiation_type")?, 0)?,
454 field_of_view: read::f32_at(read::child(column, "field_of_view")?, 0)?,
455 min_range: read::f32_at(read::child(column, "min_range")?, 0)?,
456 max_range: read::f32_at(read::child(column, "max_range")?, 0)?,
457 range: read::f32_at(read::child(column, "range")?, 0)?,
458 })
459 }
460}
461
462super::impl_from_payload!(Range);
463
464#[derive(Debug, Clone, Default, PartialEq)]
475pub struct PointCloud {
476 columns: Vec<(String, Vec<f32>)>,
478}
479
480impl PointCloud {
481 pub fn from_columns(columns: Vec<(String, Vec<f32>)>) -> Result<Self> {
489 let value = Self { columns };
490 value.check()?;
491 Ok(value)
492 }
493
494 pub fn xyz(x: Vec<f32>, y: Vec<f32>, z: Vec<f32>) -> Result<Self> {
500 Self::from_columns(vec![
501 ("x".to_owned(), x),
502 ("y".to_owned(), y),
503 ("z".to_owned(), z),
504 ])
505 }
506
507 #[must_use]
509 pub fn field_names(&self) -> Vec<&str> {
510 self.columns.iter().map(|(name, _)| name.as_str()).collect()
511 }
512
513 #[must_use]
515 pub fn column(&self, name: &str) -> Option<&[f32]> {
516 self.columns
517 .iter()
518 .find(|(field, _)| field == name)
519 .map(|(_, values)| values.as_slice())
520 }
521
522 #[must_use]
524 pub fn len(&self) -> usize {
525 self.columns.first().map_or(0, |(_, values)| values.len())
526 }
527
528 #[must_use]
530 pub fn is_empty(&self) -> bool {
531 self.len() == 0
532 }
533
534 #[must_use]
536 pub fn urn(&self) -> String {
537 format!(
538 "std/sensor/v1/PointCloud[fields={}]",
539 self.field_names().join(":")
540 )
541 }
542
543 #[must_use]
545 pub fn layout(&self) -> DataType {
546 DataType::strukt(self.columns.iter().map(|(name, _)| {
547 Field::required(
548 name,
549 DataType::list(Field::required("v", DataType::Float32)),
550 )
551 }))
552 }
553
554 pub fn check(&self) -> Result<()> {
560 let mut seen: Vec<&str> = Vec::with_capacity(self.columns.len());
561 let expected = self.len();
562 for (index, (name, values)) in self.columns.iter().enumerate() {
563 if seen.contains(&name.as_str()) {
564 return Err(DataError::DuplicateFieldName { name: name.clone() });
565 }
566 seen.push(name);
567 if values.len() != expected {
568 return Err(DataError::ColumnLengthMismatch {
569 index,
570 name: name.clone(),
571 expected,
572 actual: values.len(),
573 });
574 }
575 }
576 Ok(())
577 }
578
579 pub fn to_record_batch(&self) -> Result<RecordBatch> {
589 self.check()?;
590 let mut children: Vec<(&str, astrs_data::ArrayRef)> =
591 Vec::with_capacity(self.columns.len());
592 for (name, values) in &self.columns {
593 children.push((
594 name.as_str(),
595 build::primitive_lists::<f32>("v", &[values])?,
596 ));
597 }
598 Ok(RecordBatch::from_payload(build::structure(children)?))
599 }
600
601 pub fn from_record_batch(batch: &RecordBatch) -> Result<Self> {
607 let column = single_row_column(batch)?;
608 let strukt = read::structure(column)?;
609 let mut columns = Vec::with_capacity(strukt.fields().len());
610 for field in strukt.fields() {
611 let name = field.name().to_owned();
612 let child = read::field(strukt, &name)?;
613 columns.push((name, read::primitive_list_at::<f32>(child, 0)?));
614 }
615 Self::from_columns(columns)
616 }
617}
618
619impl super::FromPayload for PointCloud {
620 fn from_batch(batch: &RecordBatch) -> Result<Self> {
621 Self::from_record_batch(batch)
622 }
623}
624
625#[must_use]
627fn covariance_layout(n: i32) -> DataType {
628 DataType::fixed_size_list(Field::required("v", DataType::Float64), n * n)
629}
630
631fn covariance_at(column: &astrs_data::ArrayRef, field: &str) -> Result<[f64; COVARIANCE_3X3]> {
633 let values = read::fixed_tuple_at::<f64>(column, 0, COVARIANCE_3X3)?;
634 <[f64; COVARIANCE_3X3]>::try_from(values.as_slice()).map_err(|_| {
635 DataError::MessageFixedArrayLength {
636 field: field.to_owned(),
637 expected: COVARIANCE_3X3,
638 actual: values.len(),
639 }
640 })
641}
642
643#[cfg(test)]
644mod tests {
645 #![allow(clippy::unwrap_used, clippy::expect_used, clippy::panic)]
646
647 use super::*;
648 use crate::message::{assert_layout_matches, assert_registry_layout};
649
650 #[test]
651 fn every_fixed_sensor_type_conforms_to_the_registry() {
652 assert_registry_layout::<LaserScan>().unwrap();
653 assert_registry_layout::<Imu>().unwrap();
654 assert_registry_layout::<NavSatFix>().unwrap();
655 assert_registry_layout::<Range>().unwrap();
656 }
657
658 #[test]
659 fn laser_scans_round_trip() {
660 let scan = LaserScan {
661 angle_min: -1.5,
662 angle_max: 1.5,
663 angle_increment: 0.5,
664 time_increment: 0.001,
665 scan_time: 0.1,
666 range_min: 0.05,
667 range_max: 30.0,
668 ranges: vec![1.0, 2.0, 3.0],
669 intensities: vec![10.0, 20.0, 30.0],
670 };
671 let batch = scan.to_record_batch().unwrap();
672 assert_eq!(LaserScan::from_record_batch(&batch).unwrap(), scan);
673 assert_eq!(scan.beam_count(), 3);
674 assert!(scan.has_intensities());
675 assert_eq!(scan.angle_of(0), Some(-1.5));
676 assert_eq!(scan.angle_of(2), Some(-0.5));
677 assert_eq!(scan.angle_of(3), None);
678
679 let bare = LaserScan {
680 intensities: Vec::new(),
681 ..scan
682 };
683 assert!(!bare.has_intensities());
684 assert_eq!(
685 LaserScan::from_record_batch(&bare.to_record_batch().unwrap()).unwrap(),
686 bare
687 );
688 assert_eq!(LaserScan::default().beam_count(), 0);
689 }
690
691 #[test]
692 fn imu_readings_round_trip_with_their_covariances() {
693 let mut imu = Imu::from_readings(
694 Quaternion::identity(),
695 Vector3::new(0.0, 0.0, 0.1),
696 Vector3::new(0.0, 0.0, -9.81),
697 );
698 imu.orientation_covariance = [1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0];
699 let batch = imu.to_record_batch().unwrap();
700 let decoded = Imu::from_record_batch(&batch).unwrap();
701 assert_eq!(decoded, imu);
702 assert_eq!(decoded.orientation_covariance[4], 2.0);
703 assert_eq!(Imu::default().linear_acceleration, Vector3::ZERO);
704 }
705
706 #[test]
707 fn gnss_fixes_round_trip() {
708 let fix = NavSatFix::at_coordinate(35.681_2, 139.767_1, 40.0);
709 let batch = fix.to_record_batch().unwrap();
710 assert_eq!(NavSatFix::from_record_batch(&batch).unwrap(), fix);
711 assert_eq!(NavSatFix::default().latitude, 0.0);
712 }
713
714 #[test]
715 fn ranges_round_trip_and_report_their_window() {
716 let reading = Range {
717 radiation_type: 1,
718 field_of_view: 0.26,
719 min_range: 0.1,
720 max_range: 4.0,
721 range: 1.2,
722 };
723 let batch = reading.to_record_batch().unwrap();
724 assert_eq!(Range::from_record_batch(&batch).unwrap(), reading);
725 assert!(reading.is_in_range());
726 assert!(
727 !Range {
728 range: 9.0,
729 ..reading
730 }
731 .is_in_range()
732 );
733 }
734
735 #[test]
736 fn point_clouds_round_trip_and_render_their_urn() {
737 let cloud = PointCloud::xyz(vec![1.0, 2.0], vec![0.0, 0.5], vec![-1.0, -2.0]).unwrap();
738 assert_eq!(cloud.urn(), "std/sensor/v1/PointCloud[fields=x:y:z]");
739 assert_layout_matches(&cloud.urn(), &cloud.layout()).unwrap();
740
741 let batch = cloud.to_record_batch().unwrap();
742 let decoded = PointCloud::from_record_batch(&batch).unwrap();
743 assert_eq!(decoded, cloud);
744 assert_eq!(decoded.len(), 2);
745 assert!(!decoded.is_empty());
746 assert_eq!(decoded.field_names(), vec!["x", "y", "z"]);
747 assert_eq!(decoded.column("y"), Some(&[0.0, 0.5][..]));
748 assert_eq!(decoded.column("intensity"), None);
749 }
750
751 #[test]
752 fn a_malformed_point_cloud_is_refused() {
753 let error = PointCloud::from_columns(vec![
754 ("x".to_owned(), vec![1.0, 2.0]),
755 ("y".to_owned(), vec![1.0]),
756 ])
757 .unwrap_err();
758 assert!(matches!(error, DataError::ColumnLengthMismatch { .. }));
759
760 let error = PointCloud::from_columns(vec![
761 ("x".to_owned(), vec![1.0]),
762 ("x".to_owned(), vec![2.0]),
763 ])
764 .unwrap_err();
765 assert!(matches!(error, DataError::DuplicateFieldName { .. }));
766
767 assert!(PointCloud::default().is_empty());
768 assert_eq!(PointCloud::default().len(), 0);
769 }
770
771 #[test]
772 fn a_wrong_layout_is_refused() {
773 let range = Range::default().to_record_batch().unwrap();
774 assert!(Imu::from_record_batch(&range).is_err());
775 assert!(LaserScan::from_record_batch(&range).is_err());
776 assert!(NavSatFix::from_record_batch(&range).is_err());
777
778 let imu = Imu::default().to_record_batch().unwrap();
779 assert!(Range::from_record_batch(&imu).is_err());
780 assert!(PointCloud::from_record_batch(&imu).is_err());
781 }
782}