openrr_teleop/
control_modes_config.rs

1use std::{collections::HashMap, path::PathBuf, sync::Arc};
2
3use arci::{JointTrajectoryClient, MoveBase, Speaker};
4use openrr_client::{ArcRobotClient, Error, IkSolverWithChain, JointsPose};
5use schemars::JsonSchema;
6use serde::{Deserialize, Serialize};
7
8use crate::{
9    ControlMode, IkMode, IkModeConfig, JointsPoseSender, JointsPoseSenderConfig,
10    JoyJointTeleopMode, JoyJointTeleopModeConfig, MoveBaseMode, RobotCommandConfig,
11    RobotCommandExecutor,
12};
13
14#[derive(Debug, Serialize, Deserialize, Clone, JsonSchema)]
15#[serde(deny_unknown_fields)]
16pub struct JoyJointTeleopConfig {
17    pub client_name: String,
18    pub config: JoyJointTeleopModeConfig,
19}
20
21#[derive(Debug, Serialize, Deserialize, Clone, JsonSchema)]
22#[serde(deny_unknown_fields)]
23pub struct IkModeTeleopConfig {
24    pub solver_name: String,
25    pub joint_trajectory_client_name: String,
26    pub config: IkModeConfig,
27}
28
29#[derive(Debug, Serialize, Deserialize, Clone, Default, JsonSchema)]
30#[serde(deny_unknown_fields)]
31pub struct ControlModesConfig {
32    pub move_base_mode: Option<String>,
33    #[serde(default)]
34    // https://github.com/alexcrichton/toml-rs/issues/258
35    #[serde(skip_serializing_if = "Vec::is_empty")]
36    pub joy_joint_teleop_configs: Vec<JoyJointTeleopConfig>,
37    #[serde(default)]
38    // https://github.com/alexcrichton/toml-rs/issues/258
39    #[serde(skip_serializing_if = "Vec::is_empty")]
40    pub ik_mode_teleop_configs: Vec<IkModeTeleopConfig>,
41    pub joints_pose_sender_config: Option<JointsPoseSenderConfig>,
42    #[serde(default)]
43    // https://github.com/alexcrichton/toml-rs/issues/258
44    #[serde(skip_serializing_if = "Vec::is_empty")]
45    pub command_configs: Vec<RobotCommandConfig>,
46}
47
48impl ControlModesConfig {
49    #[allow(clippy::too_many_arguments)]
50    pub fn create_control_modes(
51        &self,
52        base_path: Option<PathBuf>,
53        robot_client: Arc<ArcRobotClient>,
54        speaker: Arc<dyn Speaker>,
55        joint_trajectory_client_map: &HashMap<String, Arc<dyn JointTrajectoryClient>>,
56        ik_solver_with_chain_map: &HashMap<String, Arc<IkSolverWithChain>>,
57        move_base: Option<Arc<dyn MoveBase>>,
58        joints_poses: Vec<JointsPose>,
59    ) -> Result<Vec<Arc<dyn ControlMode>>, Error> {
60        let mut modes: Vec<Arc<dyn ControlMode>> = vec![];
61
62        for joy_joint_teleop_config in &self.joy_joint_teleop_configs {
63            modes.push(Arc::new(JoyJointTeleopMode::new_from_config(
64                joy_joint_teleop_config.config.clone(),
65                joint_trajectory_client_map
66                    .get(&joy_joint_teleop_config.client_name)
67                    .ok_or_else(|| {
68                        Error::NoMapKey(
69                            format!("joint_trajectory_client_map(required from joy_joint_teleop_configs)[{} l.{}]", file!(), line!()),
70                            joy_joint_teleop_config.client_name.to_string(),
71                        )
72                    })?
73                    .clone(),
74                speaker.clone(),
75            )));
76        }
77
78        if let Some(mode) = &self.move_base_mode
79            && let Some(m) = move_base
80        {
81            modes.push(Arc::new(MoveBaseMode::new(mode.clone(), m.clone())));
82        }
83
84        for ik_mode_teleop_config in &self.ik_mode_teleop_configs {
85            let ik_mode = IkMode::new_from_config(
86                ik_mode_teleop_config.config.clone(),
87                joint_trajectory_client_map
88                    .get(&ik_mode_teleop_config.joint_trajectory_client_name)
89                    .ok_or_else(|| {
90                        Error::NoMapKey(
91                            format!(
92                                "joint_trajectory_client_map(required from ik_mode_teleop_configs)[{} l.{}]",
93                                file!(),
94                                line!()
95                            ),
96                            ik_mode_teleop_config
97                                .joint_trajectory_client_name
98                                .to_string(),
99                        )
100                    })?
101                    .clone(),
102                speaker.clone(),
103                ik_solver_with_chain_map
104                    .get(&ik_mode_teleop_config.solver_name)
105                    .ok_or_else(|| {
106                        Error::NoMapKey(
107                            format!(
108                                "ik_solver_with_chain_map(required from ik_mode_teleop_configs)[{} l.{}]",
109                                file!(),
110                                line!()
111                            ),
112                            ik_mode_teleop_config.solver_name.to_string(),
113                        )
114                    })?
115                    .clone(),
116            );
117            modes.push(Arc::new(ik_mode));
118        }
119
120        if !joints_poses.is_empty()
121            && let Some(sender_config) = &self.joints_pose_sender_config
122        {
123            let mut joint_trajectory_clients = HashMap::new();
124            for joints_pose in &joints_poses {
125                joint_trajectory_clients.insert(
126                        joints_pose.client_name.to_owned(),
127                        joint_trajectory_client_map
128                            .get(&joints_pose.client_name)
129                            .ok_or_else(|| {
130                                Error::NoMapKey(
131                                    format!(
132                                        "joint_trajectory_client_map(required form joint pose)[{} l.{}]",
133                                        file!(),
134                                        line!()
135                                    ),
136                                    joints_pose.client_name.to_string(),
137                                )
138                            })?
139                            .clone(),
140                    );
141            }
142            modes.push(Arc::new(JointsPoseSender::new_from_config(
143                sender_config.clone(),
144                joints_poses,
145                joint_trajectory_clients,
146                speaker.clone(),
147            )));
148        }
149
150        if !self.command_configs.is_empty()
151            && let Some(base_path) = base_path
152            && let Some(e) = RobotCommandExecutor::new(
153                base_path,
154                self.command_configs.clone(),
155                robot_client,
156                speaker.clone(),
157            )
158        {
159            modes.push(Arc::new(e));
160        }
161
162        Ok(modes)
163    }
164}
165
166#[cfg(test)]
167mod tests {
168    use std::collections::HashMap;
169
170    use arci::{
171        DummyJointTrajectoryClient, DummyLocalization, DummyMoveBase, DummyNavigation, DummySpeaker,
172    };
173    use openrr_client::{
174        IkSolverParameters, IkSolverWithChain, OpenrrClientsConfig,
175        create_random_jacobian_ik_solver,
176    };
177
178    use super::*;
179
180    #[test]
181    fn test_gen() {
182        // JoyJointTeleopConfig
183        let joy_joint_teleop_mode_config = JoyJointTeleopModeConfig {
184            mode: String::from("a"),
185            joint_step: 1.0_f64,
186            step_duration_secs: 0.1_f64,
187        };
188        let _joy_joint_teleop_config = JoyJointTeleopConfig {
189            client_name: String::from("b"),
190            config: joy_joint_teleop_mode_config,
191        };
192
193        // IkModeTeleopConfig
194        let ik_mode_config = IkModeConfig {
195            mode: String::from("c"),
196            move_step_angular: [1.0_f64, 1.0, 1.0],
197            move_step_linear: [1.0_f64, 1.0, 1.0],
198            step_duration_secs: 0.1_f64,
199        };
200
201        let _ik_mode_teleop_config = IkModeTeleopConfig {
202            solver_name: String::from("d"),
203            joint_trajectory_client_name: String::from("f"),
204            config: ik_mode_config,
205        };
206    }
207
208    #[test]
209    fn test_control_mode_config_create_error() {
210        let joy_joint_teleop_config = JoyJointTeleopConfig {
211            client_name: String::from("jj config"),
212            config: JoyJointTeleopModeConfig {
213                mode: String::from("joy joint teleop mode config"),
214                joint_step: 1.0_f64,
215                step_duration_secs: 0.1_f64,
216            },
217        };
218        let ik_mode_teleop_config = IkModeTeleopConfig {
219            solver_name: String::from("ik config solver"),
220            joint_trajectory_client_name: String::from("ik trajectory"),
221            config: IkModeConfig {
222                mode: String::from("ik mode config"),
223                move_step_angular: [1.0_f64, 1.0, 1.0],
224                move_step_linear: [1.0_f64, 1.0, 1.0],
225                step_duration_secs: 0.1_f64,
226            },
227        };
228        let robot_command_config = RobotCommandConfig {
229            name: String::from("rc config"),
230            file_path: String::from("sample path"),
231        };
232        let ctrl_mode_config = ControlModesConfig {
233            move_base_mode: Some(String::from("a")),
234            joy_joint_teleop_configs: vec![joy_joint_teleop_config],
235            ik_mode_teleop_configs: vec![ik_mode_teleop_config],
236            joints_pose_sender_config: None,
237            command_configs: vec![robot_command_config],
238        };
239
240        // Check create control mode(Error:No HashMap item[Key=jj config])
241        // HashMap key is required by ControlModesConfig
242        let speaker = Arc::new(DummySpeaker::default());
243        let robot_client = ArcRobotClient::new(
244            OpenrrClientsConfig::default(),
245            HashMap::new(),
246            HashMap::new(),
247            None,
248            None,
249            None,
250        );
251        assert!(robot_client.is_ok());
252        let robot_client = robot_client.unwrap();
253
254        // the 4th argument `joint_trajectory_client_map` had to have keys same with the items in `joy_joint_teleop_configs`
255        let modes = ctrl_mode_config.create_control_modes(
256            None,
257            Arc::new(robot_client),
258            speaker.clone(),
259            &HashMap::new(),
260            &HashMap::new(),
261            Some(Arc::new(DummyMoveBase::default())),
262            Vec::new(),
263        );
264        assert!(modes.is_err());
265        let err = modes.err().unwrap();
266        println!("{err}");
267
268        // Check create control mode(Error:No HashMap item[Key=ik_trajectory])
269        // HashMap key is required by ControlModesConfig
270        let robot_client = ArcRobotClient::new(
271            OpenrrClientsConfig::default(),
272            HashMap::new(),
273            HashMap::new(),
274            None,
275            None,
276            None,
277        );
278        let robot_client = robot_client.unwrap();
279        let joint_trajectory_client: Arc<dyn JointTrajectoryClient> =
280            Arc::new(DummyJointTrajectoryClient::new(vec!["a".to_owned()]));
281        let mut joint_trajectory_map = HashMap::new();
282        joint_trajectory_map.insert("jj config".to_owned(), joint_trajectory_client);
283
284        let chain = k::Chain::<f64>::from_urdf_file("../openrr-planner/sample.urdf").unwrap();
285        let end_link = chain.find("l_tool_fixed").unwrap();
286        let arm = k::SerialChain::from_end(end_link);
287        let params = IkSolverParameters {
288            allowable_position_error: 0.01,
289            allowable_angle_error: 0.02,
290            jacobian_multiplier: 0.1,
291            num_max_try: 100,
292        };
293        let ik_solver = create_random_jacobian_ik_solver(&params);
294        let constraints = k::Constraints::default();
295        let ik_solver = IkSolverWithChain::new(arm, Arc::new(ik_solver), constraints);
296        let mut ik_map = HashMap::new();
297        ik_map.insert("ik config solver".to_owned(), Arc::new(ik_solver));
298
299        // the 5th argument `joint_trajectory_client_map` had to have keys same with the items in `ik_mode_teleop_configs`
300        let modes = ctrl_mode_config.create_control_modes(
301            None,
302            Arc::new(robot_client),
303            speaker,
304            &joint_trajectory_map,
305            &ik_map,
306            Some(Arc::new(DummyMoveBase::default())),
307            Vec::new(),
308        );
309        assert!(modes.is_err());
310        let err = modes.err().unwrap();
311        println!("{err}");
312    }
313
314    #[test]
315    fn test_control_mode_config_create_ok() {
316        let joy_joint_teleop_config = JoyJointTeleopConfig {
317            client_name: String::from("jj config"),
318            config: JoyJointTeleopModeConfig {
319                mode: String::from("joy joint teleop mode config"),
320                joint_step: 1.0_f64,
321                step_duration_secs: 0.1_f64,
322            },
323        };
324        let ik_mode_teleop_config = IkModeTeleopConfig {
325            solver_name: String::from("ik config solver"),
326            joint_trajectory_client_name: String::from("jj config"),
327            config: IkModeConfig {
328                mode: String::from("ik mode config"),
329                move_step_angular: [1.0_f64, 1.0, 1.0],
330                move_step_linear: [1.0_f64, 1.0, 1.0],
331                step_duration_secs: 0.1_f64,
332            },
333        };
334        let robot_command_config = RobotCommandConfig {
335            name: String::from("rc config"),
336            file_path: String::from("sample path"),
337        };
338        let ctrl_mode_config = ControlModesConfig {
339            move_base_mode: Some(String::from("a")),
340            joy_joint_teleop_configs: vec![joy_joint_teleop_config],
341            ik_mode_teleop_configs: vec![ik_mode_teleop_config],
342            joints_pose_sender_config: None,
343            command_configs: vec![robot_command_config],
344        };
345
346        // Check create control mode(Ok)
347        let speaker = Arc::new(DummySpeaker::default());
348        let robot_client = ArcRobotClient::new(
349            OpenrrClientsConfig::default(),
350            HashMap::new(),
351            HashMap::new(),
352            Some(Arc::new(DummyLocalization::default())),
353            Some(Arc::new(DummyMoveBase::default())),
354            Some(Arc::new(DummyNavigation::default())),
355        );
356        assert!(robot_client.is_ok());
357        let robot_client = robot_client.unwrap();
358
359        let joint_trajectory_client: Arc<dyn JointTrajectoryClient> =
360            Arc::new(DummyJointTrajectoryClient::new(vec!["a".to_owned()]));
361        let mut joint_trajectory_map = HashMap::new();
362        joint_trajectory_map.insert("jj config".to_owned(), joint_trajectory_client);
363
364        let chain = k::Chain::<f64>::from_urdf_file("../openrr-planner/sample.urdf").unwrap();
365        let end_link = chain.find("l_tool_fixed").unwrap();
366        let arm = k::SerialChain::from_end(end_link);
367        let params = IkSolverParameters {
368            allowable_position_error: 0.01,
369            allowable_angle_error: 0.02,
370            jacobian_multiplier: 0.1,
371            num_max_try: 100,
372        };
373        let ik_solver = create_random_jacobian_ik_solver(&params);
374        let constraints = k::Constraints::default();
375        let ik_solver = IkSolverWithChain::new(arm, Arc::new(ik_solver), constraints);
376        let mut ik_map = HashMap::new();
377        ik_map.insert("ik config solver".to_owned(), Arc::new(ik_solver));
378
379        let modes = ctrl_mode_config.create_control_modes(
380            None,
381            Arc::new(robot_client),
382            speaker,
383            &joint_trajectory_map,
384            &ik_map,
385            Some(Arc::new(DummyMoveBase::default())),
386            Vec::new(),
387        );
388        assert!(modes.is_ok());
389    }
390}
openrr_teleop/control_modes_config.rs

openrr_teleop/
control_modes_config.rs
