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
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151

use std::sync::{Arc, Mutex};

use arci::*;

use crate::msg;

#[derive(Clone)]
pub struct RosRobotClient(Arc<RosRobotClientInner>);

struct RosRobotClientInner {
    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: Mutex<Arc<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(Arc::new(RosRobotClientInner {
            joint_names,
            trajectory_publisher,
            _joint_state_subscriber,
            joint_state_message,
            complete_condition: Mutex::new(Arc::new(TotalJointDiffCondition::default())),
        }))
    }
}

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

    fn current_joint_positions(&self) -> Result<Vec<f64>, Error> {
        let message = self.0.joint_state_message.lock().unwrap();
        Ok(message.position.clone())
    }

    fn send_joint_positions(
        &self,
        positions: Vec<f64>,
        duration: std::time::Duration,
    ) -> Result<WaitFuture, Error> {
        if let Some(ref publisher) = self.0.trajectory_publisher {
            if self.0.joint_names.len() != positions.len() {
                return Err(arci::Error::LengthMismatch {
                    model: self.0.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.0.joint_names.clone(),
                points: vec![point],
                ..Default::default()
            };
            publisher.send(traj).unwrap();
            let this = self.clone();
            Ok(WaitFuture::new(async move {
                // Clone to avoid holding the lock for a long time.
                let complete_condition = this.0.complete_condition.lock().unwrap().clone();
                complete_condition
                    .wait(&this, &positions, duration.as_secs_f64())
                    .await
            }))
        } else {
            Ok(WaitFuture::ready())
        }
    }

    fn send_joint_trajectory(&self, trajectory: Vec<TrajectoryPoint>) -> Result<WaitFuture, Error> {
        if let Some(ref publisher) = self.0.trajectory_publisher {
            let traj = msg::trajectory_msgs::JointTrajectory {
                joint_names: self.0.joint_names.clone(),
                points: trajectory.iter().map(|t| (*t).clone().into()).collect(),
                ..Default::default()
            };
            publisher.send(traj).unwrap();
            let this = self.clone();
            Ok(WaitFuture::new(async move {
                // Clone to avoid holding the lock for a long time.
                let complete_condition = this.0.complete_condition.lock().unwrap().clone();
                complete_condition
                    .wait(
                        &this,
                        &trajectory.last().unwrap().positions,
                        trajectory.last().unwrap().time_from_start.as_secs_f64(),
                    )
                    .await
            }))
        } else {
            Ok(WaitFuture::ready())
        }
    }
}

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