Add service introspection

rclrs/src/client.rs

@@ -10,7 +10,8 @@ use rosidl_runtime_rs::Message;
 use crate::{
     error::ToResult, log_fatal, rcl_bindings::*, IntoPrimitiveOptions, MessageCow, Node, Promise,
     QoSProfile, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclReturnCode, RclrsError,
-    ReadyKind, ServiceInfo, Waitable, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX,
+    ReadyKind, ServiceInfo, ServiceIntrospectionState, Waitable, WaitableLifecycle,
+    ENTITY_LIFECYCLE_MUTEX,
 };
 
 mod client_async_callback;
@@ -369,6 +370,41 @@ where
             lifecycle,
         }))
     }
+
+    /// Configure service introspection for this client.
+    /// Service introspection allows tools to monitor service requests and responses.
+    /// Service introspection can be set to either
+    ///     - Off: Disabled
+    ///     - Metadata: Only metadata without any user data contents
+    ///     - Contents: User data contents with metadata
+    pub fn configure_introspection(
+        &self,
+        introspection_state: ServiceIntrospectionState,
+    ) -> Result<(), RclrsError> {
+        let client = &mut *self.handle.rcl_client.lock().unwrap();
+        let node = &mut *self.handle.node.handle().rcl_node.lock().unwrap();
+        let clock = self.handle.node.get_clock();
+        let rcl_clock = &mut *clock.get_rcl_clock().lock().unwrap();
+        let type_support = <T as rosidl_runtime_rs::Service>::get_type_support()
+            as *const rosidl_service_type_support_t;
+
+        // SAFETY: No preconditions for this function.
+        let publisher_options = unsafe { rcl_publisher_get_default_options() };
+
+        unsafe {
+            rcl_client_configure_service_introspection(
+                client,
+                node,
+                rcl_clock,
+                type_support,
+                publisher_options,
+                introspection_state.into(),
+            )
+            .ok()?;
+        }
+
+        Ok(())
+    }
 }
 
 /// `ClientOptions` are used by [`Node::create_client`][1] to initialize a

rclrs/src/node.rs

@@ -580,7 +580,7 @@ impl NodeState {
     /// The advantage of creating a service directly from the [`NodeState`] is you
     /// can create async services using [`NodeState::create_async_service`].
     pub fn create_service<'a, T, Args>(
-        &self,
+        self: &Arc<Self>,
         options: impl Into<ServiceOptions<'a>>,
         callback: impl IntoNodeServiceCallback<T, Args>,
     ) -> Result<Service<T>, RclrsError>
@@ -590,7 +590,7 @@ impl NodeState {
         ServiceState::<T, Node>::create(
             options,
             callback.into_node_service_callback(),
-            &self.handle,
+            self,
             self.commands.async_worker_commands(),
         )
     }
@@ -671,7 +671,7 @@ impl NodeState {
     /// # Ok::<(), RclrsError>(())
     /// ```
     pub fn create_async_service<'a, T, Args>(
-        &self,
+        self: &Arc<Self>,
         options: impl Into<ServiceOptions<'a>>,
         callback: impl IntoAsyncServiceCallback<T, Args>,
     ) -> Result<Service<T>, RclrsError>
@@ -681,7 +681,7 @@ impl NodeState {
         ServiceState::<T, Node>::create(
             options,
             callback.into_async_service_callback(),
-            &self.handle,
+            self,
             self.commands.async_worker_commands(),
         )
     }

rclrs/src/service.rs

