arci_ros/ros_control/
ros_control_client.rs

Help
1use std::{
2    sync::{Arc, LazyLock, Mutex},
3    time::Duration,
4};
5
6use arci::{
7    CompleteCondition, JointTrajectoryClient, SetCompleteCondition, TotalJointDiffCondition,
8    TrajectoryPoint, WaitFuture,
9};
10use msg::trajectory_msgs::JointTrajectory;
11
12use crate::{
13    create_joint_trajectory_message_for_send_joint_positions,
14    create_joint_trajectory_message_for_send_joint_trajectory,
15    extract_current_joint_positions_from_state, msg, JointStateProvider,
16    JointStateProviderFromJointTrajectoryControllerState, LazyJointStateProvider,
17    SubscriberHandler,
18};
19
20#[derive(Clone)]
21pub struct RosControlClient(Arc<RosControlClientInner>);
22
23struct RosControlClientInner {
24    joint_names: Vec<String>,
25    send_partial_joints_goal: bool,
26    joint_state_provider: Arc<LazyJointStateProvider>,
27    complete_condition: Mutex<Arc<dyn CompleteCondition>>,
28    trajectory_publisher: rosrust::Publisher<JointTrajectory>,
29}
30
31impl RosControlClient {
32    pub fn new_with_joint_state_provider(
33        joint_names: Vec<String>,
34        controller_name: &str,
35        send_partial_joints_goal: bool,
36        joint_state_provider: Arc<LazyJointStateProvider>,
37    ) -> Self {
38        let (state_joint_names, _) = joint_state_provider.get_joint_state().unwrap();
39        for joint_name in &joint_names {
40            if !state_joint_names.iter().any(|name| **name == *joint_name) {
41                panic!(
42                    "Invalid configuration : Joint ({joint_name}) is not found in state ({state_joint_names:?})",
43                );
44            }
45        }
46
47        let trajectory_publisher =
48            rosrust::publish(&format!("{controller_name}/command"), 1).unwrap();
49
50        let rate = rosrust::rate(10.0);
51        while rosrust::is_ok() && trajectory_publisher.subscriber_count() == 0 {
52            rosrust::ros_info!("waiting trajectory subscriber");
53            rate.sleep();
54        }
55
56        Self(Arc::new(RosControlClientInner {
57            joint_names,
58            trajectory_publisher,
59            send_partial_joints_goal,
60            joint_state_provider,
61            complete_condition: Mutex::new(Arc::new(TotalJointDiffCondition::default())),
62        }))
63    }
64
65    pub fn new(
66        joint_names: Vec<String>,
67        controller_name: &str,
68        send_partial_joints_goal: bool,
69    ) -> Self {
70        let joint_state_topic_name = Self::state_topic_name(controller_name);
71        Self::new_with_state_topic_name(
72            joint_names,
73            controller_name,
74            &joint_state_topic_name,
75            send_partial_joints_goal,
76        )
77    }
78
79    pub(crate) fn state_topic_name(controller_name: &str) -> String {
80        format!("{controller_name}/state")
81    }
82
83    pub(crate) fn create_joint_state_provider(
84        joint_state_topic_name: impl Into<String>,
85    ) -> Arc<LazyJointStateProvider> {
86        let joint_state_topic_name = joint_state_topic_name.into();
87        Arc::new(LazyLock::new(Box::new(move || {
88            Box::new(JointStateProviderFromJointTrajectoryControllerState::new(
89                SubscriberHandler::new(&joint_state_topic_name, 1),
90            ))
91        })))
92    }
93
94    pub fn new_with_state_topic_name(
95        joint_names: Vec<String>,
96        controller_name: &str,
97        joint_state_topic_name: &str,
98        send_partial_joints_goal: bool,
99    ) -> Self {
100        let joint_state_provider = Self::create_joint_state_provider(joint_state_topic_name);
101        Self::new_with_joint_state_provider(
102            joint_names,
103            controller_name,
104            send_partial_joints_goal,
105            joint_state_provider,
106        )
107    }
108}
109
110impl JointStateProvider for RosControlClient {
111    fn get_joint_state(&self) -> Result<(Vec<String>, Vec<f64>), arci::Error> {
112        self.0.joint_state_provider.get_joint_state()
113    }
114}
115
116impl JointTrajectoryClient for RosControlClient {
117    fn joint_names(&self) -> Vec<String> {
118        self.0.joint_names.clone()
119    }
120
121    fn current_joint_positions(&self) -> Result<Vec<f64>, arci::Error> {
122        extract_current_joint_positions_from_state(self, self)
123    }
124
125    fn send_joint_positions(
126        &self,
127        positions: Vec<f64>,
128        duration: Duration,
129    ) -> Result<WaitFuture, arci::Error> {
130        let traj = create_joint_trajectory_message_for_send_joint_positions(
131            self,
132            self,
133            &positions,
134            duration,
135            self.0.send_partial_joints_goal,
136        )?;
137        self.0.trajectory_publisher.send(traj).unwrap();
138        let this = self.clone();
139        Ok(WaitFuture::new(async move {
140            // Clone to avoid holding the lock for a long time.
141            let complete_condition = this.0.complete_condition.lock().unwrap().clone();
142            complete_condition
143                .wait(&this, &positions, duration.as_secs_f64())
144                .await
145        }))
146    }
147
148    fn send_joint_trajectory(
149        &self,
150        trajectory: Vec<TrajectoryPoint>,
151    ) -> Result<WaitFuture, arci::Error> {
152        let traj = create_joint_trajectory_message_for_send_joint_trajectory(
153            self,
154            self,
155            &trajectory,
156            self.0.send_partial_joints_goal,
157        )?;
158        self.0.trajectory_publisher.send(traj).unwrap();
159        let this = self.clone();
160        Ok(WaitFuture::new(async move {
161            // Clone to avoid holding the lock for a long time.
162            let complete_condition = this.0.complete_condition.lock().unwrap().clone();
163            complete_condition
164                .wait(
165                    &this,
166                    &trajectory.last().unwrap().positions,
167                    trajectory.last().unwrap().time_from_start.as_secs_f64(),
168                )
169                .await
170        }))
171    }
172}
173
174impl SetCompleteCondition for RosControlClient {
175    fn set_complete_condition(&mut self, condition: Box<dyn CompleteCondition>) {
176        *self.0.complete_condition.lock().unwrap() = condition.into();
177    }
178}
arci_ros/ros_control/ros_control_client.rs

arci_ros/ros_control/
ros_control_client.rs
