feat: add qos handler in pub/sub

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

Author: YoshiRi

common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp

@@ -51,6 +51,7 @@
 #include <string>
 #include <unordered_map>
 #include <vector>
+#include <optional>
 
 namespace autoware::test_utils
 {
@@ -402,6 +403,7 @@ void createPublisherWithQoS(
   rclcpp::Node::SharedPtr test_node, std::string topic_name,
   std::shared_ptr<rclcpp::Publisher<T>> & publisher)
 {
+  // override QoS settings for specific message types
   if constexpr (
     std::is_same_v<T, LaneletRoute> || std::is_same_v<T, LaneletMapBin> ||
     std::is_same_v<T, OperationModeState>) {
@@ -410,7 +412,7 @@ void createPublisherWithQoS(
     qos.transient_local();
     publisher = rclcpp::create_publisher<T>(test_node, topic_name, qos);
   } else {
-    publisher = rclcpp::create_publisher<T>(test_node, topic_name, 1);
+    // publisher is already created. Do nothing
   }
 }
 
@@ -443,18 +445,23 @@ void setPublisher(
  * @param topic_name The name of the topic to subscribe to.
  * @param callback The callback function to call when a message is received.
  * @param subscriber A reference to the subscription to be created.
+ * @param qos The QoS settings for the subscription (optional).
  */
 template <typename T>
 void createSubscription(
-  rclcpp::Node::SharedPtr test_node, std::string topic_name,
+  rclcpp::Node::SharedPtr test_node, const std::string & topic_name,
   std::function<void(const typename T::ConstSharedPtr)> callback,
-  std::shared_ptr<rclcpp::Subscription<T>> & subscriber)
+  std::shared_ptr<rclcpp::Subscription<T>> & subscriber, 
+  std::optional<rclcpp::QoS> qos = std::nullopt)
 {
-  if constexpr (std::is_same_v<T, Trajectory>) {
-    subscriber = test_node->create_subscription<T>(topic_name, rclcpp::QoS{1}, callback);
-  } else {
-    subscriber = test_node->create_subscription<T>(topic_name, 10, callback);
+  if (!qos.has_value()) {
+    if constexpr (std::is_same_v<T, Trajectory>) {
+      qos = rclcpp::QoS{1};
+    } else {
+      qos = rclcpp::QoS{10};
+    }
   }
+  subscriber = test_node->create_subscription<T>(topic_name, *qos, callback);
 }
 
 /**
@@ -467,14 +474,16 @@ void createSubscription(
  * @param topic_name The name of the topic to subscribe to.
  * @param subscriber A reference to the subscription to be set.
  * @param count A reference to a counter that increments on message receipt.
+ * @param qos The QoS settings for the subscription (optional).
  */
 template <typename T>
 void setSubscriber(
   rclcpp::Node::SharedPtr test_node, std::string topic_name,
-  std::shared_ptr<rclcpp::Subscription<T>> & subscriber, size_t & count)
+  std::shared_ptr<rclcpp::Subscription<T>> & subscriber, size_t & count,
+  std::optional<rclcpp::QoS> qos = std::nullopt)
 {
   createSubscription(
-    test_node, topic_name, [&count](const typename T::ConstSharedPtr) { count++; }, subscriber);
+    test_node, topic_name, [&count](const typename T::ConstSharedPtr) { count++; }, subscriber, qos);
 }
 
 /**
@@ -534,9 +543,18 @@ class AutowareTestManager
   template <typename MessageType>
   void test_pub_msg(
     rclcpp::Node::SharedPtr target_node, const std::string & topic_name, MessageType & msg)
+  {
+    rclcpp::QoS qos(rclcpp::KeepLast(10));
+    test_pub_msg(target_node, topic_name, msg, qos);
+  }
+
+  template <typename MessageType>
+  void test_pub_msg(
+    rclcpp::Node::SharedPtr target_node, const std::string & topic_name, MessageType & msg,
+    rclcpp::QoS qos)
   {
     if (publishers_.find(topic_name) == publishers_.end()) {
-      auto publisher = test_node_->create_publisher<MessageType>(topic_name, 10);
+      auto publisher = test_node_->create_publisher<MessageType>(topic_name, qos);
       publishers_[topic_name] = std::static_pointer_cast<rclcpp::PublisherBase>(publisher);
     }
 
@@ -550,12 +568,12 @@ class AutowareTestManager
   template <typename MessageType>
   void set_subscriber(
     const std::string & topic_name,
-    std::function<void(const typename MessageType::ConstSharedPtr)> callback)
+    std::function<void(const typename MessageType::ConstSharedPtr)> callback, std::optional<rclcpp::QoS> qos = std::nullopt)
   {
     if (subscribers_.find(topic_name) == subscribers_.end()) {
       std::shared_ptr<rclcpp::Subscription<MessageType>> subscriber;
       autoware::test_utils::createSubscription<MessageType>(
-        test_node_, topic_name, callback, subscriber);
+        test_node_, topic_name, callback, subscriber, qos);
       subscribers_[topic_name] = std::static_pointer_cast<rclcpp::SubscriptionBase>(subscriber);
     } else {
       RCLCPP_WARN(test_node_->get_logger(), "Subscriber %s already set.", topic_name.c_str());