openrr_base/
robot_velocity_status.rs

Help
1use std::sync::Mutex;
2
3use arci::BaseVelocity;
4
5use crate::*;
6
7#[derive(Debug, Default)]
8pub struct RobotVelocityStatus {
9    velocity: Mutex<BaseVelocity>,
10    velocity_state: Mutex<BaseVelocity>,
11    max_velocity: BaseVelocity,
12    min_velocity: BaseVelocity,
13    max_acceleration: BaseAcceleration,
14    min_acceleration: BaseAcceleration,
15    last_sent_log: Mutex<BaseVelocityTimestamped>,
16}
17
18impl RobotVelocityStatus {
19    pub fn new(max_velocity: BaseVelocity, max_acceleration: BaseAcceleration) -> Self {
20        Self::new_as_asymmetric(
21            max_velocity,
22            max_velocity * (-1.),
23            max_acceleration,
24            max_acceleration * (-1.),
25        )
26    }
27
28    pub fn new_as_asymmetric(
29        max_velocity: BaseVelocity,
30        min_velocity: BaseVelocity,
31        max_acceleration: BaseAcceleration,
32        min_acceleration: BaseAcceleration,
33    ) -> Self {
34        Self {
35            velocity: Mutex::new(BaseVelocity::default()),
36            velocity_state: Mutex::new(BaseVelocity::default()),
37            max_velocity,
38            min_velocity,
39            max_acceleration,
40            min_acceleration,
41            last_sent_log: Mutex::new(BaseVelocityTimestamped::default()),
42        }
43    }
44
45    pub fn velocity(&self) -> BaseVelocity {
46        self.velocity.lock().unwrap().to_owned()
47    }
48
49    pub fn set_velocity(&self, velocity: &BaseVelocity) {
50        let mut vel = self.velocity.lock().unwrap();
51        *vel = *velocity;
52    }
53
54    pub fn time_since_last_sent(&self) -> f64 {
55        self.last_sent_log
56            .lock()
57            .unwrap()
58            .to_owned()
59            .timestamp
60            .elapsed()
61            .as_secs_f64()
62    }
63
64    pub fn set_log(&self, velocity_log: &BaseVelocity) {
65        let mut log = self.last_sent_log.lock().unwrap();
66        *log = BaseVelocityTimestamped {
67            base_velocity: *velocity_log,
68            timestamp: std::time::Instant::now(),
69        }
70    }
71
72    pub fn set_velocity_state(&self, velocity: BaseVelocity) {
73        let mut velocity_state = self.velocity_state.lock().unwrap();
74        *velocity_state = velocity;
75    }
76
77    pub fn get_limited_velocity(&self, velocity: &BaseVelocity) -> BaseVelocity {
78        let vel = self.limit_acceleration(velocity);
79        self.limit_velocity(&vel)
80    }
81
82    fn limit_velocity(&self, velocity: &BaseVelocity) -> BaseVelocity {
83        BaseVelocity {
84            x: velocity.x.clamp(self.min_velocity.x, self.max_velocity.x),
85            y: velocity.y.clamp(self.min_velocity.y, self.max_velocity.y),
86            theta: velocity
87                .theta
88                .clamp(self.min_velocity.theta, self.max_velocity.theta),
89        }
90    }
91
92    fn limit_acceleration(&self, velocity: &BaseVelocity) -> BaseVelocity {
93        let dt = self.time_since_last_sent();
94        let vel = self.velocity.lock().unwrap().to_owned();
95        BaseVelocity {
96            x: velocity.x.clamp(
97                vel.x + self.min_acceleration.x * dt,
98                vel.x + self.max_acceleration.x * dt,
99            ),
100            y: velocity.y.clamp(
101                vel.y + self.min_acceleration.y * dt,
102                vel.y + self.max_acceleration.y * dt,
103            ),
104            theta: velocity.theta.clamp(
105                vel.theta + self.min_acceleration.theta * dt,
106                vel.theta + self.max_acceleration.theta * dt,
107            ),
108        }
109    }
110}
111
112#[cfg(test)]
113mod test {
114    use assert_approx_eq::assert_approx_eq;
115
116    use super::*;
117
118    const LIMIT_VEL_X: f64 = 10.0;
119    const LIMIT_VEL_THETA: f64 = 10.0;
120    const LIMIT_ACC_X: f64 = 1000.0;
121    const LIMIT_ACC_THETA: f64 = 1000.0;
122    const DUMMY_VELOCITY_STATE_X: f64 = 1.23;
123
124    #[test]
125    fn test_robot_velocity_status() {
126        let limit_velocity = BaseVelocity::new(LIMIT_VEL_X, 0.0, LIMIT_VEL_THETA);
127        let limit_acceleration = BaseAcceleration::new(LIMIT_ACC_X, 0.0, LIMIT_ACC_THETA);
128
129        let status = RobotVelocityStatus::new(limit_velocity, limit_acceleration);
130
131        status.set_velocity(&BaseVelocity {
132            x: LIMIT_VEL_X,
133            y: 0.,
134            theta: -LIMIT_VEL_THETA,
135        });
136
137        let vel = status.velocity();
138        assert_approx_eq!(vel.x, LIMIT_VEL_X);
139
140        std::thread::sleep(std::time::Duration::from_millis(100));
141        let cmd_vel = BaseVelocity {
142            x: -20.,
143            y: 0.,
144            theta: 20.,
145        };
146        let limited = status.get_limited_velocity(&cmd_vel);
147
148        assert_approx_eq!(limited.x, -LIMIT_VEL_X);
149        assert_approx_eq!(limited.theta, LIMIT_VEL_THETA);
150
151        let velocity_state = BaseVelocity {
152            x: DUMMY_VELOCITY_STATE_X,
153            y: 0.,
154            theta: 0.,
155        };
156
157        status.set_velocity_state(velocity_state);
158
159        let velocity_state_x = status.velocity_state.lock().unwrap().x;
160        assert_approx_eq!(velocity_state_x, DUMMY_VELOCITY_STATE_X);
161    }
162}
openrr_base/robot_velocity_status.rs

openrr_base/
robot_velocity_status.rs
