diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index 0ebc2824f0..f682e9b44a 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -43,6 +43,7 @@ #include "../utils/rclcpp_gtest_macros.hpp" #include "test_msgs/msg/empty.hpp" +#include "test_msgs/msg/basic_types.hpp" using namespace std::chrono_literals; @@ -890,3 +891,54 @@ TEST_F(TestSubscription, disable_enable_event_callbacks) EXPECT_EQ(deadline_callback_count, 2); EXPECT_EQ(liveliness_callback_count, 2); } + +/* + Testing behavior when the queue size of the subscriber is reached. + See https://github.com/ros2/rclcpp/issues/421 + */ +TEST_F(TestSubscription, queue_size_behavior) { + initialize(); + using test_msgs::msg::BasicTypes; + + constexpr size_t depth = 3; + rclcpp::QoS sub_qos(depth); + + std::vector received_values; + auto callback = [&received_values](BasicTypes::ConstSharedPtr msg) { + received_values.push_back(msg->int32_value); + }; + + auto sub = node_->create_subscription("test_queue_size", sub_qos, callback); + + // Publisher with a larger queue size just to be safe + rclcpp::QoS pub_qos(10); + auto pub = node_->create_publisher("test_queue_size", pub_qos); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node_); + + // Spin a bit to ensure discovery + auto start_time = std::chrono::steady_clock::now(); + while (pub->get_subscription_count() == 0 && (std::chrono::steady_clock::now() - start_time) < 10s) { + executor.spin_node_some(node_); + std::this_thread::sleep_for(10ms); + } + ASSERT_GT(pub->get_subscription_count(), 0u); + + // Publish more messages than the subscriber's queue size + for (int32_t i = 1; i <= 5; ++i) { + BasicTypes msg; + msg.int32_value = i; + pub->publish(msg); + } + + // Now spin to receive messages + start_time = std::chrono::steady_clock::now(); + while ((std::chrono::steady_clock::now() - start_time) < 1s) { + executor.spin_node_some(node_); + std::this_thread::sleep_for(10ms); + } + + // We expect to have received exactly `depth` messages. + EXPECT_EQ(received_values.size(), depth); +}