arci_ros/
joy_gamepad.rs

1use std::{
2    collections::HashMap,
3    sync::{Arc, Mutex},
4};
5
6use arci::{gamepad::*, *};
7use schemars::JsonSchema;
8use serde::{Deserialize, Serialize};
9
10use crate::msg::sensor_msgs::Joy;
11
12pub struct RosJoyGamepad {
13    _last_joy_msg: Arc<Mutex<Joy>>,
14    rx: flume::Receiver<GamepadEvent>,
15    _sub: rosrust::Subscriber,
16}
17
18impl RosJoyGamepad {
19    pub fn new(
20        topic_name: &str,
21        button_mapping: HashMap<usize, arci::gamepad::Button>,
22        axis_mapping: HashMap<usize, arci::gamepad::Axis>,
23    ) -> Self {
24        const DEAD_ZONE: f32 = 0.00001;
25        const TOPIC_BUFFER_SIZE: usize = 100;
26        let last_joy_msg = Arc::new(Mutex::new(Joy::default()));
27        let (tx, rx) = flume::unbounded();
28        let tx_for_stop = tx.clone();
29        // spawn for stop by Ctrl-C
30        tokio::spawn(async move {
31            tokio::signal::ctrl_c().await.unwrap();
32            tx_for_stop.send(GamepadEvent::Unknown).unwrap();
33        });
34
35        let last_joy_msg_clone = last_joy_msg.clone();
36        let _sub = rosrust::subscribe(topic_name, TOPIC_BUFFER_SIZE, move |joy_msg: Joy| {
37            let mut last_joy = last_joy_msg_clone.lock().unwrap();
38            // initialize last_joy_msg
39            if last_joy.buttons.len() < joy_msg.buttons.len() {
40                last_joy.buttons.resize(joy_msg.buttons.len(), 0);
41            }
42            if last_joy.axes.len() < joy_msg.axes.len() {
43                last_joy.axes.resize(joy_msg.axes.len(), 0.0);
44            }
45            for (idx_ref, button) in button_mapping.iter() {
46                let idx = *idx_ref;
47                if joy_msg.buttons.len() <= idx {
48                    rosrust::ros_err!(
49                        "buttons index is out of range, ignored: input={idx}, size={}",
50                        joy_msg.buttons.len()
51                    );
52                } else if last_joy.buttons[idx] == 0 && joy_msg.buttons[idx] == 1 {
53                    tx.send(GamepadEvent::ButtonPressed(button.to_owned()))
54                        .unwrap();
55                } else if last_joy.buttons[idx] == 1 && joy_msg.buttons[idx] == 0 {
56                    tx.send(GamepadEvent::ButtonReleased(button.to_owned()))
57                        .unwrap();
58                }
59            }
60            for (idx_ref, axis) in axis_mapping.iter() {
61                let idx = *idx_ref;
62                if joy_msg.axes.len() <= idx {
63                    rosrust::ros_err!(
64                        "axes index is out of range, ignored: input={idx}, size={}",
65                        joy_msg.buttons.len()
66                    );
67                } else if (last_joy.axes[idx] - joy_msg.axes[idx]).abs() > DEAD_ZONE {
68                    tx.send(GamepadEvent::AxisChanged(
69                        axis.to_owned(),
70                        joy_msg.axes[idx] as f64,
71                    ))
72                    .unwrap();
73                }
74            }
75            *last_joy = joy_msg;
76        })
77        .unwrap();
78        Self {
79            _last_joy_msg: last_joy_msg,
80            rx,
81            _sub,
82        }
83    }
84
85    pub fn new_from_config(config: &RosJoyGamepadConfig) -> Self {
86        let topic_name = &config.topic_name;
87        let mut button_map = HashMap::new();
88        for (key, &value) in config.button_map.iter() {
89            button_map.insert(key.parse::<usize>().unwrap(), value);
90        }
91        let mut axis_map = HashMap::new();
92        for (key, &value) in config.axis_map.iter() {
93            axis_map.insert(key.parse::<usize>().unwrap(), value);
94        }
95        Self::new(topic_name, button_map, axis_map)
96    }
97}
98
99#[derive(Debug, Serialize, Deserialize, Clone, JsonSchema)]
100#[serde(deny_unknown_fields)]
101pub struct RosJoyGamepadConfig {
102    pub topic_name: String,
103    pub button_map: HashMap<String, Button>,
104    pub axis_map: HashMap<String, Axis>,
105}
106
107impl RosJoyGamepadConfig {
108    pub fn new() -> Self {
109        Self {
110            topic_name: default_topic_name(),
111            button_map: default_button_map(),
112            axis_map: default_axis_map(),
113        }
114    }
115}
116
117impl Default for RosJoyGamepadConfig {
118    fn default() -> Self {
119        RosJoyGamepadConfig::new()
120    }
121}
122
123fn default_topic_name() -> String {
124    "joy".to_string()
125}
126
127fn default_button_map() -> HashMap<String, Button> {
128    let mut button_map = HashMap::new();
129    button_map.insert("0".to_string(), arci::gamepad::Button::South);
130    button_map.insert("1".to_string(), arci::gamepad::Button::East);
131    button_map.insert("2".to_string(), arci::gamepad::Button::West);
132    button_map.insert("3".to_string(), arci::gamepad::Button::North);
133    button_map.insert("4".to_string(), arci::gamepad::Button::LeftTrigger2);
134    button_map.insert("5".to_string(), arci::gamepad::Button::RightTrigger2);
135    button_map
136}
137
138fn default_axis_map() -> HashMap<String, Axis> {
139    let mut axis_map = HashMap::new();
140    axis_map.insert("0".to_string(), arci::gamepad::Axis::LeftStickX);
141    axis_map.insert("1".to_string(), arci::gamepad::Axis::LeftStickY);
142    axis_map.insert("2".to_string(), arci::gamepad::Axis::LeftTrigger);
143    axis_map.insert("3".to_string(), arci::gamepad::Axis::RightStickX);
144    axis_map.insert("4".to_string(), arci::gamepad::Axis::RightStickY);
145    axis_map.insert("5".to_string(), arci::gamepad::Axis::RightTrigger);
146    axis_map.insert("6".to_string(), arci::gamepad::Axis::DPadX);
147    axis_map.insert("7".to_string(), arci::gamepad::Axis::DPadY);
148    axis_map
149}
150
151#[async_trait]
152impl Gamepad for RosJoyGamepad {
153    async fn next_event(&self) -> GamepadEvent {
154        if let Ok(ev) = self.rx.recv_async().await {
155            ev
156        } else {
157            GamepadEvent::Unknown
158        }
159    }
160
161    fn stop(&self) {}
162}
arci_ros/joy_gamepad.rs

arci_ros/
joy_gamepad.rs
