Migrate subscription to use shared callback instead of an async task

Signed-off-by: Michael X. Grey <[email protected]>

Author: mxgrey

rclrs/src/parameter/service.rs

@@ -514,8 +514,6 @@ mod tests {
         dbg!();
         std::io::stdout().lock().flush().unwrap();
 
-        return Ok(());
-
         // Limit depth, namespaced parameter is not returned
         let callback_ran = Arc::new(AtomicBool::new(false));
         let callback_ran_inner = Arc::clone(&callback_ran);
@@ -793,6 +791,12 @@ mod tests {
         let request = SetParameters_Request {
             parameters: seq![undeclared_bool],
         };
+
+        // Clone test.node here so that we don't move the whole test bundle into
+        // the closure, which would cause the test node to be fully dropped
+        // after the closure is called.
+        let test_node = Arc::clone(&test.node);
+
         let promise = set_client
             .call_then(
                 &request,
@@ -801,7 +805,7 @@ mod tests {
                     // Setting the undeclared parameter is now allowed
                     assert!(response.results[0].successful);
                     assert_eq!(
-                        test.node.use_undeclared_parameters().get("undeclared_bool"),
+                        test_node.use_undeclared_parameters().get("undeclared_bool"),
                         Some(ParameterValue::Bool(true))
                     );
                     callback_ran_inner.store(true, Ordering::Release);

rclrs/src/subscription.rs

@@ -5,14 +5,12 @@ use std::{
 
 use rosidl_runtime_rs::{Message, RmwMessage};
 
-use futures::channel::mpsc::{unbounded, UnboundedSender, TrySendError};
-
 use crate::{
     error::ToResult,
     qos::QoSProfile,
     rcl_bindings::*,
     ExecutorCommands, NodeHandle, RclrsError, Waitable, Executable, ExecutableHandle,
-    ExecutableKind, GuardCondition, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX,
+    ExecutableKind, WaitableLifecycle, ENTITY_LIFECYCLE_MUTEX,
 };
 
 mod any_subscription_callback;
@@ -30,9 +28,6 @@ pub use message_info::*;
 mod readonly_loaned_message;
 pub use readonly_loaned_message::*;
 
-mod subscription_task;
-use subscription_task::*;
-
 /// Struct for receiving messages of type `T`.
 ///
 /// The only way to instantiate a subscription is via [`Node::create_subscription()`][2]
@@ -59,7 +54,7 @@ where
     ///
     /// Holding onto this sender will keep the subscription task alive. Once
     /// this sender is dropped, the subscription task will end itself.
-    action: UnboundedSender<SubscriptionAction<T>>,
+    callback: Arc<Mutex<AnySubscriptionCallback<T>>>,
     /// Holding onto this keeps the waiter for this subscription alive in the
     /// wait set of the executor.
     lifecycle: WaitableLifecycle,
@@ -91,9 +86,9 @@ where
     pub fn set_callback<Args>(
         &self,
         callback: impl SubscriptionCallback<T, Args>,
-    ) -> Result<(), TrySendError<SubscriptionAction<T>>> {
+    ) {
         let callback = callback.into_subscription_callback();
-        self.action.unbounded_send(SubscriptionAction::SetCallback(callback))
+        *self.callback.lock().unwrap() = callback;
     }
 
     /// Set the callback of this subscription, replacing the callback that was
@@ -103,9 +98,9 @@ where
     pub fn set_async_callback<Args>(
         &self,
         callback: impl SubscriptionAsyncCallback<T, Args>,
-    ) -> Result<(), TrySendError<SubscriptionAction<T>>> {
+    ) {
         let callback = callback.into_subscription_async_callback();
-        self.action.unbounded_send(SubscriptionAction::SetCallback(callback))
+        *self.callback.lock().unwrap() = callback;
     }
 
     /// Used by [`Node`][crate::Node] to create a new subscription.
@@ -116,40 +111,8 @@ where
         node_handle: &Arc<NodeHandle>,
         commands: &Arc<ExecutorCommands>,
     ) -> Result<Arc<Self>, RclrsError> {
-        let (sender, receiver) = unbounded();
-        let (subscription, waiter) = Self::new(
-            topic,
-            qos,
-            sender,
-            Arc::clone(&node_handle),
-            Arc::clone(commands.get_guard_condition()),
-        )?;
+        let callback = Arc::new(Mutex::new(callback));
 
-        commands.run(subscription_task(
-            callback,
-            receiver,
-            Arc::clone(&subscription.handle),
-            Arc::clone(&commands),
-        ));
-
-        commands.add_to_wait_set(waiter);
-
-        Ok(subscription)
-    }
-
-    /// Instantiate the Subscription.
-    fn new(
-        topic: &str,
-        qos: QoSProfile,
-        action: UnboundedSender<SubscriptionAction<T>>,
-        node_handle: Arc<NodeHandle>,
-        guard_condition: Arc<GuardCondition>,
-    ) -> Result<(Arc<Self>, Waitable), RclrsError>
-    // This uses pub(crate) visibility to avoid instantiating this struct outside
-    // [`Node::create_subscription`], see the struct's documentation for the rationale
-    where
-        T: Message,
-    {
         // SAFETY: Getting a zero-initialized value is always safe.
         let mut rcl_subscription = unsafe { rcl_get_zero_initialized_subscription() };
         let type_support =
@@ -186,35 +149,35 @@ where
 
         let handle = Arc::new(SubscriptionHandle {
             rcl_subscription: Mutex::new(rcl_subscription),
-            node_handle,
+            node_handle: Arc::clone(node_handle),
         });
 
-        let (waiter, lifecycle) = Waitable::new(
+        let (waitable, lifecycle) = Waitable::new(
             Box::new(SubscriptionExecutable {
                 handle: Arc::clone(&handle),
-                action: action.clone(),
+                callback: Arc::clone(&callback),
+                commands: Arc::clone(commands),
             }),
-            Some(guard_condition),
+            Some(Arc::clone(commands.get_guard_condition())),
         );
+        commands.add_to_wait_set(waitable);
 
-        let subscription = Arc::new(Self { handle, action, lifecycle });
-
-        Ok((subscription, waiter))
+        Ok(Arc::new(Self { handle, callback, lifecycle }))
     }
 }
 
 struct SubscriptionExecutable<T: Message> {
     handle: Arc<SubscriptionHandle>,
-    action: UnboundedSender<SubscriptionAction<T>>,
+    callback: Arc<Mutex<AnySubscriptionCallback<T>>>,
+    commands: Arc<ExecutorCommands>,
 }
 
 impl<T> Executable for SubscriptionExecutable<T>
 where
     T: Message,
 {
     fn execute(&mut self) -> Result<(), RclrsError> {
-        self.action.unbounded_send(SubscriptionAction::Execute).ok();
-        Ok(())
+        self.callback.lock().unwrap().execute(&self.handle, &self.commands)
     }
 
     fn kind(&self) -> crate::ExecutableKind {

rclrs/src/wait.rs

@@ -31,8 +31,8 @@ pub use waitable::*;
 
 /// A struct for waiting on subscriptions and other waitable entities to become ready.
 pub struct WaitSet {
-    entities: HashMap<ExecutableKind, Vec<Waitable>>,
-    pub(crate) handle: WaitSetHandle,
+    primitives: HashMap<ExecutableKind, Vec<Waitable>>,
+    handle: WaitSetHandle,
 }
 
 // SAFETY: While the rcl_wait_set_t does have some interior mutability (because it has
@@ -53,8 +53,8 @@ impl WaitSet {
             context_handle: Arc::clone(&context.handle),
         };
 
-        let mut wait_set = Self { entities: HashMap::new(), handle };
-        wait_set.register_rcl_entities()?;
+        let mut wait_set = Self { primitives: HashMap::new(), handle };
+        wait_set.register_rcl_primitives()?;
         Ok(wait_set)
     }
 
@@ -68,10 +68,10 @@ impl WaitSet {
                 return Err(RclrsError::AlreadyAddedToWaitSet);
             }
             let kind = entity.executable.kind();
-            self.entities.entry(kind).or_default().push(entity);
+            self.primitives.entry(kind).or_default().push(entity);
         }
         self.resize_rcl_containers()?;
-        self.register_rcl_entities()?;
+        self.register_rcl_primitives()?;
         Ok(())
     }
 
@@ -80,13 +80,8 @@ impl WaitSet {
     /// This effectively resets the wait set to the state it was in after being created by
     /// [`WaitSet::new`].
     pub fn clear(&mut self) {
-        self.entities.clear();
-        // This cannot fail – the rcl_wait_set_clear function only checks that the input handle is
-        // valid, which it always is in our case. Hence, only debug_assert instead of returning
-        // Result.
-        // SAFETY: No preconditions for this function (besides passing in a valid wait set).
-        let ret = unsafe { rcl_wait_set_clear(&mut self.handle.rcl_wait_set) };
-        debug_assert_eq!(ret, 0);
+        self.primitives.clear();
+        self.rcl_clear();
     }
 
     /// Blocks until the wait set is ready, or until the timeout has been exceeded.
@@ -147,13 +142,13 @@ impl WaitSet {
         }
 
         // Remove any waitables that are no longer being used
-        for waiter in self.entities.values_mut() {
-            waiter.retain(|w| w.in_use());
+        for waitable in self.primitives.values_mut() {
+            waitable.retain(|w| w.in_use());
         }
 
         // For the remaining entities, check if they were activated and then run
         // the callback for those that were.
-        for waiter in self.entities.values_mut().flat_map(|v| v) {
+        for waiter in self.primitives.values_mut().flat_map(|v| v) {
             if waiter.is_ready(&self.handle.rcl_wait_set) {
                 f(&mut *waiter.executable)?;
             }
@@ -168,16 +163,19 @@ impl WaitSet {
         // Note that self.clear() will not change the allocated size of each rcl
         // entity container, so we do not need to resize before re-registering
         // the rcl entities.
-        self.clear();
-        self.register_rcl_entities();
+        self.rcl_clear();
+        if let Err(err) = self.register_rcl_primitives() {
+            // TODO(@mxgrey): Log this error when logging is available
+            eprintln!("Error while registering rcl primitives: {err}");
+        }
 
         Ok(())
     }
 
     /// Get a count of the different kinds of entities in the wait set.
     pub fn count(&self) -> WaitableCount {
         let mut c = WaitableCount::new();
-        for (kind, collection) in &self.entities {
+        for (kind, collection) in &self.primitives {
             c.add(*kind, collection.len());
         }
         c
@@ -191,6 +189,18 @@ impl WaitSet {
         Ok(())
     }
 
+    /// Clear only the rcl_wait_set. This is done so that we can safely repopulate
+    /// it to perform another wait. This does not effect the entities that we
+    /// consider to still be in the wait set.
+    fn rcl_clear(&mut self) {
+        // This cannot fail – the rcl_wait_set_clear function only checks that the input handle is
+        // valid, which it always is in our case. Hence, only debug_assert instead of returning
+        // Result.
+        // SAFETY: No preconditions for this function (besides passing in a valid wait set).
+        let ret = unsafe { rcl_wait_set_clear(&mut self.handle.rcl_wait_set) };
+        debug_assert_eq!(ret, 0);
+    }
+
     /// Registers all the waitable entities with the rcl wait set.
     ///
     /// # Errors
@@ -199,8 +209,8 @@ impl WaitSet {
     ///   then there is a bug in rclrs.
     ///
     /// [1]: crate::RclReturnCode
-    fn register_rcl_entities(&mut self) -> Result<(), RclrsError> {
-        for entity in self.entities.values_mut().flat_map(|c| c) {
+    fn register_rcl_primitives(&mut self) -> Result<(), RclrsError> {
+        for entity in self.primitives.values_mut().flat_map(|c| c) {
             entity.add_to_wait_set(&mut self.handle.rcl_wait_set)?;
         }
         Ok(())