Adds play until functionality -- follow up to ros2#962 (ros2#1005)

* Implements play until on top of play for. Signed-off-by: Agustin Alba Chicar <ag.albachicar@ekumenlabs.com> * Adds python bindings Signed-off-by: Agustin Alba Chicar <ag.albachicar@ekumenlabs.com> * Adds test for play until. Signed-off-by: Agustin Alba Chicar <ag.albachicar@ekumenlabs.com> * Fixes variable name. Signed-off-by: Agustin Alba Chicar <ag.albachicar@ekumenlabs.com> * Adds the same docstring for play_until and play_duration options than in PlayOptions to clarify usage. Signed-off-by: Agustin Alba Chicar <ag.albachicar@ekumenlabs.com> * Updates the CLI to combine the seconds and nanoseconds part of the playback_until stamp Signed-off-by: Agustin Alba Chicar <ag.albachicar@ekumenlabs.com> * Changes to play verb parameters: - Unifies the use of '-' instead of '_' to split words in arguments. - Uses mutually exclusive arguments: --playback-until-sec and --playback-until-nsec to pass the timestamp. Signed-off-by: Agustin Alba Chicar <ag.albachicar@ekumenlabs.com> * Solves review comments. - Adds test cases to the player. - Renames play_until and similar occurrences to contain timestamp. - Renames play_for and similar occurrences to not contain for and include duration. Signed-off-by: Agustin Alba Chicar <ag.albachicar@ekumenlabs.com> * Solves review comments to tests. Signed-off-by: Agustin Alba Chicar <ag.albachicar@ekumenlabs.com> * Modifies play_until_is_equal_to_the_total_duration The test body now evaluates a bag whose duration is exactly the same as given timestamp in PlayOptions. Signed-off-by: Agustin Alba Chicar <ag.albachicar@ekumenlabs.com>
DensoADAS · Jun 21, 2022 · ff482f0 · ff482f0
1 parent e127f97
commit ff482f0
Show file tree

Hide file tree

Showing 8 changed files with 492 additions and 12 deletions.
diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py
@@ -86,9 +86,32 @@ def add_arguments(self, parser, cli_name):  # noqa: D102
             '-d', '--delay', type=positive_float, default=0.0,
             help='Sleep duration before play (each loop), in seconds. Negative durations invalid.')
         parser.add_argument(
-            '--playback_duration', type=float, default=-1.0,
+            '--playback-duration', type=float, default=-1.0,
             help='Playback duration, in seconds. Negative durations mark an infinite playback. '
-                 'Default is -1.0.')
+                 'Default is -1.0. When positive, the maximum of `playback-until-*` and the one '
+                 'that this attribute yields will be used to determine which one stops playback '
+                 'execution.')
+
+        playback_until_arg_group = parser.add_mutually_exclusive_group()
+        playback_until_arg_group.add_argument(
+            '--playback-until-sec', type=float, default=-1.,
+            help='Playback until timestamp, expressed in seconds since epoch. '
+                 'Mutually exclusive argument with `--playback-until-nsec`. '
+                 'Use this argument when floating point to integer conversion error is not a '
+                 'problem for your application. Negative stamps disable this feature. Default is '
+                 '-1.0. When positive, the maximum of the effective time that '
+                 '`--playback-duration` yields and this attribute will be used to determine which '
+                 'one stops playback execution.')
+        playback_until_arg_group.add_argument(
+            '--playback-until-nsec', type=int, default=-1,
+            help='Playback until timestamp, expressed in nanoseconds since epoch.  '
+                 'Mutually exclusive argument with `--playback-until-sec`. '
+                 'Use this argument when floating point to integer conversion error matters for '
+                 'your application. Negative stamps disable this feature. Default is -1. When '
+                 'positive, the maximum of the effective time that `--playback-duration` yields '
+                 'and this attribute will be used to determine which one stops playback '
+                 'execution.')
+
         parser.add_argument(
             '--disable-keyboard-controls', action='store_true',
             help='disables keyboard controls for playback')
@@ -120,6 +143,14 @@ def add_arguments(self, parser, cli_name):  # noqa: D102
             help='Play for SEC seconds. Default is None, meaning that playback will continue '
                  'until the end of the bag. Valid range > 0.0')
 
