diff --git a/common/tier4_autoware_utils/test/src/ros/test_published_time_publisher.cpp b/common/tier4_autoware_utils/test/src/ros/test_published_time_publisher.cpp index 3b55a87fd47bb..23d4f194f15c7 100644 --- a/common/tier4_autoware_utils/test/src/ros/test_published_time_publisher.cpp +++ b/common/tier4_autoware_utils/test/src/ros/test_published_time_publisher.cpp @@ -25,10 +25,17 @@ class PublishedTimePublisherTest : public ::testing::Test protected: std::shared_ptr node_{nullptr}; std::shared_ptr published_time_publisher_{nullptr}; - std::shared_ptr> test_publisher_{nullptr}; + + std::shared_ptr> first_test_publisher_{nullptr}; + std::shared_ptr> second_test_publisher_{nullptr}; + + std::shared_ptr> + first_test_subscriber_{nullptr}; std::shared_ptr> - test_subscriber_{nullptr}; - autoware_internal_msgs::msg::PublishedTime::ConstSharedPtr published_time_{nullptr}; + second_test_subscriber_{nullptr}; + + autoware_internal_msgs::msg::PublishedTime::ConstSharedPtr first_published_time_{nullptr}; + autoware_internal_msgs::msg::PublishedTime::ConstSharedPtr second_published_time_{nullptr}; void SetUp() override { @@ -37,25 +44,39 @@ class PublishedTimePublisherTest : public ::testing::Test // Simplify node and topic names for brevity and uniqueness const std::string test_name = ::testing::UnitTest::GetInstance()->current_test_info()->name(); const std::string base_name = - "published_time_publisher_" + test_name; // Base name for node and topics - const std::string suffix = "/debug/published_time"; + "published_time_publisher_" + test_name; // Base name for node and topics + const std::string suffix = "/debug/published_time"; // Suffix for published time topics // Create a node node_ = std::make_shared(base_name + "_node"); - // Create a publisher - test_publisher_ = node_->create_publisher(base_name, 1); + // Create the first publisher + first_test_publisher_ = + node_->create_publisher(base_name + "_topic1", 1); + + // Create the second publisher + second_test_publisher_ = + node_->create_publisher(base_name + "_topic2", 1); // Create a PublishedTimePublisher published_time_publisher_ = std::make_shared(node_.get()); - // Create a subscriber - test_subscriber_ = node_->create_subscription( - base_name + suffix, 1, + // Create the first subscriber + first_test_subscriber_ = node_->create_subscription( + base_name + "_topic1" + suffix, 1, [this](autoware_internal_msgs::msg::PublishedTime::ConstSharedPtr msg) { - this->published_time_ = std::move(msg); + this->first_published_time_ = std::move(msg); }); + + // Create the second subscriber + second_test_subscriber_ = + node_->create_subscription( + base_name + "_topic2" + suffix, 1, + [this](autoware_internal_msgs::msg::PublishedTime::ConstSharedPtr msg) { + this->second_published_time_ = std::move(msg); + }); + rclcpp::spin_some(node_); } @@ -70,15 +91,15 @@ TEST_F(PublishedTimePublisherTest, PublishMsgWithHeader) std_msgs::msg::Header header; header.stamp = rclcpp::Time(1234); - // Use Published Time Publisher with a timestamp - published_time_publisher_->publish(test_publisher_, header); + // Use Published Time Publisher .publish() with a header + published_time_publisher_->publish(first_test_publisher_, header); rclcpp::spin_some(node_); // Check if the published_time_ is created - ASSERT_TRUE(published_time_ != nullptr); + ASSERT_TRUE(first_published_time_ != nullptr); // Check if the published time is the same as the header - EXPECT_EQ(published_time_->header.stamp, header.stamp); + EXPECT_EQ(first_published_time_->header.stamp, header.stamp); } TEST_F(PublishedTimePublisherTest, PublishMsgWithTimestamp) @@ -89,13 +110,57 @@ TEST_F(PublishedTimePublisherTest, PublishMsgWithTimestamp) std_msgs::msg::Header header; header.stamp = rclcpp::Time(4321); - // Use Published Time Publisher with a timestamp - published_time_publisher_->publish(test_publisher_, header.stamp); + // Use Published Time Publisher .publish() with a timestamp + published_time_publisher_->publish(first_test_publisher_, header.stamp); + rclcpp::spin_some(node_); + + // Check if the published_time_ is created + ASSERT_TRUE(first_published_time_ != nullptr); + + // Check if the published time is the same as the header + EXPECT_EQ(first_published_time_->header.stamp, header.stamp); +} + +TEST_F(PublishedTimePublisherTest, MultiplePublishMsgWithHeader) +{ + // Check if the PublishedTimePublisher is created + ASSERT_TRUE(published_time_publisher_ != nullptr); + + std_msgs::msg::Header header; + header.stamp = rclcpp::Time(12345); + + // Use Published Time Publisher .publish() with a header for multiple publishers + published_time_publisher_->publish(first_test_publisher_, header); + published_time_publisher_->publish(second_test_publisher_, header); + rclcpp::spin_some(node_); + + // Check if the published_time_ is created + ASSERT_TRUE(first_published_time_ != nullptr); + ASSERT_TRUE(second_published_time_ != nullptr); + + // Check if the published time is the same as the header + EXPECT_EQ(first_published_time_->header.stamp, header.stamp); + EXPECT_EQ(second_published_time_->header.stamp, header.stamp); +} + +TEST_F(PublishedTimePublisherTest, MultiplePublishMsgWithTimestamp) +{ + // Check if the PublishedTimePublisher is created + ASSERT_TRUE(published_time_publisher_ != nullptr); + + std_msgs::msg::Header header; + header.stamp = rclcpp::Time(12345); + + // Use Published Time Publisher .publish() with a timestamp for multiple publishers + published_time_publisher_->publish(first_test_publisher_, header.stamp); + published_time_publisher_->publish(second_test_publisher_, header.stamp); rclcpp::spin_some(node_); // Check if the published_time_ is created - ASSERT_TRUE(published_time_ != nullptr); + ASSERT_TRUE(first_published_time_ != nullptr); + ASSERT_TRUE(second_published_time_ != nullptr); // Check if the published time is the same as the header - EXPECT_EQ(published_time_->header.stamp, header.stamp); + EXPECT_EQ(first_published_time_->header.stamp, header.stamp); + EXPECT_EQ(second_published_time_->header.stamp, header.stamp); }