Add set_rate to PlayerClock (ros2#727)

* Add set_rate to PlayerClock

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

rosbag2_cpp/include/rosbag2_cpp/clocks/player_clock.hpp

@@ -59,6 +59,12 @@ class PlayerClock
   ROSBAG2_CPP_PUBLIC
   virtual bool sleep_until(rcutils_time_point_value_t until) = 0;
 
+  /**
+   * Change the rate of the flow of time for the clock
+   */
+  ROSBAG2_CPP_PUBLIC
+  virtual void set_rate(double rate) = 0;
+
   /**
    * Return the current playback rate.
    */

rosbag2_cpp/include/rosbag2_cpp/clocks/time_controller_clock.hpp

@@ -70,6 +70,12 @@ class TimeControllerClock : public PlayerClock
   ROSBAG2_CPP_PUBLIC
   bool sleep_until(rcutils_time_point_value_t until) override;
 
+  /**
+   * Change the rate of the flow of time for the clock
+   */
+  ROSBAG2_CPP_PUBLIC
+  void set_rate(double rate) override;
+
   /**
    * Return the current playback rate.
    */

rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp

@@ -154,6 +154,17 @@ bool TimeControllerClock::sleep_until(rcutils_time_point_value_t until)
   return now() >= until;
 }
 
+void TimeControllerClock::set_rate(double rate)
+{
+  std::lock_guard<std::mutex> lock(impl_->state_mutex);
+  if (impl_->rate == rate) {
+    return;
+  }
+  impl_->snapshot();
+  impl_->rate = rate;
+  impl_->cv.notify_all();
+}
+
 double TimeControllerClock::get_rate() const
 {
   std::lock_guard<std::mutex> lock(impl_->state_mutex);

rosbag2_cpp/test/rosbag2_cpp/test_time_controller_clock.cpp

@@ -200,3 +200,31 @@ TEST_F(TimeControllerClockTest, resumes_at_correct_ros_time)
   clock.resume();
   EXPECT_EQ(clock.now(), RCUTILS_S_TO_NS(1));
 }
+
+TEST_F(TimeControllerClockTest, change_rate)
+{
+  rosbag2_cpp::TimeControllerClock clock(ros_start_time, 1.0, now_fn);
+  rcutils_time_point_value_t expected_ros_time = 0;
+
+  // 1 second passes for both
+  return_time += std::chrono::seconds(1);
+  expected_ros_time += RCUTILS_S_TO_NS(1);
+  EXPECT_EQ(clock.now(), expected_ros_time);
+
+  // 1 steady second passes, 2 ros seconds
+  clock.set_rate(2.0);
+  return_time += std::chrono::seconds(1);
+  expected_ros_time += RCUTILS_S_TO_NS(2);
+  EXPECT_EQ(clock.now(), expected_ros_time);
+
+  clock.pause();
+  clock.set_rate(0.5);
+  return_time += std::chrono::seconds(1);
+  // ROS time doesn't proceed while paused
+  EXPECT_EQ(clock.now(), expected_ros_time);
+
+  clock.resume();
+  return_time += std::chrono::seconds(2);
+  expected_ros_time += RCUTILS_S_TO_NS(1);
+  EXPECT_EQ(clock.now(), expected_ros_time);
+}