phoxal 0.67.0

Phoxal - production-oriented autonomous robot framework: the one framework library, holding the runtime engine, the api contract tree, the typed bus, the canonical model, and the bundle.
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
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159

crate::endpoints! {
    sample: Sample<Sample>;
}

#[derive(
    phoxal_macros::DescribeWire,
    Copy,
    Eq,
    Clone,
    Debug,
    PartialEq,
    serde::Serialize,
    serde::Deserialize,
)]
#[serde(rename_all = "snake_case")]
pub enum SensorHealth {
    Nominal,
    Degraded,
    Fault,
}
#[derive(
    phoxal_macros::DescribeWire, Copy, Clone, Debug, PartialEq, serde::Serialize, serde::Deserialize,
)]
pub struct Bias {
    pub angular_velocity_radps: [f32; 3],
    pub linear_acceleration_mps2: [f32; 3],
}
#[derive(
    phoxal_macros::DescribeWire, Clone, Debug, PartialEq, serde::Serialize, serde::Deserialize,
)]
#[serde(try_from = "SampleWire")]
pub struct Sample {
    pub orientation: Option<[f32; 4]>,
    pub angular_velocity_radps: [f32; 3],
    pub linear_acceleration_mps2: [f32; 3],
    pub covariance: Option<[f32; 9]>,
    pub noise_density: Option<[f32; 3]>,
    pub sensor_frame_id: Option<String>,
    pub health: SensorHealth,
    pub bias: Option<Bias>,
}
#[derive(serde::Deserialize)]
struct SampleWire {
    orientation: Option<[f32; 4]>,
    angular_velocity_radps: [f32; 3],
    linear_acceleration_mps2: [f32; 3],
    covariance: Option<[f32; 9]>,
    noise_density: Option<[f32; 3]>,
    sensor_frame_id: Option<String>,
    health: SensorHealth,
    bias: Option<Bias>,
}
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct InvalidSample(&'static str);
impl std::fmt::Display for InvalidSample {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        f.write_str(self.0)
    }
}
impl std::error::Error for InvalidSample {}
impl Sample {
    #[expect(
        clippy::too_many_arguments,
        reason = "the validating constructor receives the complete wire sample atomically"
    )]
    pub fn try_new(
        orientation: Option<[f32; 4]>,
        angular_velocity_radps: [f32; 3],
        linear_acceleration_mps2: [f32; 3],
        covariance: Option<[f32; 9]>,
        noise_density: Option<[f32; 3]>,
        sensor_frame_id: Option<String>,
        health: SensorHealth,
        bias: Option<Bias>,
    ) -> Result<Self, InvalidSample> {
        if !angular_velocity_radps
            .iter()
            .chain(linear_acceleration_mps2.iter())
            .all(|v| v.is_finite())
            || !covariance
                .iter()
                .flatten()
                .all(|v| v.is_finite() && *v >= 0.0)
            || !noise_density
                .iter()
                .flatten()
                .all(|v| v.is_finite() && *v >= 0.0)
            || bias.is_some_and(|b| {
                !b.angular_velocity_radps
                    .iter()
                    .chain(b.linear_acceleration_mps2.iter())
                    .all(|v| v.is_finite())
            })
        {
            return Err(InvalidSample(
                "IMU vectors, covariance, noise, and bias must be finite; covariance and noise nonnegative",
            ));
        }
        if let Some(q) = orientation {
            let n2: f32 = q.iter().map(|x| x * x).sum();
            if !q.iter().all(|v| v.is_finite()) || (n2.sqrt() - 1.0).abs() > 1e-3 {
                return Err(InvalidSample(
                    "IMU orientation must be a unit finite quaternion",
                ));
            }
        }
        if sensor_frame_id
            .as_ref()
            .is_some_and(|id| id.trim().is_empty())
        {
            return Err(InvalidSample("IMU frame id must be nonempty"));
        }
        Ok(Self {
            orientation,
            angular_velocity_radps,
            linear_acceleration_mps2,
            covariance,
            noise_density,
            sensor_frame_id,
            health,
            bias,
        })
    }
}
impl TryFrom<SampleWire> for Sample {
    type Error = InvalidSample;
    fn try_from(v: SampleWire) -> Result<Self, Self::Error> {
        Self::try_new(
            v.orientation,
            v.angular_velocity_radps,
            v.linear_acceleration_mps2,
            v.covariance,
            v.noise_density,
            v.sensor_frame_id,
            v.health,
            v.bias,
        )
    }
}
#[cfg(test)]
mod tests {
    use super::*;
    #[test]
    fn constructor_requires_unit_quaternion() {
        assert!(
            Sample::try_new(
                Some([2.0, 0.0, 0.0, 0.0]),
                [0.0; 3],
                [0.0; 3],
                None,
                None,
                None,
                SensorHealth::Nominal,
                None
            )
            .is_err()
        );
    }
}