feat(autoware_test_utils): add autoware test manager (#7597)

* feat(detected_object_validation): add test

Signed-off-by: kminoda <[email protected]>

* move to autoware_test_utils

Signed-off-by: kminoda <[email protected]>

* remove perception

Signed-off-by: kminoda <[email protected]>

* update cmake

Signed-off-by: kminoda <[email protected]>

* style(pre-commit): autofix

* remove perception change

Signed-off-by: kminoda <[email protected]>

* add include

Signed-off-by: kminoda <[email protected]>

* refactored

Signed-off-by: kminoda <[email protected]>

* avoid using void and static_pointer_cast

Signed-off-by: kminoda <[email protected]>

---------

Signed-off-by: kminoda <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

common/autoware_test_utils/CMakeLists.txt

@@ -15,11 +15,10 @@ target_link_libraries(mock_data_parser
 )
 
 if(BUILD_TESTING)
-  ament_add_ros_isolated_gtest(test_mock_data_parser
-    test/test_mock_data_parser.cpp)
-
-  target_link_libraries(test_mock_data_parser
-    mock_data_parser)
+  ament_auto_add_gtest(test_autoware_test_utils
+    test/test_mock_data_parser.cpp
+    test/test_autoware_test_manager.cpp
+  )
 endif()
 
 ament_auto_package(INSTALL_TO_SHARE

common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp

@@ -48,6 +48,7 @@
 #include <limits>
 #include <memory>
 #include <string>
+#include <unordered_map>
 #include <vector>
 
 namespace autoware::test_utils
@@ -512,6 +513,62 @@ void publishToTargetNode(
   autoware::test_utils::spinSomeNodes(test_node, target_node, repeat_count);
 }
 
+/**
+ * @brief Manages publishing and subscribing to ROS topics for testing Autoware.
+ *
+ * The AutowareTestManager class provides utility functions to facilitate
+ * the publishing of messages to specified topics and the setting up of
+ * subscribers to listen for messages on specified topics. This class
+ * simplifies the setup of test environments in Autoware.
+ */
+class AutowareTestManager
+{
+public:
+  AutowareTestManager()
+  {
+    test_node_ = std::make_shared<rclcpp::Node>("autoware_test_manager_node");
+  }
+
+  template <typename MessageType>
+  void test_pub_msg(
+    rclcpp::Node::SharedPtr target_node, const std::string & topic_name, MessageType & msg)
+  {
+    if (publishers_.find(topic_name) == publishers_.end()) {
+      auto publisher = test_node_->create_publisher<MessageType>(topic_name, 10);
+      publishers_[topic_name] = std::static_pointer_cast<rclcpp::PublisherBase>(publisher);
+    }
+
+    auto publisher =
+      std::dynamic_pointer_cast<rclcpp::Publisher<MessageType>>(publishers_[topic_name]);
+
+    autoware::test_utils::publishToTargetNode(test_node_, target_node, topic_name, publisher, msg);
+    RCLCPP_INFO(test_node_->get_logger(), "Published message on topic '%s'", topic_name.c_str());
+  }
+
+  template <typename MessageType>
+  void set_subscriber(
+    const std::string & topic_name,
+    std::function<void(const typename MessageType::ConstSharedPtr)> callback)
+  {
+    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);
+      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());
+    }
+  }
+
+protected:
+  // Publisher
+  std::unordered_map<std::string, std::shared_ptr<rclcpp::PublisherBase>> publishers_;
+  std::unordered_map<std::string, std::shared_ptr<rclcpp::SubscriptionBase>> subscribers_;
+
+  // Node
+  rclcpp::Node::SharedPtr test_node_;
+};  // class AutowareTestManager
+
 }  // namespace autoware::test_utils
 
 #endif  // AUTOWARE_TEST_UTILS__AUTOWARE_TEST_UTILS_HPP_

common/autoware_test_utils/test/test_autoware_test_manager.cpp

@@ -0,0 +1,71 @@
+// Copyright 2024 TIER IV
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware_test_utils/autoware_test_utils.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <std_msgs/msg/string.hpp>
+
+#include <gtest/gtest.h>
+
+class RelayNode : public rclcpp::Node
+{
+public:
+  RelayNode() : Node("relay_node")
+  {
+    subscription_ = this->create_subscription<std_msgs::msg::String>(
+      "input_topic", 10, [this](const std_msgs::msg::String::ConstSharedPtr msg) {
+        RCLCPP_INFO(this->get_logger(), "Received message: %s", msg->data.c_str());
+        auto new_msg = std::make_shared<std_msgs::msg::String>();
+        new_msg->data = msg->data;
+        publisher_->publish(*new_msg);
+      });
+
+    publisher_ = this->create_publisher<std_msgs::msg::String>("output_topic", 10);
+  }
+
+private:
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
+};
+
+TEST(AutowareTestManagerTest, TestRelayNode)
+{
+  rclcpp::init(0, nullptr);
+
+  const std::string input_topic_name = "input_topic";
+  const std::string output_topic_name = "output_topic";
+
+  // Setup target node and its test manager
+  auto target_node = std::make_shared<RelayNode>();
+  auto manager = std::make_shared<autoware::test_utils::AutowareTestManager>();
+
+  // Setup subscriber for test manager
+  std::string received_msg;
+  manager->set_subscriber<std_msgs::msg::String>(
+    "output_topic",
+    [&received_msg](const std_msgs::msg::String::ConstSharedPtr msg) { received_msg = msg->data; });
+
+  // Publish a message to the relay node
+  std_msgs::msg::String msg;
+  msg.data = "Hello, Relay!";
+  manager->test_pub_msg<std_msgs::msg::String>(target_node, input_topic_name, msg);
+
+  // Spin to process callbacks
+  rclcpp::spin_some(target_node);
+
+  // Check that the message was relayed and received by the test node
+  EXPECT_EQ(received_msg, "Hello, Relay!");
+}