phoxal_api/api/robot/
navigation.rs

1const MAX_REQUEST_ID_LEN: usize = 128;
2const MAX_PATH_POSES: usize = 4096;
3
4fn finite(value: f64) -> bool {
5    value.is_finite()
6}
7
8fn finite_f32(value: f32) -> bool {
9    value.is_finite()
10}
11
12fn canonical_yaw(value: f64) -> bool {
13    value.is_finite() && (-std::f64::consts::PI..=std::f64::consts::PI).contains(&value)
14}
15
16fn optional_canonical_yaw(value: Option<f64>) -> bool {
17    value.is_none_or(canonical_yaw)
18}
19
20fn valid_request_id(value: &str) -> bool {
21    let trimmed = value.trim();
22    !trimmed.is_empty()
23        && trimmed.len() <= MAX_REQUEST_ID_LEN
24        && trimmed
25            .bytes()
26            .all(|byte| byte.is_ascii_alphanumeric() || matches!(byte, b'-' | b'_' | b'.'))
27}
28#[derive(Copy, Eq, Clone, Debug, PartialEq, serde::Serialize, serde::Deserialize)]
29#[serde(rename_all = "snake_case")]
30pub enum FailureReason {
31    LocalizationUnavailable,
32    MapUnavailable,
33    MapChanged,
34    NoPath,
35    Blocked,
36    Internal,
37}
38
39#[derive(Clone, Debug, PartialEq, serde::Serialize, serde::Deserialize)]
40pub enum Outcome {
41    Succeeded,
42    Failed(FailureReason),
43    Refused(RefusalReason),
44    Cancelled,
45    TimedOut,
46}
47
48/// A bounded caller-chosen request identity. The wire representation remains
49/// the historic `{ "value": "..." }` object, but callers cannot construct an
50/// invalid identity by writing the field directly.
51#[derive(Eq, PartialOrd, Ord, Clone, Debug, PartialEq, serde::Serialize, serde::Deserialize)]
52#[serde(try_from = "RequestIdWire")]
53pub struct RequestId {
54    value: String,
55}
56
57impl RequestId {
58    pub fn try_new(value: impl Into<String>) -> std::result::Result<Self, RequestIdError> {
59        let value = value.into();
60        valid_request_id(&value)
61            .then_some(Self { value })
62            .ok_or(RequestIdError::Invalid)
63    }
64
65    #[must_use]
66    pub fn as_str(&self) -> &str {
67        &self.value
68    }
69
70    #[must_use]
71    pub fn is_valid(&self) -> bool {
72        valid_request_id(&self.value)
73    }
74}
75
76#[derive(Clone, Debug, serde::Deserialize)]
77#[serde(deny_unknown_fields)]
78struct RequestIdWire {
79    value: String,
80}
81
82#[derive(Clone, Copy, Debug, PartialEq, Eq)]
83pub enum RequestIdError {
84    Invalid,
85}
86
87impl std::fmt::Display for RequestIdError {
88    fn fmt(&self, formatter: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
89        formatter.write_str("request id must be a non-empty bounded ASCII token")
90    }
91}
92impl std::error::Error for RequestIdError {}
93
94impl TryFrom<RequestIdWire> for RequestId {
95    type Error = RequestIdError;
96    fn try_from(value: RequestIdWire) -> std::result::Result<Self, Self::Error> {
97        Self::try_new(value.value)
98    }
99}
100
101/// A server-issued operation identity. It is scoped to the producer
102/// incarnation and sequence zero is reserved as the absent value.
103#[derive(Copy, Eq, Hash, Clone, Debug, PartialEq, serde::Serialize, serde::Deserialize)]
104#[serde(try_from = "NavigationOperationIdWire")]
105pub struct NavigationOperationId {
106    producer: ::phoxal_bus::ProducerId,
107    sequence: u64,
108}
109
110impl NavigationOperationId {
111    pub fn new(producer: ::phoxal_bus::ProducerId, sequence: u64) -> Option<Self> {
112        (sequence != 0).then_some(Self { producer, sequence })
113    }
114    #[must_use]
115    pub const fn producer(&self) -> ::phoxal_bus::ProducerId {
116        self.producer
117    }
118    #[must_use]
119    pub const fn sequence(&self) -> u64 {
120        self.sequence
121    }
122}
123
124#[derive(Clone, Debug, serde::Deserialize)]
125#[serde(deny_unknown_fields)]
126struct NavigationOperationIdWire {
127    producer: ::phoxal_bus::ProducerId,
128    sequence: u64,
129}
130
131impl TryFrom<NavigationOperationIdWire> for NavigationOperationId {
132    type Error = &'static str;
133    fn try_from(value: NavigationOperationIdWire) -> std::result::Result<Self, Self::Error> {
134        Self::new(value.producer, value.sequence)
135            .ok_or("navigation operation sequence must be nonzero")
136    }
137}
138
139#[derive(Clone, Debug, PartialEq, serde::Serialize, serde::Deserialize)]
140#[serde(try_from = "PoseWire")]
141pub struct Pose {
142    pub x_m: f64,
143    pub y_m: f64,
144    pub yaw_rad: Option<f64>,
145}
146
147impl Pose {
148    pub fn try_new(
149        x_m: f64,
150        y_m: f64,
151        yaw_rad: Option<f64>,
152    ) -> std::result::Result<Self, NavigationError> {
153        (finite(x_m) && finite(y_m) && optional_canonical_yaw(yaw_rad))
154            .then_some(Self { x_m, y_m, yaw_rad })
155            .ok_or(NavigationError::InvalidPose)
156    }
157}
158
159#[derive(Clone, Debug, serde::Deserialize)]
160#[serde(deny_unknown_fields)]
161struct PoseWire {
162    x_m: f64,
163    y_m: f64,
164    yaw_rad: Option<f64>,
165}
166impl TryFrom<PoseWire> for Pose {
167    type Error = NavigationError;
168    fn try_from(value: PoseWire) -> std::result::Result<Self, Self::Error> {
169        Self::try_new(value.x_m, value.y_m, value.yaw_rad)
170    }
171}
172
173#[derive(Clone, Debug, PartialEq, serde::Serialize, serde::Deserialize)]
174#[serde(try_from = "PathWire")]
175pub struct Path {
176    pub poses: Vec<Pose>,
177    pub map_revision: Option<u64>,
178}
179
180impl Path {
181    pub fn try_new(
182        poses: Vec<Pose>,
183        map_revision: Option<u64>,
184    ) -> std::result::Result<Self, NavigationError> {
185        (!poses.is_empty() && poses.len() <= MAX_PATH_POSES)
186            .then_some(Self {
187                poses,
188                map_revision,
189            })
190            .ok_or(NavigationError::PathBoundExceeded)
191    }
192}
193
194#[derive(Clone, Debug, serde::Deserialize)]
195#[serde(deny_unknown_fields)]
196struct PathWire {
197    poses: Vec<Pose>,
198    map_revision: Option<u64>,
199}
200impl TryFrom<PathWire> for Path {
201    type Error = NavigationError;
202    fn try_from(value: PathWire) -> std::result::Result<Self, Self::Error> {
203        Self::try_new(value.poses, value.map_revision)
204    }
205}
206
207#[derive(Clone, Debug, PartialEq, serde::Serialize, serde::Deserialize)]
208pub enum StartKind {
209    GotoPose(Pose),
210    FollowPath(Path),
211}
212
213#[derive(Clone, Debug, PartialEq, serde::Serialize, serde::Deserialize)]
214pub struct StartRequest {
215    pub request_id: RequestId,
216    pub kind: StartKind,
217}
218
219#[derive(Clone, Debug, PartialEq, serde::Serialize, serde::Deserialize)]
220pub enum StartResponse {
221    Accepted { operation_id: NavigationOperationId },
222    Refused(RefusalReason),
223}
224
225#[derive(Clone, Debug, PartialEq, serde::Serialize, serde::Deserialize)]
226pub struct CancelRequest {
227    pub operation_id: NavigationOperationId,
228}
229
230#[derive(Clone, Debug, PartialEq, serde::Serialize, serde::Deserialize)]
231pub enum CancelResponse {
232    Accepted,
233    Refused(RefusalReason),
234}
235
236#[derive(Clone, Debug, PartialEq, serde::Serialize, serde::Deserialize)]
237#[serde(rename_all = "snake_case")]
238pub enum RefusalReason {
239    Busy,
240    InvalidRequest,
241    Unsupported,
242    Unavailable,
243    NotOwner,
244    NotFound,
245}
246
247#[derive(Clone, Debug, PartialEq, serde::Serialize, serde::Deserialize)]
248pub enum State {
249    Idle,
250    Accepted(NavigationOperationId),
251    Running(NavigationOperationId),
252}
253
254#[derive(Clone, Debug, PartialEq, serde::Serialize, serde::Deserialize)]
255#[serde(try_from = "ProgressWire")]
256pub struct Progress {
257    pub operation_id: NavigationOperationId,
258    pub request_id: RequestId,
259    pub distance_remaining_m: f64,
260    pub path_index: u32,
261}
262
263#[derive(Clone, Debug, serde::Deserialize)]
264#[serde(deny_unknown_fields)]
265struct ProgressWire {
266    operation_id: NavigationOperationId,
267    request_id: RequestId,
268    distance_remaining_m: f64,
269    path_index: u32,
270}
271impl TryFrom<ProgressWire> for Progress {
272    type Error = NavigationError;
273    fn try_from(value: ProgressWire) -> std::result::Result<Self, Self::Error> {
274        (finite(value.distance_remaining_m)
275            && usize::try_from(value.path_index).is_ok_and(|n| n < MAX_PATH_POSES))
276        .then_some(Self {
277            operation_id: value.operation_id,
278            request_id: value.request_id,
279            distance_remaining_m: value.distance_remaining_m,
280            path_index: value.path_index,
281        })
282        .ok_or(NavigationError::InvalidProgress)
283    }
284}
285
286impl Progress {
287    pub fn try_new(
288        operation_id: NavigationOperationId,
289        request_id: RequestId,
290        distance_remaining_m: f64,
291        path_index: u32,
292    ) -> std::result::Result<Self, NavigationError> {
293        (finite(distance_remaining_m)
294            && usize::try_from(path_index).is_ok_and(|n| n < MAX_PATH_POSES))
295        .then_some(Self {
296            operation_id,
297            request_id,
298            distance_remaining_m,
299            path_index,
300        })
301        .ok_or(NavigationError::InvalidProgress)
302    }
303}
304
305#[derive(Clone, Debug, PartialEq, serde::Serialize, serde::Deserialize)]
306#[serde(try_from = "ResultWire")]
307pub struct Result {
308    pub operation_id: NavigationOperationId,
309    pub request_id: RequestId,
310    pub outcome: Outcome,
311}
312
313#[derive(Clone, Debug, serde::Deserialize)]
314#[serde(deny_unknown_fields)]
315struct ResultWire {
316    operation_id: NavigationOperationId,
317    request_id: RequestId,
318    outcome: Outcome,
319}
320impl TryFrom<ResultWire> for Result {
321    type Error = NavigationError;
322    fn try_from(value: ResultWire) -> std::result::Result<Self, Self::Error> {
323        Ok(Self {
324            operation_id: value.operation_id,
325            request_id: value.request_id,
326            outcome: value.outcome,
327        })
328    }
329}
330
331#[derive(Clone, Debug, PartialEq, serde::Serialize, serde::Deserialize)]
332#[serde(try_from = "CandidateWire")]
333pub struct Candidate {
334    pub operation_id: NavigationOperationId,
335    pub linear_x_mps: f32,
336    pub angular_z_radps: f32,
337}
338
339#[derive(Clone, Debug, serde::Deserialize)]
340#[serde(deny_unknown_fields)]
341struct CandidateWire {
342    operation_id: NavigationOperationId,
343    linear_x_mps: f32,
344    angular_z_radps: f32,
345}
346impl TryFrom<CandidateWire> for Candidate {
347    type Error = NavigationError;
348    fn try_from(value: CandidateWire) -> std::result::Result<Self, Self::Error> {
349        (finite_f32(value.linear_x_mps) && finite_f32(value.angular_z_radps))
350            .then_some(Self {
351                operation_id: value.operation_id,
352                linear_x_mps: value.linear_x_mps,
353                angular_z_radps: value.angular_z_radps,
354            })
355            .ok_or(NavigationError::InvalidCandidate)
356    }
357}
358
359impl Candidate {
360    pub fn try_new(
361        operation_id: NavigationOperationId,
362        linear_x_mps: f32,
363        angular_z_radps: f32,
364    ) -> std::result::Result<Self, NavigationError> {
365        (finite_f32(linear_x_mps) && finite_f32(angular_z_radps))
366            .then_some(Self {
367                operation_id,
368                linear_x_mps,
369                angular_z_radps,
370            })
371            .ok_or(NavigationError::InvalidCandidate)
372    }
373}
374
375#[derive(Clone, Debug, PartialEq, serde::Serialize, serde::Deserialize)]
376pub struct FrontierRequest {
377    pub map_revision: Option<u64>,
378}
379
380#[derive(Clone, Debug, PartialEq, serde::Serialize, serde::Deserialize)]
381#[serde(try_from = "FrontierWire")]
382pub struct Frontier {
383    pub x_m: f64,
384    pub y_m: f64,
385    pub score: f32,
386    pub size: u32,
387}
388
389#[derive(Clone, Debug, serde::Deserialize)]
390#[serde(deny_unknown_fields)]
391struct FrontierWire {
392    x_m: f64,
393    y_m: f64,
394    score: f32,
395    size: u32,
396}
397impl TryFrom<FrontierWire> for Frontier {
398    type Error = NavigationError;
399    fn try_from(value: FrontierWire) -> std::result::Result<Self, Self::Error> {
400        (finite(value.x_m)
401            && finite(value.y_m)
402            && finite_f32(value.score)
403            && (0.0..=1.0).contains(&value.score)
404            && value.size != 0)
405            .then_some(Self {
406                x_m: value.x_m,
407                y_m: value.y_m,
408                score: value.score,
409                size: value.size,
410            })
411            .ok_or(NavigationError::InvalidFrontier)
412    }
413}
414
415impl Frontier {
416    pub fn try_new(
417        x_m: f64,
418        y_m: f64,
419        score: f32,
420        size: u32,
421    ) -> std::result::Result<Self, NavigationError> {
422        (finite(x_m)
423            && finite(y_m)
424            && finite_f32(score)
425            && (0.0..=1.0).contains(&score)
426            && size != 0)
427            .then_some(Self {
428                x_m,
429                y_m,
430                score,
431                size,
432            })
433            .ok_or(NavigationError::InvalidFrontier)
434    }
435}
436
437#[derive(Clone, Debug, PartialEq, serde::Serialize, serde::Deserialize)]
438pub struct FrontierResponse {
439    pub frontier: Option<Frontier>,
440    pub map_revision: Option<u64>,
441}
442
443#[derive(Clone, Copy, Debug, PartialEq, Eq)]
444pub enum NavigationError {
445    InvalidPose,
446    PathBoundExceeded,
447    InvalidProgress,
448    InvalidCandidate,
449    InvalidFrontier,
450}
451impl std::fmt::Display for NavigationError {
452    fn fmt(&self, formatter: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
453        formatter.write_str(match self {
454            Self::InvalidPose => "navigation pose must contain finite values and canonical yaw",
455            Self::PathBoundExceeded => "navigation path must contain between one and 4096 poses",
456            Self::InvalidProgress => "navigation progress must be finite and bounded",
457            Self::InvalidCandidate => "navigation candidate speeds must be finite",
458            Self::InvalidFrontier => "navigation frontier must be finite, bounded, and non-empty",
459        })
460    }
461}
462impl std::error::Error for NavigationError {}
463
464phoxal_macros::phoxal_api_fragment! {
465    path robot / navigation;
466
467    topic state: State<State>;
468    topic progress: State<Progress>;
469    topic result: Event<Result>;
470    topic candidate: State<Candidate>;
471    query start: StartRequest => StartResponse;
472    query cancel: CancelRequest => CancelResponse;
473    query next_frontier: FrontierRequest => FrontierResponse;
474}
phoxal_api/api/robot/navigation.rs

phoxal_api/api/robot/
navigation.rs
