openrr_base/
odometry.rs

Help
1use std::sync::Mutex;
2
3use arci::{BaseVelocity, Isometry2, Vector2};
4
5use crate::Error;
6
7const DEFAULT_TIMEOUT_MILLIS: u128 = 5000;
8
9#[derive(Debug)]
10pub struct Odometry {
11    /// If `None`, it is odometry lost.
12    position: Mutex<Option<Isometry2<f64>>>,
13    last_update_timestamp: Mutex<Option<std::time::Instant>>,
14    timeout_millis: Mutex<u128>,
15}
16
17impl Odometry {
18    pub fn new_from_pose(position: Isometry2<f64>) -> Self {
19        Self {
20            position: Mutex::new(Some(position)),
21            last_update_timestamp: Mutex::new(None),
22            // If `0`, timeout is invalid
23            timeout_millis: Mutex::new(DEFAULT_TIMEOUT_MILLIS),
24        }
25    }
26
27    pub fn update_by_velocity(&self, velocity: &BaseVelocity) -> Result<(), Error> {
28        let mut position = self.position.lock().unwrap();
29        let mut locked_update_time = self.last_update_timestamp.lock().unwrap();
30        let timeout_millis = *self.timeout_millis.lock().unwrap();
31
32        let last_update_time = match *locked_update_time {
33            Some(t) => t,
34            None => std::time::Instant::now(),
35        };
36
37        match *position {
38            Some(p) => {
39                if last_update_time.elapsed().as_millis() > timeout_millis && timeout_millis != 0 {
40                    *position = None;
41                    *locked_update_time = Some(std::time::Instant::now());
42                    Err(Error::CurrentPositionUnknown)
43                } else {
44                    let dt = last_update_time.elapsed().as_secs_f64();
45                    let dx = Isometry2::new(
46                        Vector2::new(velocity.x * dt, velocity.y * dt),
47                        velocity.theta * dt,
48                    );
49
50                    *position = Some(p * dx);
51                    *locked_update_time = Some(std::time::Instant::now());
52
53                    Ok(())
54                }
55            }
56            None => Err(Error::CurrentPositionUnknown),
57        }
58    }
59
60    /// Recover or update odometry
61    pub fn resolve_lost(&self, current_pose: Isometry2<f64>) {
62        let mut pose = self.position.lock().unwrap();
63        let mut update_time = self.last_update_timestamp.lock().unwrap();
64
65        *pose = Some(current_pose);
66        *update_time = Some(std::time::Instant::now());
67    }
68
69    pub fn set_timeout_millis(&self, millis: u128) {
70        let mut timeout_millis = self.timeout_millis.lock().unwrap();
71        *timeout_millis = millis;
72    }
73
74    pub fn current_pose(&self) -> Result<Isometry2<f64>, Error> {
75        match self.position.lock().unwrap().to_owned() {
76            Some(pose) => Ok(pose),
77            None => Err(Error::CurrentPositionUnknown),
78        }
79    }
80}
81
82impl Default for Odometry {
83    fn default() -> Self {
84        Self {
85            position: Mutex::new(None),
86            last_update_timestamp: Mutex::new(None),
87            timeout_millis: Mutex::new(DEFAULT_TIMEOUT_MILLIS),
88        }
89    }
90}
91
92#[cfg(test)]
93mod test {
94    use super::*;
95
96    #[test]
97    fn test_base_odometry() {
98        let vel = BaseVelocity::new(100.0, 200.0, 100.0 * std::f64::consts::FRAC_PI_2);
99        let odom = Odometry::new_from_pose(Isometry2::default());
100
101        std::thread::sleep(std::time::Duration::from_millis(10));
102
103        odom.update_by_velocity(&vel).unwrap();
104        let pose = odom.current_pose();
105
106        assert!(
107            pose.as_ref().unwrap().translation.x > 1.00
108                || pose.as_ref().unwrap().translation.x < 1.05
109        );
110        assert!(
111            pose.as_ref().unwrap().translation.y > 2.00
112                || pose.as_ref().unwrap().translation.y < 2.10
113        );
114    }
115}
openrr_base/odometry.rs

openrr_base/
odometry.rs
