Initial stab, builds docs

Signed-off-by: Luca Della Vedova <[email protected]>

Author: luca-della-vedova

rclrs/src/dynamic_message/dynamic_subscription.rs

@@ -9,14 +9,23 @@ use super::{
 };
 use crate::rcl_bindings::*;
 use crate::{
-    Node, QoSProfile, RclReturnCode, RclrsError, SubscriptionBase, SubscriptionHandle, ToResult,
+    Node, QoSProfile, RclReturnCode, RclrsError, ToResult, NodeHandle, WorkerCommands, WaitableLifecycle, SubscriptionHandle,
 };
 
 /// Struct for receiving messages whose type is only known at runtime.
 pub struct DynamicSubscription {
-    pub(crate) handle: Arc<SubscriptionHandle>,
-    /// The callback function that runs when a message was received.
+    /// This handle is used to access the data that rcl holds for this subscription.
+    handle: Arc<SubscriptionHandle>,
+    /// This allows us to replace the callback in the subscription task.
+    ///
+    /// Holding onto this sender will keep the subscription task alive. Once
+    /// this sender is dropped, the subscription task will end itself.
+    // callback: Arc<Mutex<AnySubscriptionCallback<T, Scope::Payload>>>,
     pub callback: Mutex<Box<dyn FnMut(DynamicMessage) + 'static + Send>>,
+    /// Holding onto this keeps the waiter for this subscription alive in the
+    /// wait set of the executor.
+    #[allow(unused)]
+    lifecycle: WaitableLifecycle,
     metadata: DynamicMessageMetadata,
     // This is the regular type support library, not the introspection one.
     #[allow(dead_code)]
@@ -27,16 +36,18 @@ impl DynamicSubscription {
     /// Creates a new dynamic subscription.
     ///
     /// This is not a public function, by the same rationale as `Subscription::new()`.
-    pub(crate) fn new<F>(
-        node: &Node,
+    pub(crate) fn create_dynamic<F>(
         topic: &str,
         topic_type: &str,
         qos: QoSProfile,
         callback: F,
-    ) -> Result<Self, RclrsError>
+        node_handle: &Arc<NodeHandle>,
+        commands: &Arc<WorkerCommands>,
+    ) -> Result<Arc<Self>, RclrsError>
     where
         F: FnMut(DynamicMessage) + 'static + Send,
     {
+        // TODO(luca) a lot of duplication with nomral, refactor
         // This loads the introspection type support library.
         let metadata = DynamicMessageMetadata::new(topic_type)?;
         // However, we also need the regular type support library –
@@ -62,38 +73,62 @@ impl DynamicSubscription {
         let rcl_node = &mut *node.rcl_node_mtx.lock().unwrap();
 
         // SAFETY: No preconditions for this function.
-        let mut subscription_options = unsafe { rcl_subscription_get_default_options() };
-        subscription_options.qos = qos.into();
+        let mut rcl_subscription_options = unsafe { rcl_subscription_get_default_options() };
+        rcl_subscription_options.qos = qos.into();
         // SAFETY: Getting a zero-initialized value is always safe.
         let mut rcl_subscription = unsafe { rcl_get_zero_initialized_subscription() };
-        unsafe {
-            // SAFETY: The rcl_subscription is zero-initialized as expected by this function.
-            // The rcl_node is kept alive because it is co-owned by the subscription.
-            // The topic name and the options are copied by this function, so they can be dropped
-            // afterwards.
-            // TODO: type support?
-            rcl_subscription_init(
-                &mut rcl_subscription,
-                rcl_node,
-                type_support_ptr,
-                topic_c_string.as_ptr(),
-                &subscription_options,
-            )
-            .ok()?;
+        {
+            let rcl_node = node_handle.rcl_node.lock().unwrap();
+            let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap();
+            unsafe {
+                // SAFETY:
+                // * The rcl_subscription is zero-initialized as mandated by this function.
+                // * The rcl_node is kept alive by the NodeHandle because it is a dependency of the subscription.
+                // * The topic name and the options are copied by this function, so they can be dropped afterwards.
+                // * The entity lifecycle mutex is locked to protect against the risk of global
+                //   variables in the rmw implementation being unsafely modified during cleanup.
+                rcl_subscription_init(
+                    &mut rcl_subscription,
+                    &*rcl_node,
+                    type_support_ptr,
+                    topic_c_string.as_ptr(),
+                    &rcl_subscription_options,
+                )
+                .ok()?;
+            }
         }
 
         let handle = Arc::new(SubscriptionHandle {
-            rcl_subscription_mtx: Mutex::new(rcl_subscription),
-            rcl_node_mtx: node.rcl_node_mtx.clone(),
-            in_use_by_wait_set: Arc::new(AtomicBool::new(false)),
+            rcl_subscription: Mutex::new(rcl_subscription),
+            node_handle: Arc::clone(node_handle),
         });
 
+        let (waitable, lifecycle) = Waitable::new(
+            Box::new(SubscriptionExecutable {
+                handle: Arc::clone(&handle),
+                callback: Arc::clone(&callback),
+                commands: Arc::clone(commands),
+            }),
+            Some(Arc::clone(commands.get_guard_condition())),
+        );
+        commands.add_to_wait_set(waitable);
+
+        Ok(Arc::new(Self {
+            handle,
+            callback,
+            lifecycle,
+            metadata,
+            type_support_library,
+        }))
+
+        /*
         Ok(Self {
             handle,
             callback: Mutex::new(Box::new(callback)),
             metadata,
             type_support_library,
         })
+        */
     }
 
     /// Returns the topic name of the subscription.
@@ -158,29 +193,6 @@ impl DynamicSubscription {
     }
 }
 
-impl SubscriptionBase for DynamicSubscription {
-    fn handle(&self) -> &SubscriptionHandle {
-        &self.handle
-    }
-
-    fn execute(&self) -> Result<(), RclrsError> {
-        let msg = match self.take() {
-            Ok(msg) => msg,
-            Err(RclrsError::RclError {
-                code: RclReturnCode::SubscriptionTakeFailed,
-                ..
-            }) => {
-                // Spurious wakeup – this may happen even when a waitset indicated that this
-                // subscription was ready, so it shouldn't be an error.
-                return Ok(());
-            }
-            Err(e) => return Err(e),
-        };
-        (*self.callback.lock().unwrap())(msg);
-        Ok(())
-    }
-}
-
 #[cfg(test)]
 mod tests {
     use super::*;

rclrs/src/node.rs

@@ -12,6 +12,7 @@ pub use self::builder::*;
 pub use self::graph::*;
 =======
 */
+use crate::QoSProfile;
 #[cfg(feature = "dyn_msg")]
 use crate::dynamic_message::{DynamicMessage, DynamicSubscription};
 
@@ -796,6 +797,27 @@ impl NodeState {
         )
     }
 
+    // TODO(luca) introduce subscription options and perhaps an into callback
+    #[cfg(feature = "dyn_msg")]
+    pub fn create_dynamic_subscription<F>(
+        &self,
+        // options: impl Into<DynamicSubscriptionOptions<'a>>,
+        topic: &str,
+        topic_type: &str,
+        qos: QoSProfile,
+        callback: F,
+    ) -> Result<Arc<DynamicSubscription>, RclrsError>
+    where
+        F: FnMut(DynamicMessage) + 'static + Send,
+    {
+        let subscription = Arc::new(DynamicSubscription::new(
+            self, topic, topic_type, qos, callback,
+        )?);
+        self.subscriptions
+            .push(Arc::downgrade(&subscription) as Weak<dyn SubscriptionBase>);
+        Ok(subscription)
+    }
+
     /// Creates a [`DynamicSubscription`][1].
     ///
     /// [1]: crate::dynamic_message::DynamicSubscription

rclrs/src/subscription.rs

@@ -12,6 +12,8 @@ use crate::{
     WorkScope, Worker, WorkerCommands, ENTITY_LIFECYCLE_MUTEX,
 };
 
+use crate::dynamic_message::DynamicMessage;
+
 mod any_subscription_callback;
 pub use any_subscription_callback::*;
 
@@ -33,43 +35,6 @@ pub use message_info::*;
 mod readonly_loaned_message;
 pub use readonly_loaned_message::*;
 
-/*
-// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread
-// they are running in. Therefore, this type can be safely sent to another thread.
-unsafe impl Send for rcl_subscription_t {}
-
-/// Internal struct used by subscriptions.
-pub struct SubscriptionHandle {
-    pub(crate) rcl_subscription_mtx: Mutex<rcl_subscription_t>,
-    pub(crate) rcl_node_mtx: Arc<Mutex<rcl_node_t>>,
-    pub(crate) in_use_by_wait_set: Arc<AtomicBool>,
-}
-
-impl SubscriptionHandle {
-    pub(crate) fn lock(&self) -> MutexGuard<rcl_subscription_t> {
-        self.rcl_subscription_mtx.lock().unwrap()
-    }
-}
-
-impl Drop for SubscriptionHandle {
-    fn drop(&mut self) {
-        let rcl_subscription = self.rcl_subscription_mtx.get_mut().unwrap();
-        let rcl_node = &mut *self.rcl_node_mtx.lock().unwrap();
-        // SAFETY: No preconditions for this function (besides the arguments being valid).
-        unsafe {
-            rcl_subscription_fini(rcl_subscription, rcl_node);
-        }
-    }
-}
-
-/// Trait to be implemented by concrete [`Subscription`]s.
-pub trait SubscriptionBase: Send + Sync {
-    /// Internal function to get a reference to the `rcl` handle.
-    fn handle(&self) -> &SubscriptionHandle;
-    /// Tries to take a new message and run the callback with it.
-    fn execute(&self) -> Result<(), RclrsError>;
-}
-*/
 mod worker_subscription_callback;
 pub use worker_subscription_callback::*;
 
@@ -288,6 +253,41 @@ impl<'a, T: IntoPrimitiveOptions<'a>> From<T> for SubscriptionOptions<'a> {
     }
 }
 
+/// `SubscriptionOptions` are used by [`Node::create_subscription`][1] to initialize
+/// a [`Subscription`].
+///
+/// [1]: crate::NodeState::create_subscription
+#[derive(Debug, Clone)]
+#[non_exhaustive]
+pub struct DynamicSubscriptionOptions<'a> {
+    /// The topic name for the subscription.
+    pub topic: &'a str,
+    /// The topic type for the subscription.
+    pub topic_type: &'a str,
+    /// The quality of service settings for the subscription.
+    pub qos: QoSProfile,
+}
+
+impl<'a> DynamicSubscriptionOptions<'a> {
+    /// Initialize a new [`SubscriptionOptions`] with default settings.
+    pub fn new(topic: &'a str, topic_type: &'a str) -> Self {
+        Self {
+            topic,
+            topic_type,
+            qos: QoSProfile::topics_default(),
+        }
+    }
+}
+
+impl<'a, T: IntoPrimitiveOptions<'a>> From<T> for DynamicSubscriptionOptions<'a> {
+    fn from(value: T) -> Self {
+        let primitive = value.into_primitive_options();
+        let mut options = Self::new(primitive.name);
+        primitive.apply_to(&mut options.qos);
+        options
+    }
+}
+
 struct SubscriptionExecutable<T: Message, Payload> {
     handle: Arc<SubscriptionHandle>,
     callback: Arc<Mutex<AnySubscriptionCallback<T, Payload>>>,
@@ -323,7 +323,7 @@ unsafe impl Send for rcl_subscription_t {}
 /// [dropped after][1] the `rcl_subscription_t`.
 ///
 /// [1]: <https://doc.rust-lang.org/reference/destructors.html>
-struct SubscriptionHandle {
+pub(crate) struct SubscriptionHandle {
     rcl_subscription: Mutex<rcl_subscription_t>,
     node_handle: Arc<NodeHandle>,
 }