+    def get_playback_until_from_arg_group(self, playback_until_sec, playback_until_nsec) -> int:
+        nano_scale = 1000 * 1000 * 1000
+        if playback_until_sec and playback_until_sec >= 0.0:
+            return int(playback_until_sec * nano_scale)
+        if playback_until_nsec and playback_until_nsec >= 0:
+            return playback_until_nsec
+        return -1
+
     def main(self, *, args):  # noqa: D102
         qos_profile_overrides = {}  # Specify a valid default
         if args.qos_profile_overrides_path:
@@ -155,6 +186,8 @@ def main(self, *, args):  # noqa: D102
         play_options.clock_publish_frequency = args.clock
         play_options.delay = args.delay
         play_options.playback_duration = args.playback_duration
+        play_options.playback_until_timestamp = self.get_playback_until_from_arg_group(
+            args.playback_until_sec, args.playback_until_nsec)
         play_options.disable_keyboard_controls = args.disable_keyboard_controls
         play_options.start_paused = args.start_paused
         play_options.start_offset = args.start_offset

diff --git a/rosbag2_interfaces/srv/Play.srv b/rosbag2_interfaces/srv/Play.srv
@@ -2,6 +2,8 @@
 builtin_interfaces/Time start_offset
 # See rosbag2_transport::PlayOptions::playback_duration
 builtin_interfaces/Duration playback_duration
+# See rosbag2_transport::PlayOptions::playback_until_timestamp
+builtin_interfaces/Time playback_until_timestamp
 ---
 # returns false when another playback execution is running, otherwise true
 bool success
diff --git a/rosbag2_py/src/rosbag2_py/_transport.cpp b/rosbag2_py/src/rosbag2_py/_transport.cpp
@@ -95,6 +95,17 @@ struct OptionsWrapper : public T
     return RCUTILS_NS_TO_S(static_cast<double>(this->start_offset));
   }
 
+  void setPlaybackUntilTimestamp(int64_t playback_until_timestamp)
+  {
+    this->playback_until_timestamp =
+      static_cast<rcutils_time_point_value_t>(playback_until_timestamp);
+  }
+
+  int64_t getPlaybackUntilTimestamp() const
+  {
+    return this->playback_until_timestamp;
+  }
+
   void setTopicQoSProfileOverrides(const py::dict & overrides)
   {
     py_dict = overrides;
@@ -288,6 +299,10 @@ PYBIND11_MODULE(_transport, m) {
     "start_offset",
     &PlayOptions::getStartOffset,
     &PlayOptions::setStartOffset)
+  .def_property(
+    "playback_until_timestamp",
+    &PlayOptions::getPlaybackUntilTimestamp,
+    &PlayOptions::setPlaybackUntilTimestamp)
   .def_readwrite("wait_acked_timeout", &PlayOptions::wait_acked_timeout)
   .def_readwrite("disable_loan_message", &PlayOptions::disable_loan_message)
   ;

diff --git a/rosbag2_transport/CMakeLists.txt b/rosbag2_transport/CMakeLists.txt
@@ -160,7 +160,13 @@ function(create_tests_for_rmw_implementation)
       LINK_LIBS rosbag2_transport
       AMENT_DEPS test_msgs rosbag2_test_common)
 
