arci/clients/
dummy_navigation.rs

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

arci/clients/
dummy_navigation.rs
