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

@@ -26,6 +26,9 @@ use std::vec::Vec;
 mod exclusivity_guard;
 use exclusivity_guard::*;
 
+mod guard_condition;
+use guard_condition::*;
+
 /// A struct for waiting on subscriptions and other waitable entities to become ready.
 pub struct WaitSet {
     rcl_wait_set: rcl_wait_set_t,
@@ -36,6 +39,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<Mutex<GuardCondition>>>>,
     services: Vec<ExclusivityGuard<Arc<dyn ServiceBase>>>,
 }
 
@@ -45,6 +50,8 @@ pub struct ReadyEntities {
     pub subscriptions: Vec<Arc<dyn SubscriptionBase>>,
     /// A list of clients that have potentially received responses.
     pub clients: Vec<Arc<dyn ClientBase>>,
+    /// A list of guard conditions that have potentially been triggered.
+    pub guard_conditions: Vec<Arc<Mutex<GuardCondition>>>,
     /// A list of services that have potentially received requests.
     pub services: Vec<Arc<dyn ServiceBase>>,
 }
@@ -96,6 +103,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 +115,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 +159,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 condition 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<Mutex<GuardCondition>>,
+    ) -> Result<(), RclrsError> {
+        let exclusive_guard_condition = ExclusivityGuard::new(
+            Arc::clone(&guard_condition),
+            Arc::clone(&guard_condition.lock().unwrap().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.lock().unwrap().lock(),
+                std::ptr::null_mut(),
+            )
+            .ok()?;
+        }
+        self.guard_conditions.push(exclusive_guard_condition);
+        Ok(())
+    }
+
     /// Adds a client to the wait set.
     ///
     /// # Errors
@@ -252,6 +293,7 @@ impl WaitSet {
         unsafe { rcl_wait(&mut self.rcl_wait_set, timeout_ns) }.ok()?;
         let mut ready_entities = ReadyEntities {
             subscriptions: Vec::new(),
+            guard_conditions: Vec::new(),
             clients: Vec::new(),
             services: Vec::new(),
         };
@@ -266,6 +308,19 @@ impl WaitSet {
                     .push(Arc::clone(&subscription.waitable));
             }
         }
+
+        for (i, guard_condition) in self.guard_conditions.iter().enumerate() {
+            // SAFETY: The `guard_conditions` entry is an array of pointers, and this dereferencing is
+            // equivalent to
+            // https://github.com/ros2/rcl/blob/35a31b00a12f259d492bf53c0701003bd7f1745c/rcl/include/rcl/wait.h#L419
+            let wait_set_entry = unsafe { *self.rcl_wait_set.guard_conditions.add(i) };
+            if !wait_set_entry.is_null() {
+                ready_entities
+                    .guard_conditions
+                    .push(Arc::clone(&guard_condition.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,133 @@
+use crate::rcl_bindings::*;
+use crate::{Context, RclrsError, ToResult};
+
+use std::sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard};
+
+/// A struct for encapsulating a guard condition - a waitable trigger
+pub struct GuardCondition {
+    /// The rcl_guard_condition_t that this struct encapsulates.
+    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(usize)>>,
+    /// 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>,
+    /// A count for the number of times this guard condition was triggered, but no callback was assigned.
+    unread_count: usize,
+}
+
+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());
+        }
+    }
+}
+
+impl GuardCondition {
+    /// Creates a new guard condition
+    pub fn new(context: &Context) -> Self {
+        let mut guard_condition = unsafe { rcl_get_zero_initialized_guard_condition() };
+        unsafe {
+            // SAFETY: The context must be valid. No other preconditions for this function.
+            rcl_guard_condition_init(
+                &mut guard_condition,
+                &mut *context.rcl_context_mtx.lock().unwrap(),
+                rcl_guard_condition_get_default_options(),
+            );
+        }
+
+        Self {
+            rcl_guard_condition: Arc::new(Mutex::new(guard_condition)),
+            callback: None,
+            in_use_by_wait_set: Arc::new(AtomicBool::new(false)),
+            unread_count: 0,
+        }
+    }
+
+    /// Locks the underlying guard condition and returns it.
+    pub fn lock(&self) -> MutexGuard<rcl_guard_condition_t> {
+        self.rcl_guard_condition.lock().unwrap()
+    }
+
+    /// Sets the callback to call when this guard condition is triggered.
+    pub fn set_on_trigger_callback(&mut self, callback: Option<Box<dyn Fn(usize)>>) {
+        match callback {
+            Some(callback) => {
+                if self.unread_count > 0 {
+                    callback(self.unread_count);
+                    self.unread_count = 0;
+                }
+                self.callback = Some(callback);
+            }
+            None => {
+                self.callback = None;
+                self.unread_count = 0;
+            }
+        }
+    }
+
+    /// Triggers this guard condition, activating the wait set, and calling the optionally assigned callback.
+    pub fn trigger(&mut self) -> Result<(), RclrsError> {
+        unsafe {
+            // SAFETY: The rcl_guard_condition_t is valid.
+            rcl_trigger_guard_condition(&mut *self.rcl_guard_condition.lock().unwrap()).ok()?;
+        }
+        match &self.callback {
+            Some(callback) => callback(1),
+            None => self.unread_count += 1,
+        }
+        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 mut gc = GuardCondition::new(&context);
+
+        let atomic_usize = Arc::new(std::sync::atomic::AtomicUsize::new(0));
+        let atomic_usize_for_closure = Arc::clone(&atomic_usize);
+
+        gc.set_on_trigger_callback(Some(Box::new(move |count| {
+            atomic_usize_for_closure.store(count, Ordering::Relaxed);
+        })));
+
+        gc.trigger()?;
+
+        assert_eq!(atomic_usize.load(Ordering::Relaxed), 1);
+
+        Ok(())
+    }
+
+    #[test]
+    fn test_guard_condition_wait() -> Result<(), RclrsError> {
+        let context = Context::new([])?;
+        let gc = Arc::new(Mutex::new(GuardCondition::new(&context)));
+
+        let atomic_usize = Arc::new(std::sync::atomic::AtomicUsize::new(0));
+        let atomic_usize_for_closure = Arc::clone(&atomic_usize);
+
+        gc.lock()
+            .unwrap()
+            .set_on_trigger_callback(Some(Box::new(move |count| {
+                atomic_usize_for_closure.store(count, Ordering::Relaxed);
+            })));
+
+        let mut ws = WaitSet::new(0, 1, 0, 0, 0, 0, &context)?;
+        ws.add_guard_condition(Arc::clone(&gc))?;
+        gc.lock().unwrap().trigger()?;
+
+        assert_eq!(atomic_usize.load(Ordering::Relaxed), 1);
+        let wait_result = ws.wait(Some(std::time::Duration::from_millis(10)))?;
+        assert_eq!(wait_result.guard_conditions.len(), 1);
+
+        Ok(())
+    }
+}