Added support for Guard Conditions (#249)

* Added support for Guard Conditions

Author: christopherjreid

rclrs/CHANGELOG.rst

@@ -5,11 +5,13 @@ Changelog for package rclrs
 0.3
 ----------------
 * Loaned messages (zero-copy) (`#212 <https://github.com/ros2-rust/ros2_rust/pull/212>`_)
+* Graph queries (`#234 <https://github.com/ros2-rust/ros2_rust/pull/234>`_)
+* Guard conditions (`#249 <https://github.com/ros2-rust/ros2_rust/pull/249>`_)
 
 0.2 (2022-07-21)
 ----------------
 * First release
 * Build based on `colcon-ros-cargo`
 * Message generation packages `rosidl_generator_rs` and `rosidl_runtime_rs`
 * Publisher, Subscription, Client and Service
-* Tunable QoS settings
+* Tunable QoS settings

rclrs/src/lib.rs

@@ -49,13 +49,14 @@ pub use rcl_bindings::rmw_request_id_t;
 pub fn spin_once(node: &Node, timeout: Option<Duration>) -> Result<(), RclrsError> {
     let live_subscriptions = node.live_subscriptions();
     let live_clients = node.live_clients();
+    let live_guard_conditions = node.live_guard_conditions();
     let live_services = node.live_services();
     let ctx = Context {
         rcl_context_mtx: node.rcl_context_mtx.clone(),
     };
     let mut wait_set = WaitSet::new(
         live_subscriptions.len(),
-        0,
+        live_guard_conditions.len(),
         0,
         live_clients.len(),
         live_services.len(),
@@ -71,6 +72,10 @@ pub fn spin_once(node: &Node, timeout: Option<Duration>) -> Result<(), RclrsErro
         wait_set.add_client(live_client.clone())?;
     }
 
+    for live_guard_condition in &live_guard_conditions {
+        wait_set.add_guard_condition(live_guard_condition.clone())?;
+    }
+
     for live_service in &live_services {
         wait_set.add_service(live_service.clone())?;
     }

rclrs/src/node.rs

@@ -5,8 +5,9 @@ pub use self::graph::*;
 
 use crate::rcl_bindings::*;
 use crate::{
-    Client, ClientBase, Context, ParameterOverrideMap, Publisher, QoSProfile, RclrsError, Service,
-    ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, ToResult,
+    Client, ClientBase, Context, GuardCondition, ParameterOverrideMap, Publisher, QoSProfile,
+    RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback,
+    ToResult,
 };
 
 use std::cmp::PartialEq;
@@ -68,6 +69,7 @@ pub struct Node {
     pub(crate) rcl_node_mtx: Arc<Mutex<rcl_node_t>>,
     pub(crate) rcl_context_mtx: Arc<Mutex<rcl_context_t>>,
     pub(crate) clients: Vec<Weak<dyn ClientBase>>,
+    pub(crate) guard_conditions: Vec<Weak<GuardCondition>>,
     pub(crate) services: Vec<Weak<dyn ServiceBase>>,
     pub(crate) subscriptions: Vec<Weak<dyn SubscriptionBase>>,
     _parameter_map: ParameterOverrideMap,
@@ -189,6 +191,47 @@ impl Node {
         Ok(client)
     }
 
+    /// Creates a [`GuardCondition`][1] with no callback.
+    ///
+    /// A weak pointer to the `GuardCondition` is stored within this node.
+    /// When this node is added to a wait set (e.g. when calling `spin_once`[2]
+    /// with this node as an argument), the guard condition can be used to
+    /// interrupt the wait.
+    ///
+    /// [1]: crate::GuardCondition
+    /// [2]: crate::spin_once
+    pub fn create_guard_condition(&mut self) -> Arc<GuardCondition> {
+        let guard_condition = Arc::new(GuardCondition::new_with_rcl_context(
+            &mut self.rcl_context_mtx.lock().unwrap(),
+            None,
+        ));
+        self.guard_conditions
+            .push(Arc::downgrade(&guard_condition) as Weak<GuardCondition>);
+        guard_condition
+    }
+
+    /// Creates a [`GuardCondition`][1] with a callback.
+    ///
+    /// A weak pointer to the `GuardCondition` is stored within this node.
+    /// When this node is added to a wait set (e.g. when calling `spin_once`[2]
+    /// with this node as an argument), the guard condition can be used to
+    /// interrupt the wait.
+    ///
+    /// [1]: crate::GuardCondition
+    /// [2]: crate::spin_once
+    pub fn create_guard_condition_with_callback<F>(&mut self, callback: F) -> Arc<GuardCondition>
+    where
+        F: Fn() + Send + Sync + 'static,
+    {
+        let guard_condition = Arc::new(GuardCondition::new_with_rcl_context(
+            &mut self.rcl_context_mtx.lock().unwrap(),
+            Some(Box::new(callback) as Box<dyn Fn() + Send + Sync>),
+        ));
+        self.guard_conditions
+            .push(Arc::downgrade(&guard_condition) as Weak<GuardCondition>);
+        guard_condition
+    }
+
     /// Creates a [`Publisher`][1].
     ///
     /// [1]: crate::Publisher
@@ -254,6 +297,13 @@ impl Node {
         self.clients.iter().filter_map(Weak::upgrade).collect()
     }
 
+    pub(crate) fn live_guard_conditions(&self) -> Vec<Arc<GuardCondition>> {
+        self.guard_conditions
+            .iter()
+            .filter_map(Weak::upgrade)
+            .collect()
+    }
+
     pub(crate) fn live_services(&self) -> Vec<Arc<dyn ServiceBase>> {
         self.services.iter().filter_map(Weak::upgrade).collect()
     }

rclrs/src/node/builder.rs

@@ -276,6 +276,7 @@ impl NodeBuilder {
             rcl_node_mtx,
             rcl_context_mtx: self.context.clone(),
             clients: vec![],
+            guard_conditions: vec![],
             services: vec![],
             subscriptions: vec![],
             _parameter_map,

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>>>,
 }
 
@@ -45,6 +49,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 been triggered.
+    pub guard_conditions: Vec<Arc<GuardCondition>>,
     /// A list of services that have potentially received requests.
     pub services: Vec<Arc<dyn ServiceBase>>,
 }
@@ -105,6 +111,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(),
         })
