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
89
90
91
92
93
94
95
96
97
use nalgebra::{Isometry2, Vector2};
use parking_lot::Mutex;
use crate::{error::Error, traits::Navigation, WaitFuture};
#[derive(Debug)]
pub struct DummyNavigation {
pub goal_pose: Mutex<Isometry2<f64>>,
canceled: Mutex<bool>,
}
impl DummyNavigation {
pub fn new() -> Self {
Self {
goal_pose: Mutex::new(Isometry2::new(Vector2::new(0.0, 0.0), 0.0)),
canceled: Mutex::default(),
}
}
pub fn current_goal_pose(&self) -> Result<Isometry2<f64>, Error> {
Ok(self.goal_pose.lock().to_owned())
}
pub fn is_canceled(&self) -> bool {
*self.canceled.lock()
}
}
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.canceled.lock() = false;
*self.goal_pose.lock() = goal;
Ok(WaitFuture::ready())
}
fn cancel(&self) -> Result<(), Error> {
*self.canceled.lock() = true;
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();
drop(
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);
}
}