arci-ros 0.0.5

arci implementation using ROS1
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
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131

use crate::msg;
use arci::*;
use async_trait::async_trait;
use std::sync::{Arc, Mutex};

pub struct RosRobotClient {
    joint_names: Vec<String>,
    trajectory_publisher: Option<rosrust::Publisher<msg::trajectory_msgs::JointTrajectory>>,
    _joint_state_subscriber: rosrust::Subscriber,
    joint_state_message: Arc<Mutex<msg::sensor_msgs::JointState>>,
    complete_condition: Box<dyn CompleteCondition>,
}

impl From<TrajectoryPoint> for msg::trajectory_msgs::JointTrajectoryPoint {
    fn from(tp: TrajectoryPoint) -> Self {
        let mut message = msg::trajectory_msgs::JointTrajectoryPoint {
            positions: tp.positions,
            ..Default::default()
        };
        message.time_from_start.sec = tp.time_from_start.as_secs() as i32;
        message.time_from_start.nsec = tp.time_from_start.subsec_nanos() as i32;
        message
    }
}

impl RosRobotClient {
    pub fn new(
        joint_names: Vec<String>,
        joint_state_topic_name: &str,
        trajectory_topic_name: &str,
    ) -> Self {
        let joint_state_message = Arc::new(Mutex::new(msg::sensor_msgs::JointState::default()));
        let joint_state_message_for_sub = joint_state_message.clone();
        let _joint_state_subscriber = rosrust::subscribe(
            joint_state_topic_name,
            1,
            move |joint_state: msg::sensor_msgs::JointState| {
                let mut aaa = joint_state_message_for_sub.lock().unwrap();
                *aaa = joint_state;
            },
        )
        .unwrap();
        while joint_state_message.lock().unwrap().name.is_empty() {
            rosrust::ros_info!("waiting joint state publisher");
            std::thread::sleep(std::time::Duration::from_millis(100));
        }

        let trajectory_publisher = if trajectory_topic_name.is_empty() {
            None
        } else {
            let publisher = rosrust::publish(trajectory_topic_name, 1).unwrap();

            let rate = rosrust::rate(10.0);
            while rosrust::is_ok() && publisher.subscriber_count() == 0 {
                rosrust::ros_info!("waiting trajectory subscriber");
                rate.sleep();
            }
            Some(publisher)
        };

        Self {
            joint_names,
            trajectory_publisher,
            _joint_state_subscriber,
            joint_state_message,
            complete_condition: Box::new(TotalJointDiffCondition::default()),
        }
    }
}

#[async_trait]
impl JointTrajectoryClient for RosRobotClient {
    fn joint_names(&self) -> &[String] {
        &self.joint_names
    }
    fn current_joint_positions(&self) -> Result<Vec<f64>, Error> {
        let message = self.joint_state_message.lock().unwrap();
        Ok(message.position.clone())
    }

    async fn send_joint_positions(
        &self,
        positions: Vec<f64>,
        duration: std::time::Duration,
    ) -> Result<(), Error> {
        if let Some(ref publisher) = self.trajectory_publisher {
            if self.joint_names.len() != positions.len() {
                return Err(arci::Error::LengthMismatch {
                    model: self.joint_names.len(),
                    input: positions.len(),
                });
            }
            let point = msg::trajectory_msgs::JointTrajectoryPoint {
                positions: positions.to_vec(),
                time_from_start: rosrust::Duration::from_nanos(duration.as_nanos() as i64),
                ..Default::default()
            };
            let traj = msg::trajectory_msgs::JointTrajectory {
                joint_names: self.joint_names.clone(),
                points: vec![point],
                ..Default::default()
            };
            publisher.send(traj).unwrap();
            self.complete_condition
                .wait(self, &positions, duration.as_secs_f64())?;
        }
        Ok(())
    }
    async fn send_joint_trajectory(&self, trajectory: Vec<TrajectoryPoint>) -> Result<(), Error> {
        if let Some(ref publisher) = self.trajectory_publisher {
            let traj = msg::trajectory_msgs::JointTrajectory {
                joint_names: self.joint_names.clone(),
                points: trajectory.iter().map(|t| (*t).clone().into()).collect(),
                ..Default::default()
            };
            publisher.send(traj).unwrap();
            self.complete_condition.wait(
                self,
                &trajectory.last().unwrap().positions,
                trajectory.last().unwrap().time_from_start.as_secs_f64(),
            )?;
        }
        Ok(())
    }
}

impl SetCompleteCondition for RosRobotClient {
    fn set_complete_condition(&mut self, condition: Box<dyn CompleteCondition>) {
        self.complete_condition = condition;
    }
}