Update PlayOptions::delay to rclcpp::Duration to get nanosecond r…

…esolution (ros2#843)

* Change type of PlayOptions::delay to rclcpp::Duration

Signed-off-by: Emerson Knapp <[email protected]>
Signed-off-by: Wojciech Jaworski <[email protected]>

ros2bag/ros2bag/verb/play.py

@@ -83,8 +83,8 @@ def add_arguments(self, parser, cli_name):  # noqa: D102
             help='Publish to /clock at a specific frequency in Hz, to act as a ROS Time Source. '
                  'Value must be positive. Defaults to not publishing.')
         parser.add_argument(
-            '-d', '--delay', type=float, default=0.0,
-            help='Sleep SEC seconds before play. Valid range > 0.0')
+            '-d', '--delay', type=positive_float, default=0.0,
+            help='Sleep duration before play (each loop), in seconds. Negative durations invalid.')
 
     def main(self, *, args):  # noqa: D102
         qos_profile_overrides = {}  # Specify a valid default

rosbag2_py/src/rosbag2_py/_transport.cpp

@@ -67,14 +67,24 @@ template<class T>
 struct OptionsWrapper : public T
 {
 public:
-  void setTopicQoSProfileOverrides(
-    const py::dict & overrides)
+  void setDelay(double delay)
+  {
+    this->delay = rclcpp::Duration::from_nanoseconds(
+      static_cast<rcl_duration_value_t>(RCUTILS_S_TO_NS(delay)));
+  }
+
+  double getDelay() const
+  {
+    return RCUTILS_NS_TO_S(static_cast<double>(this->delay.nanoseconds()));
+  }
+
+  void setTopicQoSProfileOverrides(const py::dict & overrides)
   {
     py_dict = overrides;
     this->topic_qos_profile_overrides = qos_map_from_py_dict(overrides);
   }
 
-  const py::dict & getTopicQoSProfileOverrides()
+  const py::dict & getTopicQoSProfileOverrides() const
   {
     return py_dict;
   }
@@ -225,7 +235,10 @@ PYBIND11_MODULE(_transport, m) {
   .def_readwrite("loop", &PlayOptions::loop)
   .def_readwrite("topic_remapping_options", &PlayOptions::topic_remapping_options)
   .def_readwrite("clock_publish_frequency", &PlayOptions::clock_publish_frequency)
-  .def_readwrite("delay", &PlayOptions::delay)
+  .def_property(
+    "delay",
+    &PlayOptions::getDelay,
+    &PlayOptions::setDelay)
   ;
 
   py::class_<RecordOptions>(m, "RecordOptions")

rosbag2_transport/include/rosbag2_transport/play_options.hpp

@@ -20,6 +20,7 @@
 #include <unordered_map>
 #include <vector>
 
+#include "rclcpp/duration.hpp"
 #include "rclcpp/qos.hpp"
 
 namespace rosbag2_transport
@@ -45,8 +46,8 @@ struct PlayOptions
   // 0 (or negative) means that no publisher will be created
   double clock_publish_frequency = 0.0;
 
-  // Sleep SEC seconds before play. Valid range > 0.0.
-  float delay = 0.0;
+  // Sleep before play. Negative durations invalid. Will delay at the beginning of each loop.
+  rclcpp::Duration delay = rclcpp::Duration(0, 0);
 };
 
 }  // namespace rosbag2_transport

rosbag2_transport/src/rosbag2_transport/player.cpp

