diff --git a/rosbag2_cpp/include/rosbag2_cpp/clocks/player_clock.hpp b/rosbag2_cpp/include/rosbag2_cpp/clocks/player_clock.hpp index 78f25f95c3..34e68e084a 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/clocks/player_clock.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/clocks/player_clock.hpp @@ -52,9 +52,10 @@ class PlayerClock /** * Try to sleep (non-busy) the current thread until the provided time is reached - according to this Clock * - * Return true if the time has been reached, false if it was not successfully reached after sleeping - * for the appropriate duration. * The user should not take action based on this sleep until it returns true. + * + * \param until: The ROS time to sleep until + * \return true if the `until` has been reached, false if timeout or awakened early. */ ROSBAG2_CPP_PUBLIC virtual bool sleep_until(rcutils_time_point_value_t until) = 0; diff --git a/rosbag2_cpp/include/rosbag2_cpp/clocks/time_controller_clock.hpp b/rosbag2_cpp/include/rosbag2_cpp/clocks/time_controller_clock.hpp index 980e43545f..2768950e7e 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/clocks/time_controller_clock.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/clocks/time_controller_clock.hpp @@ -36,6 +36,10 @@ class TimeControllerClock : public PlayerClock * * \param starting_time: provides an initial offset for managing time * This will likely be the timestamp of the first message in the bag + * \param sleep_timeout: maximum amount of time to sleep even if time is not reached + * This helps dictate spin rate when clock is paused, as well as allowing periodic return + * of control even if messages are far apart. + * Value must be greater than 0. * \param rate: Rate of playback, a unit-free ratio. 1.0 is real-time * \param now_fn: Function used to get the current steady time * defaults to std::chrono::steady_clock::now @@ -45,6 +49,7 @@ class TimeControllerClock : public PlayerClock ROSBAG2_CPP_PUBLIC TimeControllerClock( rcutils_time_point_value_t starting_time, + std::chrono::nanoseconds sleep_timeout, double rate = 1.0, NowFunction now_fn = std::chrono::steady_clock::now); @@ -60,9 +65,10 @@ class TimeControllerClock : public PlayerClock /** * Try to sleep (non-busy) the current thread until the provided time is reached - according to this Clock * - * Return true if the time has been reached, false if it was not successfully reached after sleeping - * for the appropriate duration. * The user should not take action based on this sleep until it returns true. + * + * \param until: The ROS time to sleep until + * \return true if the `until` has been reached, false if timeout or awakened early. */ ROSBAG2_CPP_PUBLIC bool sleep_until(rcutils_time_point_value_t until) override; diff --git a/rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp b/rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp index a51eb95938..681ee09a9b 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -55,8 +56,11 @@ class TimeControllerClockImpl std::chrono::steady_clock::time_point steady; }; - explicit TimeControllerClockImpl(PlayerClock::NowFunction now_fn) - : now_fn(now_fn) + explicit TimeControllerClockImpl( + PlayerClock::NowFunction now_fn, + std::chrono::nanoseconds sleep_timeout) + : now_fn(now_fn), + sleep_timeout(sleep_timeout) {} virtual ~TimeControllerClockImpl() = default; @@ -82,6 +86,7 @@ class TimeControllerClockImpl } const PlayerClock::NowFunction now_fn; + const std::chrono::nanoseconds sleep_timeout; std::mutex state_mutex; std::condition_variable cv RCPPUTILS_TSA_GUARDED_BY(state_mutex); @@ -91,11 +96,15 @@ class TimeControllerClockImpl TimeControllerClock::TimeControllerClock( rcutils_time_point_value_t starting_time, + std::chrono::nanoseconds sleep_timeout, double rate, NowFunction now_fn) -: impl_(std::make_unique(now_fn)) +: impl_(std::make_unique(now_fn, sleep_timeout)) { std::lock_guard lock(impl_->state_mutex); + if (sleep_timeout <= std::chrono::nanoseconds{0}) { + throw std::runtime_error("Sleep timeout cannot be negative or 0"); + } impl_->reference.ros = starting_time; impl_->reference.steady = impl_->now_fn(); impl_->rate = rate; @@ -112,9 +121,10 @@ rcutils_time_point_value_t TimeControllerClock::now() const bool TimeControllerClock::sleep_until(rcutils_time_point_value_t until) { + const auto until_timeout = impl_->now_fn() + impl_->sleep_timeout; { TSAUniqueLock lock(impl_->state_mutex); - const auto steady_until = impl_->ros_to_steady(until); + const auto steady_until = std::min(impl_->ros_to_steady(until), until_timeout); impl_->cv.wait_until(lock, steady_until); } return now() >= until; diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_time_controller_clock.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_time_controller_clock.cpp index 9953bc0550..3eb69abf4a 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_time_controller_clock.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_time_controller_clock.cpp @@ -14,6 +14,7 @@ #include +#include "rcutils/time.h" #include "rosbag2_cpp/clocks/time_controller_clock.hpp" using namespace testing; // NOLINT @@ -30,20 +31,34 @@ class TimeControllerClockTest : public Test }; } + template + rcutils_time_point_value_t as_nanos(Duration d) + { + return std::chrono::duration_cast(d).count(); + } + rcutils_time_point_value_t as_nanos(const SteadyTimePoint & time) { - return std::chrono::duration_cast(time.time_since_epoch()).count(); + return as_nanos(time.time_since_epoch()); + } + + template + rcutils_duration_value_t abs_diff(Duration1 a, Duration2 b) + { + return std::abs(as_nanos(a) - as_nanos(b)); } NowFunction now_fn; + double playback_rate = 1.0; SteadyTimePoint return_time; // defaults to 0 + std::chrono::nanoseconds sleep_timeout{25000000}; // 40Hz rcutils_time_point_value_t ros_start_time = 0; }; TEST_F(TimeControllerClockTest, steadytime_precision) { const double playback_rate = 1.0; - rosbag2_cpp::TimeControllerClock pclock(ros_start_time, playback_rate, now_fn); + rosbag2_cpp::TimeControllerClock pclock(ros_start_time, sleep_timeout, playback_rate, now_fn); const SteadyTimePoint begin_time(std::chrono::seconds(0)); return_time = begin_time; @@ -75,7 +90,7 @@ TEST_F(TimeControllerClockTest, nonzero_start_time) { ros_start_time = 1234567890LL; const double playback_rate = 1.0; - rosbag2_cpp::TimeControllerClock pclock(ros_start_time, playback_rate, now_fn); + rosbag2_cpp::TimeControllerClock pclock(ros_start_time, sleep_timeout, playback_rate, now_fn); const SteadyTimePoint begin_time(std::chrono::seconds(0)); return_time = begin_time; @@ -88,7 +103,7 @@ TEST_F(TimeControllerClockTest, nonzero_start_time) TEST_F(TimeControllerClockTest, fast_rate) { const double playback_rate = 2.5; - rosbag2_cpp::TimeControllerClock pclock(ros_start_time, playback_rate, now_fn); + rosbag2_cpp::TimeControllerClock pclock(ros_start_time, sleep_timeout, playback_rate, now_fn); const SteadyTimePoint begin_time(std::chrono::seconds(0)); return_time = begin_time; @@ -102,7 +117,7 @@ TEST_F(TimeControllerClockTest, fast_rate) TEST_F(TimeControllerClockTest, slow_rate) { const double playback_rate = 0.4; - rosbag2_cpp::TimeControllerClock pclock(ros_start_time, playback_rate, now_fn); + rosbag2_cpp::TimeControllerClock pclock(ros_start_time, sleep_timeout, playback_rate, now_fn); const SteadyTimePoint begin_time(std::chrono::seconds(0)); return_time = begin_time; @@ -112,3 +127,40 @@ TEST_F(TimeControllerClockTest, slow_rate) return_time = some_time; EXPECT_EQ(pclock.now(), as_nanos(some_time) * playback_rate); } + +TEST_F(TimeControllerClockTest, sleep_rate_checking) +{ + sleep_timeout = std::chrono::nanoseconds{0}; + EXPECT_THROW( + rosbag2_cpp::TimeControllerClock( + ros_start_time, sleep_timeout, playback_rate, now_fn), std::runtime_error); + + sleep_timeout = std::chrono::nanoseconds{-5}; + EXPECT_THROW( + rosbag2_cpp::TimeControllerClock( + ros_start_time, sleep_timeout, playback_rate, now_fn), std::runtime_error); +} + +TEST_F(TimeControllerClockTest, sleep_rate_control) +{ + const rcutils_time_point_value_t one_second = RCUTILS_S_TO_NS(1); + const unsigned int call_count_goal = 10; + sleep_timeout = std::chrono::nanoseconds{RCUTILS_S_TO_NS(1) / 40}; // 40Hz + const auto expected_elapsed = sleep_timeout * call_count_goal; + // Giving 10% error tolerance because sleeping is not exact. + // An incorrect result would be orders of magnitude off (free loop with no wait). + const auto error_tolerance = static_cast( + as_nanos(expected_elapsed) * 0.1); + + // Don't override now_fn, we need to use the real clock in order to have sleep work + rosbag2_cpp::TimeControllerClock clock(ros_start_time, sleep_timeout); + + const auto start_time = std::chrono::steady_clock::now(); + for (unsigned int call_count = 0; call_count < call_count_goal; call_count++) { + EXPECT_FALSE(clock.sleep_until(one_second)); + } + const auto elapsed_time = std::chrono::steady_clock::now() - start_time; + + const auto time_error = abs_diff(elapsed_time, expected_elapsed); + EXPECT_LE(time_error, error_tolerance); +} diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 6088cf200b..5c4cdc7a36 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -217,7 +217,8 @@ void Player::prepare_publishers(const PlayOptions & options) void Player::prepare_clock(const PlayOptions & options, rcutils_time_point_value_t starting_time) { double rate = options.rate > 0.0 ? options.rate : 1.0; - clock_ = std::make_unique(starting_time, rate); + std::chrono::nanoseconds sleep_timeout{RCUTILS_S_TO_NS(1)}; // default 1Hz spin on paused + clock_ = std::make_unique(starting_time, sleep_timeout, rate); } } // namespace rosbag2_transport