1use std::sync::Mutex;
2
3use arci::{
4 gamepad::{Axis, Button, GamepadEvent},
5 BaseVelocity, MoveBase,
6};
7use async_trait::async_trait;
8
9use super::control_mode::ControlMode;
10
11const BASE_LINEAR_VEL_AXIS_GAIN: f64 = 0.5;
12const BASE_ANGULAR_VEL_AXIS_GAIN: f64 = 1.5;
13const BASE_TURBO_GAIN: f64 = 2.0;
14
15struct MoveBaseModeInner {
16 vel: BaseVelocity,
17 is_enabled: bool,
18 is_turbo: bool,
19}
20
21impl MoveBaseModeInner {
22 fn new() -> Self {
23 Self {
24 vel: BaseVelocity::default(),
25 is_enabled: false,
26 is_turbo: false,
27 }
28 }
29
30 fn handle_event(&mut self, ev: GamepadEvent) -> bool {
31 let mut should_stop = false;
32 match ev {
33 GamepadEvent::AxisChanged(Axis::LeftStickX, v) => {
34 self.vel.y = v * BASE_LINEAR_VEL_AXIS_GAIN
35 }
36 GamepadEvent::AxisChanged(Axis::LeftStickY, v) => {
37 self.vel.x = v * BASE_LINEAR_VEL_AXIS_GAIN
38 }
39 GamepadEvent::AxisChanged(Axis::RightStickX, v) => {
40 self.vel.theta = v * BASE_ANGULAR_VEL_AXIS_GAIN
41 }
42 GamepadEvent::ButtonPressed(Button::RightTrigger2) => {
43 self.is_enabled = true;
44 }
45 GamepadEvent::ButtonReleased(Button::RightTrigger2) => {
46 self.is_enabled = false;
47 self.vel = BaseVelocity::default();
48 should_stop = true;
49 }
50 GamepadEvent::ButtonPressed(Button::LeftTrigger2) => {
51 self.is_turbo = true;
52 }
53 GamepadEvent::ButtonReleased(Button::LeftTrigger2) => {
54 self.is_turbo = false;
55 }
56 GamepadEvent::Disconnected => {
57 self.is_enabled = false;
58 self.is_turbo = false;
59 self.vel = BaseVelocity::default();
60 should_stop = true;
61 }
62 _ => {}
63 }
64 should_stop
65 }
66
67 fn get_target_velocity(&self) -> Option<BaseVelocity> {
68 if self.is_enabled {
69 if self.is_turbo {
70 let turbo_vel = self.vel * BASE_TURBO_GAIN;
71 Some(turbo_vel)
72 } else {
73 Some(self.vel)
74 }
75 } else {
76 None
77 }
78 }
79}
80
81pub struct MoveBaseMode<T: MoveBase> {
82 move_base: T,
83 mode: String,
84 submode: String,
85 inner: Mutex<MoveBaseModeInner>,
86}
87
88impl<T> MoveBaseMode<T>
89where
90 T: MoveBase,
91{
92 pub fn new(mode: String, move_base: T) -> Self {
93 Self {
94 move_base,
95 mode,
96 submode: "".to_string(),
97 inner: Mutex::new(MoveBaseModeInner::new()),
98 }
99 }
100}
101
102#[async_trait]
103impl<T> ControlMode for MoveBaseMode<T>
104where
105 T: MoveBase,
106{
107 fn handle_event(&self, ev: GamepadEvent) {
108 if self.inner.lock().unwrap().handle_event(ev) {
109 self.move_base
111 .send_velocity(&BaseVelocity::default())
112 .unwrap();
113 }
114 }
115
116 async fn proc(&self) {
117 if let Some(v) = self.inner.lock().unwrap().get_target_velocity() {
118 self.move_base.send_velocity(&v).unwrap();
119 }
120 }
121
122 fn mode(&self) -> &str {
123 &self.mode
124 }
125
126 fn submode(&self) -> String {
127 self.submode.to_owned()
128 }
129}
130
131#[cfg(test)]
132mod tests {
133 use arci::DummyMoveBase;
134 use assert_approx_eq::*;
135
136 use super::*;
137
138 #[test]
139 fn test_move_mode_new() {
140 let mode_name = String::from("tested");
141 let base = DummyMoveBase::new();
142 let mode = MoveBaseMode::new(mode_name.clone(), base);
143
144 assert_eq!(
145 format!("{:?}", mode.move_base),
146 format!("{:?}", DummyMoveBase::new())
147 );
148 assert_eq!(mode.mode, mode_name);
149 assert_eq!(mode.submode, String::from(""));
150 assert_eq!(
151 format!("{:?}", mode.inner.lock().unwrap().vel),
152 format!("{:?}", BaseVelocity::default())
153 );
154 assert!(!mode.inner.lock().unwrap().is_enabled);
155 assert!(!mode.inner.lock().unwrap().is_turbo);
156 }
157
158 #[test]
159 fn test_move_mode_get() {
160 let mode_name = String::from("tested");
161 let base = DummyMoveBase::new();
162 let mode = MoveBaseMode::new(mode_name.clone(), base);
163
164 let base_mode = mode.mode();
165 assert_eq!(base_mode, mode_name);
166 let base_sub_mode = mode.submode();
167 assert_eq!(base_sub_mode, String::from(""));
168 }
169
170 #[tokio::test]
171 async fn test_move_mode_proc() {
172 let mode_name = String::from("tested");
173 const X: f64 = 1.2;
174 const Y: f64 = 3.5;
175 const THETA: f64 = 1.8;
176 let mode = MoveBaseMode {
177 move_base: DummyMoveBase::new(),
178 mode: mode_name.clone(),
179 submode: "".to_string(),
180 inner: Mutex::new(MoveBaseModeInner {
181 vel: BaseVelocity {
182 x: X,
183 y: Y,
184 theta: THETA,
185 },
186 is_enabled: false,
187 is_turbo: false,
188 }),
189 };
190 mode.proc().await;
191 let current = mode.move_base.current_velocity().unwrap();
192 assert_approx_eq!(current.x, 0.0);
193 assert_approx_eq!(current.y, 0.0);
194 assert_approx_eq!(current.theta, 0.0);
195 println!("{:?} {current:?}", mode.inner.lock().unwrap().vel);
196
197 let mode = MoveBaseMode {
198 move_base: DummyMoveBase::new(),
199 mode: mode_name.clone(),
200 submode: "".to_string(),
201 inner: Mutex::new(MoveBaseModeInner {
202 vel: BaseVelocity {
203 x: 1.2,
204 y: 3.5,
205 theta: 1.8,
206 },
207 is_enabled: false,
208 is_turbo: true,
209 }),
210 };
211 mode.proc().await;
212 let current = mode.move_base.current_velocity().unwrap();
213 assert_approx_eq!(current.x, 0.0);
214 assert_approx_eq!(current.y, 0.0);
215 assert_approx_eq!(current.theta, 0.0);
216 println!("{:?} {current:?}", mode.inner.lock().unwrap().vel);
217
218 let mode = MoveBaseMode {
219 move_base: DummyMoveBase::new(),
220 mode: mode_name.clone(),
221 submode: "".to_string(),
222 inner: Mutex::new(MoveBaseModeInner {
223 vel: BaseVelocity {
224 x: 1.2,
225 y: 3.5,
226 theta: 1.8,
227 },
228 is_enabled: true,
229 is_turbo: false,
230 }),
231 };
232 mode.proc().await;
233 let current = mode.move_base.current_velocity().unwrap();
234 assert_approx_eq!(current.x, X);
235 assert_approx_eq!(current.y, Y);
236 assert_approx_eq!(current.theta, THETA);
237 println!("{:?} {current:?}", mode.inner.lock().unwrap().vel);
238
239 let mode = MoveBaseMode {
240 move_base: DummyMoveBase::new(),
241 mode: mode_name.clone(),
242 submode: "".to_string(),
243 inner: Mutex::new(MoveBaseModeInner {
244 vel: BaseVelocity {
245 x: 1.2_f64,
246 y: 3.5,
247 theta: 1.8,
248 },
249 is_enabled: true,
250 is_turbo: true,
251 }),
252 };
253 mode.proc().await;
254 let current = mode.move_base.current_velocity().unwrap();
255 assert_approx_eq!(current.x, X * 2.0);
256 assert_approx_eq!(current.y, Y * 2.0);
257 assert_approx_eq!(current.theta, THETA * 2.0);
258 println!("{:?} {current:?}", mode.inner.lock().unwrap().vel);
259 }
260}