keyboard controls for pause/resume toggle and play-next:

- supports custom keyboard bindings set through commandline into play_options_

Signed-off-by: Sonia Jin <[email protected]>

ros2bag/ros2bag/verb/play.py

@@ -85,6 +85,12 @@ def add_arguments(self, parser, cli_name):  # noqa: D102
         parser.add_argument(
             '-d', '--delay', type=float, default=0.0,
             help='Sleep SEC seconds before play. Valid range > 0.0')
+        parser.add_argument(
+            '--pause-resume-toggle-key', type=str, default='SPACE',
+            help='Keybinding for toggling pause/resume.')
+        parser.add_argument(
+            '--play-next-key', type=str, default='s',
+            help='Keybinding for playing next message in paused mode')
 
     def main(self, *, args):  # noqa: D102
         qos_profile_overrides = {}  # Specify a valid default
@@ -120,6 +126,8 @@ def main(self, *, args):  # noqa: D102
         play_options.topic_remapping_options = topic_remapping
         play_options.clock_publish_frequency = args.clock
         play_options.delay = args.delay
+        play_options.pause_resume_toggle_key = args.pause_resume_toggle_key
+        play_options.play_next_key = args.play_next_key
 
         player = Player()
         player.play(storage_options, play_options)

rosbag2_py/src/rosbag2_py/_transport.cpp

@@ -226,6 +226,8 @@ PYBIND11_MODULE(_transport, m) {
   .def_readwrite("topic_remapping_options", &PlayOptions::topic_remapping_options)
   .def_readwrite("clock_publish_frequency", &PlayOptions::clock_publish_frequency)
   .def_readwrite("delay", &PlayOptions::delay)
+  .def_readwrite("pause_resume_toggle_key", &PlayOptions::pause_resume_toggle_key)
+  .def_readwrite("play_next_key", &PlayOptions::play_next_key)
   ;
 
   py::class_<RecordOptions>(m, "RecordOptions")

rosbag2_transport/CMakeLists.txt

@@ -33,6 +33,7 @@ find_package(rosbag2_storage REQUIRED)
 find_package(rmw_implementation_cmake REQUIRED)
 find_package(shared_queues_vendor REQUIRED)
 find_package(yaml_cpp_vendor REQUIRED)
+find_package(keyboard_handler REQUIRED)
 
 add_library(${PROJECT_NAME} SHARED
   src/rosbag2_transport/player.cpp
@@ -43,6 +44,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
 ament_target_dependencies(${PROJECT_NAME}
+  keyboard_handler
   rcl
   rclcpp
   rcutils
@@ -75,6 +77,7 @@ ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME})
 ament_export_targets(export_${PROJECT_NAME})
 ament_export_dependencies(
+  keyboard_handler
   rosbag2_cpp
   rosbag2_compression
   rosbag2_interfaces

rosbag2_transport/include/rosbag2_transport/play_options.hpp

@@ -20,6 +20,7 @@
 #include <unordered_map>
 #include <vector>
 
+#include "keyboard_handler/keyboard_handler.hpp"
 #include "rclcpp/qos.hpp"
 
 namespace rosbag2_transport
@@ -47,6 +48,10 @@ struct PlayOptions
 
   // Sleep SEC seconds before play. Valid range > 0.0.
   float delay = 0.0;
+
+  // keybindings
+  std::string pause_resume_toggle_key = enum_key_code_to_str(KeyboardHandler::KeyCode::SPACE);
+  std::string play_next_key = enum_key_code_to_str(KeyboardHandler::KeyCode::S);
 };
 
 }  // namespace rosbag2_transport

rosbag2_transport/include/rosbag2_transport/player.hpp

@@ -42,6 +42,8 @@
 #include "rosbag2_transport/play_options.hpp"
 #include "rosbag2_transport/visibility_control.hpp"
 
+#include "keyboard_handler/keyboard_handler.hpp"
+
 #include "rosgraph_msgs/msg/clock.hpp"
 
 namespace rosbag2_cpp
@@ -160,6 +162,11 @@ class Player : public rclcpp::Node
   rclcpp::Service<rosbag2_interfaces::srv::GetRate>::SharedPtr srv_get_rate_;
   rclcpp::Service<rosbag2_interfaces::srv::SetRate>::SharedPtr srv_set_rate_;
   rclcpp::Service<rosbag2_interfaces::srv::PlayNext>::SharedPtr srv_play_next_;
+
+  // defaults
+  KeyboardHandler keyboard_handler_;
+
+  void add_keyboard_callbacks();
 };
 
 }  // namespace rosbag2_transport

