Add Arc and Mutex (#51)

* Initial refactor to make multithreadable
Distro A, OPSEC #4584. You may have additional rights; please see https://rosmilitary.org/faq/?category=ros-2-license

Signed-off-by: Jacob Hassold <[email protected]>

* Formatter pass
Distro A, OPSEC #4584. You may have additional rights; please see https://rosmilitary.org/faq/?category=ros-2-license

Signed-off-by: Jacob Hassold <[email protected]>

Author: jhdcs

rclrs/Cargo.toml

@@ -5,9 +5,10 @@ authors = ["Esteve Fernandez <[email protected]>"]
 edition = "2018"
 
 [dependencies]
+anyhow = {version = "1", features = ["backtrace"]}
 libc = "0.2.43"
+parking_lot = "0.11"
 thiserror = "1"
-anyhow = {version = "1", features = ["backtrace"]}
 
 [build-dependencies]
 bindgen = "0.59.1"

rclrs/build.rs

@@ -8,7 +8,9 @@ fn main() {
         .header("src/rcl_wrapper.h")
         .derive_copy(false)
         .size_t_is_usize(true)
-        .default_enum_style(bindgen::EnumVariation::Rust{non_exhaustive: false});
+        .default_enum_style(bindgen::EnumVariation::Rust {
+            non_exhaustive: false,
+        });
 
     let ament_prefix_var_name = "AMENT_PREFIX_PATH";
     let ament_prefix_var = env::var(ament_prefix_var_name);

rclrs/src/context.rs

@@ -1,49 +1,49 @@
-use crate::error::{RclError, ToResult};
-use crate::{Handle, Node};
+use crate::error::ToResult;
 use crate::rcl_bindings::*;
-use std::cell::{Ref, RefCell, RefMut};
+use crate::Node;
+use parking_lot::{Mutex, MutexGuard};
+use rclrs_common::error::RclError;
 use std::env;
 use std::ffi::CString;
 use std::os::raw::c_char;
-use std::rc::Rc;
-use anyhow::Result;
+use std::sync::Arc;
 
-pub struct ContextHandle(RefCell<rcl_context_t>);
+pub struct ContextHandle(Mutex<rcl_context_t>);
 
-impl<'a> Handle<rcl_context_t> for &'a ContextHandle {
-    type DerefT = Ref<'a, rcl_context_t>;
-    type DerefMutT = RefMut<'a, rcl_context_t>;
+impl ContextHandle {
+    pub fn get_mut(&mut self) -> &mut rcl_context_t {
+        self.0.get_mut()
+    }
 
-    fn get(self) -> Self::DerefT {
-        self.0.borrow()
+    pub fn lock(&self) -> MutexGuard<rcl_context_t> {
+        self.0.lock()
     }
 
-    fn get_mut(self) -> Self::DerefMutT {
-        self.0.borrow_mut()
+    pub fn try_lock(&self) -> Option<MutexGuard<rcl_context_t>> {
+        self.0.try_lock()
     }
 }
 
 impl Drop for ContextHandle {
     fn drop(&mut self) {
-        let handle = &mut *self.get_mut();
         unsafe {
-            rcl_shutdown(handle as *mut _);
+            rcl_shutdown(&mut *self.get_mut() as *mut _);
         }
     }
 }
 
 pub struct Context {
-    pub handle: Rc<ContextHandle>,
+    pub handle: Arc<ContextHandle>,
 }
 
 impl Context {
-    fn init(&mut self) -> Result<(), RclError> {
+    fn init(&self) -> Result<(), RclError> {
         let args: Vec<CString> = env::args()
             .filter_map(|arg| CString::new(arg).ok())
             .collect();
 
         let c_args: Vec<*const c_char> = args.iter().map(|arg| arg.as_ptr()).collect();
-        let handle = &mut *self.handle.get_mut();
+        let handle = &mut *self.handle.lock();
 
         unsafe {
             let allocator = rcutils_get_default_allocator();
@@ -62,9 +62,9 @@ impl Context {
         Ok(())
     }
 
-    pub fn ok(&self) -> bool {
-        let handle = &mut *self.handle.get_mut();
-        unsafe { rcl_context_is_valid(handle as *mut _) }
+    pub fn ok(&self) -> Result<bool, RclError> {
+        let handle = &mut *self.handle.lock();
+        unsafe { Ok(rcl_context_is_valid(handle as *mut _)) }
     }
 
     pub fn create_node(&self, node_name: &str) -> Result<Node, RclError> {
@@ -74,8 +74,8 @@ impl Context {
 
 impl Default for Context {
     fn default() -> Self {
-        let mut context = Self {
-            handle: Rc::new(ContextHandle(RefCell::new(unsafe {
+        let context = Self {
+            handle: Arc::new(ContextHandle(Mutex::new(unsafe {
                 rcl_get_zero_initialized_context()
             }))),
         };

rclrs/src/error.rs

@@ -1,7 +1,5 @@
-use anyhow::Result;
-
 use crate::rcl_bindings::*;
-pub use rclrs_common::error::{RclError, to_rcl_result};
+pub use rclrs_common::error::{to_rcl_result, RclError};
 
 pub(crate) trait ToResult {
     fn ok(&self) -> Result<(), RclError>;

rclrs/src/lib.rs

@@ -12,9 +12,8 @@ pub use self::node::*;
 pub use self::qos::*;
 
 use self::rcl_bindings::*;
-use std::ops::{Deref, DerefMut};
-use anyhow::Result;
 use rclrs_common::error::WaitSetError;
+use std::ops::{Deref, DerefMut};
 use wait::WaitSet;
 
 pub trait Handle<T> {
@@ -26,21 +25,21 @@ pub trait Handle<T> {
 }
 
 /// Wrapper around [`spin_once`]
-pub fn spin(node: &Node) -> Result<(), RclError> {
-    while unsafe { rcl_context_is_valid(&mut *node.context.get_mut() as *mut _) } {
+pub fn spin<'node>(node: &'node node::Node) -> Result<(), WaitSetError> {
+    while unsafe { rcl_context_is_valid(&mut *node.context.lock() as *mut _) } {
         if let Some(error) = spin_once(node, 500).err() {
             match error {
-                WaitSetError::DroppedSubscription => continue,
-                WaitSetError::RclError(RclError::Timeout) => continue,
-                WaitSetError::RclError(rclerr) => return Err(rclerr),
-            }
+                WaitSetError::DroppedSubscription | WaitSetError::RclError(RclError::Timeout) => {
+                    continue
+                }
+                error => return Err(error),
+            };
         }
     }
 
     Ok(())
 }
 
-
 /// Main function for waiting.
 ///
 /// Following is a schematic representation of the interation of [`spin_once`] with ROS RCL FFI
@@ -78,15 +77,15 @@ pub fn spin(node: &Node) -> Result<(), RclError> {
 ///         +--------------------+
 ///
 ///
-pub fn spin_once(node: &Node, timeout: i64) -> Result<(), WaitSetError> {
+pub fn spin_once<'node>(node: &'node Node, timeout: i64) -> Result<(), WaitSetError> {
     let number_of_subscriptions = node.subscriptions.len();
     let number_of_guard_conditions = 0;
     let number_of_timers = 0;
     let number_of_clients = 0;
     let number_of_services = 0;
     let number_of_events = 0;
 
-    let context = &mut *node.context.get_mut();
+    let context = &mut *node.context.lock();
 
     let mut wait_set = WaitSet::new(
         number_of_subscriptions,
@@ -95,13 +94,14 @@ pub fn spin_once(node: &Node, timeout: i64) -> Result<(), WaitSetError> {
         number_of_clients,
         number_of_services,
         number_of_events,
-        context)?;
+        context,
+    )?;
 
     for subscription in &node.subscriptions {
         match wait_set.add_subscription(subscription) {
             Ok(()) => (),
             Err(WaitSetError::DroppedSubscription) => (),
-            err => return err,
+            Err(err) => return Err(err),
         };
     }
 

rclrs/src/node/mod.rs

@@ -1,54 +1,54 @@
-use crate::{RclError, ToResult};
 use crate::qos::QoSProfile;
 use crate::rcl_bindings::*;
-use crate::{Context, ContextHandle, Handle};
-use std::cell::{Ref, RefCell, RefMut};
+use crate::ToResult;
+use crate::{Context, ContextHandle};
+use rclrs_common::error::RclError;
 use std::ffi::CString;
-use std::rc::{Rc, Weak};
-use anyhow::Result;
+use std::sync::{Arc, Weak};
+
+use parking_lot::{Mutex, MutexGuard};
 
 pub mod publisher;
 pub use self::publisher::*;
 pub mod subscription;
 pub use self::subscription::*;
 
-pub struct NodeHandle(RefCell<rcl_node_t>);
+pub struct NodeHandle(Mutex<rcl_node_t>);
 
-impl<'a> Handle<rcl_node_t> for &'a NodeHandle {
-    type DerefT = Ref<'a, rcl_node_t>;
-    type DerefMutT = RefMut<'a, rcl_node_t>;
+impl NodeHandle {
+    pub fn get_mut(&mut self) -> &mut rcl_node_t {
+        self.0.get_mut()
+    }
 
-    fn get(self) -> Self::DerefT {
-        self.0.borrow()
+    pub fn lock(&self) -> MutexGuard<rcl_node_t> {
+        self.0.lock()
     }
 
-    fn get_mut(self) -> Self::DerefMutT {
-        self.0.borrow_mut()
+    pub fn try_lock(&self) -> Option<MutexGuard<rcl_node_t>> {
+        self.0.try_lock()
     }
 }
 
 impl Drop for NodeHandle {
     fn drop(&mut self) {
         let handle = &mut *self.get_mut();
-        unsafe {
-            rcl_node_fini(handle as *mut _).unwrap();
-        }
+        unsafe { rcl_node_fini(handle as *mut _).unwrap() };
     }
 }
 
 pub struct Node {
-    handle: Rc<NodeHandle>,
-    pub(crate) context: Rc<ContextHandle>,
+    handle: Arc<NodeHandle>,
+    pub(crate) context: Arc<ContextHandle>,
     pub(crate) subscriptions: Vec<Weak<dyn SubscriptionBase>>,
 }
 
 impl Node {
     #[allow(clippy::new_ret_no_self)]
-    pub fn new(node_name: &str, context: &Context) -> Result<Node, RclError> {
+    pub fn new<'ctxt>(node_name: &str, context: &Context) -> Result<Node, RclError> {
         Self::new_with_namespace(node_name, "", context)
     }
 
-    pub fn new_with_namespace(
+    pub fn new_with_namespace<'ctxt>(
         node_name: &str,
         node_ns: &str,
         context: &Context,
@@ -57,7 +57,7 @@ impl Node {
         let raw_node_ns = CString::new(node_ns).unwrap();
 
         let mut node_handle = unsafe { rcl_get_zero_initialized_node() };
-        let context_handle = &mut *context.handle.get_mut();
+        let context_handle = &mut *context.handle.lock();
 
         unsafe {
             let node_options = rcl_node_get_default_options();
@@ -71,7 +71,7 @@ impl Node {
             .ok()?;
         }
 
-        let handle = Rc::new(NodeHandle(RefCell::new(node_handle)));
+        let handle = Arc::new(NodeHandle(Mutex::new(node_handle)));
 
         Ok(Node {
             handle,
@@ -81,7 +81,11 @@ impl Node {
     }
 
     // TODO: make publisher's lifetime depend on node's lifetime
-    pub fn create_publisher<T>(&self, topic: &str, qos: QoSProfile) -> Result<Publisher<T>, RclError>
+    pub fn create_publisher<T>(
+        &self,
+        topic: &str,
+        qos: QoSProfile,
+    ) -> Result<Publisher<T>, RclError>
     where
         T: rclrs_common::traits::MessageDefinition<T>,
     {
@@ -94,14 +98,14 @@ impl Node {
         topic: &str,
         qos: QoSProfile,
         callback: F,
-    ) -> Result<Rc<Subscription<T>>, RclError>
+    ) -> Result<Arc<Subscription<T>>, RclError>
     where
         T: rclrs_common::traits::MessageDefinition<T> + Default,
         F: FnMut(&T) + Sized + 'static,
     {
-        let subscription = Rc::new(Subscription::<T>::new(self, topic, qos, callback)?);
+        let subscription = Arc::new(Subscription::<T>::new(self, topic, qos, callback)?);
         self.subscriptions
-            .push(Rc::downgrade(&subscription) as Weak<dyn SubscriptionBase>);
+            .push(Arc::downgrade(&subscription) as Weak<dyn SubscriptionBase>);
         Ok(subscription)
     }
 }