Finished initial Worker implementation - needs testing

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

Author: mxgrey

rclrs/src/executor.rs

@@ -267,24 +267,21 @@ pub(crate) struct WorkerCommands {
 }
 
 impl WorkerCommands {
-    pub(crate) fn new(
-        channel: Arc<dyn WorkerChannel>,
-        wakeup_wait_set: Arc<GuardCondition>,
-    ) -> Self {
-        Self { channel, wakeup_wait_set }
-    }
-
     pub(crate) fn add_to_wait_set(&self, waitable: Waitable) {
         self.channel.add_to_waitset(waitable);
     }
 
-    pub(crate) fn run<F>(&self, f: F)
+    pub(crate) fn run_async<F>(&self, f: F)
     where
         F: 'static + Future<Output = ()> + Send,
     {
         self.channel.add_async_task(Box::pin(f));
     }
 
+    pub(crate) fn run_on_payload(&self, task: PayloadTask) {
+        self.channel.send_payload_task(task);
+    }
+
     /// 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,8 +295,14 @@ pub trait WorkerChannel: Send + Sync {
 
     /// Add new entities to the waitset of the executor.
     fn add_to_waitset(&self, new_entity: Waitable);
+
+    /// Send a one-time task for the worker to run with its payload.
+    fn send_payload_task(&self, f: PayloadTask);
 }
 
+/// Encapsulates a task that can operate on the payload of a worker
+pub type PayloadTask = Box<dyn FnOnce(&mut dyn Any) + Send>;
+
 /// This is constructed by [`ExecutorCommands`] and passed to the [`ExecutorRuntime`]
 /// to create a new worker. Downstream users of rclrs should not be using this class
 /// unless you are implementing your own [`ExecutorRuntime`].

rclrs/src/executor/basic_executor.rs

