Add simple keyboard control for playback rate (ros2#893)

* Keyboard controls for playback rate

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

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

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

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

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);
 }