mavspec/rust/microservices/utils/mission/
mission_plan.rs

1use super::consts::{DEFAULT_MISSION_PLAN_VERSION, MISSION_PLAN_FILE_PREFIX};
2use super::error::MissionError;
3use super::Waypoint;
4
5/// <sup>`⍚`</sup> Mission plan for drones that support MAVLink [mission](https://mavlink.io/en/services/mission.html)
6/// protocol.
7///
8/// Supports unofficial [mission file format](https://mavlink.io/en/file_formats/#mission_plain_text_file).
9///
10/// For `no-alloc` targets use [`no_alloc::MissionPlan`](super::no_alloc::MissionPlan).
11#[cfg_attr(all(feature = "specta", feature = "unstable"), derive(specta::Type))]
12#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
13#[cfg(feature = "alloc")]
14#[derive(Clone, Debug)]
15pub struct MissionPlan {
16    version: u16,
17    waypoints: alloc::vec::Vec<Waypoint>,
18}
19
20impl MissionPlan {
21    /// Creates a new empty mission plan.
22    pub fn new() -> Self {
23        Self::default()
24    }
25
26    /// Constructs mission plan from the slice waypoints.
27    pub fn from_waypoints(waypoints: &[Waypoint]) -> Self {
28        Self {
29            version: DEFAULT_MISSION_PLAN_VERSION,
30            waypoints: waypoints.to_vec(),
31        }
32    }
33
34    /// Attempts to construct mission plan from the contents of the
35    /// [mission file](https://mavlink.io/en/file_formats/#mission_plain_text_file).
36    pub fn try_from_mission_planner_str(content: &str) -> Result<Self, MissionError> {
37        let mut waypoints = MissionPlan::new();
38
39        for (idx, line) in content
40            .split('\n')
41            .map(|s| s.trim())
42            .filter(|s| !s.is_empty())
43            .enumerate()
44        {
45            if line.starts_with(MISSION_PLAN_FILE_PREFIX) {
46                waypoints.version = line
47                    .trim_start_matches(MISSION_PLAN_FILE_PREFIX)
48                    .parse::<u16>()
49                    .unwrap_or(DEFAULT_MISSION_PLAN_VERSION);
50                continue;
51            }
52
53            match Waypoint::try_from(line) {
54                Ok(waypoint) => {
55                    if waypoints.push(waypoint).is_none() {
56                        return Err(MissionError::OutOfOrder(waypoint.index));
57                    }
58                }
59                Err(_) if idx == 0 => continue,
60                Err(err) => return Err(err),
61            }
62        }
63
64        Ok(waypoints)
65    }
66
67    /// Attempts to construct mission plan from the binary contents of the
68    /// [mission file](https://mavlink.io/en/file_formats/#mission_plain_text_file).
69    pub fn try_from_mission_planner_buf(buf: &[u8]) -> Result<Self, MissionError> {
70        use core::str;
71        Self::try_from_mission_planner_str(
72            str::from_utf8(buf).map_err(|_| MissionError::InvalidUtf8)?,
73        )
74    }
75
76    /// Returns a slice of waypoints.
77    pub fn waypoints(&self) -> &[Waypoint] {
78        self.waypoints.as_slice()
79    }
80
81    /// Returns the length of available waypoints.
82    pub fn len(&self) -> usize {
83        self.waypoints.len()
84    }
85
86    /// Returns `true` if the list of waypoints is empty.
87    pub fn is_empty(&self) -> bool {
88        self.waypoints.is_empty()
89    }
90
91    /// Puts waypoint into the mission plan.
92    ///
93    /// This function will either insert a waypoint to an already existing position or append list
94    /// of waypoints.
95    pub fn put(&mut self, waypoint: Waypoint) -> Option<u16> {
96        if waypoint.index > self.len() as u16 {
97            return None;
98        }
99
100        let idx = waypoint.index;
101        if idx as usize >= self.waypoints.len() {
102            self.waypoints.push(waypoint);
103        } else {
104            self.waypoints[waypoint.index as usize] = waypoint;
105        }
106
107        Some(idx)
108    }
109
110    /// Inserts waypoint into the mission plan.
111    ///
112    /// This function will insert a waypoint and adjust other waypoint positions accordingly. If the
113    /// `index` of a waypoint is greater than length of the current mission plan, it will be
114    /// ignored.
115    pub fn insert(&mut self, waypoint: Waypoint) -> Option<u16> {
116        if waypoint.index > self.len() as u16 {
117            return None;
118        }
119
120        let idx = waypoint.index;
121        self.waypoints.insert(idx as usize, waypoint);
122        let len = self.waypoints.len();
123
124        for (n, entry) in self.waypoints[idx as usize..len].iter_mut().enumerate() {
125            entry.index = n as u16;
126        }
127
128        Some(idx)
129    }
130
131    /// Append a new waypoint.
132    ///
133    /// This will update the `index` field of a [`Waypoint`].
134    pub fn push(&mut self, waypoint: Waypoint) -> Option<u16> {
135        let mut waypoint = waypoint;
136        let idx = self.len() as u16;
137        waypoint.index = idx;
138        let waypoint = waypoint;
139
140        self.waypoints.push(waypoint);
141
142        Some(idx)
143    }
144
145    /// Takes the last [`Waypoint`] from the mission list.
146    pub fn pop(&mut self) -> Option<Waypoint> {
147        self.waypoints.pop()
148    }
149}
150
151impl Default for MissionPlan {
152    fn default() -> Self {
153        Self {
154            version: DEFAULT_MISSION_PLAN_VERSION,
155            waypoints: Default::default(),
156        }
157    }
158}
159
160impl From<&[Waypoint]> for MissionPlan {
161    #[inline(always)]
162    fn from(value: &[Waypoint]) -> Self {
163        Self::from_waypoints(value)
164    }
165}
166
167impl TryFrom<&[u8]> for MissionPlan {
168    type Error = MissionError;
169
170    #[inline(always)]
171    fn try_from(value: &[u8]) -> Result<Self, Self::Error> {
172        Self::try_from_mission_planner_buf(value)
173    }
174}
175
176impl TryFrom<&str> for MissionPlan {
177    type Error = MissionError;
178
179    fn try_from(value: &str) -> Result<Self, Self::Error> {
180        Self::try_from_mission_planner_str(value)
181    }
182}
183
184impl core::fmt::Display for MissionPlan {
185    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
186        f.write_fmt(format_args!(
187            "{}{}\n",
188            MISSION_PLAN_FILE_PREFIX, self.version
189        ))?;
190        for waypoint in self.waypoints.iter() {
191            f.write_fmt(format_args!("{}\n", waypoint))?;
192        }
193        Ok(())
194    }
195}
196
197#[cfg(test)]
198mod mission_plan_tests {
199    use super::*;
200
201    #[test]
202    fn test_parse_mission_plan_from_str() {
203        let contents = "\
204        0\t1\t0\t16\t0.149999999999999994\t0\t0\t0\t8.54800000000000004\t47.3759999999999977\t550\t1\n\
205        1\t0\t0\t16\t0.149999999999999994\t0\t0\t0\t8.54800000000000004\t47.3759999999999977\t550\t1\n";
206
207        MissionPlan::try_from(contents).unwrap();
208    }
209
210    #[test]
211    fn test_parse_mission_plan_from_bytes() {
212        let contents = "\
213        0\t1\t0\t16\t0.149999999999999994\t0\t0\t0\t8.54800000000000004\t47.3759999999999977\t550\t1\n\
214        1\t0\t0\t16\t0.149999999999999994\t0\t0\t0\t8.54800000000000004\t47.3759999999999977\t550\t1\n";
215
216        MissionPlan::try_from(contents.as_bytes()).unwrap();
217    }
218
219    #[test]
220    #[cfg(feature = "alloc")]
221    fn test_mission_plan_to_string() {
222        use crate::alloc::string::ToString;
223
224        let original = "QGC WPL 110\n\
225        0\t1\t0\t16\t0\t0\t0\t0\t50.4338103\t30.5059493\t12.19\t1\n\
226        1\t0\t3\t22\t15\t0\t0\t0\t50.4338203\t50.4338203\t50\t1\n\
227        2\t0\t3\t16\t0\t0\t0\t0\t50.4338303\t50.4338303\t50\t1\n";
228
229        let mission = MissionPlan::try_from(original).unwrap();
230        assert_eq!(mission.version, 110);
231
232        let dumped = mission.to_string();
233        assert_eq!(original, dumped);
234    }
235
236    #[test]
237    #[cfg(feature = "alloc")]
238    fn test_new_mission_plan_empty() {
239        let mission = MissionPlan::new();
240        assert!(mission.is_empty());
241    }
242
243    #[test]
244    #[cfg(feature = "alloc")]
245    fn test_mission_paln_push_pop() {
246        let mut mission = MissionPlan::new();
247        assert!(mission.is_empty());
248
249        assert!(matches!(mission.push(Waypoint::default()), Some(0)));
250        assert!(matches!(mission.push(Waypoint::default()), Some(1)));
251
252        let waypoint = mission.pop().unwrap();
253        assert_eq!(waypoint.index, 1);
254    }
255
256    #[test]
257    #[cfg(feature = "alloc")]
258    fn test_mission_plan_put() {
259        let mut mission = MissionPlan::new();
260        assert!(mission.is_empty());
261
262        assert!(matches!(mission.push(Waypoint::default()), Some(0)));
263        assert!(matches!(mission.push(Waypoint::default()), Some(1)));
264        assert_eq!(mission.len(), 2);
265
266        assert!(matches!(
267            mission.put(Waypoint {
268                index: 0,
269                ..Default::default()
270            }),
271            Some(0)
272        ));
273        assert_eq!(mission.len(), 2);
274
275        assert!(matches!(
276            mission.put(Waypoint {
277                index: 1,
278                ..Default::default()
279            }),
280            Some(1)
281        ));
282        assert_eq!(mission.len(), 2);
283
284        assert!(matches!(
285            mission.put(Waypoint {
286                index: 2,
287                ..Default::default()
288            }),
289            Some(2)
290        ));
291        assert_eq!(mission.len(), 3);
292
293        assert!(matches!(
294            mission.put(Waypoint {
295                index: 4,
296                ..Default::default()
297            }),
298            None
299        ));
300        assert_eq!(mission.len(), 3);
301    }
302
303    #[test]
304    #[cfg(feature = "alloc")]
305    fn test_mission_plan_insert() {
306        let mut mission = MissionPlan::new();
307
308        assert!(matches!(
309            mission.insert(Waypoint {
310                index: 0,
311                param1: 2.0,
312                ..Default::default()
313            }),
314            Some(0)
315        ));
316        assert_eq!(mission.len(), 1);
317
318        assert!(matches!(
319            mission.insert(Waypoint {
320                index: 0,
321                param1: 1.0,
322                ..Default::default()
323            }),
324            Some(0)
325        ));
326        assert_eq!(mission.len(), 2);
327
328        assert!(matches!(
329            mission.insert(Waypoint {
330                index: 2,
331                param1: 3.0,
332                ..Default::default()
333            }),
334            Some(2)
335        ));
336        assert_eq!(mission.len(), 3);
337
338        assert!(matches!(
339            mission.insert(Waypoint {
340                index: 4,
341                param1: 5.0,
342                ..Default::default()
343            }),
344            None
345        ));
346        assert_eq!(mission.len(), 3);
347
348        assert_eq!(mission.waypoints()[0].param1, 1.0);
349        assert_eq!(mission.waypoints()[1].param1, 2.0);
350        assert_eq!(mission.waypoints()[2].param1, 3.0);
351    }
352}
mavspec/rust/microservices/utils/mission/mission_plan.rs

mavspec/rust/microservices/utils/mission/
mission_plan.rs
