openrr_teleop/
switcher.rs

1use std::{
2    sync::{
3        atomic::{AtomicBool, Ordering},
4        Arc,
5    },
6    time::Duration,
7};
8
9use arci::{
10    gamepad::{Button, Gamepad, GamepadEvent},
11    Speaker,
12};
13use parking_lot::Mutex;
14use tokio::sync::Mutex as TokioMutex;
15use tracing::{debug, warn};
16
17use super::control_mode::ControlMode;
18
19pub struct ControlModeSwitcher<S>
20where
21    S: Speaker,
22{
23    current_index: Arc<Mutex<usize>>,
24    control_modes: Arc<TokioMutex<Vec<Arc<dyn ControlMode>>>>,
25    speaker: S,
26    is_running: Arc<AtomicBool>,
27}
28
29impl<S> ControlModeSwitcher<S>
30where
31    S: Speaker,
32{
33    #[track_caller]
34    pub fn new(
35        control_modes: Vec<Arc<dyn ControlMode>>,
36        speaker: S,
37        initial_mode_index: usize,
38    ) -> Self {
39        assert!(!control_modes.is_empty());
40        Self {
41            current_index: Arc::new(Mutex::new(initial_mode_index)),
42            control_modes: Arc::new(TokioMutex::new(control_modes)),
43            speaker,
44            is_running: Arc::new(AtomicBool::new(false)),
45        }
46    }
47
48    pub async fn increment_mode(&self) -> Result<(), arci::Error> {
49        let len = self.control_modes.lock().await.len();
50        {
51            let mut index = self.current_index.lock();
52            *index = (*index + 1) % len;
53        }
54        self.speak_current_mode().await
55    }
56
57    pub async fn speak_current_mode(&self) -> Result<(), arci::Error> {
58        let modes = self.control_modes.lock().await;
59        let i = self.current_index();
60        let mode = modes[i].mode();
61        let submode = modes[i].submode();
62        self.speaker.speak(&format!("{mode}{submode}"))?.await
63    }
64
65    fn current_index(&self) -> usize {
66        *self.current_index.lock()
67    }
68
69    fn is_running(&self) -> bool {
70        self.is_running.load(Ordering::Relaxed)
71    }
72
73    pub fn stop(&self) {
74        self.is_running.store(false, Ordering::Relaxed);
75    }
76
77    pub async fn main<G>(&self, gamepad: G)
78    where
79        G: 'static + Gamepad,
80    {
81        let modes = self.control_modes.clone();
82        let index = self.current_index.clone();
83        let is_running = self.is_running.clone();
84        self.is_running.store(true, Ordering::Relaxed);
85        self.speak_current_mode().await.unwrap();
86        let gamepad = Arc::new(gamepad);
87        let gamepad_cloned = gamepad.clone();
88        tokio::spawn(async move {
89            let mut interval = tokio::time::interval(Duration::from_millis(50));
90            while is_running.load(Ordering::Relaxed) {
91                debug!("tick");
92                let mode = { modes.lock().await[*index.lock()].clone() };
93                mode.proc().await;
94                interval.tick().await;
95            }
96            gamepad_cloned.stop();
97        });
98        while self.is_running() {
99            let ev = gamepad.next_event().await;
100            debug!("event: {ev:?}");
101            match ev {
102                GamepadEvent::ButtonPressed(Button::North) => {
103                    self.increment_mode().await.unwrap();
104                }
105                GamepadEvent::Unknown => {
106                    warn!("gamepad Unknown");
107                    self.stop();
108                }
109                _ => {
110                    let mode = { self.control_modes.lock().await[self.current_index()].clone() };
111                    mode.handle_event(ev);
112                }
113            }
114        }
115    }
116}
openrr_teleop/switcher.rs

openrr_teleop/
switcher.rs
