openrr_teleop/
robot_command_executor.rs

Help
1use std::{
2    path::PathBuf,
3    sync::{Arc, Mutex},
4};
5
6use arci::{
7    gamepad::{Button, GamepadEvent},
8    Speaker,
9};
10use async_trait::async_trait;
11use clap::Parser;
12use openrr_client::{resolve_relative_path, ArcRobotClient};
13use openrr_command::{load_command_file_and_filter, RobotCommand};
14use schemars::JsonSchema;
15use serde::{Deserialize, Serialize};
16use tracing::{error, info, warn};
17
18use crate::ControlMode;
19
20const MODE: &str = "command";
21
22struct RobotCommandExecutorInner {
23    commands: Vec<RobotCommandConfig>,
24    submode: String,
25    command_index: usize,
26    is_trigger_holding: bool,
27    is_sending: bool,
28}
29
30impl RobotCommandExecutorInner {
31    fn new(commands: Vec<RobotCommandConfig>) -> Self {
32        let submode = commands[0].name.clone();
33        Self {
34            commands,
35            submode,
36            command_index: 0,
37            is_trigger_holding: false,
38            is_sending: false,
39        }
40    }
41
42    fn handle_event(&mut self, event: arci::gamepad::GamepadEvent) -> Option<&str> {
43        match event {
44            GamepadEvent::ButtonPressed(Button::East) => {
45                self.command_index = (self.command_index + 1) % self.commands.len();
46                let command = &self.commands[self.command_index];
47                self.submode.clone_from(&command.name);
48                return Some(&self.submode);
49            }
50            GamepadEvent::ButtonPressed(Button::RightTrigger2) => {
51                self.is_trigger_holding = true;
52            }
53            GamepadEvent::ButtonReleased(Button::RightTrigger2) => {
54                self.is_trigger_holding = false;
55                self.is_sending = false;
56            }
57            GamepadEvent::ButtonPressed(Button::West) => {
58                self.is_sending = true;
59            }
60            GamepadEvent::ButtonReleased(Button::West) => {
61                self.is_sending = false;
62            }
63            GamepadEvent::Disconnected => {
64                self.is_trigger_holding = false;
65                self.is_sending = false;
66            }
67            _ => {}
68        }
69        None
70    }
71
72    fn get_command(&self) -> &RobotCommandConfig {
73        &self.commands[self.command_index]
74    }
75}
76
77pub struct RobotCommandExecutor<S>
78where
79    S: Speaker,
80{
81    base_path: PathBuf,
82    robot_client: Arc<ArcRobotClient>,
83    speaker: S,
84    inner: Mutex<RobotCommandExecutorInner>,
85}
86
87impl<S> RobotCommandExecutor<S>
88where
89    S: Speaker,
90{
91    pub fn new(
92        base_path: PathBuf,
93        commands: Vec<RobotCommandConfig>,
94        robot_client: Arc<ArcRobotClient>,
95        speaker: S,
96    ) -> Option<Self> {
97        if commands.is_empty() {
98            None
99        } else {
100            Some(Self {
101                base_path,
102                robot_client,
103                speaker,
104                inner: Mutex::new(RobotCommandExecutorInner::new(commands)),
105            })
106        }
107    }
108}
109
110#[async_trait]
111impl<S> ControlMode for RobotCommandExecutor<S>
112where
113    S: Speaker,
114{
115    fn handle_event(&self, event: arci::gamepad::GamepadEvent) {
116        if let Some(submode) = self.inner.lock().unwrap().handle_event(event) {
117            // do not wait
118            drop(self.speaker.speak(&format!("{MODE} {submode}")).unwrap());
119        }
120    }
121
122    async fn proc(&self) {
123        let command = {
124            let inner = self.inner.lock().unwrap();
125            if inner.is_trigger_holding && inner.is_sending {
126                inner.get_command().clone()
127            } else {
128                return;
129            }
130        };
131        let executor = openrr_command::RobotCommandExecutor {};
132        match resolve_relative_path(&self.base_path, command.file_path.clone()) {
133            Ok(path) => {
134                match load_command_file_and_filter(path) {
135                    Ok(commands) => {
136                        let commands_len = commands.len() as f64;
137                        for (i, command) in commands.iter().enumerate() {
138                            if !self.inner.lock().unwrap().is_trigger_holding {
139                                warn!("Remaining commands are canceled.");
140                                return;
141                            }
142                            let command_parsed_iter = command.split_whitespace();
143                            // Parse the command
144                            let read_opt = RobotCommand::parse_from(command_parsed_iter);
145                            // Execute the parsed command
146                            info!("Executing {command} {i}/{commands_len}");
147
148                            match executor.execute(&self.robot_client, &read_opt).await {
149                                Ok(_) => {
150                                    info!("finished command {command}");
151                                }
152                                Err(e) => {
153                                    error!("failed command {command} {e:?}");
154                                    break;
155                                }
156                            }
157                        }
158                    }
159                    Err(e) => {
160                        error!("failed to load file {e:?}");
161                    }
162                }
163            }
164            Err(e) => {
165                error!("failed to resolve_relative_path {e:?}");
166            }
167        }
168    }
169
170    fn mode(&self) -> &str {
171        MODE
172    }
173
174    fn submode(&self) -> String {
175        self.inner.lock().unwrap().submode.to_owned()
176    }
177}
178
179#[derive(Debug, Serialize, Deserialize, Clone, JsonSchema)]
180#[serde(deny_unknown_fields)]
181pub struct RobotCommandConfig {
182    pub name: String,
183    pub file_path: String,
184}
185
186#[cfg(test)]
187mod test {
188    use std::collections::HashMap;
189
190    use arci::{
191        DummyJointTrajectoryClient, DummyLocalization, DummyMoveBase, DummyNavigation,
192        DummySpeaker, JointTrajectoryClient, Localization, MoveBase, Navigation,
193    };
194    use openrr_client::RobotClient;
195
196    use super::*;
197
198    #[test]
199    fn test_robot_command_executor_inner() {
200        let commands = vec![
201            RobotCommandConfig {
202                name: String::from("name0"),
203                file_path: String::from("path0"),
204            },
205            RobotCommandConfig {
206                name: String::from("name1"),
207                file_path: String::from("path1"),
208            },
209        ];
210        let mut inner = RobotCommandExecutorInner::new(commands);
211
212        assert_eq!(inner.get_command().name, "name0");
213        assert_eq!(inner.get_command().file_path, "path0");
214
215        // Changed submode
216        inner.handle_event(GamepadEvent::ButtonPressed(Button::East));
217        assert_eq!(inner.submode, "name1");
218        assert_eq!(inner.command_index, 1);
219
220        // Case that enable switch is on
221        inner.handle_event(GamepadEvent::ButtonPressed(Button::RightTrigger2));
222        assert!(inner.is_trigger_holding);
223        assert!(!inner.is_sending);
224
225        // Case that enable switch becomes off
226        inner.handle_event(GamepadEvent::ButtonReleased(Button::RightTrigger2));
227        assert!(!inner.is_trigger_holding);
228        assert!(!inner.is_sending);
229
230        // Send command
231        inner.handle_event(GamepadEvent::ButtonPressed(Button::West));
232        assert!(inner.is_sending);
233
234        // Stop send command
235        inner.handle_event(GamepadEvent::ButtonReleased(Button::West));
236        assert!(!inner.is_sending);
237
238        // Disconnected
239        inner.handle_event(GamepadEvent::Disconnected);
240        assert!(!inner.is_trigger_holding);
241        assert!(!inner.is_sending);
242    }
243
244    #[test]
245    fn test_robot_command_executor_new() {
246        let robot_client = Arc::new(
247            RobotClient::new(
248                toml::from_str("urdf_path = \"path\"").unwrap(),
249                {
250                    HashMap::from([(
251                        String::from("arm"),
252                        Arc::new(DummyJointTrajectoryClient::new(vec![String::from(
253                            "test_joint1",
254                        )])) as Arc<dyn JointTrajectoryClient>,
255                    )])
256                },
257                {
258                    HashMap::from([(
259                        String::from("speaker"),
260                        Arc::new(DummySpeaker::new()) as Arc<dyn Speaker>,
261                    )])
262                },
263                Some(Arc::new(DummyLocalization::new()) as Arc<dyn Localization>),
264                Some(Arc::new(DummyMoveBase::new()) as Arc<dyn MoveBase>),
265                Some(Arc::new(DummyNavigation::new()) as Arc<dyn Navigation>),
266            )
267            .unwrap(),
268        );
269
270        let robot_command_executor = RobotCommandExecutor::new(
271            PathBuf::from("path"),
272            vec![RobotCommandConfig {
273                name: String::from("name"),
274                file_path: String::from("file_path"),
275            }],
276            robot_client.clone(),
277            DummySpeaker::new(),
278        )
279        .unwrap();
280
281        assert_eq!(robot_command_executor.base_path, PathBuf::from("path"));
282        assert_eq!(
283            robot_command_executor.inner.lock().unwrap().commands[0].name,
284            String::from("name")
285        );
286        assert_eq!(
287            robot_command_executor.inner.lock().unwrap().commands[0].file_path,
288            String::from("file_path")
289        );
290
291        assert!(RobotCommandExecutor::new(
292            PathBuf::from("path"),
293            vec![],
294            robot_client,
295            DummySpeaker::new()
296        )
297        .is_none());
298    }
299}
openrr_teleop/robot_command_executor.rs

openrr_teleop/
robot_command_executor.rs