rosbag2_transport/package.xml

@@ -20,6 +20,7 @@
   <depend>rmw</depend>
   <depend>shared_queues_vendor</depend>
   <depend>yaml_cpp_vendor</depend>
+  <depend>keyboard_handler</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_index_cpp</test_depend>

rosbag2_transport/src/rosbag2_transport/player.cpp

@@ -104,7 +104,8 @@ Player::Player(
     rclcpp::NodeOptions(node_options).arguments(play_options.topic_remapping_options)),
   reader_(std::move(reader)),
   storage_options_(storage_options),
-  play_options_(play_options)
+  play_options_(play_options),
+  keyboard_handler_()
 {
   {
     reader_->open(storage_options_, {"", rmw_get_serialization_format()});
@@ -118,7 +119,7 @@ Player::Player(
 
     reader_->close();
   }
-
+  // service callbacks
   srv_pause_ = create_service<rosbag2_interfaces::srv::Pause>(
     "~/pause",
     [this](
@@ -182,6 +183,8 @@ Player::Player(
     {
       response->success = play_next();
     });
+  // keyboard callbacks
+  add_keyboard_callbacks();
 }
 
 Player::~Player()
@@ -222,6 +225,13 @@ void Player::play()
   }
 
   try {
+    RCLCPP_INFO_STREAM(
+      this->get_logger(),
+      "Press " << play_options_.pause_resume_toggle_key << " to toggle Pause/Resume.");
+    RCLCPP_INFO_STREAM(
+      this->get_logger(),
+      "Press " << play_options_.play_next_key <<
+        " to play the next message if you are in PAUSED state.");
     do {
       if (delay > 0.0) {
         RCLCPP_INFO_STREAM(this->get_logger(), "Sleep " << delay << " sec");
@@ -255,16 +265,18 @@ void Player::play()
 void Player::pause()
 {
   clock_->pause();
+  RCLCPP_INFO_STREAM(this->get_logger(), "Player paused.");
 }
 
 void Player::resume()
 {
   clock_->resume();
+  RCLCPP_INFO_STREAM(this->get_logger(), "Player resumed.");
 }
 
 void Player::toggle_paused()
 {
-  clock_->is_paused() ? clock_->resume() : clock_->pause();
+  this->is_paused() ? this->resume() : this->pause();
 }
 
 bool Player::is_paused() const
@@ -304,6 +316,8 @@ bool Player::play_next()
     return false;
   }
 
+  RCLCPP_INFO_STREAM(this->get_logger(), "Playing next message.");
+
   // Temporary take over playback from play_messages_from_queue()
   std::lock_guard<std::mutex> main_play_loop_lk(skip_message_in_main_play_loop_mutex_);
   skip_message_in_main_play_loop_ = true;
@@ -466,4 +480,38 @@ bool Player::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr mess
   return message_published;
 }
 
+void Player::add_keyboard_callbacks()
+{
+  RCLCPP_INFO_STREAM(this->get_logger(), "Adding keyboard callbacks.");
+  // translated options into keybindings
+  const KeyboardHandler::KeyCode pause_resume_toggle_key = enum_str_to_key_code(
+    play_options_.pause_resume_toggle_key);
+  const KeyboardHandler::KeyCode play_next_key = enum_str_to_key_code(play_options_.play_next_key);
+  // check keybindings
+  if (
+    (pause_resume_toggle_key == KeyboardHandler::KeyCode::UNKNOWN) ||
+    (play_next_key == KeyboardHandler::KeyCode::UNKNOWN))
+  {
+    throw std::invalid_argument("UNKNOWN keyboard bindings!");
+  }
+  // create callback
+  KeyboardHandler::callback_t callback = [this, pause_resume_toggle_key, play_next_key](
+    KeyboardHandler::KeyCode key_code,
+    KeyboardHandler::KeyModifiers /*key_modifiers*/)
+    {
+      if (key_code == pause_resume_toggle_key) {
+        this->toggle_paused();
+      } else if (key_code == play_next_key) {
+        this->play_next();
+      } else {
+        RCLCPP_WARN_STREAM(
+          this->get_logger(),
+          "Unbound key code = " << enum_key_code_to_str(key_code));
+      }
+    };
+  // register callbacks on key_codes
+  keyboard_handler_.add_key_press_callback(callback, pause_resume_toggle_key);
+  keyboard_handler_.add_key_press_callback(callback, play_next_key);
+}
+
 }  // namespace rosbag2_transport