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 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 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 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}