@@ -116,6 +123,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
@@ -159,6 +167,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
@@ -262,6 +302,7 @@ impl WaitSet {
         let mut ready_entities = ReadyEntities {
             subscriptions: Vec::new(),
             clients: Vec::new(),
+            guard_conditions: Vec::new(),
             services: Vec::new(),
         };
         for (i, subscription) in self.subscriptions.iter().enumerate() {
@@ -275,6 +316,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
@@ -284,6 +326,19 @@ impl WaitSet {
                 ready_entities.clients.push(Arc::clone(&client.waitable));
             }
         }
+
+        for (i, guard_condition) in self.guard_conditions.iter().enumerate() {
+            // SAFETY: The `clients` 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, service) in self.services.iter().enumerate() {
             // SAFETY: The `services` entry is an array of pointers, and this dereferencing is
             // equivalent to
@@ -309,4 +364,20 @@ mod tests {
         assert_send::<WaitSet>();
         assert_sync::<WaitSet>();
     }
+
+    #[test]
+    fn guard_condition_in_wait_set_readies() -> Result<(), RclrsError> {
+        let context = Context::new([])?;
+
+        let guard_condition = Arc::new(GuardCondition::new(&context));
+
+        let mut wait_set = WaitSet::new(0, 1, 0, 0, 0, 0, &context)?;
+        wait_set.add_guard_condition(Arc::clone(&guard_condition))?;
+        guard_condition.trigger()?;
+
+        let readies = wait_set.wait(Some(std::time::Duration::from_millis(10)))?;
+        assert!(readies.guard_conditions.contains(&guard_condition));
+
+        Ok(())
+    }
 }