arci_ros/
ros_robot_client.rs

Help
1use std::sync::{Arc, Mutex};
2
3use arci::*;
4
5use crate::msg;
6
7#[derive(Clone)]
8pub struct RosRobotClient(Arc<RosRobotClientInner>);
9
10struct RosRobotClientInner {
11    joint_names: Vec<String>,
12    trajectory_publisher: Option<rosrust::Publisher<msg::trajectory_msgs::JointTrajectory>>,
13    _joint_state_subscriber: rosrust::Subscriber,
14    joint_state_message: Arc<Mutex<msg::sensor_msgs::JointState>>,
15    complete_condition: Mutex<Arc<dyn CompleteCondition>>,
16}
17
18impl From<TrajectoryPoint> for msg::trajectory_msgs::JointTrajectoryPoint {
19    fn from(tp: TrajectoryPoint) -> Self {
20        let mut message = msg::trajectory_msgs::JointTrajectoryPoint {
21            positions: tp.positions,
22            ..Default::default()
23        };
24        message.time_from_start.sec = tp.time_from_start.as_secs() as i32;
25        message.time_from_start.nsec = tp.time_from_start.subsec_nanos() as i32;
26        message
27    }
28}
29
30impl RosRobotClient {
31    pub fn new(
32        joint_names: Vec<String>,
33        joint_state_topic_name: &str,
34        trajectory_topic_name: &str,
35    ) -> Self {
36        let joint_state_message = Arc::new(Mutex::new(msg::sensor_msgs::JointState::default()));
37        let joint_state_message_for_sub = joint_state_message.clone();
38        let _joint_state_subscriber = rosrust::subscribe(
39            joint_state_topic_name,
40            1,
41            move |joint_state: msg::sensor_msgs::JointState| {
42                let mut aaa = joint_state_message_for_sub.lock().unwrap();
43                *aaa = joint_state;
44            },
45        )
46        .unwrap();
47        while joint_state_message.lock().unwrap().name.is_empty() {
48            rosrust::ros_info!("waiting joint state publisher");
49            std::thread::sleep(std::time::Duration::from_millis(100));
50        }
51
52        let trajectory_publisher = if trajectory_topic_name.is_empty() {
53            None
54        } else {
55            let publisher = rosrust::publish(trajectory_topic_name, 1).unwrap();
56
57            let rate = rosrust::rate(10.0);
58            while rosrust::is_ok() && publisher.subscriber_count() == 0 {
59                rosrust::ros_info!("waiting trajectory subscriber");
60                rate.sleep();
61            }
62            Some(publisher)
63        };
64
65        Self(Arc::new(RosRobotClientInner {
66            joint_names,
67            trajectory_publisher,
68            _joint_state_subscriber,
69            joint_state_message,
70            complete_condition: Mutex::new(Arc::new(TotalJointDiffCondition::default())),
71        }))
72    }
73}
74
75impl JointTrajectoryClient for RosRobotClient {
76    fn joint_names(&self) -> Vec<String> {
77        self.0.joint_names.clone()
78    }
79
80    fn current_joint_positions(&self) -> Result<Vec<f64>, Error> {
81        let message = self.0.joint_state_message.lock().unwrap();
82        Ok(message.position.clone())
83    }
84
85    fn send_joint_positions(
86        &self,
87        positions: Vec<f64>,
88        duration: std::time::Duration,
89    ) -> Result<WaitFuture, Error> {
90        if let Some(ref publisher) = self.0.trajectory_publisher {
91            if self.0.joint_names.len() != positions.len() {
92                return Err(arci::Error::LengthMismatch {
93                    model: self.0.joint_names.len(),
94                    input: positions.len(),
95                });
96            }
97            let point = msg::trajectory_msgs::JointTrajectoryPoint {
98                positions: positions.to_vec(),
99                time_from_start: rosrust::Duration::from_nanos(duration.as_nanos() as i64),
100                ..Default::default()
101            };
102            let traj = msg::trajectory_msgs::JointTrajectory {
103                joint_names: self.0.joint_names.clone(),
104                points: vec![point],
105                ..Default::default()
106            };
107            publisher.send(traj).unwrap();
108            let this = self.clone();
109            Ok(WaitFuture::new(async move {
110                // Clone to avoid holding the lock for a long time.
111                let complete_condition = this.0.complete_condition.lock().unwrap().clone();
112                complete_condition
113                    .wait(&this, &positions, duration.as_secs_f64())
114                    .await
115            }))
116        } else {
117            Ok(WaitFuture::ready())
118        }
119    }
120
121    fn send_joint_trajectory(&self, trajectory: Vec<TrajectoryPoint>) -> Result<WaitFuture, Error> {
122        if let Some(ref publisher) = self.0.trajectory_publisher {
123            let traj = msg::trajectory_msgs::JointTrajectory {
124                joint_names: self.0.joint_names.clone(),
125                points: trajectory.iter().map(|t| (*t).clone().into()).collect(),
126                ..Default::default()
127            };
128            publisher.send(traj).unwrap();
129            let this = self.clone();
130            Ok(WaitFuture::new(async move {
131                // Clone to avoid holding the lock for a long time.
132                let complete_condition = this.0.complete_condition.lock().unwrap().clone();
133                complete_condition
134                    .wait(
135                        &this,
136                        &trajectory.last().unwrap().positions,
137                        trajectory.last().unwrap().time_from_start.as_secs_f64(),
138                    )
139                    .await
140            }))
141        } else {
142            Ok(WaitFuture::ready())
143        }
144    }
145}
146
147impl SetCompleteCondition for RosRobotClient {
148    fn set_complete_condition(&mut self, condition: Box<dyn CompleteCondition>) {
149        *self.0.complete_condition.lock().unwrap() = condition.into();
150    }
151}
arci_ros/ros_robot_client.rs

arci_ros/
ros_robot_client.rs
