arci/traits/
joint_trajectory_client.rs

1use auto_impl::auto_impl;
2use serde::{Deserialize, Serialize};
3
4use crate::{
5    error::Error,
6    waits::{CompleteCondition, WaitFuture},
7};
8
9#[derive(Clone, Debug, Serialize, Deserialize)]
10pub struct TrajectoryPoint {
11    pub positions: Vec<f64>,
12    pub velocities: Option<Vec<f64>>,
13    pub time_from_start: std::time::Duration,
14}
15
16impl TrajectoryPoint {
17    pub fn new(positions: Vec<f64>, time_from_start: std::time::Duration) -> Self {
18        Self {
19            positions,
20            velocities: None,
21            time_from_start,
22        }
23    }
24}
25
26#[auto_impl(Box)]
27pub trait SetCompleteCondition {
28    fn set_complete_condition(&mut self, condition: Box<dyn CompleteCondition>);
29}
30
31#[auto_impl(Box, Arc)]
32pub trait JointTrajectoryClient: Send + Sync {
33    /// Returns names of joints that this client handles.
34    fn joint_names(&self) -> Vec<String>;
35
36    /// Returns the current joint positions.
37    fn current_joint_positions(&self) -> Result<Vec<f64>, Error>;
38
39    /// Send the specified joint positions and returns a future that waits until
40    /// complete the move joints.
41    ///
42    /// # Implementation
43    ///
44    /// The returned future is expected to behave similarly to
45    /// [`std::thread::JoinHandle`] and [`tokio::task::JoinHandle`]:
46    ///
47    /// - Can wait for the operation to complete by `.await`.
48    /// - The operation does not end even if it is dropped.
49    ///
50    /// If the operation may block the current thread for an extended period of
51    /// time, consider [spawning a thread to running blocking
52    /// operations](https://docs.rs/tokio/1/tokio/index.html#cpu-bound-tasks-and-blocking-code).
53    fn send_joint_positions(
54        &self,
55        positions: Vec<f64>,
56        duration: std::time::Duration,
57    ) -> Result<WaitFuture, Error>;
58
59    /// Send the specified joint trajectory and returns a future that waits until
60    /// complete the move joints.
61    ///
62    /// # Implementation
63    ///
64    /// See the "Implementation" section of the
65    /// [`send_joint_positions`](Self::send_joint_positions) method.
66    fn send_joint_trajectory(&self, trajectory: Vec<TrajectoryPoint>) -> Result<WaitFuture, Error>;
67}
68
69#[cfg(test)]
70mod tests {
71    use assert_approx_eq::assert_approx_eq;
72
73    use super::*;
74
75    #[test]
76    fn test_trajectory_point() {
77        let mut tp = TrajectoryPoint::new(vec![1.0, -1.0], std::time::Duration::from_secs(1));
78        assert_approx_eq!(tp.positions[0], 1.0);
79        assert_approx_eq!(tp.positions[1], -1.0);
80        assert!(tp.velocities.is_none());
81        assert_eq!(tp.time_from_start, std::time::Duration::from_secs(1));
82        tp.positions = vec![-1.0, 1.0];
83        tp.velocities = Some(vec![1.0, -1.0]);
84        tp.time_from_start = std::time::Duration::from_secs(2);
85        assert_approx_eq!(tp.positions[0], -1.0);
86        assert_approx_eq!(tp.positions[1], 1.0);
87        let vels = tp.velocities.unwrap();
88        assert_approx_eq!(vels[0], 1.0);
89        assert_approx_eq!(vels[1], -1.0);
90        assert_eq!(tp.time_from_start, std::time::Duration::from_secs(2));
91    }
92
93    #[test]
94    fn test_trajectory_point_debug() {
95        let tp = TrajectoryPoint::new(vec![1.0, -1.0], std::time::Duration::from_secs(1));
96        assert_eq!(
97            format!("{tp:?}"),
98            "TrajectoryPoint { positions: [1.0, -1.0], velocities: None, time_from_start: 1s }"
99        );
100    }
101
102    #[allow(clippy::redundant_clone)] // This is intentional.
103    #[test]
104    fn test_trajectory_point_clone() {
105        let tp1 = TrajectoryPoint::new(vec![1.0, -1.0], std::time::Duration::from_secs(1));
106        let tp2 = tp1.clone();
107        assert_approx_eq!(tp2.positions[0], 1.0);
108        assert_approx_eq!(tp2.positions[1], -1.0);
109        assert_approx_eq!(tp1.positions[0], 1.0);
110        assert_approx_eq!(tp1.positions[1], -1.0);
111    }
112}