Beginning implementation of Worker concept

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

Author: mxgrey

rclrs/src/client.rs

@@ -1,4 +1,5 @@
 use std::{
+    any::Any,
     collections::HashMap,
     ffi::CString,
     sync::{Arc, Mutex, MutexGuard},
@@ -299,7 +300,7 @@ impl<T> RclPrimitive for ClientExecutable<T>
 where
     T: rosidl_runtime_rs::Service,
 {
-    fn execute(&mut self) -> Result<(), RclrsError> {
+    unsafe fn execute(&mut self, payload: &mut dyn Any) -> Result<(), RclrsError> {
         self.board.lock().unwrap().execute(&self.handle)
     }
 

rclrs/src/error.rs

@@ -35,6 +35,14 @@ pub enum RclrsError {
     /// The guard condition that you tried to trigger is not owned by the
     /// [`GuardCondition`][crate::GuardCondition] instance.
     UnownedGuardCondition,
+    /// The payload given to a primitive that belongs to a worker was the wrong
+    /// type.
+    InvalidPayload {
+        /// The payload type expected by the primitive
+        expected: std::any::TypeId,
+        /// The payload type given by the worker
+        received: std::any::TypeId,
+    }
 }
 
 impl Display for RclrsError {
@@ -57,6 +65,12 @@ impl Display for RclrsError {
                     "Could not trigger guard condition because it is not owned by rclrs"
                 )
             }
+            RclrsError::InvalidPayload { expected, received } => {
+                write!(
+                    f,
+                    "Received invalid payload: expected {expected:?}, received {received:?}",
+                )
+            }
         }
     }
 }
@@ -92,6 +106,7 @@ impl Error for RclrsError {
             // It should be easy to do this using the thiserror crate.
             RclrsError::AlreadyAddedToWaitSet => None,
             RclrsError::UnownedGuardCondition => None,
+            RclrsError::InvalidPayload { .. } => None,
         }
     }
 }
@@ -369,6 +384,10 @@ impl ToResult for rcl_ret_t {
 pub trait RclrsErrorFilter {
     /// If the result was a timeout error, change it to `Ok(())`.
     fn timeout_ok(self) -> Result<(), RclrsError>;
+
+    /// If a subscription, service, or client take failed, change the result
+    /// to be `Ok(())`.
+    fn take_failed_ok(self) -> Result<(), RclrsError>;
 }
 
 impl RclrsErrorFilter for Result<(), RclrsError> {
@@ -390,4 +409,18 @@ impl RclrsErrorFilter for Result<(), RclrsError> {
             }
         }
     }
+
+    fn take_failed_ok(self) -> Result<(), RclrsError> {
+        match self {
+            Err(RclrsError::RclError {
+                code: RclReturnCode::SubscriptionTakeFailed | RclReturnCode::ServiceTakeFailed | RclReturnCode::ClientTakeFailed,
+                ..
+            }) => {
+                // Spurious wakeup - this may happen even when a waitset indicated that
+                // work was ready, so we won't report it as an error
+                Ok(())
+            }
+            other => other,
+        }
+    }
 }

rclrs/src/executor.rs

@@ -8,6 +8,7 @@ use futures::{
     future::{select, BoxFuture, Either},
 };
 use std::{
+    any::Any,
     future::Future,
     sync::{
         atomic::{AtomicBool, Ordering},
@@ -90,7 +91,7 @@ impl Executor {
             context: Context {
                 handle: Arc::clone(&context),
             },
-            channel: runtime.channel(),
+            channel: runtime.default_channel(),
             halt_spinning: Arc::new(AtomicBool::new(false)),
             wakeup_wait_set: Arc::new(wakeup_wait_set),
         });
@@ -217,20 +218,29 @@ impl ExecutorCommands {
     }
 }
 
+pub trait WorkerChannel: Send + Sync {
+    /// Add new entities to the waitset of the executor.
+    fn add_to_waitset(&self, new_entity: Waitable);
+}
+
 /// This trait defines the interface for passing new items into an executor to
 /// run.
