WIP working through subscriptions

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

Author: luca-della-vedova

rclrs/src/dynamic_message.rs

@@ -7,6 +7,7 @@
 
 use std::{
     fmt::{self, Display},
+    ops::Deref,
     path::PathBuf,
     sync::Arc,
 };

rclrs/src/dynamic_message/dynamic_publisher.rs

@@ -9,7 +9,7 @@ use super::{
 use crate::error::{RclrsError, ToResult};
 use crate::qos::QoSProfile;
 use crate::rcl_bindings::*;
-use crate::Node;
+use crate::NodeHandle;
 
 /// Struct for sending messages of type `T`.
 ///
@@ -23,7 +23,7 @@ use crate::Node;
 /// [1]: crate::spin
 pub struct DynamicPublisher {
     rcl_publisher_mtx: Mutex<rcl_publisher_t>,
-    rcl_node_mtx: Arc<Mutex<rcl_node_t>>,
+    node_handle: Arc<NodeHandle>,
     metadata: DynamicMessageMetadata,
     // This is the regular type support library, not the introspection one.
     #[allow(dead_code)]
@@ -32,11 +32,12 @@ pub struct DynamicPublisher {
 
 impl Drop for DynamicPublisher {
     fn drop(&mut self) {
+        let mut rcl_node = self.node_handle.rcl_node.lock().unwrap();
         unsafe {
             // SAFETY: No preconditions for this function (besides the arguments being valid).
             rcl_publisher_fini(
                 self.rcl_publisher_mtx.get_mut().unwrap(),
-                &mut *self.rcl_node_mtx.lock().unwrap(),
+                &mut *rcl_node,
             );
         }
     }
@@ -54,7 +55,7 @@ impl DynamicPublisher {
     ///
     /// Node and namespace changes are always applied _before_ topic remapping.
     pub fn new(
-        node: &Node,
+        node_handle: &Arc<NodeHandle>,
         topic: &str,
         topic_type: &str,
         qos: QoSProfile,
@@ -83,7 +84,7 @@ impl DynamicPublisher {
             err,
             s: topic.into(),
         })?;
-        let rcl_node = &mut *node.rcl_node_mtx.lock().unwrap();
+        let rcl_node = node_handle.rcl_node.lock().unwrap();
 
         // SAFETY: No preconditions for this function.
         let mut publisher_options = unsafe { rcl_publisher_get_default_options() };
@@ -96,7 +97,7 @@ impl DynamicPublisher {
             // TODO: type support?
             rcl_publisher_init(
                 &mut rcl_publisher,
-                rcl_node,
+                &*rcl_node,
                 type_support_ptr,
                 topic_c_string.as_ptr(),
                 &publisher_options,
@@ -106,7 +107,7 @@ impl DynamicPublisher {
 
         Ok(Self {
             rcl_publisher_mtx: Mutex::new(rcl_publisher),
-            rcl_node_mtx: Arc::clone(&node.rcl_node_mtx),
+            node_handle: node_handle.clone(),
             metadata,
             type_support_library,
         })

rclrs/src/dynamic_message/dynamic_subscription.rs

@@ -21,8 +21,8 @@ pub struct DynamicSubscription {
     ///
     /// 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>>,
+    pub 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)]
@@ -103,6 +103,8 @@ impl DynamicSubscription {
             node_handle: Arc::clone(node_handle),
         });
 
+        let callback = Arc::new(Mutex::new(callback));
+
         let (waitable, lifecycle) = Waitable::new(
             Box::new(SubscriptionExecutable {
                 handle: Arc::clone(&handle),

rclrs/src/error.rs

@@ -4,6 +4,7 @@ use std::{
     fmt::{self, Display},
 };
 
+#[cfg(feature = "dyn_msg")]
 use crate::dynamic_message::DynamicMessageError;
 use crate::{rcl_bindings::*, DeclarationError};
 
@@ -33,6 +34,7 @@ pub enum RclrsError {
     },
     /// It was attempted to add a waitable to a wait set twice.
     AlreadyAddedToWaitSet,
+    #[cfg(feature = "dyn_msg")]
     /// An error while creating dynamic message.
     DynamicMessageError {
         /// The error containing more detailed information.
@@ -100,6 +102,7 @@ impl Display for RclrsError {
                     "Could not add entity to wait set because it was already added to a wait set"
                 )
             }
+            #[cfg(feature = "dyn_msg")]
             RclrsError::DynamicMessageError { .. } => {
                 write!(f, "Could not create dynamic message")
             }
@@ -131,6 +134,7 @@ impl Display for RclrsError {
     }
 }
 
+#[cfg(feature = "dyn_msg")]
 impl From<DynamicMessageError> for RclrsError {
     fn from(err: DynamicMessageError) -> Self {
         Self::DynamicMessageError { err }
@@ -167,6 +171,7 @@ impl Error for RclrsError {
             // TODO(@mxgrey): We should provide source information for these other types.
             // It should be easy to do this using the thiserror crate.
             RclrsError::AlreadyAddedToWaitSet => None,
+            #[cfg(feature = "dyn_msg")]
             RclrsError::DynamicMessageError { err } => Some(err).map(|e| e as &dyn Error),
             RclrsError::NegativeDuration(_) => None,
             RclrsError::UnownedGuardCondition => None,

rclrs/src/subscription.rs

@@ -12,6 +12,7 @@ use crate::{
     WorkScope, Worker, WorkerCommands, ENTITY_LIFECYCLE_MUTEX,
 };
 
+#[cfg(feature = "dyn_msg")]
 use crate::dynamic_message::DynamicMessage;
 
 mod any_subscription_callback;
@@ -279,6 +280,7 @@ impl<'a> DynamicSubscriptionOptions<'a> {
     }
 }
 
+/*
 impl<'a, T: IntoPrimitiveOptions<'a>> From<T> for DynamicSubscriptionOptions<'a> {
     fn from(value: T) -> Self {
         let primitive = value.into_primitive_options();
@@ -287,6 +289,7 @@ impl<'a, T: IntoPrimitiveOptions<'a>> From<T> for DynamicSubscriptionOptions<'a>
         options
     }
 }
+*/
 
 struct SubscriptionExecutable<T: Message, Payload> {
     handle: Arc<SubscriptionHandle>,
@@ -361,6 +364,13 @@ impl SubscriptionHandle {
         Ok((T::from_rmw_message(rmw_message), message_info))
     }
 
+    /*
+    #[cfg(feature = "dyn_msg")]
+    fn take_dynamic(&self) -> Result<(DynamicMessage, MessageInfo), RclrsError> {
+
+    }
+    */
+
     /// This is a version of take() that returns a boxed message.
     ///
     /// This can be more efficient for messages containing large arrays.