arci_ros/ros_control/
ros_control_action_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};
10
11use crate::{
12    create_joint_trajectory_message_for_send_joint_positions,
13    create_joint_trajectory_message_for_send_joint_trajectory, define_action_client,
14    extract_current_joint_positions_from_state, msg, JointStateProvider,
15    JointStateProviderFromJointState, LazyJointStateProvider, SubscriberHandler,
16};
17
18const ACTION_TIMEOUT_DURATION_RATIO: u32 = 10;
19
20define_action_client!(
21    ControlActionClient,
22    msg::control_msgs,
23    FollowJointTrajectory
24);
25
26#[derive(Clone)]
27pub struct RosControlActionClient(Arc<RosControlActionClientInner>);
28
29struct RosControlActionClientInner {
30    joint_names: Vec<String>,
31    send_partial_joints_goal: bool,
32    joint_state_provider: Arc<LazyJointStateProvider>,
33    complete_condition: Mutex<Arc<dyn CompleteCondition>>,
34    action_client: ControlActionClient,
35}
36
37impl RosControlActionClient {
38    pub fn new_with_joint_state_provider(
39        joint_names: Vec<String>,
40        controller_name: &str,
41        send_partial_joints_goal: bool,
42        joint_state_provider: Arc<LazyJointStateProvider>,
43    ) -> Self {
44        let (state_joint_names, _) = joint_state_provider.get_joint_state().unwrap();
45        for joint_name in &joint_names {
46            if !state_joint_names.iter().any(|name| **name == *joint_name) {
47                panic!(
48                    "Invalid configuration : Joint ({joint_name}) is not found in state ({state_joint_names:?})",
49                );
50            }
51        }
52
53        let action_client =
54            ControlActionClient::new(&format!("{controller_name}/follow_joint_trajectory"), 1);
55
56        Self(Arc::new(RosControlActionClientInner {
57            joint_names,
58            joint_state_provider,
59            send_partial_joints_goal,
60            action_client,
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        joint_state_topic_name: &str,
70    ) -> Self {
71        Self::new_with_joint_state_provider(
72            joint_names,
73            controller_name,
74            send_partial_joints_goal,
75            Self::create_joint_state_provider(joint_state_topic_name),
76        )
77    }
78
79    pub(crate) fn create_joint_state_provider(
80        joint_state_topic_name: impl Into<String>,
81    ) -> Arc<LazyJointStateProvider> {
82        let joint_state_topic_name = joint_state_topic_name.into();
83        Arc::new(LazyLock::new(Box::new(move || {
84            Box::new(JointStateProviderFromJointState::new(
85                SubscriberHandler::new(&joint_state_topic_name, 1),
86            ))
87        })))
88    }
89}
90
91impl JointStateProvider for RosControlActionClient {
92    fn get_joint_state(&self) -> Result<(Vec<String>, Vec<f64>), arci::Error> {
93        self.0.joint_state_provider.get_joint_state()
94    }
95}
96
97impl JointTrajectoryClient for RosControlActionClient {
98    fn joint_names(&self) -> Vec<String> {
99        self.0.joint_names.clone()
100    }
101
102    fn current_joint_positions(&self) -> Result<Vec<f64>, arci::Error> {
103        extract_current_joint_positions_from_state(self, self)
104    }
105
106    fn send_joint_positions(
107        &self,
108        positions: Vec<f64>,
109        duration: Duration,
110    ) -> Result<WaitFuture, arci::Error> {
111        let traj = create_joint_trajectory_message_for_send_joint_positions(
112            self,
113            self,
114            &positions,
115            duration,
116            self.0.send_partial_joints_goal,
117        )?;
118        let goal = msg::control_msgs::FollowJointTrajectoryGoal {
119            trajectory: traj,
120            ..Default::default()
121        };
122        let mut goal_id = self
123            .0
124            .action_client
125            .send_goal(goal)
126            .map_err(|e| anyhow::anyhow!(e.to_string()))?;
127        let this = self.clone();
128        Ok(WaitFuture::new(async move {
129            // Success of action result does not always mean joints reached to target.
130            // So check complete_condition too.
131            tokio::task::spawn_blocking(move || {
132                goal_id.wait(duration * ACTION_TIMEOUT_DURATION_RATIO)
133            })
134            .await
135            .map_err(|e| arci::Error::Other(e.into()))??;
136            // Clone to avoid holding the lock for a long time.
137            let complete_condition = this.0.complete_condition.lock().unwrap().clone();
138            complete_condition
139                .wait(&this, &positions, duration.as_secs_f64())
140                .await
141        }))
142    }
143
144    fn send_joint_trajectory(
145        &self,
146        trajectory: Vec<TrajectoryPoint>,
147    ) -> Result<WaitFuture, arci::Error> {
148        let traj = create_joint_trajectory_message_for_send_joint_trajectory(
149            self,
150            self,
151            &trajectory,
152            self.0.send_partial_joints_goal,
153        )?;
154        let goal = msg::control_msgs::FollowJointTrajectoryGoal {
155            trajectory: traj,
156            ..Default::default()
157        };
158        let mut goal_id = self
159            .0
160            .action_client
161            .send_goal(goal)
162            .map_err(|e| anyhow::anyhow!(e.to_string()))?;
163        let this = self.clone();
164        Ok(WaitFuture::new(async move {
165            let duration = if let Some(trajectory_point) = trajectory.last() {
166                trajectory_point.time_from_start
167            } else {
168                Duration::from_secs(0)
169            };
170            // Success of action result does not always mean joints reached to target.
171            // So check complete_condition too.
172            tokio::task::spawn_blocking(move || {
173                goal_id.wait(duration * ACTION_TIMEOUT_DURATION_RATIO)
174            })
175            .await
176            .map_err(|e| arci::Error::Other(e.into()))??;
177            // Clone to avoid holding the lock for a long time.
178            let complete_condition = this.0.complete_condition.lock().unwrap().clone();
179            complete_condition
180                .wait(
181                    &this,
182                    &trajectory.last().unwrap().positions,
183                    trajectory.last().unwrap().time_from_start.as_secs_f64(),
184                )
185                .await
186        }))
187    }
188}
189
190impl SetCompleteCondition for RosControlActionClient {
191    fn set_complete_condition(&mut self, condition: Box<dyn CompleteCondition>) {
192        *self.0.complete_condition.lock().unwrap() = condition.into();
193    }
194}
arci_ros/ros_control/ros_control_action_client.rs

arci_ros/ros_control/
ros_control_action_client.rs
