diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index ecf4c379..e5fb1087 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -13,6 +13,10 @@ use crate::{ ReadyKind, ServiceInfo, Waitable, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX, }; +// The API for service introspection was added in Jazzy. +#[cfg(not(ros_distro = "humble"))] +use crate::ServiceIntrospectionState; + mod client_async_callback; pub use client_async_callback::*; @@ -369,6 +373,43 @@ 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 + // The API for service introspection was added in Jazzy. + #[cfg(not(ros_distro = "humble"))] + 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 ed436df2..a48ad42f 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -642,7 +642,7 @@ impl NodeState { ServiceState::::create( options, callback.into_node_service_callback(), - &self.handle, + self, self.commands.async_worker_commands(), ) } @@ -723,7 +723,7 @@ impl NodeState { /// # Ok::<(), RclrsError>(()) /// ``` pub fn create_async_service<'a, T, Args>( - &self, + self: &Arc, options: impl Into>, callback: impl IntoAsyncServiceCallback, ) -> Result, RclrsError> @@ -733,7 +733,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 1450c272..f5e43bae 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -8,9 +8,10 @@ use std::{ use rosidl_runtime_rs::{Message, Service as ServiceIDL}; use crate::{ - error::ToResult, rcl_bindings::*, IntoPrimitiveOptions, MessageCow, Node, NodeHandle, - QoSProfile, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, ReadyKind, - Waitable, WaitableLifecycle, WorkScope, Worker, WorkerCommands, ENTITY_LIFECYCLE_MUTEX, + error::ToResult, rcl_bindings::*, Clock, IntoPrimitiveOptions, MessageCow, Node, NodeHandle, + NodeState, QoSProfile, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError, + ReadyKind, Waitable, WaitableLifecycle, WorkScope, Worker, WorkerCommands, + ENTITY_LIFECYCLE_MUTEX, }; mod any_service_callback; @@ -101,7 +102,7 @@ where pub(crate) fn create<'a>( options: impl Into>, callback: AnyServiceCallback, - node_handle: &Arc, + node: &NodeState, commands: &Arc, ) -> Result, RclrsError> { let ServiceOptions { name, qos } = options.into(); @@ -120,7 +121,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 +144,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 +166,43 @@ 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 + // The API for service introspection was added in Jazzy. + #[cfg(not(ros_distro = "humble"))] + 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 +321,7 @@ unsafe impl Send for rcl_service_t {} pub struct ServiceHandle { rcl_service: Mutex, node_handle: Arc, + clock: Clock, } impl ServiceHandle { @@ -391,6 +431,55 @@ impl Drop for ServiceHandle { } } +/// The possible service introspection configurations +// The API for service introspection was added in Jazzy. +#[cfg(not(ros_distro = "humble"))] +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum ServiceIntrospectionState { + /// Disable service introspection + Off, + /// Enable service introspection with metadata only + Metadata, + /// Enable service introspection with metadata and contents + Contents, +} + +// The API for service introspection was added in Jazzy. +#[cfg(not(ros_distro = "humble"))] +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 + } + } + } +} + +// The API for service introspection was added in Jazzy. +#[cfg(not(ros_distro = "humble"))] +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 73a3b09d..6e390129 100644 --- a/rclrs/src/worker.rs +++ b/rclrs/src/worker.rs @@ -428,7 +428,7 @@ impl WorkerState { ServiceState::>::create( options, callback.into_worker_service_callback(), - self.node.handle(), + &self.node, &self.commands, ) }