arci/clients/
dummy_trajectory_client.rs

Help
1use std::sync::{Arc, Mutex};
2
3use crate::{
4    error::Error,
5    traits::{JointTrajectoryClient, TrajectoryPoint},
6    waits::WaitFuture,
7};
8
9/// Dummy JointTrajectoryClient for debug or tests.
10#[derive(Debug)]
11pub struct DummyJointTrajectoryClient {
12    pub joint_names: Vec<String>,
13    pub positions: Arc<Mutex<Vec<f64>>>,
14    pub last_trajectory: Arc<Mutex<Vec<TrajectoryPoint>>>,
15}
16
17impl DummyJointTrajectoryClient {
18    /// Creates a new `DummyJointTrajectoryClient` with the given joint names.
19    pub fn new(joint_names: Vec<String>) -> Self {
20        let dof = joint_names.len();
21        let positions = Arc::new(Mutex::new(vec![0.0; dof]));
22        Self {
23            joint_names,
24            positions,
25            last_trajectory: Arc::new(Mutex::new(Vec::new())),
26        }
27    }
28}
29
30impl JointTrajectoryClient for DummyJointTrajectoryClient {
31    fn joint_names(&self) -> Vec<String> {
32        self.joint_names.clone()
33    }
34
35    fn current_joint_positions(&self) -> Result<Vec<f64>, Error> {
36        Ok(self.positions.lock().unwrap().clone())
37    }
38
39    fn send_joint_positions(
40        &self,
41        positions: Vec<f64>,
42        _duration: std::time::Duration,
43    ) -> Result<WaitFuture, Error> {
44        *self.positions.lock().unwrap() = positions;
45        Ok(WaitFuture::ready())
46    }
47
48    fn send_joint_trajectory(
49        &self,
50        full_trajectory: Vec<TrajectoryPoint>,
51    ) -> Result<WaitFuture, Error> {
52        if let Some(last_point) = full_trajectory.last() {
53            last_point
54                .positions
55                .clone_into(&mut self.positions.lock().unwrap());
56        }
57        *self.last_trajectory.lock().unwrap() = full_trajectory;
58        Ok(WaitFuture::ready())
59    }
60}
61
62#[cfg(test)]
63mod tests {
64    use assert_approx_eq::assert_approx_eq;
65
66    use super::*;
67
68    #[tokio::test]
69    async fn send_and_get() {
70        let client = DummyJointTrajectoryClient::new(vec!["a".to_owned(), "b".to_owned()]);
71        assert_eq!(client.joint_names().len(), 2);
72        assert_eq!(client.joint_names()[0], "a");
73        assert_eq!(client.joint_names()[1], "b");
74        let pos = client.current_joint_positions().unwrap();
75        assert_eq!(pos.len(), 2);
76        assert_approx_eq!(pos[0], 0.0);
77        assert_approx_eq!(pos[1], 0.0);
78        assert!(client
79            .send_joint_positions(vec![1.0, 2.0], std::time::Duration::from_secs(1))
80            .unwrap()
81            .await
82            .is_ok());
83        let pos2 = client.current_joint_positions().unwrap();
84        assert_eq!(pos2.len(), 2);
85        assert_approx_eq!(pos2[0], 1.0);
86        assert_approx_eq!(pos2[1], 2.0);
87    }
88
89    #[tokio::test]
90    async fn trajectory() {
91        let client = DummyJointTrajectoryClient::new(vec!["aa".to_owned(), "bb".to_owned()]);
92        assert_eq!(client.last_trajectory.lock().unwrap().len(), 0);
93        let result = client
94            .send_joint_trajectory(vec![
95                TrajectoryPoint::new(vec![1.0, -1.0], std::time::Duration::from_secs(1)),
96                TrajectoryPoint::new(vec![2.0, -3.0], std::time::Duration::from_secs(2)),
97            ])
98            .unwrap();
99        assert!(result.await.is_ok());
100        assert_eq!(client.last_trajectory.lock().unwrap().len(), 2);
101
102        let pos = client.current_joint_positions().unwrap();
103        assert_eq!(pos.len(), 2);
104        assert_approx_eq!(pos[0], 2.0);
105        assert_approx_eq!(pos[1], -3.0);
106    }
107}
arci/clients/dummy_trajectory_client.rs

arci/clients/
dummy_trajectory_client.rs
