phoxal 0.6.0

Phoxal — production-oriented autonomous robot framework (engine, model, typed bus, contracts).
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137

use serde::{Deserialize, Serialize};

pub const MILLIMETERS_PER_METER: f32 = 1000.0;

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Depth {
    /// Dense depth samples encoded as unsigned 16-bit millimeters.
    ///
    /// Contract:
    /// - resolution is static component metadata, not packet data
    /// - each sample stores perspective depth along the sensor forward axis,
    ///   not radial ray length
    /// - samples are row-major over the static depth grid
    /// - columns increase to the sensor's right and rows increase downward
    /// - producers publish only complete grids with valid non-zero samples
    /// - the camera frame uses +X forward, +Y left, +Z up after the producer's
    ///   configured sensor transform is applied into the robot frame
    ///
    /// Producers and consumers must treat this as one shared geometry rule so
    /// Webots and real hardware publish compatible depth data.
    samples_mm: Vec<u16>,
    encoding: Encoding,
    invalid_sample_policy: InvalidSamplePolicy,
    width: Option<u32>,
    height: Option<u32>,
    intrinsics: Option<super::camera::Intrinsics>,
    distortion: Option<super::camera::Distortion>,
    exposure: Option<super::camera::ExposureTiming>,
    measured_at_ns: Option<u64>,
    calibration: Option<super::camera::CalibrationIdentity>,
}

impl Depth {
    pub fn new(samples_mm: Vec<u16>) -> Self {
        Self {
            samples_mm,
            encoding: Encoding::U16Millimeters,
            invalid_sample_policy: InvalidSamplePolicy::ZeroIsInvalid,
            width: None,
            height: None,
            intrinsics: None,
            distortion: None,
            exposure: None,
            measured_at_ns: None,
            calibration: None,
        }
    }

    pub fn with_resolution(mut self, width: u32, height: u32) -> Self {
        self.width = Some(width);
        self.height = Some(height);
        self
    }

    pub fn from_meters(data_m: impl IntoIterator<Item = f32>) -> Option<Self> {
        let samples_mm = data_m
            .into_iter()
            .map(meters_to_sample_mm)
            .collect::<Option<Vec<_>>>()?;
        (!samples_mm.is_empty()).then_some(Self::new(samples_mm))
    }

    pub fn samples_mm(&self) -> &[u16] {
        &self.samples_mm
    }

    pub const fn width(&self) -> Option<u32> {
        self.width
    }

    pub const fn height(&self) -> Option<u32> {
        self.height
    }
}

#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
#[serde(rename_all = "snake_case")]
pub enum Encoding {
    U16Millimeters,
}

#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
#[serde(rename_all = "snake_case")]
pub enum InvalidSamplePolicy {
    ZeroIsInvalid,
    NonFiniteIsInvalid,
}

fn meters_to_sample_mm(value_m: f32) -> Option<u16> {
    if value_m.is_finite() && value_m > 0.0 {
        let sample_mm = (value_m * MILLIMETERS_PER_METER).round();
        if sample_mm <= f32::from(u16::MAX) {
            Some(sample_mm.max(1.0) as u16)
        } else {
            None
        }
    } else {
        None
    }
}

pub const KIND: &str = "depth";

#[cfg(test)]
mod tests {
    use super::{Depth, meters_to_sample_mm};

    #[test]
    fn converts_meters_to_millimeter_samples() {
        assert_eq!(meters_to_sample_mm(1.234), Some(1234));
        assert_eq!(meters_to_sample_mm(0.0004), Some(1));
        assert_eq!(meters_to_sample_mm(f32::NAN), None);
        assert_eq!(meters_to_sample_mm(-1.0), None);
        assert_eq!(meters_to_sample_mm(100.0), None);
    }

    #[test]
    fn builds_depth_from_meters() {
        let depth = Depth::from_meters([0.5, 2.0]).expect("valid depth");

        assert_eq!(depth.samples_mm(), &[500, 2000]);
    }

    #[test]
    fn resolution_is_carried_when_producer_sets_it() {
        let depth = Depth::new(vec![1_000; 320 * 240]).with_resolution(320, 240);

        assert_eq!(depth.width(), Some(320));
        assert_eq!(depth.height(), Some(240));
    }

    #[test]
    fn rejects_invalid_meter_samples() {
        assert!(Depth::from_meters([0.5, f32::NAN]).is_none());
        assert!(Depth::from_meters([]).is_none());
    }
}