openrr_teleop/
joints.rs
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 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 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}