arci/clients/
dummy_navigation.rs

1use std::sync::Mutex;
2
3use nalgebra::{Isometry2, Vector2};
4
5use crate::{error::Error, traits::Navigation, WaitFuture};
6
7/// Dummy Navigation for debug or tests.
8#[derive(Debug)]
9pub struct DummyNavigation {
10    pub goal_pose: Mutex<Isometry2<f64>>,
11    canceled: Mutex<bool>,
12}
13
14impl DummyNavigation {
15    /// Creates a new `DummyNavigation`.
16    pub fn new() -> Self {
17        Self {
18            goal_pose: Mutex::new(Isometry2::new(Vector2::new(0.0, 0.0), 0.0)),
19            canceled: Mutex::default(),
20        }
21    }
22
23    pub fn current_goal_pose(&self) -> Result<Isometry2<f64>, Error> {
24        Ok(self.goal_pose.lock().unwrap().to_owned())
25    }
26
27    pub fn is_canceled(&self) -> bool {
28        *self.canceled.lock().unwrap()
29    }
30}
31
32impl Default for DummyNavigation {
33    fn default() -> Self {
34        Self::new()
35    }
36}
37
38impl Navigation for DummyNavigation {
39    fn send_goal_pose(
40        &self,
41        goal: Isometry2<f64>,
42        _frame_id: &str,
43        _timeout: std::time::Duration,
44    ) -> Result<WaitFuture, Error> {
45        *self.canceled.lock().unwrap() = false;
46        *self.goal_pose.lock().unwrap() = goal;
47        Ok(WaitFuture::ready())
48    }
49
50    fn cancel(&self) -> Result<(), Error> {
51        *self.canceled.lock().unwrap() = true;
52        Ok(())
53    }
54}
55
56#[cfg(test)]
57mod tests {
58    use assert_approx_eq::assert_approx_eq;
59
60    use super::*;
61
62    #[tokio::test]
63    async fn test_set() {
64        let nav = DummyNavigation::new();
65        assert!(nav
66            .send_goal_pose(
67                Isometry2::new(Vector2::new(1.0, 2.0), 3.0),
68                "",
69                std::time::Duration::default(),
70            )
71            .unwrap()
72            .await
73            .is_ok());
74
75        let current_goal_pose = nav.current_goal_pose().unwrap();
76        assert_approx_eq!(current_goal_pose.translation.x, 1.0);
77        assert_approx_eq!(current_goal_pose.translation.y, 2.0);
78        assert_approx_eq!(current_goal_pose.rotation.angle(), 3.0);
79    }
80
81    #[test]
82    fn test_set_no_wait() {
83        let nav = DummyNavigation::new();
84        drop(
85            nav.send_goal_pose(
86                Isometry2::new(Vector2::new(1.0, 2.0), 3.0),
87                "",
88                std::time::Duration::default(),
89            )
90            .unwrap(),
91        );
92
93        let current_goal_pose = nav.current_goal_pose().unwrap();
94        assert_approx_eq!(current_goal_pose.translation.x, 1.0);
95        assert_approx_eq!(current_goal_pose.translation.y, 2.0);
96        assert_approx_eq!(current_goal_pose.rotation.angle(), 3.0);
97    }
98}
arci/clients/dummy_navigation.rs

arci/clients/
dummy_navigation.rs
