Adds Guard Conditions

- Adds the Guard Condition struct encapsulating rcl_guard_condition_t.
- Adds optional callbacks and a trigger method to approximate the rclcpp
  implementation.
- Adds an `add_guard_condition` method to WaitSet to add the
  GuardCondition to the WaitSet

Author: christopherjreid

rclrs/src/wait.rs

@@ -24,7 +24,9 @@ use std::time::Duration;
 use std::vec::Vec;
 
 mod exclusivity_guard;
+mod guard_condition;
 use exclusivity_guard::*;
+pub use guard_condition::*;
 
 /// A struct for waiting on subscriptions and other waitable entities to become ready.
 pub struct WaitSet {
@@ -36,6 +38,8 @@ pub struct WaitSet {
     // even in the error case.
     subscriptions: Vec<ExclusivityGuard<Arc<dyn SubscriptionBase>>>,
     clients: Vec<ExclusivityGuard<Arc<dyn ClientBase>>>,
+    // The guard conditions that are currently registered in the wait set.
+    guard_conditions: Vec<ExclusivityGuard<Arc<GuardCondition>>>,
     services: Vec<ExclusivityGuard<Arc<dyn ServiceBase>>>,
 }
 
@@ -96,6 +100,7 @@ impl WaitSet {
             rcl_wait_set,
             _rcl_context_mtx: context.rcl_context_mtx.clone(),
             subscriptions: Vec::new(),
+            guard_conditions: Vec::new(),
             clients: Vec::new(),
             services: Vec::new(),
         })
