Finished and tested basic features of Worker

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

Author: mxgrey

rclrs/src/executor.rs

@@ -1,7 +1,7 @@
 mod basic_executor;
 pub use self::basic_executor::*;
 
-use crate::{Context, ContextHandle, GuardCondition, IntoNodeOptions, Node, RclrsError, Waitable};
+use crate::{WeakActivityListener, Context, ContextHandle, GuardCondition, IntoNodeOptions, Node, RclrsError, Waitable};
 pub use futures::channel::oneshot::Receiver as Promise;
 use futures::{
     channel::oneshot,
@@ -210,6 +210,23 @@ impl ExecutorCommands {
         receiver
     }
 
+    /// Pass in a promise to get a second promise that will notify when the main
+    /// promise is fulfilled. This second promise can be passed into
+    /// [`SpinOptions::until_promise_resolved`].
+    pub fn create_notice<Out>(&self, promise: Promise<Out>) -> (Promise<Out>, Promise<()>)
+    where
+        Out: 'static + Send,
+    {
+        let (main_sender, main_receiver) = oneshot::channel();
+        let notice_receiver = self.run(async move {
+            if let Ok(out) = promise.await {
+                main_sender.send(out).ok();
+            }
+        });
+
+        (main_receiver, notice_receiver)
+    }
+
     /// Get the context that the executor is associated with.
     pub fn context(&self) -> &Context {
         &self.context
@@ -282,6 +299,10 @@ impl WorkerCommands {
         self.channel.send_payload_task(task);
     }
 
+    pub(crate) fn add_activity_listener(&self, listener: WeakActivityListener) {
+        self.channel.add_activity_listener(listener);
+    }
+
     /// Get a guard condition that can be used to wake up the wait set of the executor.
     pub(crate) fn get_guard_condition(&self) -> &Arc<GuardCondition> {
         &self.wakeup_wait_set
@@ -298,6 +319,9 @@ pub trait WorkerChannel: Send + Sync {
 
     /// Send a one-time task for the worker to run with its payload.
     fn send_payload_task(&self, f: PayloadTask);
+
+    /// Send something to listen to worker activity.
+    fn add_activity_listener(&self, listener: WeakActivityListener);
 }
 
 /// Encapsulates a task that can operate on the payload of a worker

rclrs/src/executor/basic_executor.rs

@@ -16,7 +16,7 @@ use std::{
 };
 
 use crate::{
-    ExecutorChannel, ExecutorRuntime, SpinConditions, WorkerChannel,
+    WeakActivityListener, ExecutorChannel, ExecutorRuntime, SpinConditions, WorkerChannel,
     RclrsError, WaitSetRunner, WaitSetRunConditions, Waitable, log_warn, log_fatal, ToLogParams,
     GuardCondition, ExecutorWorkerOptions, PayloadTask,
 };
@@ -280,6 +280,7 @@ impl ExecutorChannel for BasicExecutorChannel {
         let runner = WaitSetRunner::new(options);
         let waitable_sender = runner.waitable_sender();
         let payload_task_sender = runner.payload_task_sender();
+        let activity_listeners = runner.activity_listeners();
 
         if let Err(err) = self.new_worker_sender.unbounded_send(runner) {
             log_fatal!(
@@ -292,6 +293,7 @@ impl ExecutorChannel for BasicExecutorChannel {
             waitable_sender,
             task_sender: self.task_sender.clone(),
             payload_task_sender,
+            activity_listeners,
         })
     }
 
@@ -304,6 +306,7 @@ struct BasicWorkerChannel {
     task_sender: TaskSender,
     waitable_sender: UnboundedSender<Waitable>,
     payload_task_sender: UnboundedSender<PayloadTask>,
+    activity_listeners: Arc<Mutex<Vec<WeakActivityListener>>>,
 }
 
 impl WorkerChannel for BasicWorkerChannel {
@@ -319,6 +322,10 @@ impl WorkerChannel for BasicWorkerChannel {
     fn send_payload_task(&self, f: PayloadTask) {
         self.payload_task_sender.unbounded_send(f).ok();
     }
+
+    fn add_activity_listener(&self, listener: WeakActivityListener) {
+        self.activity_listeners.lock().unwrap().push(listener);
+    }
 }
 
 #[derive(Clone)]

rclrs/src/service/worker_service_callback.rs

@@ -36,6 +36,7 @@ where
         any_payload: &mut dyn Any,
     ) -> Result<(), RclrsError> {
         let Some(payload) = any_payload.downcast_mut::<Payload>() else {
+            dbg!();
             return Err(RclrsError::InvalidPayload {
                 expected: std::any::TypeId::of::<Payload>(),
                 received: (*any_payload).type_id(),

rclrs/src/subscription/worker_subscription_callback.rs

@@ -40,6 +40,7 @@ impl<T: Message, Payload: 'static> WorkerSubscriptionCallback<T, Payload> {
         any_payload: &mut dyn Any,
     ) -> Result<(), RclrsError> {
         let Some(payload) = any_payload.downcast_mut::<Payload>() else {
+            dbg!();
             return Err(RclrsError::InvalidPayload {
                 expected: std::any::TypeId::of::<Payload>(),
                 received: (*any_payload).type_id(),

rclrs/src/wait_set/wait_set_runner.rs

@@ -5,13 +5,13 @@ use futures::channel::{
 
 use std::{
     any::Any,
-    sync::{atomic::{AtomicBool, Ordering}, Arc},
+    sync::{atomic::{AtomicBool, Ordering}, Arc, Mutex},
     time::{Duration, Instant},
 };
 
 use crate::{
     Context, Promise, RclrsError, WaitSet, Waitable, GuardCondition, ExecutorWorkerOptions,
-    PayloadTask,
+    PayloadTask, WeakActivityListener, ActivityListenerCallback,
 };
 
 /// This is a utility class that executors can use to easily run and manage
@@ -22,6 +22,7 @@ pub struct WaitSetRunner {
     waitable_receiver: UnboundedReceiver<Waitable>,
     task_sender: UnboundedSender<PayloadTask>,
     task_receiver: UnboundedReceiver<PayloadTask>,
+    activity_listeners: Arc<Mutex<Vec<WeakActivityListener>>>,
     guard_condition: Arc<GuardCondition>,
     payload: Box<dyn Any + Send>,
 }
@@ -62,6 +63,7 @@ impl WaitSetRunner {
             waitable_receiver,
             task_sender,
             task_receiver,
+            activity_listeners: Arc::default(),
             guard_condition: worker_options.guard_condition,
             payload: worker_options.payload,
         }
@@ -79,6 +81,12 @@ impl WaitSetRunner {
         self.task_sender.clone()
     }
 
+    /// Get the group of senders that will be triggered each time the wait set
+    /// is woken up. This is used
+    pub fn activity_listeners(&self) -> Arc<Mutex<Vec<WeakActivityListener>>> {
+        Arc::clone(&self.activity_listeners)
+    }
+
     /// Get the guard condition associated with the wait set of this runner.
     pub fn guard_condition(&self) -> &Arc<GuardCondition> {
         &self.guard_condition
@@ -111,6 +119,7 @@ impl WaitSetRunner {
     /// will be triggered after the user-provided promise is resolved.
     pub fn run_blocking(&mut self, conditions: WaitSetRunConditions) -> Result<(), RclrsError> {
         let mut first_spin = true;
+        let mut listeners = Vec::new();
         loop {
             // TODO(@mxgrey): SmallVec would be better suited here if we are
             // okay with adding that as a dependency.
@@ -124,7 +133,7 @@ impl WaitSetRunner {
             }
 
             while let Ok(Some(task)) = self.task_receiver.try_next() {
-                task(&mut self.payload);
+                task(&mut *self.payload);
             }
 
             if conditions.only_next_available_work && !first_spin {
@@ -154,14 +163,57 @@ impl WaitSetRunner {
                 }
             });
 
+            let mut at_least_one = false;
             self.wait_set.wait(timeout, |executable| {
+                at_least_one = true;
                 // SAFETY: The user of WaitSetRunner is responsible for ensuring
                 // the runner has the same payload type as the executables that
                 // are given to it.
                 unsafe {
-                    executable.execute(&mut self.payload)
+                    executable.execute(&mut *self.payload)
                 }
             })?;
+
+            if at_least_one {
+                // We drain all listeners from activity_listeners to ensure that we
+                // don't get a deadlock from double-locking the activity_listeners
+                // mutex while executing one of the listeners. If the listener has
+                // access to the Worker<T> then it could attempt to add another
+                // listener while we have the vector locked, which would cause a
+                // deadlock.
+                listeners.extend(
+                    self.activity_listeners.lock().unwrap().drain(..)
+                        .filter_map(|x| x.upgrade())
+                );
+
+                for arc_listener in &listeners {
+                    // We pull the callback out of its mutex entirely and release
+                    // the lock on the mutex before executing the callback. Otherwise
+                    // if the callback triggers its own WorkerActivity to change the
+                    // callback then we would get a deadlock from double-locking the
+                    // mutex.
+                    let listener = { arc_listener.lock().unwrap().take() };
+                    if let Some(mut listener) = listener {
+                        match &mut listener {
+                            ActivityListenerCallback::Listen(listen) => {
+                                listen(&mut *self.payload);
+                            }
+                            ActivityListenerCallback::Inert => {
+                                // Do nothing
+                            }
+                        }
+
+                        // We replace instead of assigning in case the callback
+                        // inserted its own
+                        arc_listener.lock().unwrap().replace(listener);
+                    }
+                }
+
+                self.activity_listeners.lock().unwrap().extend(
+                    listeners.drain(..)
+                        .map(|x| Arc::downgrade(&x))
+                );
+            }
         }
     }
 }