arci 0.1.0

Abstract Robot Control Interface
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101

use std::time::Duration;

use arci::{
    utils::{get_joint_index, move_joint_until_stop},
    DummyJointTrajectoryClient, Error, JointTrajectoryClient, TrajectoryPoint, WaitFuture,
};
use assert_approx_eq::assert_approx_eq;

struct TestJointTrajectoryClient {
    joint_names: Vec<String>,
}

impl TestJointTrajectoryClient {
    pub fn new() -> Self {
        Self {
            joint_names: vec![String::from("j0"), String::from("j1")],
        }
    }
}

impl JointTrajectoryClient for TestJointTrajectoryClient {
    fn joint_names(&self) -> Vec<String> {
        self.joint_names.clone()
    }

    fn current_joint_positions(&self) -> Result<Vec<f64>, Error> {
        Ok(vec![1.0, 1.0])
    }

    fn send_joint_positions(
        &self,
        _: Vec<f64>,
        _: std::time::Duration,
    ) -> Result<WaitFuture, Error> {
        Ok(WaitFuture::ready())
    }

    fn send_joint_trajectory(&self, _: Vec<TrajectoryPoint>) -> Result<WaitFuture, Error> {
        Ok(WaitFuture::ready())
    }
}

#[test]
fn test_get_joint_index() {
    let client = DummyJointTrajectoryClient::new(vec![String::from("j0"), String::from("j1")]);
    let j0 = get_joint_index(&client, "j0");
    assert!(j0.is_ok());
    assert_eq!(j0.unwrap(), 0);
    let j1 = get_joint_index(&client, "j1");
    assert!(j1.is_ok());
    assert_eq!(j1.unwrap(), 1);
    assert!(get_joint_index(&client, "j2").is_err());
}

#[tokio::test]
async fn test_move_joint_until_stop() {
    let client = DummyJointTrajectoryClient::new(vec![String::from("j0"), String::from("j1")]);

    let stopped_position = move_joint_until_stop(
        &client,
        "j0",
        2.0,
        Duration::from_secs_f64(1.0),
        0.01,
        Duration::from_secs_f64(1.0),
        Duration::from_secs_f64(0.1),
        Duration::from_secs_f64(0.1),
    )
    .await;
    assert!(stopped_position.is_ok());
    assert_approx_eq!(stopped_position.unwrap(), 2.0);

    let stopped_position = move_joint_until_stop(
        &client,
        "j2",
        2.0,
        Duration::from_secs_f64(1.0),
        0.01,
        Duration::from_secs_f64(1.0),
        Duration::from_secs_f64(0.1),
        Duration::from_secs_f64(0.1),
    )
    .await;
    assert!(stopped_position.is_err());

    let client = TestJointTrajectoryClient::new();

    let stopped_position = move_joint_until_stop(
        &client,
        "j0",
        2.0,
        Duration::from_secs_f64(1.0),
        0.01,
        Duration::from_secs_f64(1.0),
        Duration::from_secs_f64(0.1),
        Duration::from_secs_f64(0.1),
    )
    .await;
    assert!(stopped_position.is_ok());
    assert_approx_eq!(stopped_position.unwrap(), 1.0);
}