@@ -107,6 +112,7 @@ impl WaitSet {
     /// [`WaitSet::new`].
     pub fn clear(&mut self) {
         self.subscriptions.clear();
+        self.guard_conditions.clear();
         self.clients.clear();
         self.services.clear();
         // This cannot fail – the rcl_wait_set_clear function only checks that the input handle is
@@ -150,6 +156,38 @@ impl WaitSet {
         Ok(())
     }
 
+    /// Adds a guard condition to the wait set.
+    ///
+    /// # Errors
+    /// - If the guard condition was already added to this wait set or another one,
+    ///   [`AlreadyAddedToWaitSet`][1] will be returned
+    /// - If the number of guard conditions in the wait set is larger than the
+    ///   capacity set in [`WaitSet::new`], [`WaitSetFull`][2] will be returned
+    ///
+    /// [1]: crate::RclrsError
+    /// [2]: crate::RclReturnCode
+    pub fn add_guard_condition(
+        &mut self,
+        guard_condition: Arc<GuardCondition>,
+    ) -> Result<(), RclrsError> {
+        let exclusive_guard_condition = ExclusivityGuard::new(
+            Arc::clone(&guard_condition),
+            Arc::clone(&guard_condition.in_use_by_wait_set),
+        )?;
+
+        unsafe {
+            // SAFETY: Safe if the wait set and guard condition are initialized
+            rcl_wait_set_add_guard_condition(
+                &mut self.rcl_wait_set,
+                &*guard_condition.rcl_guard_condition.lock().unwrap(),
+                std::ptr::null_mut(),
+            )
+            .ok()?;
+        }
+        self.guard_conditions.push(exclusive_guard_condition);
+        Ok(())
+    }
+
     /// Adds a client to the wait set.
     ///
     /// # Errors
@@ -266,6 +304,7 @@ impl WaitSet {
                     .push(Arc::clone(&subscription.waitable));
             }
         }
+
         for (i, client) in self.clients.iter().enumerate() {
             // SAFETY: The `clients` entry is an array of pointers, and this dereferencing is
             // equivalent to

rclrs/src/wait/guard_condition.rs

@@ -0,0 +1,156 @@
+use crate::rcl_bindings::*;
+use crate::{Context, RclrsError, ToResult};
+
+use std::sync::{atomic::AtomicBool, Arc, Mutex};
+
+/// A waitiable entity used for waking up a wait set manually
+///
+/// # Example
+/// ```
+/// # use rclrs::{Context, GuardCondition, WaitSet, RclrsError};
+/// # use std::sync::{Arc, atomic::Ordering};
+///
+/// let context = Context::new([])?;
+///
+/// let atomic_bool = Arc::new(std::sync::atomic::AtomicBool::new(false));
+/// let atomic_bool_for_closure = Arc::clone(&atomic_bool);
+///
+/// let gc = Arc::new(GuardCondition::new(
+///     &context,
+///     Some(Box::new(move || {
+///         atomic_bool_for_closure.store(true, Ordering::Relaxed);
+///     })),
+/// ));
+///
+/// let mut ws = WaitSet::new(0, 1, 0, 0, 0, 0, &context)?;
+/// ws.add_guard_condition(Arc::clone(&gc))?;
+///
+/// // Trigger the guard condition, firing the callback and waking the wait set being waited on, if any.
+/// gc.trigger()?;
+///
+/// // The provided callback has now been called.
+/// assert_eq!(atomic_bool.load(Ordering::Relaxed), true);
+///
+/// // The wait call will now immediately return.
+/// ws.wait(Some(std::time::Duration::from_millis(10)))?;
+///
+/// # Ok::<(), RclrsError>(())
+/// ```
+pub struct GuardCondition {
+    /// The rcl_guard_condition_t that this struct encapsulates.
+    pub(crate) rcl_guard_condition: Arc<Mutex<rcl_guard_condition_t>>,
+    /// An optional callback to call when this guard condition is triggered.
+    callback: Option<Box<dyn Fn() + Send + Sync>>,
+    /// A flag to indicate if this guard condition has already been assigned to a wait set.
+    pub(crate) in_use_by_wait_set: Arc<AtomicBool>,
+}
+
+impl Drop for GuardCondition {
+    fn drop(&mut self) {
+        unsafe {
+            // SAFETY: No precondition for this function (besides passing in a valid guard condition)
+            rcl_guard_condition_fini(&mut *self.rcl_guard_condition.lock().unwrap());
+        }
+    }
+}
+
+// SAFETY: rcl_guard_condition is the only member that doesn't implement Send, and it is designed to be accessed from other threads
+unsafe impl Send for rcl_guard_condition_t {}
+
+// SAFETY: The mutexes and atomic members ensure synchronized access to members, and the callback is reentrant
+unsafe impl Sync for GuardCondition {}
+
+impl GuardCondition {
+    /// Creates a new guard condition.
+    pub fn new(context: &Context, callback: Option<Box<dyn Fn() + Send + Sync>>) -> Arc<Self> {
+        // SAFETY: Getting a zero initialized value is always safe
+        let mut guard_condition = unsafe { rcl_get_zero_initialized_guard_condition() };
+        unsafe {
+            // SAFETY: The context must be valid, and the guard condition must be zero-initialized
+            rcl_guard_condition_init(
+                &mut guard_condition,
+                &mut *context.rcl_context_mtx.lock().unwrap(),
+                rcl_guard_condition_get_default_options(),
+            );
+        }
+
+        Arc::new(Self {
+            rcl_guard_condition: Arc::new(Mutex::new(guard_condition)),
+            callback,
+            in_use_by_wait_set: Arc::new(AtomicBool::new(false)),
+        })
+    }
+
+    /// Triggers this guard condition, activating the wait set, and calling the optionally assigned callback.
+    pub fn trigger(&self) -> Result<(), RclrsError> {
+        unsafe {
+            // SAFETY: The rcl_guard_condition_t is valid.
+            rcl_trigger_guard_condition(&mut *self.rcl_guard_condition.lock().unwrap()).ok()?;
+        }
+        if let Some(callback) = &self.callback {
+            callback();
+        }
+        Ok(())
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::WaitSet;
+    use std::sync::atomic::Ordering;
+
+    #[test]
+    fn test_guard_condition() -> Result<(), RclrsError> {
+        let context = Context::new([])?;
+
+        let atomic_bool = Arc::new(std::sync::atomic::AtomicBool::new(false));
+        let atomic_bool_for_closure = Arc::clone(&atomic_bool);
+
+        let gc = GuardCondition::new(
+            &context,
+            Some(Box::new(move || {
+                atomic_bool_for_closure.store(true, Ordering::Relaxed);
+            })),
+        );
+
+        gc.trigger()?;
+
+        assert_eq!(atomic_bool.load(Ordering::Relaxed), true);
+
+        Ok(())
+    }
+
+    #[test]
+    fn test_guard_condition_wait() -> Result<(), RclrsError> {
+        let context = Context::new([])?;
+
+        let atomic_bool = Arc::new(std::sync::atomic::AtomicBool::new(false));
+        let atomic_bool_for_closure = Arc::clone(&atomic_bool);
+
+        let gc = GuardCondition::new(
+            &context,
+            Some(Box::new(move || {
+                atomic_bool_for_closure.store(true, Ordering::Relaxed);
+            })),
+        );
+
+        let mut ws = WaitSet::new(0, 1, 0, 0, 0, 0, &context)?;
+        ws.add_guard_condition(Arc::clone(&gc))?;
+        gc.trigger()?;
+
+        assert_eq!(atomic_bool.load(Ordering::Relaxed), true);
+        ws.wait(Some(std::time::Duration::from_millis(10)))?;
+
+        Ok(())
+    }
+
+    fn assert_send<T: Send>() {}
+    fn assert_sync<T: Sync>() {}
+
+    #[test]
+    fn test_guard_condition_is_send_and_sync() {
+        assert_send::<GuardCondition>();
+        assert_sync::<GuardCondition>();
+    }
+}