1use futures::stream::FuturesOrdered;
2
3use crate::{
4 error::Error,
5 traits::{JointTrajectoryClient, TrajectoryPoint},
6 waits::WaitFuture,
7};
8
9#[derive(Debug)]
10pub struct JointTrajectoryClientsContainer<T: JointTrajectoryClient> {
11 joint_names: Vec<String>,
12 clients: Vec<T>,
13}
14
15impl<T> JointTrajectoryClientsContainer<T>
16where
17 T: JointTrajectoryClient,
18{
19 pub fn new(clients: Vec<T>) -> Self {
20 let mut joint_names = vec![];
21 for c in &clients {
22 joint_names.append(&mut c.joint_names().to_vec());
23 }
24 Self {
25 joint_names,
26 clients,
27 }
28 }
29}
30
31impl<T> JointTrajectoryClient for JointTrajectoryClientsContainer<T>
32where
33 T: JointTrajectoryClient + Sync,
34{
35 fn joint_names(&self) -> Vec<String> {
36 self.joint_names.clone()
37 }
38
39 fn current_joint_positions(&self) -> Result<Vec<f64>, Error> {
40 let mut ret = vec![];
41 for c in &self.clients {
42 let mut positions = c.current_joint_positions()?;
43 ret.append(&mut positions);
44 }
45 Ok(ret)
46 }
47
48 fn send_joint_positions(
49 &self,
50 positions: Vec<f64>,
51 duration: std::time::Duration,
52 ) -> Result<WaitFuture, Error> {
53 let mut offset = 0;
54 let mut waits = FuturesOrdered::new();
55 for c in &self.clients {
56 let mut current_positions = c.current_joint_positions()?;
57 for i in 0..current_positions.len() {
58 if positions.len() > (offset + i) {
59 current_positions[i] = positions[offset + i];
60 }
61 }
62 offset += current_positions.len();
63 waits.push_back(c.send_joint_positions(current_positions, duration)?);
64 }
65 Ok(WaitFuture::from_stream(waits))
66 }
67
68 fn send_joint_trajectory(
69 &self,
70 full_trajectory: Vec<TrajectoryPoint>,
71 ) -> Result<WaitFuture, Error> {
72 let mut offset = 0;
73 let full_dof = self.joint_names().len();
74 let mut waits = FuturesOrdered::new();
75 for client in &self.clients {
76 let mut current_positions = client.current_joint_positions()?;
77 let partial_dof = current_positions.len();
78 let mut partial_trajectory: Vec<TrajectoryPoint> = vec![];
79 for full_point in &full_trajectory {
80 for (i, current) in current_positions.iter_mut().enumerate().take(partial_dof) {
81 if full_dof > (offset + i) {
82 *current = full_point.positions[offset + i];
83 }
84 }
85 let partial_velocities = if let Some(full_velocities) = &full_point.velocities {
86 let mut partial_velocities = vec![0.0; partial_dof];
87 for (i, partial_velocity) in
88 partial_velocities.iter_mut().enumerate().take(partial_dof)
89 {
90 if full_dof > (offset + i) {
91 *partial_velocity = full_velocities[offset + i];
92 }
93 }
94 Some(partial_velocities)
95 } else {
96 None
97 };
98 partial_trajectory.push(TrajectoryPoint {
99 positions: current_positions.clone(),
100 velocities: partial_velocities,
101 time_from_start: full_point.time_from_start,
102 });
103 }
104 waits.push_back(client.send_joint_trajectory(partial_trajectory)?);
105 offset += partial_dof;
106 }
107 Ok(WaitFuture::from_stream(waits))
108 }
109}
110
111#[cfg(test)]
112mod tests {
113 use std::sync::{Arc, Mutex};
114
115 use super::*;
116 #[derive(Debug, Clone)]
117 struct DummyFull {
118 name: Vec<String>,
119 pos: Arc<Mutex<Vec<f64>>>,
120 last_trajectory: Arc<Mutex<Vec<TrajectoryPoint>>>,
121 }
122 impl JointTrajectoryClient for DummyFull {
123 fn joint_names(&self) -> Vec<String> {
124 self.name.clone()
125 }
126
127 fn current_joint_positions(&self) -> Result<Vec<f64>, Error> {
128 Ok(self.pos.lock().unwrap().clone())
129 }
130
131 fn send_joint_positions(
132 &self,
133 positions: Vec<f64>,
134 _duration: std::time::Duration,
135 ) -> Result<WaitFuture, Error> {
136 *self.pos.lock().unwrap() = positions;
137 Ok(WaitFuture::ready())
138 }
139
140 fn send_joint_trajectory(
141 &self,
142 full_trajectory: Vec<TrajectoryPoint>,
143 ) -> Result<WaitFuture, Error> {
144 if let Some(last_point) = full_trajectory.last() {
145 last_point
146 .positions
147 .clone_into(&mut self.pos.lock().unwrap());
148 }
149 *self.last_trajectory.lock().unwrap() = full_trajectory;
150 Ok(WaitFuture::ready())
151 }
152 }
153
154 #[test]
155 fn test_container_new() {
156 #[derive(Debug, Clone)]
157 struct Dummy {
158 name: Vec<String>,
159 }
160 impl JointTrajectoryClient for Dummy {
161 fn joint_names(&self) -> Vec<String> {
162 self.name.clone()
163 }
164
165 fn current_joint_positions(&self) -> Result<Vec<f64>, Error> {
166 unimplemented!();
167 }
168
169 fn send_joint_positions(
170 &self,
171 _positions: Vec<f64>,
172 _duration: std::time::Duration,
173 ) -> Result<WaitFuture, Error> {
174 unimplemented!();
175 }
176
177 fn send_joint_trajectory(
178 &self,
179 _trajectory: Vec<TrajectoryPoint>,
180 ) -> Result<WaitFuture, Error> {
181 unimplemented!()
182 }
183 }
184
185 let clients = vec![
186 Dummy {
187 name: vec![String::from("part1"), String::from("high")],
188 },
189 Dummy {
190 name: vec![String::from("part2"), String::from("low")],
191 },
192 ];
193
194 let container = JointTrajectoryClientsContainer::new(clients.clone());
195
196 assert_eq!(
197 format!("{:?}", container.joint_names),
198 "[\"part1\", \"high\", \"part2\", \"low\"]"
199 );
200 assert_eq!(
201 format!("{:?}", clients[0]),
202 format!("{:?}", container.clients[0])
203 );
204 assert_eq!(
205 format!("{:?}", clients[1]),
206 format!("{:?}", container.clients[1])
207 );
208 }
209
210 #[test]
211 fn test_container_joint_name() {
212 struct Dummy {
213 name: Vec<String>,
214 }
215 impl JointTrajectoryClient for Dummy {
216 fn joint_names(&self) -> Vec<String> {
217 self.name.clone()
218 }
219
220 fn current_joint_positions(&self) -> Result<Vec<f64>, Error> {
221 unimplemented!();
222 }
223
224 fn send_joint_positions(
225 &self,
226 _positions: Vec<f64>,
227 _duration: std::time::Duration,
228 ) -> Result<WaitFuture, Error> {
229 unimplemented!();
230 }
231
232 fn send_joint_trajectory(
233 &self,
234 _trajectory: Vec<TrajectoryPoint>,
235 ) -> Result<WaitFuture, Error> {
236 unimplemented!()
237 }
238 }
239
240 let clients = vec![
241 Dummy {
242 name: vec![String::from("part1"), String::from("high")],
243 },
244 Dummy {
245 name: vec![String::from("part2"), String::from("low")],
246 },
247 Dummy {
248 name: vec![String::from("part3"), String::from("high")],
249 },
250 Dummy {
251 name: vec![String::from("part4"), String::from("middle")],
252 },
253 ];
254
255 let container = JointTrajectoryClientsContainer::new(clients);
256
257 assert_eq!(
258 format!("{:?}", container.joint_names()),
259 "[\"part1\", \"high\", \"part2\", \"low\", \"part3\", \"high\", \"part4\", \"middle\"]"
260 );
261 }
262
263 #[test]
264 fn test_container_current_pos_ok() {
265 use assert_approx_eq::assert_approx_eq;
266 let clients = vec![
267 DummyFull {
268 name: vec![String::from("part1"), String::from("high")],
269 pos: Arc::new(Mutex::new(vec![1.0_f64, 3.0_f64])),
270 last_trajectory: Arc::new(Mutex::new(Vec::new())),
271 },
272 DummyFull {
273 name: vec![String::from("part2"), String::from("low")],
274 pos: Arc::new(Mutex::new(vec![2.2_f64, 4.0_f64])),
275 last_trajectory: Arc::new(Mutex::new(Vec::new())),
276 },
277 ];
278
279 let container = JointTrajectoryClientsContainer::new(clients);
280 let current = container.current_joint_positions();
281 let expect = vec![1.0_f64, 3.0_f64, 2.2_f64, 4.0_f64];
282
283 let positions = current.unwrap();
284
285 positions
286 .iter()
287 .zip(expect)
288 .for_each(|(pos, correct)| assert_approx_eq!(*pos, correct));
289 }
290
291 #[test]
292 fn test_container_current_pos_err() {
293 #[derive(Debug, Clone)]
294 struct Dummy {
295 name: Vec<String>,
296 }
297 impl JointTrajectoryClient for Dummy {
298 fn joint_names(&self) -> Vec<String> {
299 self.name.clone()
300 }
301
302 fn current_joint_positions(&self) -> Result<Vec<f64>, Error> {
303 Err(Error::Uninitialized {
304 message: String::from("current pos is not exist."),
305 })
306 }
307
308 fn send_joint_positions(
309 &self,
310 _positions: Vec<f64>,
311 _duration: std::time::Duration,
312 ) -> Result<WaitFuture, Error> {
313 unimplemented!();
314 }
315
316 fn send_joint_trajectory(
317 &self,
318 _trajectory: Vec<TrajectoryPoint>,
319 ) -> Result<WaitFuture, Error> {
320 unimplemented!()
321 }
322 }
323 let clients = vec![
324 Dummy {
325 name: vec![String::from("part1"), String::from("high")],
326 },
327 Dummy {
328 name: vec![String::from("part2"), String::from("low")],
329 },
330 ];
331
332 let container = JointTrajectoryClientsContainer::new(clients);
333 let current = container.current_joint_positions();
334
335 assert!(current.is_err());
336 }
337
338 #[tokio::test]
339 async fn test_container_send_pos_ok() {
340 use assert_approx_eq::assert_approx_eq;
341 let clients = vec![
342 DummyFull {
343 name: vec![String::from("part1"), String::from("high")],
344 pos: Arc::new(Mutex::new(vec![1.0_f64, 3.0_f64])),
345 last_trajectory: Arc::new(Mutex::new(Vec::new())),
346 },
347 DummyFull {
348 name: vec![String::from("part2"), String::from("low")],
349 pos: Arc::new(Mutex::new(vec![2.2_f64, 4.0_f64])),
350 last_trajectory: Arc::new(Mutex::new(Vec::new())),
351 },
352 ];
353
354 let container = JointTrajectoryClientsContainer::new(clients);
355 let correct = vec![3.4_f64, 5.8_f64, 0.1_f64, 2.5_f64];
356 let duration = std::time::Duration::from_secs(5);
357
358 let result = container.send_joint_positions(correct.clone(), duration);
359 assert!(result.is_ok());
360 assert!(result.unwrap().await.is_ok());
361
362 let positions = container.current_joint_positions().unwrap();
363 positions
364 .iter()
365 .zip(correct)
366 .for_each(|(pos, correct)| assert_approx_eq!(*pos, correct));
367 }
368
369 #[test]
370 fn test_container_send_pos_err() {
371 #[derive(Debug, Clone)]
372 struct Dummy {
373 name: Vec<String>,
374 pos: Vec<f64>,
375 }
376 impl JointTrajectoryClient for Dummy {
377 fn joint_names(&self) -> Vec<String> {
378 self.name.clone()
379 }
380
381 fn current_joint_positions(&self) -> Result<Vec<f64>, Error> {
382 Ok(self.pos.clone())
383 }
384
385 fn send_joint_positions(
386 &self,
387 _positions: Vec<f64>,
388 _duration: std::time::Duration,
389 ) -> Result<WaitFuture, Error> {
390 Err(Error::Uninitialized {
391 message: String::from("error pattern."),
392 })
393 }
394
395 fn send_joint_trajectory(
396 &self,
397 _trajectory: Vec<TrajectoryPoint>,
398 ) -> Result<WaitFuture, Error> {
399 unimplemented!()
400 }
401 }
402 let clients = vec![
403 Dummy {
404 name: vec![String::from("part1"), String::from("high")],
405 pos: vec![1.0_f64, 3.0_f64],
406 },
407 Dummy {
408 name: vec![String::from("part2"), String::from("low")],
409 pos: vec![2.2_f64, 4.0_f64],
410 },
411 ];
412
413 let container = JointTrajectoryClientsContainer::new(clients);
414 let pos = vec![3.4_f64, 5.8_f64, 0.1_f64, 2.5_f64];
415 let duration = std::time::Duration::from_secs(5);
416
417 assert!(container.send_joint_positions(pos, duration).is_err());
418 }
419
420 #[tokio::test]
421 async fn test_trajectory_ok() {
422 use assert_approx_eq::assert_approx_eq;
423 let clients = vec![
424 DummyFull {
425 name: vec![String::from("part1"), String::from("high")],
426 pos: Arc::new(Mutex::new(vec![1.0_f64, 3.0_f64])),
427 last_trajectory: Arc::new(Mutex::new(Vec::new())),
428 },
429 DummyFull {
430 name: vec![String::from("part2"), String::from("low")],
431 pos: Arc::new(Mutex::new(vec![2.2_f64, 4.0_f64])),
432 last_trajectory: Arc::new(Mutex::new(Vec::new())),
433 },
434 ];
435
436 let container = JointTrajectoryClientsContainer::new(clients);
437 let correct = vec![3.4_f64, 5.8_f64, 0.1_f64, 2.5_f64];
438 let trajectories = vec![
439 TrajectoryPoint::new(
440 vec![1.0_f64, 3.0_f64, 2.2_f64, 4.0_f64],
441 std::time::Duration::from_secs(1),
442 ),
443 TrajectoryPoint::new(
444 vec![3.4_f64, 5.8_f64, 0.1_f64, 2.5_f64],
445 std::time::Duration::from_secs(2),
446 ),
447 ];
448
449 let result = container.send_joint_trajectory(trajectories);
450 assert!(result.is_ok());
451 assert!(result.unwrap().await.is_ok());
452
453 let positions = container.current_joint_positions().unwrap();
454 println!("{positions:?}");
455 positions
456 .iter()
457 .zip(correct)
458 .for_each(|(pos, correct)| assert_approx_eq!(*pos, correct));
459 }
460
461 #[test]
462 fn test_trajectory_err() {
463 #[derive(Debug, Clone)]
464 struct Dummy {
465 name: Vec<String>,
466 pos: Vec<f64>,
467 }
468 impl JointTrajectoryClient for Dummy {
469 fn joint_names(&self) -> Vec<String> {
470 self.name.clone()
471 }
472
473 fn current_joint_positions(&self) -> Result<Vec<f64>, Error> {
474 Ok(self.pos.clone())
475 }
476
477 fn send_joint_positions(
478 &self,
479 _positions: Vec<f64>,
480 _duration: std::time::Duration,
481 ) -> Result<WaitFuture, Error> {
482 Ok(WaitFuture::ready())
483 }
484
485 fn send_joint_trajectory(
486 &self,
487 _trajectory: Vec<TrajectoryPoint>,
488 ) -> Result<WaitFuture, Error> {
489 Err(Error::Uninitialized {
490 message: String::from("error pattern."),
491 })
492 }
493 }
494 let clients = vec![
495 Dummy {
496 name: vec![String::from("part1"), String::from("high")],
497 pos: vec![1.0_f64, 3.0_f64],
498 },
499 Dummy {
500 name: vec![String::from("part2"), String::from("low")],
501 pos: vec![2.2_f64, 4.0_f64],
502 },
503 ];
504
505 let container = JointTrajectoryClientsContainer::new(clients);
506 let trajectories = vec![
507 TrajectoryPoint::new(vec![3.4_f64, 5.8_f64], std::time::Duration::from_secs(1)),
508 TrajectoryPoint::new(vec![0.1_f64, 2.5_f64], std::time::Duration::from_secs(2)),
509 ];
510
511 assert!(container.send_joint_trajectory(trajectories).is_err());
512 }
513}