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            if let Some(m) = move_base {
80                modes.push(Arc::new(MoveBaseMode::new(mode.clone(), m.clone())));
81            }
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            if let Some(sender_config) = &self.joints_pose_sender_config {
122                let mut joint_trajectory_clients = HashMap::new();
123                for joints_pose in &joints_poses {
124                    joint_trajectory_clients.insert(
125                        joints_pose.client_name.to_owned(),
126                        joint_trajectory_client_map
127                            .get(&joints_pose.client_name)
128                            .ok_or_else(|| {
129                                Error::NoMapKey(
130                                    format!(
131                                        "joint_trajectory_client_map(required form joint pose)[{} l.{}]",
132                                        file!(),
133                                        line!()
134                                    ),
135                                    joints_pose.client_name.to_string(),
136                                )
137                            })?
138                            .clone(),
139                    );
140                }
141                modes.push(Arc::new(JointsPoseSender::new_from_config(
142                    sender_config.clone(),
143                    joints_poses,
144                    joint_trajectory_clients,
145                    speaker.clone(),
146                )));
147            }
148        }
149
150        if !self.command_configs.is_empty() {
151            if let Some(base_path) = base_path {
152                if let Some(e) = RobotCommandExecutor::new(
153                    base_path,
154                    self.command_configs.clone(),
155                    robot_client,
156                    speaker.clone(),
157                ) {
158                    modes.push(Arc::new(e));
159                }
160            }
161        }
162
163        Ok(modes)
164    }
165}
166
167#[cfg(test)]
168mod tests {
169    use std::collections::HashMap;
170
171    use arci::{
172        DummyJointTrajectoryClient, DummyLocalization, DummyMoveBase, DummyNavigation, DummySpeaker,
173    };
174    use openrr_client::{
175        create_random_jacobian_ik_solver, IkSolverParameters, IkSolverWithChain,
176        OpenrrClientsConfig,
177    };
178
179    use super::*;
180
181    #[test]
182    fn test_gen() {
183        // JoyJointTeleopConfig
184        let joy_joint_teleop_mode_config = JoyJointTeleopModeConfig {
185            mode: String::from("a"),
186            joint_step: 1.0_f64,
187            step_duration_secs: 0.1_f64,
188        };
189        let _joy_joint_teleop_config = JoyJointTeleopConfig {
190            client_name: String::from("b"),
191            config: joy_joint_teleop_mode_config,
192        };
193
194        // IkModeTeleopConfig
195        let ik_mode_config = IkModeConfig {
196            mode: String::from("c"),
197            move_step_angular: [1.0_f64, 1.0, 1.0],
198            move_step_linear: [1.0_f64, 1.0, 1.0],
199            step_duration_secs: 0.1_f64,
200        };
201
202        let _ik_mode_teleop_config = IkModeTeleopConfig {
203            solver_name: String::from("d"),
204            joint_trajectory_client_name: String::from("f"),
205            config: ik_mode_config,
206        };
207    }
208
209    #[test]
210    fn test_control_mode_config_create_error() {
211        let joy_joint_teleop_config = JoyJointTeleopConfig {
212            client_name: String::from("jj config"),
213            config: JoyJointTeleopModeConfig {
214                mode: String::from("joy joint teleop mode config"),
215                joint_step: 1.0_f64,
216                step_duration_secs: 0.1_f64,
217            },
218        };
219        let ik_mode_teleop_config = IkModeTeleopConfig {
220            solver_name: String::from("ik config solver"),
221            joint_trajectory_client_name: String::from("ik trajectory"),
222            config: IkModeConfig {
223                mode: String::from("ik mode config"),
224                move_step_angular: [1.0_f64, 1.0, 1.0],
225                move_step_linear: [1.0_f64, 1.0, 1.0],
226                step_duration_secs: 0.1_f64,
227            },
228        };
229        let robot_command_config = RobotCommandConfig {
230            name: String::from("rc config"),
231            file_path: String::from("sample path"),
232        };
233        let ctrl_mode_config = ControlModesConfig {
234            move_base_mode: Some(String::from("a")),
235            joy_joint_teleop_configs: vec![joy_joint_teleop_config],
236            ik_mode_teleop_configs: vec![ik_mode_teleop_config],
237            joints_pose_sender_config: None,
238            command_configs: vec![robot_command_config],
239        };
240
241        // Check create control mode(Error:No HashMap item[Key=jj config])
242        // HashMap key is required by ControlModesConfig
243        let speaker = Arc::new(DummySpeaker::default());
244        let robot_client = ArcRobotClient::new(
245            OpenrrClientsConfig::default(),
246            HashMap::new(),
247            HashMap::new(),
248            None,
249            None,
250            None,
251        );
252        assert!(robot_client.is_ok());
253        let robot_client = robot_client.unwrap();
254
255        // the 4th argument `joint_trajectory_client_map` had to have keys same with the items in `joy_joint_teleop_configs`
256        let modes = ctrl_mode_config.create_control_modes(
257            None,
258            Arc::new(robot_client),
259            speaker.clone(),
260            &HashMap::new(),
261            &HashMap::new(),
262            Some(Arc::new(DummyMoveBase::default())),
263            Vec::new(),
264        );
265        assert!(modes.is_err());
266        let err = modes.err().unwrap();
267        println!("{err}");
268
269        // Check create control mode(Error:No HashMap item[Key=ik_trajectory])
270        // HashMap key is required by ControlModesConfig
271        let robot_client = ArcRobotClient::new(
272            OpenrrClientsConfig::default(),
273            HashMap::new(),
274            HashMap::new(),
275            None,
276            None,
277            None,
278        );
279        let robot_client = robot_client.unwrap();
280        let joint_trajectory_client: Arc<dyn JointTrajectoryClient> =
281            Arc::new(DummyJointTrajectoryClient::new(vec!["a".to_owned()]));
282        let mut joint_trajectory_map = HashMap::new();
283        joint_trajectory_map.insert("jj config".to_owned(), joint_trajectory_client);
284
285        let chain = k::Chain::<f64>::from_urdf_file("../openrr-planner/sample.urdf").unwrap();
286        let end_link = chain.find("l_tool_fixed").unwrap();
287        let arm = k::SerialChain::from_end(end_link);
288        let params = IkSolverParameters {
289            allowable_position_error: 0.01,
290            allowable_angle_error: 0.02,
291            jacobian_multiplier: 0.1,
292            num_max_try: 100,
293        };
294        let ik_solver = create_random_jacobian_ik_solver(&params);
295        let constraints = k::Constraints::default();
296        let ik_solver = IkSolverWithChain::new(arm, Arc::new(ik_solver), constraints);
297        let mut ik_map = HashMap::new();
298        ik_map.insert("ik config solver".to_owned(), Arc::new(ik_solver));
299
300        // the 5th argument `joint_trajectory_client_map` had to have keys same with the items in `ik_mode_teleop_configs`
301        let modes = ctrl_mode_config.create_control_modes(
302            None,
303            Arc::new(robot_client),
304            speaker,
305            &joint_trajectory_map,
306            &ik_map,
307            Some(Arc::new(DummyMoveBase::default())),
308            Vec::new(),
309        );
310        assert!(modes.is_err());
311        let err = modes.err().unwrap();
312        println!("{err}");
313    }
314
315    #[test]
316    fn test_control_mode_config_create_ok() {
317        let joy_joint_teleop_config = JoyJointTeleopConfig {
318            client_name: String::from("jj config"),
319            config: JoyJointTeleopModeConfig {
320                mode: String::from("joy joint teleop mode config"),
321                joint_step: 1.0_f64,
322                step_duration_secs: 0.1_f64,
323            },
324        };
325        let ik_mode_teleop_config = IkModeTeleopConfig {
326            solver_name: String::from("ik config solver"),
327            joint_trajectory_client_name: String::from("jj config"),
328            config: IkModeConfig {
329                mode: String::from("ik mode config"),
330                move_step_angular: [1.0_f64, 1.0, 1.0],
331                move_step_linear: [1.0_f64, 1.0, 1.0],
332                step_duration_secs: 0.1_f64,
333            },
334        };
335        let robot_command_config = RobotCommandConfig {
336            name: String::from("rc config"),
337            file_path: String::from("sample path"),
338        };
339        let ctrl_mode_config = ControlModesConfig {
340            move_base_mode: Some(String::from("a")),
341            joy_joint_teleop_configs: vec![joy_joint_teleop_config],
342            ik_mode_teleop_configs: vec![ik_mode_teleop_config],
343            joints_pose_sender_config: None,
344            command_configs: vec![robot_command_config],
345        };
346
347        // Check create control mode(Ok)
348        let speaker = Arc::new(DummySpeaker::default());
349        let robot_client = ArcRobotClient::new(
350            OpenrrClientsConfig::default(),
351            HashMap::new(),
352            HashMap::new(),
353            Some(Arc::new(DummyLocalization::default())),
354            Some(Arc::new(DummyMoveBase::default())),
355            Some(Arc::new(DummyNavigation::default())),
356        );
357        assert!(robot_client.is_ok());
358        let robot_client = robot_client.unwrap();
359
360        let joint_trajectory_client: Arc<dyn JointTrajectoryClient> =
361            Arc::new(DummyJointTrajectoryClient::new(vec!["a".to_owned()]));
362        let mut joint_trajectory_map = HashMap::new();
363        joint_trajectory_map.insert("jj config".to_owned(), joint_trajectory_client);
364
365        let chain = k::Chain::<f64>::from_urdf_file("../openrr-planner/sample.urdf").unwrap();
366        let end_link = chain.find("l_tool_fixed").unwrap();
367        let arm = k::SerialChain::from_end(end_link);
368        let params = IkSolverParameters {
369            allowable_position_error: 0.01,
370            allowable_angle_error: 0.02,
371            jacobian_multiplier: 0.1,
372            num_max_try: 100,
373        };
374        let ik_solver = create_random_jacobian_ik_solver(&params);
375        let constraints = k::Constraints::default();
376        let ik_solver = IkSolverWithChain::new(arm, Arc::new(ik_solver), constraints);
377        let mut ik_map = HashMap::new();
378        ik_map.insert("ik config solver".to_owned(), Arc::new(ik_solver));
379
380        let modes = ctrl_mode_config.create_control_modes(
381            None,
382            Arc::new(robot_client),
383            speaker,
384            &joint_trajectory_map,
385            &ik_map,
386            Some(Arc::new(DummyMoveBase::default())),
387            Vec::new(),
388        );
389        assert!(modes.is_ok());
390    }
391}