1use std::time::Duration;
2
3use tokio::time::interval as AsyncInterval;
4use tracing::{debug, info};
5
6use crate::{Error, JointTrajectoryClient, TrajectoryPoint};
7
8pub fn get_joint_index<J>(joint_trajectory_client: &J, joint_name: &str) -> Result<usize, Error>
9where
10 J: JointTrajectoryClient,
11{
12 joint_trajectory_client
13 .joint_names()
14 .iter()
15 .position(|name| name == joint_name)
16 .ok_or_else(|| Error::NoJoint(joint_name.to_owned()))
17}
18
19#[allow(clippy::too_many_arguments)]
20pub async fn move_joint_until_stop<J>(
21 joint_trajectory_client: &J,
22 joint_name: &str,
23 target_position: f64,
24 target_duration: Duration,
25 diff_threshold_for_stop: f64,
26 stopped_duration: Duration,
27 monitor_interval: Duration,
28 duration_to_cancel: Duration,
29) -> Result<f64, Error>
30where
31 J: JointTrajectoryClient,
32{
33 let joint_index = get_joint_index(joint_trajectory_client, joint_name)?;
34 let mut positions = joint_trajectory_client.current_joint_positions()?;
35 let mut prev_current_position = positions[joint_index];
36 positions[joint_index] = target_position;
37 drop(
39 joint_trajectory_client
40 .send_joint_trajectory(vec![TrajectoryPoint::new(positions, target_duration)])?,
41 );
42 let mut stopped_count: u64 = 0;
43 let stopped_count_threshold =
44 (stopped_duration.as_secs_f64() / monitor_interval.as_secs_f64()).ceil() as u64;
45 let mut interval = AsyncInterval(monitor_interval);
46
47 loop {
48 let current_position = joint_trajectory_client.current_joint_positions()?[joint_index];
49 let diff_position = (prev_current_position - current_position).abs();
50 debug!(
51 "diff={diff_position:.4} diff_threshold={diff_threshold_for_stop:.4} stopped_count={stopped_count}",
52 );
53 if diff_position < diff_threshold_for_stop {
54 stopped_count += 1;
55 } else {
56 stopped_count = 0;
57 }
58 if stopped_count > stopped_count_threshold {
59 info!("Stopped.");
60 break;
61 }
62 prev_current_position = current_position;
63 interval.tick().await;
64 }
65 let stopped_position = joint_trajectory_client.current_joint_positions()?[joint_index];
66 joint_trajectory_client
67 .send_joint_positions(
68 joint_trajectory_client.current_joint_positions()?,
69 duration_to_cancel,
70 )?
71 .await?;
72 Ok(stopped_position)
73}