-  rosbag2_transport_add_gmock(test_burst
+  rosbag2_transport_add_gmock(test_play_until
+      test/rosbag2_transport/test_play_until.cpp
+      INCLUDE_DIRS $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/rosbag2_transport>
+      LINK_LIBS rosbag2_transport
+      AMENT_DEPS test_msgs rosbag2_test_common)
+
+    rosbag2_transport_add_gmock(test_burst
       test/rosbag2_transport/test_burst.cpp
       INCLUDE_DIRS $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/rosbag2_transport>
       LINK_LIBS rosbag2_transport

diff --git a/rosbag2_transport/include/rosbag2_transport/play_options.hpp b/rosbag2_transport/include/rosbag2_transport/play_options.hpp
@@ -22,6 +22,7 @@
 
 #include "keyboard_handler/keyboard_handler.hpp"
 #include "rclcpp/duration.hpp"
+#include "rclcpp/time.hpp"
 #include "rclcpp/qos.hpp"
 
 namespace rosbag2_transport
@@ -52,8 +53,16 @@ struct PlayOptions
 
   // Determines the maximum duration of the playback. Negative durations will make the playback to
   // not stop. Default configuration makes the player to not stop execution.
+  // When positive, the maximum of `play_until_timestamp` and the one that this attribute yields
+  // will be used to determine which one stops playback execution.
   rclcpp::Duration playback_duration = rclcpp::Duration(-1, 0);
 
+  // Determines the timestamp at which the playback will stop. Negative timestamps will make the
+  // playback to not stop. Default configuration makes the player to not stop execution.
+  // When positive, the maximum of the effective time that `playback_duration` yields and this
+  // attribute will be used to determine which one stops playback execution.
+  rcutils_time_point_value_t playback_until_timestamp = -1;
+
   // Start paused.
   bool start_paused = false;
 

diff --git a/rosbag2_transport/include/rosbag2_transport/player.hpp b/rosbag2_transport/include/rosbag2_transport/player.hpp
@@ -20,7 +20,6 @@
 #include <functional>
 #include <future>
 #include <memory>
-#include <optional>
 #include <queue>
 #include <string>
 #include <unordered_map>
@@ -268,9 +267,11 @@ class Player : public rclcpp::Node
 
   void create_control_services();
 
+  void configure_play_until_timestamp();
+
   rosbag2_storage::StorageOptions storage_options_;
   rosbag2_transport::PlayOptions play_options_;
-  rcutils_time_point_value_t play_until_time_ = -1;
+  rcutils_time_point_value_t play_until_timestamp_ = -1;
   moodycamel::ReaderWriterQueue<rosbag2_storage::SerializedBagMessageSharedPtr> message_queue_;
   mutable std::future<void> storage_loading_future_;
   std::atomic_bool load_storage_content_{true};

diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp
@@ -160,10 +160,7 @@ Player::Player(
     set_rate(play_options_.rate);
     topic_qos_profile_overrides_ = play_options_.topic_qos_profile_overrides;
     prepare_publishers();
-
-    if (play_options_.playback_duration >= rclcpp::Duration(0, 0)) {
-      play_until_time_ = starting_time_ + play_options_.playback_duration.nanoseconds();
-    }
+    configure_play_until_timestamp();
   }
   create_control_services();
   add_keyboard_callbacks();
@@ -212,7 +209,7 @@ bool Player::play()
       "Invalid delay value: " << play_options_.delay.nanoseconds() << ". Delay is disabled.");
   }
 
-  RCLCPP_INFO_STREAM(get_logger(), "Playback duration value: " << play_until_time_);
+  RCLCPP_INFO_STREAM(get_logger(), "Playback until timestamp: " << play_until_timestamp_);
 
   try {
     do {
@@ -367,7 +364,9 @@ bool Player::play_next()
 
   bool next_message_published = false;
   while (message_ptr != nullptr && !next_message_published) {
-    if (play_until_time_ >= starting_time_ && message_ptr->time_stamp > play_until_time_) {
+    if (play_until_timestamp_ >= starting_time_ &&
+      message_ptr->time_stamp > play_until_timestamp_)
+    {
       break;
     }
     {
@@ -531,7 +530,9 @@ void Player::play_messages_from_queue()
     ready_to_play_from_queue_cv_.notify_all();
   }
   while (message_ptr != nullptr && rclcpp::ok()) {
-    if (play_until_time_ >= starting_time_ && message_ptr->time_stamp > play_until_time_) {
+    if (play_until_timestamp_ >= starting_time_ &&
+      message_ptr->time_stamp > play_until_timestamp_)
+    {
       break;
     }
     // Do not move on until sleep_until returns true
@@ -781,6 +782,9 @@ void Player::create_control_services()
     {
       play_options_.start_offset = rclcpp::Time(request->start_offset).nanoseconds();
       play_options_.playback_duration = rclcpp::Duration(request->playback_duration);
+      play_options_.playback_until_timestamp =
+      rclcpp::Time(request->playback_until_timestamp).nanoseconds();
+      configure_play_until_timestamp();
       response->success = play();
     });
   srv_play_next_ = create_service<rosbag2_interfaces::srv::PlayNext>(
@@ -810,4 +814,17 @@ void Player::create_control_services()
     });
 }
 
+void Player::configure_play_until_timestamp()
+{
+  if (play_options_.playback_duration >= rclcpp::Duration(0, 0) ||
+    play_options_.playback_until_timestamp >= rcutils_time_point_value_t{0})
+  {
+    play_until_timestamp_ = std::max(
+      starting_time_ + play_options_.playback_duration.nanoseconds(),
+      play_options_.playback_until_timestamp);
+  } else {
+    play_until_timestamp_ = -1;
+  }
+}
+
 }  // namespace rosbag2_transport