openrr_apps_robot_teleop/
robot_teleop.rs

Help
1#[cfg(feature = "ros")]
2use std::thread;
3use std::{fs, path::PathBuf, sync::Arc};
4
5use anyhow::{format_err, Result};
6use arci_gamepad_gilrs::GilGamepad;
7use clap::Parser;
8use openrr_apps::{
9    utils::{init_tracing, init_tracing_with_file_appender, LogConfig},
10    BuiltinGamepad, Error, GamepadKind, RobotTeleopConfig,
11};
12use openrr_client::ArcRobotClient;
13use openrr_plugin::PluginProxy;
14use openrr_teleop::ControlModeSwitcher;
15use tracing::info;
16
17/// An openrr teleoperation tool.
18#[derive(Parser, Debug)]
19#[clap(name = env!("CARGO_BIN_NAME"))]
20pub struct RobotTeleopArgs {
21    /// Path to the setting file.
22    #[clap(short, long, value_parser)]
23    config_path: Option<PathBuf>,
24    /// Set options from command line. These settings take priority over the
25    /// setting file specified by --config-path.
26    #[clap(long)]
27    teleop_config: Option<String>,
28    /// Set options from command line. These settings take priority over the
29    /// setting file specified by --config-path.
30    #[clap(long)]
31    robot_config: Option<String>,
32    /// Prints the default setting as TOML.
33    #[clap(long)]
34    show_default_config: bool,
35    /// Path to log directory for tracing FileAppender.
36    #[clap(long, value_parser)]
37    log_directory: Option<PathBuf>,
38}
39
40#[tokio::main]
41async fn main() -> Result<()> {
42    let args = RobotTeleopArgs::parse();
43
44    if args.show_default_config {
45        print!(
46            "{}",
47            toml::to_string(&RobotTeleopConfig::default()).unwrap()
48        );
49        return Ok(());
50    }
51
52    if args.log_directory.is_none() {
53        init_tracing();
54    }
55    #[cfg(not(feature = "ros"))]
56    let _guard = args.log_directory.map(|log_directory| {
57        init_tracing_with_file_appender(
58            LogConfig {
59                directory: log_directory,
60                ..Default::default()
61            },
62            env!("CARGO_BIN_NAME").to_string(),
63        )
64    });
65
66    let teleop_config = openrr_apps::utils::resolve_teleop_config(
67        args.config_path.as_deref(),
68        args.teleop_config.as_deref(),
69    )?;
70    let robot_config_path = teleop_config.robot_config_full_path();
71    let robot_config = openrr_apps::utils::resolve_robot_config(
72        robot_config_path.as_deref(),
73        args.robot_config.as_deref(),
74    )?;
75
76    openrr_apps::utils::init(env!("CARGO_BIN_NAME"), &robot_config);
77    #[cfg(feature = "ros")]
78    let use_ros = robot_config.has_ros_clients();
79    #[cfg(feature = "ros")]
80    let _guard = args.log_directory.map(|log_directory| {
81        init_tracing_with_file_appender(
82            LogConfig {
83                directory: log_directory,
84                ..Default::default()
85            },
86            if use_ros {
87                arci_ros::name()
88            } else {
89                env!("CARGO_BIN_NAME").to_string()
90            },
91        )
92    });
93    let client: Arc<ArcRobotClient> = Arc::new(robot_config.create_robot_client()?);
94
95    let speaker = client.speakers().values().next().unwrap();
96
97    let modes = teleop_config
98        .control_modes_config
99        .create_control_modes(
100            args.config_path,
101            client.clone(),
102            speaker.clone(),
103            client.joint_trajectory_clients(),
104            client.ik_solvers(),
105            Some(client.clone()),
106            robot_config.openrr_clients_config.joints_poses,
107        )
108        .unwrap();
109    if modes.is_empty() {
110        panic!("No valid modes");
111    }
112
113    let initial_mode_index = if teleop_config.initial_mode.is_empty() {
114        info!("Use first mode as initial mode");
115        0
116    } else if let Some(initial_mode_index) = modes
117        .iter()
118        .position(|mode| mode.mode() == teleop_config.initial_mode)
119    {
120        initial_mode_index
121    } else {
122        return Err(Error::NoSpecifiedMode(teleop_config.initial_mode).into());
123    };
124
125    let switcher = Arc::new(ControlModeSwitcher::new(
126        modes,
127        speaker.clone(),
128        initial_mode_index,
129    ));
130    #[cfg(feature = "ros")]
131    if use_ros {
132        let switcher_cloned = switcher.clone();
133        thread::spawn(move || {
134            let rate = arci_ros::rate(1.0);
135            while arci_ros::is_ok() {
136                rate.sleep();
137            }
138            switcher_cloned.stop();
139        });
140    }
141
142    match teleop_config.gamepad {
143        GamepadKind::Builtin(BuiltinGamepad::Gilrs) => {
144            switcher
145                .main(GilGamepad::new_from_config(
146                    teleop_config.gil_gamepad_config,
147                ))
148                .await;
149        }
150        #[cfg(unix)]
151        GamepadKind::Builtin(BuiltinGamepad::Keyboard) => {
152            switcher
153                .main(arci_gamepad_keyboard::KeyboardGamepad::new())
154                .await;
155        }
156        #[cfg(windows)]
157        GamepadKind::Builtin(BuiltinGamepad::Keyboard) => {
158            anyhow::bail!("`gamepad = \"keyboard\"` is not supported on windows");
159        }
160        #[cfg(feature = "ros")]
161        GamepadKind::Builtin(BuiltinGamepad::RosJoyGamepad) => {
162            if !use_ros {
163                arci_ros::init(env!("CARGO_BIN_NAME"));
164            }
165            switcher
166                .main(arci_ros::RosJoyGamepad::new_from_config(
167                    &teleop_config.ros_joy_gamepad_config,
168                ))
169                .await;
170        }
171        #[cfg(not(feature = "ros"))]
172        GamepadKind::Builtin(BuiltinGamepad::RosJoyGamepad) => {
173            anyhow::bail!("`gamepad = \"ros-joy-gamepad\"` requires \"ros\" feature");
174        }
175        GamepadKind::Plugin(name) => {
176            let mut gamepad = None;
177            for (plugin_name, config) in teleop_config.plugins {
178                if name == plugin_name {
179                    let args = if let Some(path) = &config.args_from_path {
180                        fs::read_to_string(path).map_err(|e| Error::NoFile(path.to_owned(), e))?
181                    } else {
182                        config.args.unwrap_or_default()
183                    };
184                    let plugin = PluginProxy::from_path(&config.path)?;
185                    gamepad = Some(plugin.new_gamepad(args)?.ok_or_else(|| {
186                        format_err!("failed to create `Gamepad` instance `{name}`: None")
187                    })?);
188                    break;
189                }
190            }
191            match gamepad {
192                Some(gamepad) => {
193                    switcher.main(gamepad).await;
194                }
195                None => {
196                    return Err(Error::NoPluginInstance {
197                        name,
198                        kind: "Gamepad".to_string(),
199                    }
200                    .into());
201                }
202            }
203        }
204    }
205
206    Ok(())
207}
208
209#[cfg(test)]
210mod tests {
211    use clap::CommandFactory;
212
213    use super::*;
214
215    #[test]
216    fn parse_args() {
217        let bin = env!("CARGO_BIN_NAME");
218        assert!(RobotTeleopArgs::try_parse_from([bin]).is_ok());
219        assert!(RobotTeleopArgs::try_parse_from([bin, "--show-default-config"]).is_ok());
220        assert!(RobotTeleopArgs::try_parse_from([bin, "--config-path", "path"]).is_ok());
221        assert!(RobotTeleopArgs::try_parse_from([
222            bin,
223            "--show-default-config",
224            "--config-path",
225            "path"
226        ])
227        .is_ok());
228    }
229
230    #[test]
231    fn assert_app() {
232        RobotTeleopArgs::command().debug_assert();
233    }
234}
openrr_apps_robot_teleop/robot_teleop.rs

openrr_apps_robot_teleop/
robot_teleop.rs
