openrr_remote/
lib.rs

1#![doc = include_str!("../README.md")]
2#![warn(missing_debug_implementations, rust_2018_idioms)]
3// This warns code generated by tonic.
4#![allow(clippy::derive_partial_eq_without_eq)]
5
6mod pb {
7    #[cfg(local_out_dir)]
8    include!("generated/arci.rs");
9    #[cfg(not(local_out_dir))]
10    tonic::include_proto!("arci");
11}
12
13#[rustfmt::skip]
14#[path = "gen/impls.rs"]
15mod impls;
16
17use std::{
18    future::Future,
19    net::SocketAddr,
20    time::{Duration, SystemTime},
21};
22
23use arci::nalgebra;
24use tracing::error;
25
26pub use crate::impls::*;
27
28fn block_in_place<T>(f: impl Future<Output = T>) -> T {
29    tokio::task::block_in_place(|| tokio::runtime::Handle::current().block_on(f))
30}
31
32fn wait_from_handle(
33    handle: tokio::task::JoinHandle<Result<tonic::Response<()>, tonic::Status>>,
34) -> arci::WaitFuture {
35    arci::WaitFuture::new(async move {
36        handle
37            .await
38            .map_err(|e| arci::Error::Other(e.into()))?
39            .map_err(|e| arci::Error::Other(e.into()))?;
40        Ok(())
41    })
42}
43
44// =============================================================================
45// arci::JointTrajectoryClient
46
47impl arci::JointTrajectoryClient for RemoteJointTrajectoryClientSender {
48    fn joint_names(&self) -> Vec<String> {
49        let mut client = self.client.clone();
50        block_in_place(client.joint_names(()))
51            .unwrap()
52            .into_inner()
53            .names
54    }
55
56    fn current_joint_positions(&self) -> Result<Vec<f64>, arci::Error> {
57        let mut client = self.client.clone();
58        Ok(block_in_place(client.current_joint_positions(()))
59            .map_err(|e| arci::Error::Other(e.into()))?
60            .into_inner()
61            .positions)
62    }
63
64    fn send_joint_positions(
65        &self,
66        positions: Vec<f64>,
67        duration: Duration,
68    ) -> Result<arci::WaitFuture, arci::Error> {
69        let mut client = self.client.clone();
70        Ok(wait_from_handle(tokio::spawn(async move {
71            client
72                .send_joint_positions(pb::JointPositionsRequest {
73                    positions,
74                    duration: Some(duration.try_into().unwrap()),
75                })
76                .await
77        })))
78    }
79
80    fn send_joint_trajectory(
81        &self,
82        trajectory: Vec<arci::TrajectoryPoint>,
83    ) -> Result<arci::WaitFuture, arci::Error> {
84        let mut client = self.client.clone();
85        Ok(wait_from_handle(tokio::spawn(async move {
86            client
87                .send_joint_trajectory(pb::JointTrajectoryRequest {
88                    trajectory: trajectory.into_iter().map(Into::into).collect(),
89                })
90                .await
91        })))
92    }
93}
94
95#[tonic::async_trait]
96impl<C> pb::joint_trajectory_client_server::JointTrajectoryClient
97    for RemoteJointTrajectoryClientReceiver<C>
98where
99    C: arci::JointTrajectoryClient + 'static,
100{
101    async fn joint_names(
102        &self,
103        _: tonic::Request<()>,
104    ) -> Result<tonic::Response<pb::JointNamesResponse>, tonic::Status> {
105        Ok(tonic::Response::new(pb::JointNamesResponse {
106            names: arci::JointTrajectoryClient::joint_names(&self.inner),
107        }))
108    }
109
110    async fn current_joint_positions(
111        &self,
112        _: tonic::Request<()>,
113    ) -> Result<tonic::Response<pb::JointPositionsResponse>, tonic::Status> {
114        Ok(tonic::Response::new(pb::JointPositionsResponse {
115            positions: arci::JointTrajectoryClient::current_joint_positions(&self.inner)
116                .map_err(|e| tonic::Status::unknown(e.to_string()))?,
117        }))
118    }
119
120    async fn send_joint_positions(
121        &self,
122        request: tonic::Request<pb::JointPositionsRequest>,
123    ) -> Result<tonic::Response<()>, tonic::Status> {
124        let request = request.into_inner();
125        arci::JointTrajectoryClient::send_joint_positions(
126            &self.inner,
127            request.positions,
128            request.duration.unwrap().try_into().unwrap(),
129        )
130        .map_err(|e| tonic::Status::unknown(e.to_string()))?
131        .await
132        .map_err(|e| tonic::Status::unknown(e.to_string()))?;
133        Ok(tonic::Response::new(()))
134    }
135
136    async fn send_joint_trajectory(
137        &self,
138        request: tonic::Request<pb::JointTrajectoryRequest>,
139    ) -> Result<tonic::Response<()>, tonic::Status> {
140        let request = request.into_inner();
141        arci::JointTrajectoryClient::send_joint_trajectory(
142            &self.inner,
143            request.trajectory.into_iter().map(Into::into).collect(),
144        )
145        .map_err(|e| tonic::Status::unknown(e.to_string()))?
146        .await
147        .map_err(|e| tonic::Status::unknown(e.to_string()))?;
148        Ok(tonic::Response::new(()))
149    }
150}
151
152// =============================================================================
153// arci::Gamepad
154
155#[arci::async_trait]
156impl arci::Gamepad for RemoteGamepadSender {
157    async fn next_event(&self) -> arci::gamepad::GamepadEvent {
158        let mut client = self.client.clone();
159        match client.next_event(()).await {
160            Ok(event) => event.into_inner().into(),
161            Err(e) => {
162                error!("{e}");
163                arci::gamepad::GamepadEvent::Unknown
164            }
165        }
166    }
167
168    fn stop(&self) {
169        let mut client = self.client.clone();
170        if let Err(e) = block_in_place(client.stop(())) {
171            error!("{e}");
172        }
173    }
174}
175
176#[tonic::async_trait]
177impl<C> pb::gamepad_server::Gamepad for RemoteGamepadReceiver<C>
178where
179    C: arci::Gamepad + 'static,
180{
181    async fn next_event(
182        &self,
183        _: tonic::Request<()>,
184    ) -> Result<tonic::Response<pb::GamepadEvent>, tonic::Status> {
185        Ok(tonic::Response::new(self.inner.next_event().await.into()))
186    }
187
188    async fn stop(&self, _: tonic::Request<()>) -> Result<tonic::Response<()>, tonic::Status> {
189        self.inner.stop();
190        Ok(tonic::Response::new(()))
191    }
192}
193
194// =============================================================================
195// Messages
196
197impl From<arci::TrajectoryPoint> for pb::TrajectoryPoint {
198    fn from(val: arci::TrajectoryPoint) -> Self {
199        Self {
200            positions: val.positions,
201            velocities: val.velocities.unwrap_or_default(),
202            time_from_start: Some(val.time_from_start.try_into().unwrap()),
203        }
204    }
205}
206
207impl From<pb::TrajectoryPoint> for arci::TrajectoryPoint {
208    fn from(val: pb::TrajectoryPoint) -> Self {
209        Self {
210            positions: val.positions,
211            velocities: if val.velocities.is_empty() {
212                None
213            } else {
214                Some(val.velocities)
215            },
216            time_from_start: val.time_from_start.unwrap().try_into().unwrap(),
217        }
218    }
219}
220
221impl From<arci::BaseVelocity> for pb::BaseVelocity {
222    fn from(val: arci::BaseVelocity) -> Self {
223        Self {
224            x: val.x,
225            y: val.y,
226            theta: val.theta,
227        }
228    }
229}
230
231impl From<pb::BaseVelocity> for arci::BaseVelocity {
232    fn from(val: pb::BaseVelocity) -> Self {
233        Self {
234            x: val.x,
235            y: val.y,
236            theta: val.theta,
237        }
238    }
239}
240
241impl From<arci::Isometry2<f64>> for pb::Isometry2 {
242    fn from(val: arci::Isometry2<f64>) -> Self {
243        Self {
244            rotation: Some(pb::UnitComplex {
245                re: val.rotation.re,
246                im: val.rotation.im,
247            }),
248            translation: Some(pb::Translation2 {
249                x: val.translation.x,
250                y: val.translation.y,
251            }),
252        }
253    }
254}
255
256impl From<pb::Isometry2> for arci::Isometry2<f64> {
257    fn from(val: pb::Isometry2) -> Self {
258        let translation = val.translation.unwrap();
259        let rotation = val.rotation.unwrap();
260        Self::from_parts(
261            nalgebra::Translation2::new(translation.x, translation.y),
262            nalgebra::UnitComplex::from_complex(nalgebra::Complex {
263                re: rotation.re,
264                im: rotation.im,
265            }),
266        )
267    }
268}
269
270impl From<arci::Isometry3<f64>> for pb::Isometry3 {
271    fn from(val: arci::Isometry3<f64>) -> Self {
272        Self {
273            rotation: Some(pb::UnitQuaternion {
274                x: val.rotation.coords.x,
275                y: val.rotation.coords.y,
276                z: val.rotation.coords.z,
277                w: val.rotation.coords.w,
278            }),
279            translation: Some(pb::Translation3 {
280                x: val.translation.x,
281                y: val.translation.y,
282                z: val.translation.z,
283            }),
284        }
285    }
286}
287
288impl From<pb::Isometry3> for arci::Isometry3<f64> {
289    fn from(val: pb::Isometry3) -> Self {
290        let translation = val.translation.unwrap();
291        let rotation = val.rotation.unwrap();
292        Self::from_parts(
293            nalgebra::Translation3::new(translation.x, translation.y, translation.z),
294            nalgebra::UnitQuaternion::from_quaternion(nalgebra::Quaternion::new(
295                rotation.w, rotation.x, rotation.y, rotation.z,
296            )),
297        )
298    }
299}
300
301impl From<(arci::Isometry2<f64>, &str, Duration)> for pb::GoalPoseRequest {
302    fn from((goal, frame_id, timeout): (arci::Isometry2<f64>, &str, Duration)) -> Self {
303        Self {
304            goal: Some(goal.into()),
305            frame_id: frame_id.into(),
306            timeout: Some(timeout.try_into().unwrap()),
307        }
308    }
309}
310
311impl From<(&str, &str, SystemTime)> for pb::ResolveTransformationRequest {
312    fn from((from, to, time): (&str, &str, SystemTime)) -> Self {
313        Self {
314            from: from.into(),
315            to: to.into(),
316            time: Some(time.into()),
317        }
318    }
319}
320
321impl From<arci::gamepad::GamepadEvent> for pb::GamepadEvent {
322    fn from(val: arci::gamepad::GamepadEvent) -> Self {
323        let event = match val {
324            arci::gamepad::GamepadEvent::ButtonPressed(b) => {
325                pb::gamepad_event::Event::ButtonPressed(pb::Button::from(b) as _)
326            }
327            arci::gamepad::GamepadEvent::ButtonReleased(b) => {
328                pb::gamepad_event::Event::ButtonReleased(pb::Button::from(b) as _)
329            }
330            arci::gamepad::GamepadEvent::AxisChanged(axis, value) => {
331                pb::gamepad_event::Event::AxisChanged(pb::AxisChanged {
332                    axis: pb::Axis::from(axis) as _,
333                    value,
334                })
335            }
336            arci::gamepad::GamepadEvent::Connected => pb::gamepad_event::Event::Connected(()),
337            arci::gamepad::GamepadEvent::Disconnected => pb::gamepad_event::Event::Disconnected(()),
338            arci::gamepad::GamepadEvent::Unknown => pb::gamepad_event::Event::Unknown(()),
339        };
340        Self { event: Some(event) }
341    }
342}
343
344impl From<arci::gamepad::Button> for pb::Button {
345    fn from(val: arci::gamepad::Button) -> Self {
346        match val {
347            arci::gamepad::Button::South => Self::South,
348            arci::gamepad::Button::East => Self::East,
349            arci::gamepad::Button::North => Self::North,
350            arci::gamepad::Button::West => Self::West,
351            arci::gamepad::Button::LeftTrigger => Self::LeftTrigger,
352            arci::gamepad::Button::LeftTrigger2 => Self::LeftTrigger2,
353            arci::gamepad::Button::RightTrigger => Self::RightTrigger,
354            arci::gamepad::Button::RightTrigger2 => Self::RightTrigger2,
355            arci::gamepad::Button::Select => Self::Select,
356            arci::gamepad::Button::Start => Self::Start,
357            arci::gamepad::Button::Mode => Self::Mode,
358            arci::gamepad::Button::LeftThumb => Self::LeftThumb,
359            arci::gamepad::Button::RightThumb => Self::RightThumb,
360            arci::gamepad::Button::DPadUp => Self::DPadUp,
361            arci::gamepad::Button::DPadDown => Self::DPadDown,
362            arci::gamepad::Button::DPadLeft => Self::DPadLeft,
363            arci::gamepad::Button::DPadRight => Self::DPadRight,
364            arci::gamepad::Button::Unknown => Self::Unknown,
365        }
366    }
367}
368
369impl From<arci::gamepad::Axis> for pb::Axis {
370    fn from(val: arci::gamepad::Axis) -> Self {
371        match val {
372            arci::gamepad::Axis::LeftStickX => Self::LeftStickX,
373            arci::gamepad::Axis::LeftStickY => Self::LeftStickY,
374            arci::gamepad::Axis::LeftTrigger => Self::LeftTrigger,
375            arci::gamepad::Axis::RightStickX => Self::RightStickX,
376            arci::gamepad::Axis::RightStickY => Self::RightStickY,
377            arci::gamepad::Axis::RightTrigger => Self::RightTrigger,
378            arci::gamepad::Axis::DPadX => Self::DPadX,
379            arci::gamepad::Axis::DPadY => Self::DPadY,
380            arci::gamepad::Axis::Unknown => Self::Unknown,
381        }
382    }
383}
384
385impl From<pb::GamepadEvent> for arci::gamepad::GamepadEvent {
386    fn from(val: pb::GamepadEvent) -> Self {
387        let val = val.event.unwrap();
388        match val {
389            pb::gamepad_event::Event::ButtonPressed(b) => {
390                Self::ButtonPressed(pb::Button::from_i32(b).unwrap().into())
391            }
392            pb::gamepad_event::Event::ButtonReleased(b) => {
393                Self::ButtonReleased(pb::Button::from_i32(b).unwrap().into())
394            }
395            pb::gamepad_event::Event::AxisChanged(a) => {
396                Self::AxisChanged(pb::Axis::from_i32(a.axis).unwrap().into(), a.value)
397            }
398            pb::gamepad_event::Event::Connected(()) => Self::Connected,
399            pb::gamepad_event::Event::Disconnected(()) => Self::Disconnected,
400            pb::gamepad_event::Event::Unknown(()) => Self::Unknown,
401        }
402    }
403}
404
405impl From<pb::Button> for arci::gamepad::Button {
406    fn from(val: pb::Button) -> Self {
407        match val {
408            pb::Button::South => Self::South,
409            pb::Button::East => Self::East,
410            pb::Button::North => Self::North,
411            pb::Button::West => Self::West,
412            pb::Button::LeftTrigger => Self::LeftTrigger,
413            pb::Button::LeftTrigger2 => Self::LeftTrigger2,
414            pb::Button::RightTrigger => Self::RightTrigger,
415            pb::Button::RightTrigger2 => Self::RightTrigger2,
416            pb::Button::Select => Self::Select,
417            pb::Button::Start => Self::Start,
418            pb::Button::Mode => Self::Mode,
419            pb::Button::LeftThumb => Self::LeftThumb,
420            pb::Button::RightThumb => Self::RightThumb,
421            pb::Button::DPadUp => Self::DPadUp,
422            pb::Button::DPadDown => Self::DPadDown,
423            pb::Button::DPadLeft => Self::DPadLeft,
424            pb::Button::DPadRight => Self::DPadRight,
425            pb::Button::Unknown => Self::Unknown,
426        }
427    }
428}
429
430impl From<pb::Axis> for arci::gamepad::Axis {
431    fn from(val: pb::Axis) -> Self {
432        match val {
433            pb::Axis::LeftStickX => Self::LeftStickX,
434            pb::Axis::LeftStickY => Self::LeftStickY,
435            pb::Axis::LeftTrigger => Self::LeftTrigger,
436            pb::Axis::RightStickX => Self::RightStickX,
437            pb::Axis::RightStickY => Self::RightStickY,
438            pb::Axis::RightTrigger => Self::RightTrigger,
439            pb::Axis::DPadX => Self::DPadX,
440            pb::Axis::DPadY => Self::DPadY,
441            pb::Axis::Unknown => Self::Unknown,
442        }
443    }
444}
openrr_remote/lib.rs

openrr_remote/
lib.rs
