Fix take logic, cleanup

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

Author: luca-della-vedova

rclrs/src/dynamic_message/dynamic_subscription.rs

@@ -14,7 +14,7 @@ use crate::rcl_bindings::*;
 use crate::{
     MessageInfo, Node, NodeHandle, RclPrimitive, RclPrimitiveHandle, RclPrimitiveKind, RclrsError,
     SubscriptionHandle, SubscriptionOptions, ToResult, Waitable, WaitableLifecycle, WorkScope,
-    Worker, WorkerCommands, ENTITY_LIFECYCLE_MUTEX,
+    Worker, WorkerCommands, ENTITY_LIFECYCLE_MUTEX, RclrsErrorFilter,
 };
 
 /// Struct for receiving messages whose type is not know at compile time.
@@ -172,17 +172,20 @@ impl<Payload: 'static> DynamicSubscriptionCallback<Payload> {
                 received: (*any_payload).type_id(),
             });
         };
-        match self {
-            Self::Node(cb) => {
-                let (msg, msg_info) = executable.take()?;
-                commands.run_async(cb(msg, msg_info));
+        let mut evaluate = || {
+            match self {
+                Self::Node(cb) => {
+                    let (msg, msg_info) = executable.take()?;
+                    commands.run_async(cb(msg, msg_info));
+                }
+                Self::Worker(cb) => {
+                    let (msg, msg_info) = executable.take()?;
+                    cb(payload, msg, msg_info);
+                }
             }
-            Self::Worker(cb) => {
-                let (msg, msg_info) = executable.take()?;
-                cb(payload, msg, msg_info);
-            }
-        }
-        Ok(())
+            Ok(())
+        };
+        evaluate().take_failed_ok()
     }
 }
 

rclrs/src/subscription.rs

@@ -270,17 +270,6 @@ 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();
-        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>>>,
@@ -365,13 +354,6 @@ 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.