Make context initialization and destruction more robust

rclrs/src/context.rs

@@ -11,10 +11,15 @@ use parking_lot::Mutex;
 
 impl Drop for rcl_context_t {
     fn drop(&mut self) {
-        // SAFETY: These functions have no preconditions besides a valid/initialized handle
         unsafe {
-            rcl_shutdown(self);
-            rcl_context_fini(self);
+            // The context may be invalid when rcl_init failed, e.g. because of invalid command
+            // line arguments.
+            // SAFETY: No preconditions for this function.
+            if rcl_context_is_valid(self) {
+                // SAFETY: These functions have no preconditions besides a valid handle
+                rcl_shutdown(self);
+                rcl_context_fini(self);
+            }
         }
     }
 }
@@ -54,47 +59,44 @@ impl Context {
     /// # Panics
     /// When there is an interior null byte in any of the args.
     pub fn new(args: impl IntoIterator<Item = String>) -> Result<Self, RclReturnCode> {
-        let context = Self {
-            // SAFETY: Getting a zero-initialized value is always safe
-            handle: Arc::new(Mutex::new(unsafe { rcl_get_zero_initialized_context() })),
-        };
+        // SAFETY: Getting a zero-initialized value is always safe
+        let mut rcl_context = unsafe { rcl_get_zero_initialized_context() };
         let cstring_args: Vec<CString> = args
             .into_iter()
             .map(|arg| CString::new(arg).unwrap())
             .collect();
         // Vector of pointers into cstring_args
         let c_args: Vec<*const c_char> = cstring_args.iter().map(|arg| arg.as_ptr()).collect();
-        // Scope for the handle
-        {
-            let handle = &mut *context.handle.lock();
-            unsafe {
-                // SAFETY: No preconditions for this function.
-                let allocator = rcutils_get_default_allocator();
-                // SAFETY: Getting a zero-initialized value is always safe.
-                let mut init_options = rcl_get_zero_initialized_init_options();
-                // SAFETY: Passing in a zero-initialized value is expected.
-                // In the case where this returns not ok, there's nothing to clean up.
-                rcl_init_options_init(&mut init_options, allocator).ok()?;
-                // SAFETY: This function does not store the ephemeral init_options and c_args
-                // pointers. Passing in a zero-initialized handle is expected.
-                let ret = rcl_init(
-                    c_args.len() as i32,
-                    if c_args.is_empty() {
-                        std::ptr::null()
-                    } else {
-                        c_args.as_ptr()
-                    },
-                    &init_options,
-                    handle,
-                );
-                // SAFETY: It's safe to pass in an initialized object.
-                // Early return will not leak memory, because this is the last fini function.
-                rcl_init_options_fini(&mut init_options).ok()?;
-                // Move the check after the last fini()
-                ret.ok()?;
-            }
+        unsafe {
+            // SAFETY: No preconditions for this function.
+            let allocator = rcutils_get_default_allocator();
+            // SAFETY: Getting a zero-initialized value is always safe.
+            let mut init_options = rcl_get_zero_initialized_init_options();
+            // SAFETY: Passing in a zero-initialized value is expected.
+            // In the case where this returns not ok, there's nothing to clean up.
+            rcl_init_options_init(&mut init_options, allocator).ok()?;
+            // SAFETY: This function does not store the ephemeral init_options and c_args
+            // pointers. Passing in a zero-initialized handle is expected.
+            let ret = rcl_init(
+                c_args.len() as i32,
+                if c_args.is_empty() {
+                    std::ptr::null()
+                } else {
+                    c_args.as_ptr()
+                },
+                &init_options,
+                &mut rcl_context,
+            )
+            .ok();
+            // SAFETY: It's safe to pass in an initialized object.
+            // Early return will not leak memory, because this is the last fini function.
+            rcl_init_options_fini(&mut init_options).ok()?;
+            // Move the check after the last fini()
+            ret?;
         }
-        Ok(context)
+        Ok(Self {
+            handle: Arc::new(Mutex::new(rcl_context)),
+        })
     }
 
     /// Creates a node.