feat(polling_subscriber): polling srbscriber for multiple data (autowarefoundation#7409)

* add(polling_subscriber): polling srbscriber for multiple data

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

* fix qos depth check script

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

---------

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

Author: yhisaki

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp

@@ -20,19 +20,25 @@
 #include <memory>
 #include <stdexcept>
 #include <string>
+#include <type_traits>
+#include <vector>
 
 namespace tier4_autoware_utils
 {
 
-template <typename T>
-class InterProcessPollingSubscriber
+template <typename T, int N = 1, typename Enable = void>
+class InterProcessPollingSubscriber;
+
+template <typename T, int N>
+class InterProcessPollingSubscriber<T, N, typename std::enable_if<N == 1>::type>
 {
 public:
-  using SharedPtr = std::shared_ptr<InterProcessPollingSubscriber<T>>;
+  using SharedPtr =
+    std::shared_ptr<InterProcessPollingSubscriber<T, N, typename std::enable_if<N == 1>::type>>;
   static SharedPtr create_subscription(
     rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{1})
   {
-    return std::make_shared<InterProcessPollingSubscriber<T>>(node, topic_name, qos);
+    return std::make_shared<InterProcessPollingSubscriber<T, N>>(node, topic_name, qos);
   }
 
 private:
@@ -71,6 +77,56 @@ class InterProcessPollingSubscriber
   };
 };
 
+template <typename T, int N>
+class InterProcessPollingSubscriber<T, N, typename std::enable_if<(N >= 2)>::type>
+{
+public:
+  using SharedPtr =
+    std::shared_ptr<InterProcessPollingSubscriber<T, N, typename std::enable_if<(N >= 2)>::type>>;
+  static SharedPtr create_subscription(
+    rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{N})
+  {
+    return std::make_shared<InterProcessPollingSubscriber<T, N>>(node, topic_name, qos);
+  }
+
+private:
+  typename rclcpp::Subscription<T>::SharedPtr subscriber_;
+
+public:
+  explicit InterProcessPollingSubscriber(
+    rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{N})
+  {
+    auto noexec_callback_group =
+      node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
+    auto noexec_subscription_options = rclcpp::SubscriptionOptions();
+    noexec_subscription_options.callback_group = noexec_callback_group;
+
+    subscriber_ = node->create_subscription<T>(
+      topic_name, qos,
+      [node]([[maybe_unused]] const typename T::ConstSharedPtr msg) { assert(false); },
+      noexec_subscription_options);
+    if (qos.get_rmw_qos_profile().depth < N) {
+      throw std::invalid_argument(
+        "InterProcessPollingSubscriber will be used with depth == " + std::to_string(N) +
+        ", which may cause inefficient serialization while updateLatestData()");
+    }
+  };
+  std::vector<typename T::ConstSharedPtr> takeData()
+  {
+    std::vector<typename T::ConstSharedPtr> data;
+    rclcpp::MessageInfo message_info;
+    for (int i = 0; i < N; ++i) {
+      auto datum = std::make_shared<T>();
+      if (subscriber_->take(*datum, message_info)) {
+        data.push_back(datum);
+      } else {
+        break;
+      }
+    }
+    return data;
+  };
+};
+
 }  // namespace tier4_autoware_utils
 
 #endif  // TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_