@@ -8,7 +8,7 @@ use std::{
 use rosidl_runtime_rs::{Message, Service as ServiceIDL};
 
 use crate::{
-    error::ToResult, rcl_bindings::*, IntoPrimitiveOptions, MessageCow, Node, NodeHandle,
+    error::ToResult, rcl_bindings::*, Clock, IntoPrimitiveOptions, MessageCow, Node, NodeHandle,
     QoSProfile, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, ReadyKind,
     Waitable, WaitableLifecycle, WorkScope, Worker, WorkerCommands, ENTITY_LIFECYCLE_MUTEX,
 };
@@ -101,7 +101,7 @@ where
     pub(crate) fn create<'a>(
         options: impl Into<ServiceOptions<'a>>,
         callback: AnyServiceCallback<T, Scope::Payload>,
-        node_handle: &Arc<NodeHandle>,
+        node: &Node,
         commands: &Arc<WorkerCommands>,
     ) -> Result<Arc<Self>, RclrsError> {
         let ServiceOptions { name, qos } = options.into();
@@ -120,7 +120,7 @@ where
         service_options.qos = qos.into();
 
         {
-            let rcl_node = node_handle.rcl_node.lock().unwrap();
+            let rcl_node = node.handle().rcl_node.lock().unwrap();
             let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap();
             unsafe {
                 // SAFETY:
@@ -143,7 +143,8 @@ where
 
         let handle = Arc::new(ServiceHandle {
             rcl_service: Mutex::new(rcl_service),
-            node_handle: Arc::clone(&node_handle),
+            node_handle: Arc::clone(node.handle()),
+            clock: node.get_clock(),
         });
 
         let (waitable, lifecycle) = Waitable::new(
@@ -164,6 +165,41 @@ where
 
         Ok(service)
     }
+
+    /// Configure service introspection for this service.
+    /// Service introspection allows tools to monitor service requests and responses.
+    /// Service introspection can be set to either
+    ///     - Off: Disabled
+    ///     - Metadata: Only metadata without any user data contents
+    ///     - Contents: User data contents with metadata
+    pub fn configure_introspection(
+        &self,
+        introspection_state: ServiceIntrospectionState,
+    ) -> Result<(), RclrsError> {
+        let service = &mut *self.handle.rcl_service.lock().unwrap();
+        let node = &mut *self.handle.node_handle.rcl_node.lock().unwrap();
+        let clock = &self.handle.clock;
+        let rcl_clock = &mut *clock.get_rcl_clock().lock().unwrap();
+        let type_support = <T as rosidl_runtime_rs::Service>::get_type_support()
+            as *const rosidl_service_type_support_t;
+
+        // SAFETY: No preconditions for this function.
+        let publisher_options = unsafe { rcl_publisher_get_default_options() };
+
+        unsafe {
+            rcl_service_configure_service_introspection(
+                service,
+                node,
+                rcl_clock,
+                type_support,
+                publisher_options,
+                introspection_state.into(),
+            )
+            .ok()?;
+        }
+
+        Ok(())
+    }
 }
 
 impl<T: ServiceIDL> ServiceState<T, Node> {
@@ -282,6 +318,7 @@ unsafe impl Send for rcl_service_t {}
 pub struct ServiceHandle {
     rcl_service: Mutex<rcl_service_t>,
     node_handle: Arc<NodeHandle>,
+    clock: Clock,
 }
 
 impl ServiceHandle {
@@ -391,6 +428,45 @@ impl Drop for ServiceHandle {
     }
 }
 
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub enum ServiceIntrospectionState {
+    Off,
+    Metadata,
+    Contents,
+}
+
+impl From<rcl_service_introspection_state_e> for ServiceIntrospectionState {
+    fn from(value: rcl_service_introspection_state_e) -> Self {
+        match value {
+            rcl_service_introspection_state_e::RCL_SERVICE_INTROSPECTION_OFF => {
+                ServiceIntrospectionState::Off
+            }
+            rcl_service_introspection_state_e::RCL_SERVICE_INTROSPECTION_METADATA => {
+                ServiceIntrospectionState::Metadata
+            }
+            rcl_service_introspection_state_e::RCL_SERVICE_INTROSPECTION_CONTENTS => {
+                ServiceIntrospectionState::Contents
+            }
+        }
+    }
+}
+
+impl From<ServiceIntrospectionState> for rcl_service_introspection_state_e {
+    fn from(value: ServiceIntrospectionState) -> Self {
+        match value {
+            ServiceIntrospectionState::Off => {
+                rcl_service_introspection_state_e::RCL_SERVICE_INTROSPECTION_OFF
+            }
+            ServiceIntrospectionState::Metadata => {
+                rcl_service_introspection_state_e::RCL_SERVICE_INTROSPECTION_METADATA
+            }
+            ServiceIntrospectionState::Contents => {
+                rcl_service_introspection_state_e::RCL_SERVICE_INTROSPECTION_CONTENTS
+            }
+        }
+    }
+}
+
 #[cfg(test)]
 mod tests {
     use super::*;

rclrs/src/worker.rs

@@ -367,7 +367,7 @@ impl<Payload: 'static + Send + Sync> WorkerState<Payload> {
         ServiceState::<T, Worker<Payload>>::create(
             options,
             callback.into_worker_service_callback(),
-            self.node.handle(),
+            &self.node,
             &self.commands,
         )
     }