WIP dynamic-serialized subscription

Signed-off-by: Emerson Knapp <[email protected]>

Author: emersonknapp

rclcpp/include/rclcpp/dynamic_subscription.hpp

@@ -37,6 +37,13 @@
 namespace rclcpp
 {
 
+typedef std::variant<
+    std::function<void (dynamic_typesupport::DynamicMessage::SharedPtr)>,
+    std::function<void (dynamic_typesupport::DynamicMessage::SharedPtr, const MessageInfo &)>,
+    std::function<void (std::shared_ptr<SerializedMessage>)>,
+    std::function<void (std::shared_ptr<SerializedMessage>, const MessageInfo &)>
+> AnyDynamicSubscriptionCallback;
+
 /// %Subscription for messages whose type descriptions are obtained at runtime.
 /**
  * Since the type is not known at compile time, this is not a template, and the dynamic library
@@ -56,10 +63,7 @@ class DynamicSubscription : public rclcpp::SubscriptionBase
     rclcpp::dynamic_typesupport::DynamicMessageTypeSupport::SharedPtr type_support,
     const std::string & topic_name,
     const rclcpp::QoS & qos,
-    std::function<void(
-      rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr,
-      const rosidl_runtime_c__type_description__TypeDescription &
-    )> callback,
+    AnyDynamicSubscriptionCallback callback,
     const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options)
   : SubscriptionBase(
       node_base,
@@ -69,6 +73,8 @@ class DynamicSubscription : public rclcpp::SubscriptionBase
         qos),
       options.event_callbacks,
       options.use_default_callbacks,
+      is_callback_serialized(callback) ?
+      DeliveredMessageKind::SERIALIZED_MESSAGE :
       DeliveredMessageKind::DYNAMIC_MESSAGE),
     ts_(type_support),
     callback_(callback),
@@ -159,11 +165,10 @@ class DynamicSubscription : public rclcpp::SubscriptionBase
 private:
   RCLCPP_DISABLE_COPY(DynamicSubscription)
 
+  static bool is_callback_serialized(const rclcpp::AnyDynamicSubscriptionCallback &);
+
   rclcpp::dynamic_typesupport::DynamicMessageTypeSupport::SharedPtr ts_;
-  std::function<void(
-      rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr,
-      const rosidl_runtime_c__type_description__TypeDescription &
-    )> callback_;
+  rclcpp::AnyDynamicSubscriptionCallback callback_;
 
   rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr serialization_support_;
   rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr dynamic_message_;

rclcpp/src/rclcpp/dynamic_subscription.cpp

@@ -41,10 +41,17 @@ void DynamicSubscription::handle_message(std::shared_ptr<void> &, const rclcpp::
 }
 
 void DynamicSubscription::handle_serialized_message(
-  const std::shared_ptr<rclcpp::SerializedMessage> &, const rclcpp::MessageInfo &)
+  const std::shared_ptr<rclcpp::SerializedMessage> & message,
+  const rclcpp::MessageInfo & message_info)
 {
-  throw rclcpp::exceptions::UnimplementedError(
-          "handle_serialized_message is not implemented for DynamicSubscription");
+  using PlainCallback = std::function<void (std::shared_ptr<SerializedMessage>)>;
+  using InfoCallback = std::function<
+    void (std::shared_ptr<SerializedMessage>, const MessageInfo &)>;
+  if (std::holds_alternative<PlainCallback>(callback_)) {
+    std::get<PlainCallback>(callback_)(message);
+  } else {
+    std::get<InfoCallback>(callback_)(message, message_info);
+  }
 }
 
 void DynamicSubscription::handle_loaned_message(void *, const rclcpp::MessageInfo &)
@@ -106,8 +113,25 @@ void DynamicSubscription::handle_dynamic_message(
   const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
   const rclcpp::MessageInfo & message_info)
 {
-  (void) message_info;
-  callback_(message, ts_->get_rosidl_runtime_c_type_description());
+  using PlainCallback = std::function<void (dynamic_typesupport::DynamicMessage::SharedPtr)>;
+  using InfoCallback = std::function<
+    void (dynamic_typesupport::DynamicMessage::SharedPtr, const MessageInfo &)>;
+  if (std::holds_alternative<PlainCallback>(callback_)) {
+    std::get<PlainCallback>(callback_)(message);
+  } else {
+    std::get<InfoCallback>(callback_)(message, message_info);
+  }
+}
+
+bool DynamicSubscription::is_callback_serialized(
+  const rclcpp::AnyDynamicSubscriptionCallback & callback)
+{
+  using PlainCallback = std::function<void (std::shared_ptr<SerializedMessage>)>;
+  using InfoCallback =
+    std::function<void (std::shared_ptr<SerializedMessage>, const MessageInfo &)>;
+  return
+    std::holds_alternative<PlainCallback>(callback) ||
+    std::holds_alternative<InfoCallback>(callback);
 }
 
 }  // namespace rclcpp