arci/clients/
joint_trajectory_clients_container.rs

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}