From 26a440ce06c5c254a669a0b164a5747f15ef3fd1 Mon Sep 17 00:00:00 2001
From: Emerson Knapp <537409+emersonknapp@users.noreply.github.com>
Date: Wed, 27 Oct 2021 16:38:28 -0700
Subject: [PATCH] Add simple keyboard control for playback rate (#893)

* Keyboard controls for playback rate

Signed-off-by: Emerson Knapp <eknapp@amazon.com>
---
 .../include/rosbag2_transport/play_options.hpp |  2 ++
 .../include/rosbag2_transport/player.hpp       |  2 +-
 .../src/rosbag2_transport/player.cpp           | 18 +++++++++++++++++-
 .../test_keyboard_controls.cpp                 | 12 ++++++++++++
 4 files changed, 32 insertions(+), 2 deletions(-)

diff --git a/rosbag2_transport/include/rosbag2_transport/play_options.hpp b/rosbag2_transport/include/rosbag2_transport/play_options.hpp
index f8dc1690d6..2938098fd9 100644
--- a/rosbag2_transport/include/rosbag2_transport/play_options.hpp
+++ b/rosbag2_transport/include/rosbag2_transport/play_options.hpp
@@ -54,6 +54,8 @@ struct PlayOptions
   // keybindings
   KeyboardHandler::KeyCode pause_resume_toggle_key = KeyboardHandler::KeyCode::SPACE;
   KeyboardHandler::KeyCode play_next_key = KeyboardHandler::KeyCode::CURSOR_RIGHT;
+  KeyboardHandler::KeyCode increase_rate_key = KeyboardHandler::KeyCode::CURSOR_UP;
+  KeyboardHandler::KeyCode decrease_rate_key = KeyboardHandler::KeyCode::CURSOR_DOWN;
 };
 
 }  // namespace rosbag2_transport
diff --git a/rosbag2_transport/include/rosbag2_transport/player.hpp b/rosbag2_transport/include/rosbag2_transport/player.hpp
index 88e79d34b3..b7fcf7b15f 100644
--- a/rosbag2_transport/include/rosbag2_transport/player.hpp
+++ b/rosbag2_transport/include/rosbag2_transport/player.hpp
@@ -117,7 +117,7 @@ class Player : public rclcpp::Node
   /// \brief Set the playback rate.
   /// \return false if an invalid value was provided (<= 0).
   ROSBAG2_TRANSPORT_PUBLIC
-  bool set_rate(double);
+  virtual bool set_rate(double);
 
   /// \brief Playing next message from queue when in pause.
   /// \details This is blocking call and it will wait until next available message will be
diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp
index 644545b09d..e232f7faf9 100644
--- a/rosbag2_transport/src/rosbag2_transport/player.cpp
+++ b/rosbag2_transport/src/rosbag2_transport/player.cpp
@@ -311,7 +311,13 @@ double Player::get_rate() const
 
 bool Player::set_rate(double rate)
 {
-  return clock_->set_rate(rate);
+  bool ok = clock_->set_rate(rate);
+  if (ok) {
+    RCLCPP_INFO_STREAM(get_logger(), "Set rate to " << rate);
+  } else {
+    RCLCPP_WARN_STREAM(get_logger(), "Failed to set rate to invalid value " << rate);
+  }
+  return ok;
 }
 
 rosbag2_storage::SerializedBagMessageSharedPtr * Player::peek_next_message_from_queue()
@@ -590,6 +596,16 @@ void Player::add_keyboard_callbacks()
     [this]() {play_next();},
     "Play Next Message"
   );
+  add_key_callback(
+    play_options_.increase_rate_key,
+    [this]() {set_rate(get_rate() * 1.1);},
+    "Increase Rate 10%"
+  );
+  add_key_callback(
+    play_options_.decrease_rate_key,
+    [this]() {set_rate(get_rate() * 0.9);},
+    "Decrease Rate 10%"
+  );
 }
 
 }  // namespace rosbag2_transport
diff --git a/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp b/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp
index 8da1bb9625..d01857b723 100644
--- a/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp
+++ b/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp
@@ -72,9 +72,17 @@ class CountPlayer : public Player
     return ret;
   }
 
+  bool set_rate(double rate) override
+  {
+    bool ret = Player::set_rate(rate);
+    num_set_rate++;
+    return ret;
+  }
+
   int num_paused = 0;
   int num_resumed = 0;
   int num_played_next = 0;
+  int num_set_rate = 0;
 };
 
 
@@ -147,6 +155,9 @@ TEST_F(RosBag2PlayTestFixture, test_keyboard_controls)
   keyboard_handler->simulate_key_press(play_options_.pause_resume_toggle_key);
   EXPECT_THAT(player->is_paused(), true);
 
+  keyboard_handler->simulate_key_press(play_options_.increase_rate_key);
+  keyboard_handler->simulate_key_press(play_options_.decrease_rate_key);
+
   // start play thread
   std::thread player_thread = std::thread([player]() {player->play();});
 
@@ -164,4 +175,5 @@ TEST_F(RosBag2PlayTestFixture, test_keyboard_controls)
   EXPECT_THAT(player->num_paused, 1);
   EXPECT_THAT(player->num_resumed, 1);
   EXPECT_THAT(player->num_played_next, 1);
+  EXPECT_THAT(player->num_set_rate, 2);
 }