@@ -212,20 +212,20 @@ bool Player::is_storage_completely_loaded() const
 
 void Player::play()
 {
-  float delay;
-  if (play_options_.delay >= 0.0) {
+  rclcpp::Duration delay(0, 0);
+  if (play_options_.delay >= rclcpp::Duration(0, 0)) {
     delay = play_options_.delay;
   } else {
-    RCLCPP_WARN(
-      this->get_logger(), "Invalid delay value: %f. Delay is disabled.", play_options_.delay);
-    delay = 0.0;
+    RCLCPP_WARN_STREAM(
+      this->get_logger(),
+      "Invalid delay value: " << play_options_.delay.nanoseconds() << ". Delay is disabled.");
   }
 
   try {
     do {
-      if (delay > 0.0) {
-        RCLCPP_INFO_STREAM(this->get_logger(), "Sleep " << delay << " sec");
-        std::chrono::duration<float> duration(delay);
+      if (delay > rclcpp::Duration(0, 0)) {
+        RCLCPP_INFO_STREAM(this->get_logger(), "Sleep " << delay.nanoseconds() << " ns");
+        std::chrono::nanoseconds duration(delay.nanoseconds());
         std::this_thread::sleep_for(duration);
       }
       reader_->open(storage_options_, {"", rmw_get_serialization_format()});

rosbag2_transport/test/rosbag2_transport/test_play_loop.cpp

@@ -44,7 +44,7 @@ TEST_F(RosBag2PlayTestFixture, play_bag_file_twice) {
   const float rate = 1.0;
   const bool loop_playback = false;
   double clock_publish_frequency = 0.0;
-  const float delay = 1.0;
+  const rclcpp::Duration delay(1, 0);
 
   auto primitive_message1 = get_messages_basic_types()[0];
   primitive_message1->int32_value = test_value;
@@ -104,7 +104,7 @@ TEST_F(RosBag2PlayTestFixture, messages_played_in_loop) {
   const float rate = 1.0;
   const bool loop_playback = true;
   const double clock_publish_frequency = 0.0;
-  const float delay = 1.0;
+  const rclcpp::Duration delay(1, 0);
 
   auto primitive_message1 = get_messages_basic_types()[0];
   primitive_message1->int32_value = test_value;

rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp

@@ -194,24 +194,23 @@ TEST_F(PlayerTestFixture, playing_respects_delay)
   auto primitive_message2 = get_messages_strings()[0];
   primitive_message2->string_value = "Hello World 2";
 
-  auto message_time_difference = std::chrono::seconds(1);
+  auto message_time_difference = rclcpp::Duration(1, 0);
   auto topics_and_types =
     std::vector<rosbag2_storage::TopicMetadata>{{"topic1", "test_msgs/Strings", "", ""}};
   std::vector<std::shared_ptr<rosbag2_storage::SerializedBagMessage>> messages =
   {serialize_test_message("topic1", 0, primitive_message1),
     serialize_test_message("topic1", 0, primitive_message2)};
 
   messages[0]->time_stamp = 100;
-  messages[1]->time_stamp =
-    messages[0]->time_stamp + std::chrono::nanoseconds(message_time_difference).count();
+  messages[1]->time_stamp = messages[0]->time_stamp + message_time_difference.nanoseconds();
 
-  float delay_margin = 1.0;
+  rclcpp::Duration delay_margin(1, 0);
 
   // Sleep 5.0 seconds before play
   {
-    play_options_.delay = 5.0;
-    std::chrono::duration<float> delay(play_options_.delay);
-    std::chrono::duration<float> delay_uppper(play_options_.delay + delay_margin);
+    play_options_.delay = rclcpp::Duration(5, 0);
+    auto lower_expected_duration = message_time_difference + play_options_.delay;
+    auto upper_expected_duration = message_time_difference + play_options_.delay + delay_margin;
 
     auto prepared_mock_reader = std::make_unique<MockSequentialReader>();
     prepared_mock_reader->prepare(messages, topics_and_types);
@@ -220,16 +219,18 @@ TEST_F(PlayerTestFixture, playing_respects_delay)
       std::move(reader), storage_options_, play_options_);
     auto start = std::chrono::steady_clock::now();
     player->play();
-    auto replay_time = std::chrono::steady_clock::now() - start;
 
-    ASSERT_THAT(replay_time, Gt(message_time_difference + delay));
-    ASSERT_THAT(replay_time, Lt(message_time_difference + delay_uppper));
+    auto replay_time = std::chrono::steady_clock::now() - start;
+    auto replay_nanos = std::chrono::nanoseconds(replay_time).count();
+    EXPECT_THAT(replay_nanos, Gt(lower_expected_duration.nanoseconds()));
+    EXPECT_THAT(replay_nanos, Lt(upper_expected_duration.nanoseconds()));
   }
 
   // Invalid value should result in playing at default delay 0.0
   {
-    play_options_.delay = -5.0;
-    std::chrono::duration<float> delay_uppper(delay_margin);
+    play_options_.delay = rclcpp::Duration(-5, 0);
+    auto lower_expected_duration = message_time_difference;
+    auto upper_expected_duration = message_time_difference + delay_margin;
 
     auto prepared_mock_reader = std::make_unique<MockSequentialReader>();
     prepared_mock_reader->prepare(messages, topics_and_types);
@@ -238,9 +239,10 @@ TEST_F(PlayerTestFixture, playing_respects_delay)
       std::move(reader), storage_options_, play_options_);
     auto start = std::chrono::steady_clock::now();
     player->play();
-    auto replay_time = std::chrono::steady_clock::now() - start;
 
-    ASSERT_THAT(replay_time, Gt(message_time_difference));
-    ASSERT_THAT(replay_time, Lt(message_time_difference + delay_uppper));
+    auto replay_time = std::chrono::steady_clock::now() - start;
+    auto replay_nanos = std::chrono::nanoseconds(replay_time).count();
+    EXPECT_THAT(replay_nanos, Gt(lower_expected_duration.nanoseconds()));
+    EXPECT_THAT(replay_nanos, Lt(upper_expected_duration.nanoseconds()));
   }
 }