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

use std::sync::Mutex;

use nalgebra::{Isometry2, Vector2};

use crate::{error::Error, traits::Navigation, WaitFuture};

/// Dummy Navigation for debug or tests.
#[derive(Debug)]
pub struct DummyNavigation {
    pub goal_pose: Mutex<Isometry2<f64>>,
}

impl DummyNavigation {
    pub fn new() -> Self {
        Self {
            goal_pose: Mutex::new(Isometry2::new(Vector2::new(0.0, 0.0), 0.0)),
        }
    }

    pub fn current_goal_pose(&self) -> Result<Isometry2<f64>, Error> {
        Ok(self.goal_pose.lock().unwrap().to_owned())
    }
}

impl Default for DummyNavigation {
    fn default() -> Self {
        Self::new()
    }
}

impl Navigation for DummyNavigation {
    fn send_goal_pose(
        &self,
        goal: Isometry2<f64>,
        _frame_id: &str,
        _timeout: std::time::Duration,
    ) -> Result<WaitFuture, Error> {
        *self.goal_pose.lock().unwrap() = goal;
        Ok(WaitFuture::ready())
    }

    fn cancel(&self) -> Result<(), Error> {
        Ok(())
    }
}

#[cfg(test)]
mod tests {
    use assert_approx_eq::assert_approx_eq;

    use super::*;

    #[tokio::test]
    async fn test_set() {
        let nav = DummyNavigation::new();
        assert!(nav
            .send_goal_pose(
                Isometry2::new(Vector2::new(1.0, 2.0), 3.0),
                "",
                std::time::Duration::default(),
            )
            .unwrap()
            .await
            .is_ok());

        let current_goal_pose = nav.current_goal_pose().unwrap();
        assert_approx_eq!(current_goal_pose.translation.x, 1.0);
        assert_approx_eq!(current_goal_pose.translation.y, 2.0);
        assert_approx_eq!(current_goal_pose.rotation.angle(), 3.0);
    }

    #[test]
    fn test_set_no_wait() {
        let nav = DummyNavigation::new();
        let _ = nav
            .send_goal_pose(
                Isometry2::new(Vector2::new(1.0, 2.0), 3.0),
                "",
                std::time::Duration::default(),
            )
            .unwrap();

        let current_goal_pose = nav.current_goal_pose().unwrap();
        assert_approx_eq!(current_goal_pose.translation.x, 1.0);
        assert_approx_eq!(current_goal_pose.translation.y, 2.0);
        assert_approx_eq!(current_goal_pose.rotation.angle(), 3.0);
    }
}