diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index f0f8f05e5..b308f1de2 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -43,7 +43,7 @@ impl Drop for ClientHandle { // SAFETY: The entity lifecycle mutex is locked to protect against the risk of // global variables in the rmw implementation being unsafely modified during cleanup. unsafe { - rcl_client_fini(rcl_client, &mut **rcl_node); + rcl_client_fini(rcl_client, &mut *rcl_node); } } } @@ -115,7 +115,7 @@ where unsafe { rcl_client_init( &mut rcl_client, - &**rcl_node, + &*rcl_node, type_support, topic_c_string.as_ptr(), &client_options, @@ -263,7 +263,7 @@ where pub fn service_is_ready(&self) -> Result { let mut is_ready = false; let client = &mut *self.handle.rcl_client.lock().unwrap(); - let node = &mut **self.handle.node_handle.rcl_node.lock().unwrap(); + let node = &mut *self.handle.node_handle.rcl_node.lock().unwrap(); unsafe { // SAFETY both node and client are guaranteed to be valid here diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 0d141721c..90e229258 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -5,7 +5,7 @@ use std::{ ffi::CStr, fmt, os::raw::c_char, - sync::{Arc, Mutex, Weak}, + sync::{Arc, Mutex, Weak, atomic::AtomicBool}, vec::Vec, }; @@ -79,19 +79,37 @@ pub struct Node { /// /// [1]: pub(crate) struct NodeHandle { - pub(crate) rcl_node: Mutex>, + pub(crate) rcl_node: Mutex, pub(crate) context_handle: Arc, + /// In the humbe distro, rcl is sensitive to the address of the rcl_node_t + /// object being moved (this issue seems to be gone in jazzy), so we need + /// to initialize the rcl_node_t in-place inside this struct. In the event + /// that the initialization fails (e.g. it was created with an invalid name) + /// we need to make sure that we do not call rcl_node_fini on it while + /// dropping the NodeHandle, so we keep track of successful initialization + /// with this variable. + /// + /// We may be able to restructure this in the future when we no longer need + /// to support Humble. + pub(crate) initialized: AtomicBool, } impl Drop for NodeHandle { fn drop(&mut self) { + if !self.initialized.load(std::sync::atomic::Ordering::Acquire) { + // The node was not correctly initialized, e.g. it was created with + // an invalid name, so we must not try to finalize it or else we + // will get undefined behavior. + return; + } + let _context_lock = self.context_handle.rcl_context.lock().unwrap(); let mut rcl_node = self.rcl_node.lock().unwrap(); let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); // SAFETY: The entity lifecycle mutex is locked to protect against the risk of // global variables in the rmw implementation being unsafely modified during cleanup. - unsafe { rcl_node_fini(&mut **rcl_node) }; + unsafe { rcl_node_fini(&mut *rcl_node) }; } } @@ -376,7 +394,7 @@ impl Node { let mut domain_id: usize = 0; let ret = unsafe { // SAFETY: No preconditions for this function. - rcl_node_get_domain_id(&**rcl_node, &mut domain_id) + rcl_node_get_domain_id(&*rcl_node, &mut domain_id) }; debug_assert_eq!(ret, 0); @@ -451,7 +469,7 @@ impl Node { pub fn logger_name(&self) -> &str { let rcl_node = self.handle.rcl_node.lock().unwrap(); // SAFETY: No pre-conditions for this function - let name_raw_ptr = unsafe { rcl_node_get_logger_name(&**rcl_node) }; + let name_raw_ptr = unsafe { rcl_node_get_logger_name(&*rcl_node) }; if name_raw_ptr.is_null() { return ""; } diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs index 14d4dae27..10787c5cf 100644 --- a/rclrs/src/node/builder.rs +++ b/rclrs/src/node/builder.rs @@ -1,6 +1,6 @@ use std::{ ffi::CString, - sync::{Arc, Mutex}, + sync::{Arc, Mutex, atomic::AtomicBool}, }; use crate::{ @@ -276,8 +276,13 @@ impl NodeBuilder { let rcl_node_options = self.create_rcl_node_options()?; let rcl_context = &mut *self.context.rcl_context.lock().unwrap(); - // SAFETY: Getting a zero-initialized value is always safe. - let mut rcl_node = Box::new(unsafe { rcl_get_zero_initialized_node() }); + let handle = Arc::new(NodeHandle { + // SAFETY: Getting a zero-initialized value is always safe. + rcl_node: Mutex::new(unsafe { rcl_get_zero_initialized_node() }), + context_handle: Arc::clone(&self.context), + initialized: AtomicBool::new(false), + }); + unsafe { // SAFETY: // * The rcl_node is zero-initialized as mandated by this function. @@ -287,7 +292,7 @@ impl NodeBuilder { // global variables in the rmw implementation being unsafely modified during cleanup. let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); rcl_node_init( - &mut *rcl_node, + &mut *handle.rcl_node.lock().unwrap(), node_name.as_ptr(), node_namespace.as_ptr(), rcl_context, @@ -296,10 +301,8 @@ impl NodeBuilder { .ok()?; }; - let handle = Arc::new(NodeHandle { - rcl_node: Mutex::new(rcl_node), - context_handle: Arc::clone(&self.context), - }); + handle.initialized.store(true, std::sync::atomic::Ordering::Release); + let parameter = { let rcl_node = handle.rcl_node.lock().unwrap(); ParameterInterface::new( diff --git a/rclrs/src/node/graph.rs b/rclrs/src/node/graph.rs index e18675a21..343fa61d3 100644 --- a/rclrs/src/node/graph.rs +++ b/rclrs/src/node/graph.rs @@ -139,7 +139,7 @@ impl Node { unsafe { let rcl_node = self.handle.rcl_node.lock().unwrap(); rcl_get_topic_names_and_types( - &**rcl_node, + &*rcl_node, &mut rcutils_get_default_allocator(), false, &mut rcl_names_and_types, @@ -169,7 +169,7 @@ impl Node { unsafe { let rcl_node = self.handle.rcl_node.lock().unwrap(); rcl_get_node_names( - &**rcl_node, + &*rcl_node, rcutils_get_default_allocator(), &mut rcl_names, &mut rcl_namespaces, @@ -217,7 +217,7 @@ impl Node { unsafe { let rcl_node = self.handle.rcl_node.lock().unwrap(); rcl_get_node_names_with_enclaves( - &**rcl_node, + &*rcl_node, rcutils_get_default_allocator(), &mut rcl_names, &mut rcl_namespaces, @@ -266,7 +266,7 @@ impl Node { // SAFETY: The topic_name string was correctly allocated previously unsafe { let rcl_node = self.handle.rcl_node.lock().unwrap(); - rcl_count_publishers(&**rcl_node, topic_name.as_ptr(), &mut count).ok()? + rcl_count_publishers(&*rcl_node, topic_name.as_ptr(), &mut count).ok()? }; Ok(count) } @@ -282,7 +282,7 @@ impl Node { // SAFETY: The topic_name string was correctly allocated previously unsafe { let rcl_node = self.handle.rcl_node.lock().unwrap(); - rcl_count_subscribers(&**rcl_node, topic_name.as_ptr(), &mut count).ok()? + rcl_count_subscribers(&*rcl_node, topic_name.as_ptr(), &mut count).ok()? }; Ok(count) } @@ -333,7 +333,7 @@ impl Node { unsafe { let rcl_node = self.handle.rcl_node.lock().unwrap(); getter( - &**rcl_node, + &*rcl_node, &mut rcutils_get_default_allocator(), node_name.as_ptr(), node_namespace.as_ptr(), @@ -369,7 +369,7 @@ impl Node { unsafe { let rcl_node = self.handle.rcl_node.lock().unwrap(); getter( - &**rcl_node, + &*rcl_node, &mut rcutils_get_default_allocator(), topic.as_ptr(), false, diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index f8427a0cc..d9e55715d 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -10,7 +10,10 @@ pub use value::*; use crate::vendor::rcl_interfaces::msg::rmw::{ParameterType, ParameterValue as RmwParameterValue}; -use crate::{call_string_getter_with_rcl_node, rcl_bindings::*, Node, RclrsError}; +use crate::{ + call_string_getter_with_rcl_node, rcl_bindings::*, Node, RclrsError, + ENTITY_LIFECYCLE_MUTEX, +}; use std::{ collections::{btree_map::Entry, BTreeMap}, fmt::Debug, @@ -760,6 +763,7 @@ impl ParameterInterface { global_arguments: &rcl_arguments_t, ) -> Result { let override_map = unsafe { + let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); let fqn = call_string_getter_with_rcl_node(rcl_node, rcl_node_get_fully_qualified_name); resolve_parameter_overrides(&fqn, node_arguments, global_arguments)? }; diff --git a/rclrs/src/publisher.rs b/rclrs/src/publisher.rs index 54273bc19..2935ca322 100644 --- a/rclrs/src/publisher.rs +++ b/rclrs/src/publisher.rs @@ -38,7 +38,7 @@ impl Drop for PublisherHandle { // SAFETY: The entity lifecycle mutex is locked to protect against the risk of // global variables in the rmw implementation being unsafely modified during cleanup. unsafe { - rcl_publisher_fini(self.rcl_publisher.get_mut().unwrap(), &mut **rcl_node); + rcl_publisher_fini(self.rcl_publisher.get_mut().unwrap(), &mut *rcl_node); } } } @@ -111,7 +111,7 @@ where // variables in the rmw implementation being unsafely modified during cleanup. rcl_publisher_init( &mut rcl_publisher, - &**rcl_node, + &*rcl_node, type_support_ptr, topic_c_string.as_ptr(), &publisher_options, diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index ac8d1a2a6..ac43e51a8 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -41,7 +41,7 @@ impl Drop for ServiceHandle { // SAFETY: The entity lifecycle mutex is locked to protect against the risk of // global variables in the rmw implementation being unsafely modified during cleanup. unsafe { - rcl_service_fini(rcl_service, &mut **rcl_node); + rcl_service_fini(rcl_service, &mut *rcl_node); } } } @@ -116,7 +116,7 @@ where // variables in the rmw implementation being unsafely modified during initialization. rcl_service_init( &mut rcl_service, - &**rcl_node, + &*rcl_node, type_support, topic_c_string.as_ptr(), &service_options as *const _, diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 47d93de1a..36c241d19 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -49,7 +49,7 @@ impl Drop for SubscriptionHandle { // SAFETY: The entity lifecycle mutex is locked to protect against the risk of // global variables in the rmw implementation being unsafely modified during cleanup. unsafe { - rcl_subscription_fini(rcl_subscription, &mut **rcl_node); + rcl_subscription_fini(rcl_subscription, &mut *rcl_node); } } } @@ -129,7 +129,7 @@ where // variables in the rmw implementation being unsafely modified during cleanup. rcl_subscription_init( &mut rcl_subscription, - &**rcl_node, + &*rcl_node, type_support, topic_c_string.as_ptr(), &subscription_options,