arci/clients/
lazy.rs

1use std::{
2    sync::{Arc, LazyLock},
3    time::{Duration, SystemTime},
4};
5
6use async_trait::async_trait;
7use nalgebra::{Isometry2, Isometry3};
8
9use crate::{
10    error::Error,
11    gamepad::GamepadEvent,
12    traits::{
13        BaseVelocity, Gamepad, JointTrajectoryClient, Localization, MoveBase, Navigation, Speaker,
14        TrajectoryPoint, TransformResolver,
15    },
16    waits::WaitFuture,
17};
18
19#[allow(clippy::type_complexity)]
20#[derive(Debug)]
21pub struct Lazy<'a, T> {
22    inner: LazyLock<
23        Result<T, Arc<Error>>,
24        Box<dyn FnOnce() -> Result<T, Arc<Error>> + Send + Sync + 'a>,
25    >,
26    joint_names: Option<Vec<String>>,
27}
28
29impl<'a, T> Lazy<'a, T> {
30    /// Creates a new `Lazy` with the given constructor.
31    pub fn new(constructor: impl FnOnce() -> Result<T, Error> + Send + Sync + 'a) -> Self {
32        Self {
33            inner: LazyLock::new(Box::new(|| constructor().map_err(Arc::new))),
34            joint_names: None,
35        }
36    }
37
38    /// Creates a new `Lazy` with the given constructor and joint names.
39    ///
40    /// The specified joint names will be used as the return value of
41    /// `JointTrajectoryClient::joint_names`.
42    pub fn with_joint_names(
43        constructor: impl FnOnce() -> Result<T, Error> + Send + Sync + 'a,
44        joint_names: Vec<String>,
45    ) -> Self {
46        Self {
47            inner: LazyLock::new(Box::new(|| constructor().map_err(Arc::new))),
48            joint_names: Some(joint_names),
49        }
50    }
51
52    /// Returns a reference to the underlying value.
53    ///
54    /// - If this lazy value has not been constructed yet, this method will construct it.
55    /// - If the constructor of this lazy value fails, this method returns an error.
56    pub fn get_ref(&self) -> Result<&T, Error> {
57        self.inner.as_ref().map_err(|e| Error::Lazy(e.clone()))
58    }
59}
60
61impl<T> JointTrajectoryClient for Lazy<'_, T>
62where
63    T: JointTrajectoryClient,
64{
65    fn joint_names(&self) -> Vec<String> {
66        if let Some(joint_names) = &self.joint_names {
67            return joint_names.clone();
68        }
69        match self.get_ref() {
70            Ok(this) => this.joint_names(),
71            Err(e) => panic!("{e}"),
72        }
73    }
74
75    fn current_joint_positions(&self) -> Result<Vec<f64>, Error> {
76        self.get_ref()?.current_joint_positions()
77    }
78
79    fn send_joint_positions(
80        &self,
81        positions: Vec<f64>,
82        duration: Duration,
83    ) -> Result<WaitFuture, Error> {
84        self.get_ref()?.send_joint_positions(positions, duration)
85    }
86
87    fn send_joint_trajectory(&self, trajectory: Vec<TrajectoryPoint>) -> Result<WaitFuture, Error> {
88        self.get_ref()?.send_joint_trajectory(trajectory)
89    }
90}
91
92impl<T> Localization for Lazy<'_, T>
93where
94    T: Localization,
95{
96    fn current_pose(&self, frame_id: &str) -> Result<Isometry2<f64>, Error> {
97        self.get_ref()?.current_pose(frame_id)
98    }
99}
100
101impl<T> MoveBase for Lazy<'_, T>
102where
103    T: MoveBase,
104{
105    fn current_velocity(&self) -> Result<BaseVelocity, Error> {
106        self.get_ref()?.current_velocity()
107    }
108
109    fn send_velocity(&self, velocity: &BaseVelocity) -> Result<(), Error> {
110        self.get_ref()?.send_velocity(velocity)
111    }
112}
113
114impl<T> Navigation for Lazy<'_, T>
115where
116    T: Navigation,
117{
118    fn send_goal_pose(
119        &self,
120        goal: Isometry2<f64>,
121        frame_id: &str,
122        timeout: Duration,
123    ) -> Result<WaitFuture, Error> {
124        self.get_ref()?.send_goal_pose(goal, frame_id, timeout)
125    }
126
127    fn cancel(&self) -> Result<(), Error> {
128        self.get_ref()?.cancel()
129    }
130}
131
132impl<T> Speaker for Lazy<'_, T>
133where
134    T: Speaker,
135{
136    fn speak(&self, message: &str) -> Result<WaitFuture, Error> {
137        self.get_ref()?.speak(message)
138    }
139}
140
141impl<T> TransformResolver for Lazy<'_, T>
142where
143    T: TransformResolver,
144{
145    fn resolve_transformation(
146        &self,
147        from: &str,
148        to: &str,
149        time: SystemTime,
150    ) -> Result<Isometry3<f64>, Error> {
151        self.get_ref()?.resolve_transformation(from, to, time)
152    }
153}
154
155#[async_trait]
156impl<T> Gamepad for Lazy<'_, T>
157where
158    T: Gamepad,
159{
160    async fn next_event(&self) -> GamepadEvent {
161        match self.get_ref() {
162            Ok(this) => this.next_event().await,
163            Err(e) => panic!("{e}"),
164        }
165    }
166
167    fn stop(&self) {
168        match self.get_ref() {
169            Ok(this) => this.stop(),
170            Err(e) => panic!("{e}"),
171        }
172    }
173}