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