@@ -18,7 +18,7 @@ use std::{
 use crate::{
     ExecutorChannel, ExecutorRuntime, SpinConditions, WorkerChannel,
     RclrsError, WaitSetRunner, WaitSetRunConditions, Waitable, log_warn, log_fatal, ToLogParams,
-    GuardCondition, ExecutorWorkerOptions,
+    GuardCondition, ExecutorWorkerOptions, PayloadTask,
 };
 
 static FAILED_TO_SEND_WORKER: &'static str =
@@ -278,7 +278,8 @@ impl ExecutorChannel for BasicExecutorChannel {
         options: ExecutorWorkerOptions,
     ) -> Arc<dyn WorkerChannel> {
         let runner = WaitSetRunner::new(options);
-        let waitable_sender = runner.sender();
+        let waitable_sender = runner.waitable_sender();
+        let payload_task_sender = runner.payload_task_sender();
 
         if let Err(err) = self.new_worker_sender.unbounded_send(runner) {
             log_fatal!(
@@ -290,6 +291,7 @@ impl ExecutorChannel for BasicExecutorChannel {
         Arc::new(BasicWorkerChannel {
             waitable_sender,
             task_sender: self.task_sender.clone(),
+            payload_task_sender,
         })
     }
 
@@ -301,6 +303,7 @@ impl ExecutorChannel for BasicExecutorChannel {
 struct BasicWorkerChannel {
     task_sender: TaskSender,
     waitable_sender: UnboundedSender<Waitable>,
+    payload_task_sender: UnboundedSender<PayloadTask>,
 }
 
 impl WorkerChannel for BasicWorkerChannel {
@@ -312,6 +315,10 @@ impl WorkerChannel for BasicWorkerChannel {
     fn add_async_task(&self, f: BoxFuture<'static, ()>) {
         self.task_sender.add_async_task(f);
     }
+
+    fn send_payload_task(&self, f: PayloadTask) {
+        self.payload_task_sender.unbounded_send(f).ok();
+    }
 }
 
 #[derive(Clone)]

rclrs/src/node.rs

@@ -34,7 +34,7 @@ use crate::{
     Publisher, PublisherOptions, PublisherState, RclrsError, Service, IntoAsyncServiceCallback,
     IntoNodeServiceCallback, ServiceOptions, ServiceState, Subscription, IntoAsyncSubscriptionCallback,
     IntoNodeSubscriptionCallback, SubscriptionOptions, SubscriptionState, TimeSource, ToLogParams,
-    ENTITY_LIFECYCLE_MUTEX,
+    ENTITY_LIFECYCLE_MUTEX, IntoWorkerOptions, Worker, WorkerState,
 };
 
 /// A processing unit that can communicate with other nodes.
@@ -236,6 +236,21 @@ impl NodeState {
         self.call_string_getter(rcl_node_get_fully_qualified_name)
     }
 
+    /// Create a new [`Worker`] for this Node.
+    //
+    // TODO(@mxgrey): Write some usage examples.
+    pub fn create_worker<'a, Payload>(
+        &self,
+        options: impl IntoWorkerOptions<Payload>,
+    ) -> Worker<Payload>
+    where
+        Payload: 'static + Send,
+    {
+        let options = options.into_worker_options();
+        let commands = self.commands.create_worker_commands(Box::new(options.payload));
+        WorkerState::create(Arc::clone(&self.handle), commands)
+    }
+
     /// Creates a [`Client`][1].
     ///
     /// Pass in only the service name for the `options` argument to use all default client options:

rclrs/src/service/node_service_callback.rs

@@ -34,7 +34,7 @@ impl<T: Service> NodeServiceCallback<T> {
                 NodeServiceCallback::OnlyRequest(cb) => {
                     let (msg, mut rmw_request_id) = handle.take_request::<T>()?;
                     let response = cb(msg);
-                    commands.run(async move {
+                    commands.run_async(async move {
                         if let Err(err) = handle.send_response::<T>(&mut rmw_request_id, response.await) {
                             log_service_send_error(&*handle, rmw_request_id, err);
                         }
@@ -44,7 +44,7 @@ impl<T: Service> NodeServiceCallback<T> {
                     let (msg, mut rmw_request_id) = handle.take_request::<T>()?;
                     let request_id = RequestId::from_rmw_request_id(&rmw_request_id);
                     let response = cb(msg, request_id);
-                    commands.run(async move {
+                    commands.run_async(async move {
                         if let Err(err) = handle.send_response::<T>(&mut rmw_request_id, response.await) {
                             log_service_send_error(&*handle, rmw_request_id, err);
                         }
@@ -55,7 +55,7 @@ impl<T: Service> NodeServiceCallback<T> {
                     let mut rmw_request_id = rmw_service_info.rmw_request_id();
                     let service_info = ServiceInfo::from_rmw_service_info(&rmw_service_info);
                     let response = cb(msg, service_info);
-                    commands.run(async move {
+                    commands.run_async(async move {
                         if let Err(err) = handle.send_response::<T>(&mut rmw_request_id, response.await) {
                             log_service_send_error(&*handle, rmw_request_id, err);
                         }

rclrs/src/subscription/node_subscription_callback.rs

@@ -44,27 +44,27 @@ impl<T: Message> NodeSubscriptionCallback<T> {
             match self {
                 NodeSubscriptionCallback::Regular(cb) => {
                     let (msg, _) = handle.take::<T>()?;
-                    commands.run(cb(msg));
+                    commands.run_async(cb(msg));
                 }
                 NodeSubscriptionCallback::RegularWithMessageInfo(cb) => {
                     let (msg, msg_info) = handle.take::<T>()?;
-                    commands.run(cb(msg, msg_info));
+                    commands.run_async(cb(msg, msg_info));
                 }
                 NodeSubscriptionCallback::Boxed(cb) => {
                     let (msg, _) = handle.take_boxed::<T>()?;
-                    commands.run(cb(msg));
+                    commands.run_async(cb(msg));
                 }
                 NodeSubscriptionCallback::BoxedWithMessageInfo(cb) => {
                     let (msg, msg_info) = handle.take_boxed::<T>()?;
-                    commands.run(cb(msg, msg_info));
+                    commands.run_async(cb(msg, msg_info));
                 }
                 NodeSubscriptionCallback::Loaned(cb) => {
                     let (msg, _) = handle.take_loaned::<T>()?;
-                    commands.run(cb(msg));
+                    commands.run_async(cb(msg));
                 }
                 NodeSubscriptionCallback::LoanedWithMessageInfo(cb) => {
                     let (msg, msg_info) = handle.take_loaned::<T>()?;
-                    commands.run(cb(msg, msg_info));
+                    commands.run_async(cb(msg, msg_info));
                 }
             }
             Ok(())

rclrs/src/wait_set/wait_set_runner.rs

@@ -11,6 +11,7 @@ use std::{
 
 use crate::{
     Context, Promise, RclrsError, WaitSet, Waitable, GuardCondition, ExecutorWorkerOptions,
+    PayloadTask,
 };
 
 /// This is a utility class that executors can use to easily run and manage
@@ -19,6 +20,8 @@ pub struct WaitSetRunner {
     wait_set: WaitSet,
     waitable_sender: UnboundedSender<Waitable>,
     waitable_receiver: UnboundedReceiver<Waitable>,
+    task_sender: UnboundedSender<PayloadTask>,
+    task_receiver: UnboundedReceiver<PayloadTask>,
     guard_condition: Arc<GuardCondition>,
     payload: Box<dyn Any + Send>,
 }
@@ -49,24 +52,33 @@ impl WaitSetRunner {
         worker_options: ExecutorWorkerOptions,
     ) -> Self {
         let (waitable_sender, waitable_receiver) = unbounded();
+        let (task_sender, task_receiver) = unbounded();
         Self {
             wait_set: WaitSet::new(&worker_options.context)
                 // SAFETY: This only gets called from Context which ensures that
                 // everything is valid when creating a wait set.
                 .expect("Unable to create wait set for basic executor"),
             waitable_sender,
             waitable_receiver,
+            task_sender,
+            task_receiver,
             guard_condition: worker_options.guard_condition,
             payload: worker_options.payload,
         }
     }
 
-    /// Get the sender that allows users to send new [`Waitables`] to this
+    /// Get the sender that allows users to send new [`Waitable`]s to this
     /// `WaitSetRunner`.
-    pub fn sender(&self) -> UnboundedSender<Waitable> {
+    pub fn waitable_sender(&self) -> UnboundedSender<Waitable> {
         self.waitable_sender.clone()
     }
 
+    /// Get the sender that allows users to send new [`PayloadTask`]s to this
+    /// `WaitSetRunner`.
+    pub fn payload_task_sender(&self) -> UnboundedSender<PayloadTask> {
+        self.task_sender.clone()
+    }
+
     /// Get the guard condition associated with the wait set of this runner.
     pub fn guard_condition(&self) -> &Arc<GuardCondition> {
         &self.guard_condition
@@ -111,6 +123,10 @@ impl WaitSetRunner {
                 self.wait_set.add(new_waitables).ok();
             }
 
+            while let Ok(Some(task)) = self.task_receiver.try_next() {
+                task(&mut self.payload);
+            }
+
             if conditions.only_next_available_work && !first_spin {
                 // We've already completed a spin and were asked to only do one,
                 // so break here

rclrs/src/worker.rs

@@ -1,5 +1,12 @@
-use std::sync::Arc;
-use crate::{WorkerCommands};
+use rosidl_runtime_rs::{Message, Service as IdlService};
+use std::{any::Any, sync::Arc};
+use futures::channel::oneshot;
+use crate::{
+    WorkerCommands, NodeHandle, ToLogParams, Promise, log_fatal,
+    IntoWorkerSubscriptionCallback, IntoWorkerServiceCallback,
+    WorkerSubscription, SubscriptionState, WorkerService, ServiceState,
+    SubscriptionOptions, ServiceOptions, RclrsError,
+};
 
 /// A worker that carries a payload and synchronizes callbacks for subscriptions
 /// and services.
@@ -25,12 +32,108 @@ pub type Worker<Payload> = Arc<WorkerState<Payload>>;
 ///
 /// [1]: std::sync::Weak
 pub struct WorkerState<Payload> {
+    /// The node that this worker is associated with
+    node: Arc<NodeHandle>,
+    /// The commands to communicate with the runtime of the worker
     commands: Arc<WorkerCommands>,
     _ignore: std::marker::PhantomData<Payload>,
 }
 
-impl<T> WorkerState<T> {
-    pub(crate) fn new(commands: Arc<WorkerCommands>) -> Self {
-        Self { commands, _ignore: Default::default() }
+impl<Payload: 'static + Send> WorkerState<Payload> {
+    /// Run a task on this worker. This allows you to view and modify the payload
+    /// of the worker. You will receive a [`Promise`] which you can use to view
+    /// what happened inside the callback.
+    pub fn run<Out, F>(&self, f: F) -> Promise<Out>
+    where
+        F: FnOnce(&mut Payload) -> Out + 'static + Send,
+        Out: 'static + Send,
+    {
+        let (sender, receiver) = oneshot::channel();
+        self.commands.run_on_payload(Box::new(move |any_payload: &mut dyn Any| {
+            let Some(payload) = any_payload.downcast_mut::<Payload>() else {
+                log_fatal!(
+                    "rclrs.worker",
+                    "Received invalid payload from worker. Expected: {:?}, received: {:?}. \
+                    This should never happen. Please report this to the maintainers of rclrs \
+                    with a minimal reproducible example.",
+                    std::any::TypeId::of::<Payload>(),
+                    any_payload,
+                );
+                return;
+            };
+
+            let out = f(payload);
+            sender.send(out).ok();
+        }));
+        receiver
+    }
+
+    /// Creates a [`WorkerSubscription`].
+    pub fn create_subscription<'a, T, Args>(
+        &self,
+        options: impl Into<SubscriptionOptions<'a>>,
+        callback: impl IntoWorkerSubscriptionCallback<T, Payload, Args>,
+    ) -> Result<WorkerSubscription<T, Payload>, RclrsError>
+    where
+        T: Message,
+    {
+        SubscriptionState::<T, Worker<Payload>>::create(
+            options,
+            callback.into_worker_subscription_callback(),
+            &self.node,
+            &self.commands,
+        )
+    }
+
+    /// Creates a [`WorkerService`].
+    pub fn create_service<'a, T, Args>(
+        &self,
+        options: impl Into<ServiceOptions<'a>>,
+        callback: impl IntoWorkerServiceCallback<T, Payload, Args>,
+    ) -> Result<WorkerService<T, Payload>, RclrsError>
+    where
+        T: IdlService,
+    {
+        ServiceState::<T, Worker<Payload>>::create(
+            options,
+            callback.into_worker_service_callback(),
+            &self.node,
+            &self.commands,
+        )
+    }
+
+    /// Used by [`Node`][crate::Node] to create a `WorkerState`. Users should
+    /// call [`Node::create_worker`][crate::NodeState::create_worker] instead of
+    /// this.
+    pub(crate) fn create(
+        node: Arc<NodeHandle>,
+        commands: Arc<WorkerCommands>,
+    ) -> Arc<Self> {
+        Arc::new(Self { node, commands, _ignore: Default::default() })
+    }
+}
+
+/// Options used while creating a new [`Worker`].
+pub struct WorkerOptions<Payload> {
+    /// The value of the initial payload for the [`Worker`].
+    pub payload: Payload,
+}
+
+impl<Payload> WorkerOptions<Payload> {
+    /// Create a new `WorkerOptions`.
+    pub fn new(payload: Payload) -> Self {
+        Self { payload }
+    }
+}
+
+/// Implicitly convert something into [`WorkerOptions`].
+pub trait IntoWorkerOptions<Payload> {
+    /// Convert an object into [`WorkerOptions`]. Users do not need to call this.
+    fn into_worker_options(self) -> WorkerOptions<Payload>;
+}
+
+impl<Payload> IntoWorkerOptions<Payload> for Payload {
+    fn into_worker_options(self) -> WorkerOptions<Payload> {
+        WorkerOptions::new(self)
     }
 }