rclrs 0.7.0

A ROS 2 client library for developing robotics applications in Rust
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70

use rosidl_runtime_rs::Service;

use crate::{RclrsError, RclrsErrorFilter, RequestId, ServiceHandle, ServiceInfo};

use std::{any::Any, sync::Arc};

/// An enum capturing the various possible function signatures for service
/// callbacks that can be used by a [`Worker`][crate::Worker].
///
/// The correct enum variant is deduced by the [`IntoWorkerServiceCallback`][1] trait.
///
/// [1]: crate::IntoWorkerServiceCallback
pub enum WorkerServiceCallback<T, Payload>
where
    T: Service,
    Payload: 'static + Send,
{
    /// A callback that only takes in the request value
    OnlyRequest(Box<dyn FnMut(&mut Payload, T::Request) -> T::Response + Send>),
    /// A callback that takes in the request value and the ID of the request
    WithId(Box<dyn FnMut(&mut Payload, T::Request, RequestId) -> T::Response + Send>),
    /// A callback that takes in the request value and all available
    WithInfo(Box<dyn FnMut(&mut Payload, T::Request, ServiceInfo) -> T::Response + Send>),
}

impl<T, Payload> WorkerServiceCallback<T, Payload>
where
    T: Service,
    Payload: 'static + Send,
{
    pub(super) fn execute(
        &mut self,
        handle: &Arc<ServiceHandle>,
        any_payload: &mut dyn Any,
    ) -> Result<(), RclrsError> {
        let Some(payload) = any_payload.downcast_mut::<Payload>() else {
            return Err(RclrsError::InvalidPayload {
                expected: std::any::TypeId::of::<Payload>(),
                received: (*any_payload).type_id(),
            });
        };

        let mut evaluate = || {
            match self {
                WorkerServiceCallback::OnlyRequest(cb) => {
                    let (msg, mut rmw_request_id) = handle.take_request::<T>()?;
                    let response = cb(payload, msg);
                    handle.send_response::<T>(&mut rmw_request_id, response)?;
                }
                WorkerServiceCallback::WithId(cb) => {
                    let (msg, mut rmw_request_id) = handle.take_request::<T>()?;
                    let request_id = RequestId::from_rmw_request_id(&rmw_request_id);
                    let response = cb(payload, msg, request_id);
                    handle.send_response::<T>(&mut rmw_request_id, response)?;
                }
                WorkerServiceCallback::WithInfo(cb) => {
                    let (msg, rmw_service_info) = handle.take_request_with_info::<T>()?;
                    let mut rmw_request_id = rmw_service_info.rmw_request_id();
                    let service_info = ServiceInfo::from_rmw_service_info(&rmw_service_info);
                    let response = cb(payload, msg, service_info);
                    handle.send_response::<T>(&mut rmw_request_id, response)?;
                }
            }

            Ok(())
        };

        evaluate().take_failed_ok()
    }
}