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 #[serde(skip_serializing_if = "Vec::is_empty")]
36 pub joy_joint_teleop_configs: Vec<JoyJointTeleopConfig>,
37 #[serde(default)]
38 #[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 #[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 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 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 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 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 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(¶ms);
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 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 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(¶ms);
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}