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 6bf3a6af7c699..8c95c9ce9e955 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 @@ -20,27 +20,37 @@ #include -class PublishedTimePublisherTest : public ::testing::Test +class PublishedTimePublisherWithSubscriptionTest : public ::testing::Test { protected: std::shared_ptr node_{nullptr}; - std::shared_ptr published_time_publisher_{nullptr}; + std::shared_ptr published_time_publisher_ptr_{ + nullptr}; - std::shared_ptr> first_test_publisher_{nullptr}; - std::shared_ptr> second_test_publisher_{nullptr}; + std::shared_ptr> first_test_publisher_ptr_{nullptr}; + std::shared_ptr> second_test_publisher_ptr_{nullptr}; std::shared_ptr> - first_test_subscriber_{nullptr}; + first_test_subscriber_ptr_{nullptr}; std::shared_ptr> - second_test_subscriber_{nullptr}; + second_test_subscriber_ptr_{nullptr}; - autoware_internal_msgs::msg::PublishedTime::ConstSharedPtr first_published_time_{nullptr}; - autoware_internal_msgs::msg::PublishedTime::ConstSharedPtr second_published_time_{nullptr}; + autoware_internal_msgs::msg::PublishedTime::ConstSharedPtr first_published_time_ptr_{nullptr}; + autoware_internal_msgs::msg::PublishedTime::ConstSharedPtr second_published_time_ptr_{nullptr}; + + std_msgs::msg::Header first_header_; + std_msgs::msg::Header second_header_; void SetUp() override { ASSERT_TRUE(rclcpp::ok()); + // init headers which will be used to publish + first_header_.stamp = rclcpp::Time(0); + first_header_.frame_id = "frame_id_1"; + second_header_.stamp = rclcpp::Time(1); + second_header_.frame_id = "frame_id_2"; + // 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 = @@ -51,30 +61,31 @@ class PublishedTimePublisherTest : public ::testing::Test node_ = std::make_shared(base_name + "_node"); // Create the first publisher - first_test_publisher_ = + first_test_publisher_ptr_ = node_->create_publisher(base_name + "_topic1", 1); // Create the second publisher - second_test_publisher_ = + second_test_publisher_ptr_ = node_->create_publisher(base_name + "_topic2", 1); // Create a PublishedTimePublisher - published_time_publisher_ = + published_time_publisher_ptr_ = std::make_shared(node_.get()); // Create the first subscriber - first_test_subscriber_ = node_->create_subscription( - base_name + "_topic1" + suffix, 1, - [this](autoware_internal_msgs::msg::PublishedTime::ConstSharedPtr msg) { - this->first_published_time_ = std::move(msg); - }); + first_test_subscriber_ptr_ = + node_->create_subscription( + base_name + "_topic1" + suffix, 1, + [this](autoware_internal_msgs::msg::PublishedTime::ConstSharedPtr msg) { + this->first_published_time_ptr_ = std::move(msg); + }); // Create the second subscriber - second_test_subscriber_ = + second_test_subscriber_ptr_ = node_->create_subscription( base_name + "_topic2" + suffix, 1, [this](autoware_internal_msgs::msg::PublishedTime::ConstSharedPtr msg) { - this->second_published_time_ = std::move(msg); + this->second_published_time_ptr_ = std::move(msg); }); rclcpp::spin_some(node_); @@ -83,84 +94,233 @@ class PublishedTimePublisherTest : public ::testing::Test void TearDown() override {} }; -TEST_F(PublishedTimePublisherTest, PublishMsgWithHeader) +class PublishedTimePublisherWithoutSubscriptionTest : public ::testing::Test { - // Check if the PublishedTimePublisher is created - ASSERT_TRUE(published_time_publisher_ != nullptr); +protected: + std::shared_ptr node_{nullptr}; + std::shared_ptr published_time_publisher_ptr_{ + nullptr}; + + std::shared_ptr> first_test_publisher_ptr_{nullptr}; + std::shared_ptr> second_test_publisher_ptr_{nullptr}; + + autoware_internal_msgs::msg::PublishedTime::ConstSharedPtr first_published_time_ptr_{nullptr}; + autoware_internal_msgs::msg::PublishedTime::ConstSharedPtr second_published_time_ptr_{nullptr}; + + std_msgs::msg::Header first_header_; + std_msgs::msg::Header second_header_; + + void SetUp() override + { + ASSERT_TRUE(rclcpp::ok()); + + // init headers which will be used to publish + first_header_.stamp = rclcpp::Time(0); + first_header_.frame_id = "frame_id_1"; + second_header_.stamp = rclcpp::Time(1); + second_header_.frame_id = "frame_id_2"; + + // 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"; // Suffix for published time topics + + // Create a node + node_ = std::make_shared(base_name + "_node"); + + // Create the first publisher + first_test_publisher_ptr_ = + node_->create_publisher(base_name + "_topic1", 1); + + // Create the second publisher + second_test_publisher_ptr_ = + node_->create_publisher(base_name + "_topic2", 1); + + // Create a PublishedTimePublisher + published_time_publisher_ptr_ = + std::make_shared(node_.get()); + + rclcpp::spin_some(node_); + } + + void TearDown() override {} +}; - std_msgs::msg::Header header; - header.stamp = rclcpp::Time(1234); +TEST_F(PublishedTimePublisherWithSubscriptionTest, WithoutPublishing) +{ + // Check if the PublishedTimePublisher is created + ASSERT_TRUE(published_time_publisher_ptr_ != nullptr); // Use Published Time Publisher .publish_if_subscribed() with a header - published_time_publisher_->publish_if_subscribed(first_test_publisher_, header); + published_time_publisher_ptr_->publish_if_subscribed(first_test_publisher_ptr_, first_header_); rclcpp::spin_some(node_); // Check if the published_time_ is created - ASSERT_TRUE(first_published_time_ != nullptr); + ASSERT_TRUE(first_published_time_ptr_ != nullptr); // Check if the published time is the same as the header - EXPECT_EQ(first_published_time_->header.stamp, header.stamp); + EXPECT_EQ(first_published_time_ptr_->header.stamp, first_header_.stamp); } -TEST_F(PublishedTimePublisherTest, PublishMsgWithTimestamp) +TEST_F(PublishedTimePublisherWithSubscriptionTest, PublishMsgWithHeader) { // Check if the PublishedTimePublisher is created - ASSERT_TRUE(published_time_publisher_ != nullptr); + ASSERT_TRUE(published_time_publisher_ptr_ != nullptr); - std_msgs::msg::Header header; - header.stamp = rclcpp::Time(4321); + // Use Published Time Publisher .publish_if_subscribed() with a header + published_time_publisher_ptr_->publish_if_subscribed(first_test_publisher_ptr_, first_header_); + rclcpp::spin_some(node_); + + // Check if the published_time_ is created + ASSERT_TRUE(first_published_time_ptr_ != nullptr); + + // Check if the published time is the same as the header + EXPECT_EQ(first_published_time_ptr_->header.stamp, first_header_.stamp); +} + +TEST_F(PublishedTimePublisherWithSubscriptionTest, PublishMsgWithTimestamp) +{ + // Check if the PublishedTimePublisher is created + ASSERT_TRUE(published_time_publisher_ptr_ != nullptr); // Use Published Time Publisher .publish_if_subscribed() with a timestamp - published_time_publisher_->publish_if_subscribed(first_test_publisher_, header.stamp); + published_time_publisher_ptr_->publish_if_subscribed( + first_test_publisher_ptr_, first_header_.stamp); rclcpp::spin_some(node_); // Check if the published_time_ is created - ASSERT_TRUE(first_published_time_ != nullptr); + ASSERT_TRUE(first_published_time_ptr_ != nullptr); // Check if the published time is the same as the header - EXPECT_EQ(first_published_time_->header.stamp, header.stamp); + EXPECT_EQ(first_published_time_ptr_->header.stamp, first_header_.stamp); } -TEST_F(PublishedTimePublisherTest, MultiplePublishMsgWithHeader) +TEST_F(PublishedTimePublisherWithSubscriptionTest, MultiplePublishMsgWithHeader) { // Check if the PublishedTimePublisher is created - ASSERT_TRUE(published_time_publisher_ != nullptr); - - std_msgs::msg::Header header; - header.stamp = rclcpp::Time(12345); + ASSERT_TRUE(published_time_publisher_ptr_ != nullptr); // Use Published Time Publisher .publish_if_subscribed() with a header for multiple publishers - published_time_publisher_->publish_if_subscribed(first_test_publisher_, header); - published_time_publisher_->publish_if_subscribed(second_test_publisher_, header); + published_time_publisher_ptr_->publish_if_subscribed(first_test_publisher_ptr_, first_header_); + published_time_publisher_ptr_->publish_if_subscribed(second_test_publisher_ptr_, first_header_); rclcpp::spin_some(node_); // Check if the published_time_ is created - ASSERT_TRUE(first_published_time_ != nullptr); - ASSERT_TRUE(second_published_time_ != nullptr); + ASSERT_TRUE(first_published_time_ptr_ != nullptr); + ASSERT_TRUE(second_published_time_ptr_ != 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); + EXPECT_EQ(first_published_time_ptr_->header.stamp, first_header_.stamp); + EXPECT_EQ(second_published_time_ptr_->header.stamp, first_header_.stamp); } -TEST_F(PublishedTimePublisherTest, MultiplePublishMsgWithTimestamp) +TEST_F(PublishedTimePublisherWithSubscriptionTest, MultiplePublishMsgWithTimestamp) { // Check if the PublishedTimePublisher is created - ASSERT_TRUE(published_time_publisher_ != nullptr); - - std_msgs::msg::Header header; - header.stamp = rclcpp::Time(12345); + ASSERT_TRUE(published_time_publisher_ptr_ != nullptr); // Use Published Time Publisher .publish_if_subscribed() with a timestamp for multiple publishers - published_time_publisher_->publish_if_subscribed(first_test_publisher_, header.stamp); - published_time_publisher_->publish_if_subscribed(second_test_publisher_, header.stamp); + published_time_publisher_ptr_->publish_if_subscribed( + first_test_publisher_ptr_, first_header_.stamp); + published_time_publisher_ptr_->publish_if_subscribed( + second_test_publisher_ptr_, first_header_.stamp); rclcpp::spin_some(node_); // Check if the published_time_ is created - ASSERT_TRUE(first_published_time_ != nullptr); - ASSERT_TRUE(second_published_time_ != nullptr); + ASSERT_TRUE(first_published_time_ptr_ != nullptr); + ASSERT_TRUE(second_published_time_ptr_ != 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); + EXPECT_EQ(first_published_time_ptr_->header.stamp, first_header_.stamp); + EXPECT_EQ(second_published_time_ptr_->header.stamp, first_header_.stamp); +} + +TEST_F(PublishedTimePublisherWithoutSubscriptionTest, WithoutPublishing) +{ + // Check if the PublishedTimePublisher is created + ASSERT_TRUE(published_time_publisher_ptr_ != nullptr); + + // Use Published Time Publisher .publish_if_subscribed() with a header + published_time_publisher_ptr_->publish_if_subscribed(first_test_publisher_ptr_, first_header_); + rclcpp::spin_some(node_); + + // Check if the published_time_ is created + ASSERT_TRUE(first_published_time_ptr_ != nullptr); + + // Check if the published time is null + EXPECT_EQ(first_published_time_ptr_, nullptr); +} + +TEST_F(PublishedTimePublisherWithoutSubscriptionTest, PublishMsgWithHeader) +{ + // Check if the PublishedTimePublisher is created + ASSERT_TRUE(published_time_publisher_ptr_ != nullptr); + + // Use Published Time Publisher .publish_if_subscribed() with a header + published_time_publisher_ptr_->publish_if_subscribed(first_test_publisher_ptr_, first_header_); + rclcpp::spin_some(node_); + + // Check if the published_time_ is created + ASSERT_TRUE(first_published_time_ptr_ != nullptr); + + // Check if the published time is null + EXPECT_EQ(first_published_time_ptr_, nullptr); +} + +TEST_F(PublishedTimePublisherWithoutSubscriptionTest, PublishMsgWithTimestamp) +{ + // Check if the PublishedTimePublisher is created + ASSERT_TRUE(published_time_publisher_ptr_ != nullptr); + + // Use Published Time Publisher .publish_if_subscribed() with a timestamp + published_time_publisher_ptr_->publish_if_subscribed( + first_test_publisher_ptr_, first_header_.stamp); + rclcpp::spin_some(node_); + + // Check if the published_time_ is created + ASSERT_TRUE(first_published_time_ptr_ != nullptr); + + // Check if the published time is null + EXPECT_EQ(first_published_time_ptr_, nullptr); +} + +TEST_F(PublishedTimePublisherWithoutSubscriptionTest, MultiplePublishMsgWithHeader) +{ + // Check if the PublishedTimePublisher is created + ASSERT_TRUE(published_time_publisher_ptr_ != nullptr); + + // Use Published Time Publisher .publish_if_subscribed() with a header for multiple publishers + published_time_publisher_ptr_->publish_if_subscribed(first_test_publisher_ptr_, first_header_); + published_time_publisher_ptr_->publish_if_subscribed(second_test_publisher_ptr_, first_header_); + rclcpp::spin_some(node_); + + // Check if the published_time_ is created + ASSERT_TRUE(first_published_time_ptr_ != nullptr); + ASSERT_TRUE(second_published_time_ptr_ != nullptr); + + // Check if the published time is null + EXPECT_EQ(first_published_time_ptr_, nullptr); + EXPECT_EQ(second_published_time_ptr_, nullptr); +} + +TEST_F(PublishedTimePublisherWithoutSubscriptionTest, MultiplePublishMsgWithTimestamp) +{ + // Check if the PublishedTimePublisher is created + ASSERT_TRUE(published_time_publisher_ptr_ != nullptr); + + // Use Published Time Publisher .publish_if_subscribed() with a timestamp for multiple publishers + published_time_publisher_ptr_->publish_if_subscribed( + first_test_publisher_ptr_, first_header_.stamp); + published_time_publisher_ptr_->publish_if_subscribed( + second_test_publisher_ptr_, first_header_.stamp); + rclcpp::spin_some(node_); + + // Check if the published_time_ is created + ASSERT_TRUE(first_published_time_ptr_ != nullptr); + ASSERT_TRUE(second_published_time_ptr_ != nullptr); + + // Check if the published time is null + EXPECT_EQ(first_published_time_ptr_, nullptr); + EXPECT_EQ(second_published_time_ptr_, nullptr); }