openrr_teleop/
joints.rs

Help
1use std::{sync::Mutex, time::Duration};
2
3use arci::{
4    gamepad::{Axis, Button, GamepadEvent},
5    JointTrajectoryClient, Speaker,
6};
7use async_trait::async_trait;
8use schemars::JsonSchema;
9use serde::{Deserialize, Serialize};
10
11use super::control_mode::ControlMode;
12
13const AXIS_GAIN: f64 = 2.0;
14const JOINT_POSITION_TURBO_GAIN: f64 = 2.0;
15
16#[derive(Debug)]
17struct JoyJointTeleopModeInner {
18    submode: String,
19    velocity: f64,
20    dof: usize,
21    joint_index: usize,
22    joint_step: f64,
23    is_turbo: bool,
24    is_sending: bool,
25}
26
27impl JoyJointTeleopModeInner {
28    fn new(joint_step: f64, dof: usize) -> Self {
29        Self {
30            dof,
31            submode: "0".to_string(),
32            velocity: 0.0,
33            joint_index: 0,
34            joint_step,
35            is_turbo: false,
36            is_sending: false,
37        }
38    }
39
40    fn handle_event(&mut self, event: GamepadEvent) -> Option<&str> {
41        match event {
42            GamepadEvent::ButtonPressed(Button::East) => {
43                self.joint_index = (self.joint_index + 1) % self.dof;
44                self.submode = format!("{}", self.joint_index);
45                return Some(&self.submode);
46            }
47            GamepadEvent::ButtonPressed(Button::LeftTrigger2) => {
48                self.is_turbo = true;
49            }
50            GamepadEvent::ButtonReleased(Button::LeftTrigger2) => {
51                self.is_turbo = false;
52            }
53            GamepadEvent::ButtonPressed(Button::RightTrigger2) => {
54                self.is_sending = true;
55            }
56            GamepadEvent::ButtonReleased(Button::RightTrigger2) => {
57                self.is_sending = false;
58                self.velocity = 0.0;
59            }
60            GamepadEvent::ButtonPressed(Button::West) => {
61                self.velocity = self.joint_step;
62            }
63            GamepadEvent::ButtonReleased(Button::West) => {
64                self.velocity = 0.0;
65            }
66            GamepadEvent::ButtonPressed(Button::South) => {
67                self.velocity = -self.joint_step;
68            }
69            GamepadEvent::ButtonReleased(Button::South) => {
70                self.velocity = 0.0;
71            }
72            GamepadEvent::AxisChanged(Axis::RightStickY, v) => {
73                self.velocity = self.joint_step * v * AXIS_GAIN;
74            }
75            GamepadEvent::Disconnected => {
76                self.is_sending = false;
77                self.is_turbo = false;
78                self.velocity = 0.0;
79            }
80            _ => {}
81        }
82        None
83    }
84
85    fn get_target_positions(&self, mut current_positions: Vec<f64>) -> Vec<f64> {
86        current_positions[self.joint_index] += self.velocity
87            * if self.is_turbo {
88                JOINT_POSITION_TURBO_GAIN
89            } else {
90                1.0
91            };
92        current_positions
93    }
94}
95
96#[derive(Debug)]
97pub struct JoyJointTeleopMode<J, S>
98where
99    J: JointTrajectoryClient,
100    S: Speaker,
101{
102    joint_trajectory_client: J,
103    speaker: S,
104    mode: String,
105    step_duration: Duration,
106    inner: Mutex<JoyJointTeleopModeInner>,
107}
108
109impl<J, S> JoyJointTeleopMode<J, S>
110where
111    J: JointTrajectoryClient,
112    S: Speaker,
113{
114    pub fn new(
115        mode: String,
116        joint_trajectory_client: J,
117        joint_step: f64,
118        step_duration: Duration,
119        speaker: S,
120    ) -> Self {
121        let dof = joint_trajectory_client.joint_names().len();
122        Self {
123            joint_trajectory_client,
124            speaker,
125            mode,
126            step_duration,
127            inner: Mutex::new(JoyJointTeleopModeInner::new(joint_step, dof)),
128        }
129    }
130
131    pub fn new_from_config(
132        config: JoyJointTeleopModeConfig,
133        joint_trajectory_client: J,
134        speaker: S,
135    ) -> Self {
136        Self::new(
137            config.mode,
138            joint_trajectory_client,
139            config.joint_step,
140            Duration::from_secs_f64(config.step_duration_secs),
141            speaker,
142        )
143    }
144}
145
146#[async_trait]
147impl<N, S> ControlMode for JoyJointTeleopMode<N, S>
148where
149    N: JointTrajectoryClient,
150    S: Speaker,
151{
152    fn handle_event(&self, event: GamepadEvent) {
153        if let Some(submode) = self.inner.lock().unwrap().handle_event(event) {
154            // do not wait
155            drop(
156                self.speaker
157                    .speak(&format!("{}{submode}", self.mode))
158                    .unwrap(),
159            );
160        }
161    }
162
163    async fn proc(&self) {
164        let inner = self.inner.lock().unwrap();
165        if inner.is_sending {
166            let pos = self
167                .joint_trajectory_client
168                .current_joint_positions()
169                .unwrap();
170            let pos = inner.get_target_positions(pos);
171            // do not wait
172            drop(
173                self.joint_trajectory_client
174                    .send_joint_positions(pos, self.step_duration)
175                    .unwrap(),
176            );
177        }
178    }
179
180    fn mode(&self) -> &str {
181        &self.mode
182    }
183
184    fn submode(&self) -> String {
185        self.inner.lock().unwrap().submode.to_owned()
186    }
187}
188
189#[derive(Debug, Serialize, Deserialize, Clone, JsonSchema)]
190#[serde(deny_unknown_fields)]
191pub struct JoyJointTeleopModeConfig {
192    pub mode: String,
193    #[serde(default = "default_joint_step")]
194    pub joint_step: f64,
195    #[serde(default = "default_step_duration_secs")]
196    pub step_duration_secs: f64,
197}
198
199const fn default_joint_step() -> f64 {
200    0.02
201}
202
203const fn default_step_duration_secs() -> f64 {
204    0.1
205}
206
207#[cfg(test)]
208mod tests {
209    use assert_approx_eq::*;
210
211    use super::*;
212
213    #[test]
214    fn test_default_joint_step() {
215        let def = default_joint_step();
216
217        assert_approx_eq!(def, 0.02_f64);
218    }
219
220    #[test]
221    fn test_default_step_duration_secs() {
222        let def = default_step_duration_secs();
223
224        assert_approx_eq!(def, 0.1_f64);
225    }
226}
openrr_teleop/joints.rs

openrr_teleop/
joints.rs
