openrr_client/
utils.rs

Help
1use arci::*;
2
3pub fn wait_joint_positions(
4    client: &dyn JointTrajectoryClient,
5    target_positions: &[f64],
6    timeout: std::time::Duration,
7    allowable_total_diff: f64,
8) -> Result<(), Error> {
9    let sleep_unit = std::time::Duration::from_millis(100);
10    let max_num = timeout.as_micros() / sleep_unit.as_micros();
11    let dof = target_positions.len();
12    let mut sum_err = 0.0;
13    for _iteration in 0..max_num {
14        let cur = client.current_joint_positions()?;
15        sum_err = 0.0;
16        for i in 0..dof {
17            sum_err += (target_positions[i] - cur[i]).abs();
18        }
19        if sum_err < allowable_total_diff {
20            return Ok(());
21        }
22        std::thread::sleep(sleep_unit);
23    }
24    Err(Error::Timeout {
25        timeout,
26        allowable_total_diff,
27        err: sum_err,
28    })
29}
30
31pub fn find_nodes(joint_names: &[String], chain: &k::Chain<f64>) -> Option<Vec<k::Node<f64>>> {
32    let mut nodes = vec![];
33    for name in joint_names {
34        if let Some(node) = chain.find(name) {
35            nodes.push(node.clone());
36        } else {
37            return None;
38        }
39    }
40    Some(nodes)
41}
42
43#[cfg(test)]
44mod test {
45    use k::{Chain, Joint, JointType, Node};
46
47    use super::*;
48
49    #[test]
50    fn test_wait_joint_positions() {
51        let client = DummyJointTrajectoryClient::new(vec![String::from("joint")]);
52        let target = [1f64];
53        let timeout = std::time::Duration::from_millis(1000);
54        let allowable_total_diff = 10.;
55        let impossible_total_diff = 0.01;
56        assert!(wait_joint_positions(&client, &target, timeout, allowable_total_diff).is_ok());
57        assert!(wait_joint_positions(&client, &target, timeout, impossible_total_diff).is_err());
58    }
59
60    #[test]
61    fn test_find_nodes() {
62        let joint_names = [String::from("joint")];
63        let fake_joint_names = [String::from("fake_joint")];
64        let chain: Chain<f64> = Chain::from_nodes(vec![Node::new(Joint::new(
65            "joint",
66            JointType::Linear {
67                axis: Vector3::y_axis(),
68            },
69        ))]);
70
71        assert!(find_nodes(&joint_names, &chain).is_some());
72        assert!(find_nodes(&fake_joint_names, &chain).is_none());
73    }
74}
openrr_client/utils.rs

openrr_client/
utils.rs