-pub trait ExecutorChannel: Send + Sync {
+pub trait ExecutorChannel: WorkerChannel {
     /// Add a new item for the executor to run.
     fn add_async_task(&self, f: BoxFuture<'static, ()>);
 
-    /// Add new entities to the waitset of the executor.
-    fn add_to_waitset(&self, new_entity: Waitable);
+    /// Create a new channel specific to a worker whose payload must be
+    /// initialized with the given function.
+    fn create_worker_channel(
+        &self,
+        payload: Box<dyn Any + Send>,
+    ) -> Box<dyn ExecutorChannel>;
 }
 
 /// This trait defines the interface for having an executor run.
 pub trait ExecutorRuntime: Send {
     /// Get a channel that can add new items for the executor to run.
-    fn channel(&self) -> Box<dyn ExecutorChannel>;
+    fn default_channel(&self) -> Box<dyn ExecutorChannel>;
 
     /// Tell the runtime to spin while blocking any further execution until the
     /// spinning is complete.

rclrs/src/executor/basic_executor.rs

@@ -4,6 +4,7 @@ use futures::{
     task::{waker_ref, ArcWake},
 };
 use std::{
+    any::Any,
     sync::{
         atomic::{AtomicBool, Ordering},
         mpsc::{channel, Receiver, Sender},
@@ -13,7 +14,7 @@ use std::{
 };
 
 use crate::{
-    executor::{ExecutorChannel, ExecutorRuntime, SpinConditions},
+    executor::{ExecutorChannel, ExecutorRuntime, SpinConditions, WorkerChannel},
     Context, RclrsError, WaitSetRunner, Waitable,
 };
 
@@ -131,7 +132,7 @@ impl ExecutorRuntime for BasicExecutorRuntime {
         })
     }
 
-    fn channel(&self) -> Box<dyn ExecutorChannel> {
+    fn default_channel(&self) -> Box<dyn ExecutorChannel> {
         let waitable_sender = self.wait_set_runner.as_ref().expect(
             "The wait set runner of the basic executor is missing while creating a channel. \
             This is a critical bug in rclrs. \
@@ -195,14 +196,23 @@ struct BasicExecutorChannel {
     waitable_sender: UnboundedSender<Waitable>,
 }
 
+impl WorkerChannel for BasicExecutorChannel {
+    fn add_to_waitset(&self, new_entity: Waitable) {
+        // TODO(@mxgrey): Log errors here once logging becomes available.
+        self.waitable_sender.unbounded_send(new_entity).ok();
+    }
+}
+
 impl ExecutorChannel for BasicExecutorChannel {
     fn add_async_task(&self, f: BoxFuture<'static, ()>) {
         self.task_sender.add_async_task(f);
     }
 
-    fn add_to_waitset(&self, new_entity: Waitable) {
-        // TODO(@mxgrey): Log errors here once logging becomes available.
-        self.waitable_sender.unbounded_send(new_entity).ok();
+    fn create_worker_channel(
+        &self,
+        payload: Box<dyn Any + Send>,
+    ) -> Box<dyn ExecutorChannel> {
+
     }
 }
 

rclrs/src/lib.rs

@@ -22,6 +22,7 @@ mod time;
 mod time_source;
 mod vendor;
 mod wait_set;
+mod worker;
 
 #[cfg(test)]
 mod test_helpers;
@@ -48,3 +49,4 @@ pub use subscription::*;
 pub use time::*;
 use time_source::*;
 pub use wait_set::*;
+pub use worker::*;

rclrs/src/node.rs

@@ -32,8 +32,8 @@ use crate::{
     rcl_bindings::*, Client, ClientOptions, ClientState, Clock, ContextHandle, ExecutorCommands,
     LogParams, Logger, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Promise,
     Publisher, PublisherOptions, PublisherState, RclrsError, Service, ServiceAsyncCallback,
-    ServiceCallback, ServiceOptions, ServiceState, Subscription, SubscriptionAsyncCallback,
-    SubscriptionCallback, SubscriptionOptions, SubscriptionState, TimeSource, ToLogParams,
+    ServiceCallback, ServiceOptions, ServiceState, Subscription, IntoAsyncSubscriptionCallback,
+    IntoNodeSubscriptionCallback, SubscriptionOptions, SubscriptionState, TimeSource, ToLogParams,
     ENTITY_LIFECYCLE_MUTEX,
 };
 
@@ -492,14 +492,14 @@ impl NodeState {
     pub fn create_subscription<'a, T, Args>(
         &self,
         options: impl Into<SubscriptionOptions<'a>>,
-        callback: impl SubscriptionCallback<T, Args>,
+        callback: impl IntoNodeSubscriptionCallback<T, Args>,
     ) -> Result<Subscription<T>, RclrsError>
     where
         T: Message,
     {
         SubscriptionState::<T>::create(
             options,
-            callback.into_subscription_callback(),
+            callback.into_node_subscription_callback(),
             &self.handle,
             &self.commands,
         )
@@ -527,14 +527,14 @@ impl NodeState {
     pub fn create_async_subscription<'a, T, Args>(
         &self,
         options: impl Into<SubscriptionOptions<'a>>,
-        callback: impl SubscriptionAsyncCallback<T, Args>,
+        callback: impl IntoAsyncSubscriptionCallback<T, Args>,
     ) -> Result<Subscription<T>, RclrsError>
     where
         T: Message,
     {
         SubscriptionState::<T>::create(
             options,
-            callback.into_subscription_async_callback(),
+            callback.into_async_subscription_callback(),
             &self.handle,
             &self.commands,
         )

rclrs/src/service.rs

@@ -2,6 +2,7 @@ use std::{
     boxed::Box,
     ffi::{CStr, CString},
     sync::{Arc, Mutex, MutexGuard},
+    any::Any,
 };
 
 use crate::{
@@ -51,7 +52,7 @@ pub type Service<T> = Arc<ServiceState<T>>;
 /// The public API of the [`Service`] type is implemented via `ServiceState`.
 ///
 /// [1]: std::sync::Weak
-pub struct ServiceState<T>
+pub struct ServiceState<T, Payload = ()>
 where
     T: rosidl_runtime_rs::Service,
 {
@@ -212,7 +213,7 @@ impl<T> RclPrimitive for ServiceExecutable<T>
 where
     T: rosidl_runtime_rs::Service,
 {
-    fn execute(&mut self) -> Result<(), RclrsError> {
+    unsafe fn execute(&mut self, payload: &mut dyn Any) -> Result<(), RclrsError> {
         if let Err(err) = self
             .callback
             .lock()