More flexible clock publish test

Signed-off-by: Emerson Knapp <eknapp@amazon.com>

rosbag2_test_common/include/rosbag2_test_common/subscription_manager.hpp

@@ -55,8 +55,13 @@ class SubscriptionManager
       subscriber_node_->create_subscription<MessageT>(
         topic_name,
         qos,
-        [this, topic_name](std::shared_ptr<rclcpp::SerializedMessage> msg) {
+        [this, topic_name](std::shared_ptr<rclcpp::SerializedMessage> msg)
+        {
           subscribed_messages_[topic_name].push_back(msg);
+          // NOTE: would use MessageInfo, but
+          // source/received timestamps aren't supported on cyclone yet
+          subscribed_messages_received_time_[topic_name].push_back(
+            std::chrono::steady_clock::now());
         },
         options));
   }
@@ -75,6 +80,12 @@ class SubscriptionManager
     return received_messages_on_topic;
   }
 
+  std::vector<std::chrono::steady_clock::time_point> get_received_messages_time(
+    const std::string & topic_name)
+  {
+    return subscribed_messages_received_time_[topic_name];
+  }
+
   std::future<void> spin_subscriptions()
   {
     return async(
@@ -100,6 +111,9 @@ class SubscriptionManager
   std::vector<rclcpp::SubscriptionBase::SharedPtr> subscriptions_;
   std::unordered_map<std::string,
     std::vector<std::shared_ptr<rclcpp::SerializedMessage>>> subscribed_messages_;
+  std::unordered_map<
+    std::string, std::vector<std::chrono::steady_clock::time_point>
+  > subscribed_messages_received_time_;
   std::unordered_map<std::string, size_t> expected_topics_with_size_;
   rclcpp::Node::SharedPtr subscriber_node_;
   MemoryManagement memory_management_;

rosbag2_transport/include/rosbag2_transport/play_options.hpp

@@ -43,7 +43,7 @@ struct PlayOptions
 
   // Rate in Hz at which to publish to /clock.
   // 0 (or negative) means that no publisher will be created
-  double clock_publish_frequency = 0.f;
+  double clock_publish_frequency = 0.0;
 };
 
 }  // namespace rosbag2_transport

rosbag2_transport/test/rosbag2_transport/test_play_publish_clock.cpp

@@ -26,17 +26,28 @@
 
 using namespace ::testing;  // NOLINT
 
+namespace
+{
+template<typename Duration>
+rcutils_duration_value_t as_nanos(Duration d)
+{
+  return std::chrono::duration_cast<std::chrono::nanoseconds>(d).count();
+}
+
+rcutils_duration_value_t period_for_frequency(double frequency)
+{
+  return static_cast<rcutils_duration_value_t>(RCUTILS_S_TO_NS(1) / frequency);
+}
+}  // namespace
+
 TEST_F(RosBag2PlayTestFixture, clock_is_published_at_chosen_frequency)
 {
   // Test values
   play_options_.clock_publish_frequency = 20;
   const size_t messages_to_play = 10;
-  const int64_t milliseconds_between_messages = 200;
-  const auto bag_duration_seconds =
-    (messages_to_play - 1) * milliseconds_between_messages / 1000.f;
-  const auto error_tolerance = 0.1;
-  const size_t expected_clock_messages =
-    bag_duration_seconds * play_options_.clock_publish_frequency * (1.0 - error_tolerance);
+  const auto milliseconds_between_messages = 200;
+  // Wait for just a few clock messages, we're checking the time between them, not the total count
+  const size_t expected_clock_messages = 4;
 
   // Fake bag setup
   auto topic_types = std::vector<rosbag2_storage::TopicMetadata>{
@@ -46,7 +57,7 @@ TEST_F(RosBag2PlayTestFixture, clock_is_published_at_chosen_frequency)
   std::vector<std::shared_ptr<rosbag2_storage::SerializedBagMessage>> messages;
   for (size_t i = 0; i < messages_to_play; i++) {
     auto message = get_messages_basic_types()[0];
-    message->int32_value = i;
+    message->int32_value = static_cast<int32_t>(i);
     messages.push_back(
       serialize_test_message("topic1", milliseconds_between_messages * i, message));
   }
@@ -65,6 +76,17 @@ TEST_F(RosBag2PlayTestFixture, clock_is_published_at_chosen_frequency)
   rosbag2_transport.play(storage_options_, play_options_);
   await_received_messages.get();
 
+  // Check that we got enough messages
   auto received_clock = sub_->get_received_messages<rosgraph_msgs::msg::Clock>("/clock");
   EXPECT_THAT(received_clock, SizeIs(Ge(expected_clock_messages)));
+
+  // Check time deltas between messages
+  auto message_times = sub_->get_received_messages_time("/clock");
+  const auto expect_clock_delta = period_for_frequency(play_options_.clock_publish_frequency);
+  const auto allowed_error = static_cast<rcutils_duration_value_t>(expect_clock_delta * 0.1);
+  for (size_t i = 0; i < expected_clock_messages - 1; i++) {
+    auto delta = message_times[i + 1] - message_times[i];
+    auto error = std::abs(as_nanos(delta) - expect_clock_delta);
+    EXPECT_LE(error, allowed_error);
+  }
 }