diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index 461c5357..e71657e7 100644 --- a/rclrs/src/client.rs +++ b/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 = ::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 diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index abb69f97..04c6513d 100644 --- a/rclrs/src/node.rs +++ b/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, options: impl Into>, callback: impl IntoNodeServiceCallback, ) -> Result, RclrsError> @@ -590,7 +590,7 @@ impl NodeState { ServiceState::::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, options: impl Into>, callback: impl IntoAsyncServiceCallback, ) -> Result, RclrsError> @@ -681,7 +681,7 @@ impl NodeState { ServiceState::::create( options, callback.into_async_service_callback(), - &self.handle, + self, self.commands.async_worker_commands(), ) } diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index 596ef1f8..e469388e 100644 --- a/rclrs/src/service.rs +++ b/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>, callback: AnyServiceCallback, - node_handle: &Arc, + node: &Node, commands: &Arc, ) -> Result, 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 = ::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 ServiceState { @@ -282,6 +318,7 @@ unsafe impl Send for rcl_service_t {} pub struct ServiceHandle { rcl_service: Mutex, node_handle: Arc, + 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 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 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::*; diff --git a/rclrs/src/worker.rs b/rclrs/src/worker.rs index 5c5e3f24..94afaecd 100644 --- a/rclrs/src/worker.rs +++ b/rclrs/src/worker.rs @@ -367,7 +367,7 @@ impl WorkerState { ServiceState::>::create( options, callback.into_worker_service_callback(), - self.node.handle(), + &self.node, &self.commands, ) }