From d0c73e43de5ff1957ef7fd4a6383ad8c1b0e3ee5 Mon Sep 17 00:00:00 2001 From: Sonia Jin Date: Fri, 3 Sep 2021 09:12:05 -0400 Subject: [PATCH] adds keyboard controls for ros2 bag play Signed-off-by: Sonia Jin --- ros2bag/ros2bag/verb/play.py | 4 + rosbag2_py/src/rosbag2_py/_transport.cpp | 1 + rosbag2_transport/CMakeLists.txt | 8 + .../rosbag2_transport/play_options.hpp | 6 + .../include/rosbag2_transport/player.hpp | 31 ++- rosbag2_transport/package.xml | 1 + .../src/rosbag2_transport/player.cpp | 77 ++++++- .../mock_keyboard_handler.hpp | 98 ++++++++ .../test_keyboard_controls.cpp | 214 ++++++++++++++++++ 9 files changed, 434 insertions(+), 6 deletions(-) create mode 100644 rosbag2_transport/test/rosbag2_transport/mock_keyboard_handler.hpp create mode 100644 rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index 3b2b8d772a..a6f7bd4077 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -85,6 +85,9 @@ def add_arguments(self, parser, cli_name): # noqa: D102 parser.add_argument( '-d', '--delay', type=positive_float, default=0.0, help='Sleep duration before play (each loop), in seconds. Negative durations invalid.') + parser.add_argument( + '--disable-keyboard-controls', action='store_true', + help='disables keyboard controls for playback') def main(self, *, args): # noqa: D102 qos_profile_overrides = {} # Specify a valid default @@ -120,6 +123,7 @@ 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.disable_keyboard_controls = args.disable_keyboard_controls player = Player() player.play(storage_options, play_options) diff --git a/rosbag2_py/src/rosbag2_py/_transport.cpp b/rosbag2_py/src/rosbag2_py/_transport.cpp index 31519995bd..7819dc4aed 100644 --- a/rosbag2_py/src/rosbag2_py/_transport.cpp +++ b/rosbag2_py/src/rosbag2_py/_transport.cpp @@ -239,6 +239,7 @@ PYBIND11_MODULE(_transport, m) { "delay", &PlayOptions::getDelay, &PlayOptions::setDelay) + .def_readwrite("disable_keyboard_controls", &PlayOptions::disable_keyboard_controls) ; py::class_(m, "RecordOptions") diff --git a/rosbag2_transport/CMakeLists.txt b/rosbag2_transport/CMakeLists.txt index e6f03b6724..e4e37e663e 100644 --- a/rosbag2_transport/CMakeLists.txt +++ b/rosbag2_transport/CMakeLists.txt @@ -22,6 +22,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(ament_cmake_ros REQUIRED) +find_package(keyboard_handler REQUIRED) find_package(rcl REQUIRED) find_package(rclcpp REQUIRED) find_package(rcutils REQUIRED) @@ -43,6 +44,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC $ $) 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 @@ -156,6 +159,11 @@ function(create_tests_for_rmw_implementation) LINK_LIBS rosbag2_transport AMENT_DEPS test_msgs rosbag2_test_common) + rosbag2_transport_add_gmock(test_keyboard_controls + test/rosbag2_transport/test_keyboard_controls.cpp + LINK_LIBS rosbag2_transport + AMENT_DEPS test_msgs rosbag2_test_common) + rosbag2_transport_add_gmock(test_record_regex test/rosbag2_transport/test_record_regex.cpp LINK_LIBS rosbag2_transport diff --git a/rosbag2_transport/include/rosbag2_transport/play_options.hpp b/rosbag2_transport/include/rosbag2_transport/play_options.hpp index 2eb62c8835..11bfd01459 100644 --- a/rosbag2_transport/include/rosbag2_transport/play_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/play_options.hpp @@ -20,6 +20,7 @@ #include #include +#include "keyboard_handler/keyboard_handler.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/qos.hpp" @@ -48,6 +49,11 @@ struct PlayOptions // Sleep before play. Negative durations invalid. Will delay at the beginning of each loop. rclcpp::Duration delay = rclcpp::Duration(0, 0); + + bool disable_keyboard_controls = false; + // keybindings + KeyboardHandlerBase::KeyCode pause_resume_toggle_key = KeyboardHandlerBase::KeyCode::SPACE; + KeyboardHandlerBase::KeyCode play_next_key = KeyboardHandlerBase::KeyCode::CURSOR_RIGHT; }; } // namespace rosbag2_transport diff --git a/rosbag2_transport/include/rosbag2_transport/player.hpp b/rosbag2_transport/include/rosbag2_transport/player.hpp index b83d2c3acf..508ad4611b 100644 --- a/rosbag2_transport/include/rosbag2_transport/player.hpp +++ b/rosbag2_transport/include/rosbag2_transport/player.hpp @@ -21,6 +21,9 @@ #include #include #include +#include + +#include "keyboard_handler/keyboard_handler.hpp" #include "moodycamel/readerwriterqueue.h" @@ -75,6 +78,15 @@ class Player : public rclcpp::Node const std::string & node_name = "rosbag2_player", const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()); + ROSBAG2_TRANSPORT_PUBLIC + Player( + std::unique_ptr reader, + std::shared_ptr keyboard_handler, + const rosbag2_storage::StorageOptions & storage_options, + const rosbag2_transport::PlayOptions & play_options, + const std::string & node_name = "rosbag2_player", + const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()); + ROSBAG2_TRANSPORT_PUBLIC virtual ~Player(); @@ -87,11 +99,11 @@ class Player : public rclcpp::Node // Playback control interface /// Pause the flow of time for playback. ROSBAG2_TRANSPORT_PUBLIC - void pause(); + virtual void pause(); /// Start the flow of time for playback. ROSBAG2_TRANSPORT_PUBLIC - void resume(); + virtual void resume(); /// Pause if time running, resume if paused. ROSBAG2_TRANSPORT_PUBLIC @@ -120,7 +132,16 @@ class Player : public rclcpp::Node /// this method will wait until new element will be pushed to the queue. /// \return true if player in pause mode and successfully played next message, otherwise false. ROSBAG2_TRANSPORT_PUBLIC - bool play_next(); + virtual bool play_next(); + + ROSBAG2_TRANSPORT_PUBLIC + void add_key_callback( + KeyboardHandlerBase::KeyCode key, + const std::function & cb, + const std::string & op_name); + + ROSBAG2_TRANSPORT_PUBLIC + void add_keyboard_callbacks(); protected: bool is_ready_to_play_from_queue_{false}; @@ -160,6 +181,10 @@ class Player : public rclcpp::Node rclcpp::Service::SharedPtr srv_get_rate_; rclcpp::Service::SharedPtr srv_set_rate_; rclcpp::Service::SharedPtr srv_play_next_; + + // defaults + std::shared_ptr keyboard_handler_; + std::vector keyboard_callbacks_; }; } // namespace rosbag2_transport diff --git a/rosbag2_transport/package.xml b/rosbag2_transport/package.xml index 697d7fdd97..6c77e8bf45 100644 --- a/rosbag2_transport/package.xml +++ b/rosbag2_transport/package.xml @@ -20,6 +20,7 @@ rmw shared_queues_vendor yaml_cpp_vendor + keyboard_handler ament_cmake_gmock ament_index_cpp diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index a55347daa7..b268ce527c 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -99,12 +99,27 @@ Player::Player( const rosbag2_transport::PlayOptions & play_options, const std::string & node_name, const rclcpp::NodeOptions & node_options) +: Player(std::move(reader), + // only call KeyboardHandler when using default keyboard handler implementation + std::shared_ptr(new KeyboardHandler()), + storage_options, play_options, + node_name, node_options) +{} + +Player::Player( + std::unique_ptr reader, + std::shared_ptr keyboard_handler, + const rosbag2_storage::StorageOptions & storage_options, + const rosbag2_transport::PlayOptions & play_options, + const std::string & node_name, + const rclcpp::NodeOptions & node_options) : rclcpp::Node( node_name, 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_(keyboard_handler) { { reader_->open(storage_options_, {"", rmw_get_serialization_format()}); @@ -118,7 +133,7 @@ Player::Player( reader_->close(); } - + // service callbacks srv_pause_ = create_service( "~/pause", [this]( @@ -182,10 +197,18 @@ Player::Player( { response->success = play_next(); }); + // keyboard callbacks + add_keyboard_callbacks(); } Player::~Player() { + // remove callbacks on key_codes to prevent race conditions + // Note: keyboard_handler handles locks between removing & executing callbacks + for (auto cb = keyboard_callbacks_.begin(); cb != keyboard_callbacks_.end(); ++cb) { + keyboard_handler_->delete_key_press_callback(*cb); + } + // closes reader if (reader_) { reader_->close(); } @@ -255,16 +278,18 @@ void Player::play() void Player::pause() { clock_->pause(); + RCLCPP_INFO_STREAM(this->get_logger(), "Pausing play."); } void Player::resume() { clock_->resume(); + RCLCPP_INFO_STREAM(this->get_logger(), "Resuming play."); } void Player::toggle_paused() { - clock_->is_paused() ? clock_->resume() : clock_->pause(); + this->is_paused() ? this->resume() : this->pause(); } bool Player::is_paused() const @@ -306,9 +331,12 @@ rosbag2_storage::SerializedBagMessageSharedPtr * Player::peek_next_message_from_ bool Player::play_next() { if (!clock_->is_paused()) { + RCLCPP_WARN_STREAM(this->get_logger(), "Called play next, but not in paused state."); return false; } + RCLCPP_INFO_STREAM(this->get_logger(), "Playing next message."); + // Temporary take over playback from play_messages_from_queue() std::lock_guard main_play_loop_lk(skip_message_in_main_play_loop_mutex_); skip_message_in_main_play_loop_ = true; @@ -471,4 +499,47 @@ bool Player::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr mess return message_published; } +void Player::add_key_callback( + KeyboardHandlerBase::KeyCode key, + const std::function & cb, + const std::string & op_name) +{ + std::string key_str = enum_key_code_to_str(key); + if (key == KeyboardHandlerBase::KeyCode::UNKNOWN) { + RCLCPP_ERROR_STREAM( + this->get_logger(), + "Invalid key binding " << key_str << " for " << op_name); + throw std::invalid_argument("Invalid key binding."); + } + keyboard_callbacks_.push_back( + keyboard_handler_->add_key_press_callback( + [cb](KeyboardHandlerBase::KeyCode /*key_code*/, + KeyboardHandlerBase::KeyModifiers /*key_modifiers*/) {cb();}, + key)); + // show instructions + RCLCPP_INFO_STREAM( + this->get_logger(), + "Press " << key_str << " for " << op_name); +} + +void Player::add_keyboard_callbacks() +{ + // skip if disabled + if (play_options_.disable_keyboard_controls) { + return; + } + RCLCPP_INFO_STREAM(this->get_logger(), "Adding keyboard callbacks."); + // check keybindings + add_key_callback( + play_options_.pause_resume_toggle_key, + [this]() {this->toggle_paused();}, + "Pause/Resume" + ); + add_key_callback( + play_options_.play_next_key, + [this]() {this->play_next();}, + "Play Next Message" + ); +} + } // namespace rosbag2_transport diff --git a/rosbag2_transport/test/rosbag2_transport/mock_keyboard_handler.hpp b/rosbag2_transport/test/rosbag2_transport/mock_keyboard_handler.hpp new file mode 100644 index 0000000000..d54e3f4360 --- /dev/null +++ b/rosbag2_transport/test/rosbag2_transport/mock_keyboard_handler.hpp @@ -0,0 +1,98 @@ +// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROSBAG2_TRANSPORT__MOCK_KEYBOARD_HANDLER_HPP_ +#define ROSBAG2_TRANSPORT__MOCK_KEYBOARD_HANDLER_HPP_ + +#include +#include +#include +#include + +#include "keyboard_handler/keyboard_handler_base.hpp" + +// press one specific key code one second at a time +class MockKeyboardHandlerImpl : public KeyboardHandlerBase +{ +public: + MockKeyboardHandlerImpl( + KeyboardHandlerBase::KeyCode pause_resume_toggle_key, + KeyboardHandlerBase::KeyCode play_next_key) + : pause_resume_toggle_key_(pause_resume_toggle_key), + play_next_key_(play_next_key) + { + is_init_succeed_ = true; + key_handler_thread_ = std::thread( + [ = ]() { + try { + int i = 0; + // sleep 0.1 seconda in beginning + std::this_thread::sleep_for(std::chrono::duration(0.1)); + do { + // only press 4 times it in the beginning + // 0 - pause + // 1 - play-next = plays next message + // 2 - resume + // 3 - play-next = calls code but doesn't actually play next message because not paused + if (i < 4) { + KeyboardHandlerBase::KeyCode key_code = pause_resume_toggle_key_; + if ((i == 1) || (i == 3)) { + key_code = play_next_key_; + } + KeyModifiers key_modifiers = KeyboardHandlerBase::KeyModifiers::NONE; + std::lock_guard lk(callbacks_mutex_); + auto range = callbacks_.equal_range(KeyAndModifiers{key_code, key_modifiers}); + for (auto it = range.first; it != range.second; ++it) { + it->second.callback(key_code, key_modifiers); + } + // sleep 1 second at a time + std::this_thread::sleep_for(std::chrono::duration(0.25)); + } + i++; + } while (!exit_.load()); + } catch (...) { + thread_exception_ptr = std::current_exception(); + } + } + ); + } + + /// \brief destructor + KEYBOARD_HANDLER_PUBLIC + virtual ~MockKeyboardHandlerImpl() + { + exit_ = true; + if (key_handler_thread_.joinable()) { + key_handler_thread_.join(); + } + try { + if (thread_exception_ptr != nullptr) { + std::rethrow_exception(thread_exception_ptr); + } + } catch (const std::exception & e) { + std::cerr << "Caught exception: \"" << e.what() << "\"\n"; + } catch (...) { + std::cerr << "Caught unknown exception" << std::endl; + } + } + +private: + KeyboardHandlerBase::KeyCode pause_resume_toggle_key_; + KeyboardHandlerBase::KeyCode play_next_key_; + std::thread key_handler_thread_; + std::atomic_bool exit_; + std::exception_ptr thread_exception_ptr{nullptr}; +}; + +#endif // ROSBAG2_TRANSPORT__MOCK_KEYBOARD_HANDLER_HPP_ diff --git a/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp b/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp new file mode 100644 index 0000000000..e2f87ebd20 --- /dev/null +++ b/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp @@ -0,0 +1,214 @@ +// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "rosbag2_test_common/subscription_manager.hpp" + +#include "rosbag2_transport/player.hpp" + +#include "test_msgs/msg/arrays.hpp" +#include "test_msgs/msg/basic_types.hpp" +#include "test_msgs/message_fixtures.hpp" + +#include "rosbag2_play_test_fixture.hpp" +#include "rosbag2_transport_test_fixture.hpp" +#include "mock_keyboard_handler.hpp" + +using namespace ::testing; // NOLINT +using namespace rosbag2_transport; // NOLINT +using namespace std::chrono_literals; // NOLINT +using namespace rosbag2_test_common; // NOLINT + +// wrapper player that counts command calls +class CountPlayer : public Player +{ +public: + CountPlayer( + std::unique_ptr reader, + std::shared_ptr keyboard_handler, + const rosbag2_storage::StorageOptions & storage_options, + const rosbag2_transport::PlayOptions & play_options) + : Player(std::move(reader), keyboard_handler, storage_options, play_options) {} + + void pause() override + { + Player::pause(); + num_paused++; + } + + void resume() override + { + Player::resume(); + num_resumed++; + } + + bool play_next() override + { + bool ret = Player::play_next(); + if (ret) { + num_played_next++; + } + return ret; + } + + int num_paused = 0; + int num_resumed = 0; + int num_played_next = 0; +}; + + +TEST_F(RosBag2PlayTestFixture, invalid_keybindings) +{ + auto primitive_message1 = get_messages_strings()[0]; + primitive_message1->string_value = "Hello World 1"; + + auto primitive_message2 = get_messages_strings()[0]; + primitive_message2->string_value = "Hello World 2"; + + auto message_time_difference = rclcpp::Duration(1, 0); + auto topics_and_types = + std::vector{{"topic1", "test_msgs/Strings", "", ""}}; + std::vector> messages = + {serialize_test_message("topic1", 0, primitive_message1), + serialize_test_message("topic1", 0, primitive_message2)}; + + messages[0]->time_stamp = 100; + messages[1]->time_stamp = messages[0]->time_stamp + message_time_difference.nanoseconds(); + + play_options_.play_next_key = KeyboardHandler::KeyCode::UNKNOWN; + + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, topics_and_types); + auto reader = std::make_unique(std::move(prepared_mock_reader)); + + EXPECT_THROW( + std::make_shared( + std::move(reader), storage_options_, play_options_), + std::invalid_argument + ); +} + +TEST_F(RosBag2PlayTestFixture, test_keyboard_controls) +{ + auto primitive_message1 = get_messages_strings()[0]; + primitive_message1->string_value = "Hello World 1"; + + auto primitive_message2 = get_messages_strings()[0]; + primitive_message2->string_value = "Hello World 2"; + + auto primitive_message3 = get_messages_strings()[0]; + primitive_message3->string_value = "Hello World 3"; + + auto message_time_difference = rclcpp::Duration(1, 0); + auto topics_and_types = + std::vector{{"topic1", "test_msgs/Strings", "", ""}}; + std::vector> messages = + {serialize_test_message("topic1", 0, primitive_message1), + serialize_test_message("topic1", 0, primitive_message2), + serialize_test_message("topic1", 0, primitive_message3)}; + + messages[0]->time_stamp = 100; + messages[1]->time_stamp = messages[0]->time_stamp + message_time_difference.nanoseconds(); + messages[2]->time_stamp = messages[1]->time_stamp + message_time_difference.nanoseconds(); + + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, topics_and_types); + auto reader = std::make_unique(std::move(prepared_mock_reader)); + auto keyboard_handler = std::shared_ptr( + new MockKeyboardHandlerImpl( + play_options_.pause_resume_toggle_key, + play_options_.play_next_key)); + + rclcpp::Duration pause_time(0, 500000000); + rclcpp::Duration error_margin(0, 100000000); + auto lower_expected_duration = message_time_difference * 2 + pause_time - error_margin; + auto upper_expected_duration = message_time_difference * 2 + pause_time + error_margin; + + auto player = std::make_shared( + std::move(reader), keyboard_handler, storage_options_, play_options_); + auto start = std::chrono::steady_clock::now(); + player->play(); + + // should pause for .5 second + auto replay_time = std::chrono::steady_clock::now() - start; + auto replay_nanos = std::chrono::nanoseconds(replay_time).count(); + EXPECT_THAT(replay_nanos, Gt(lower_expected_duration.nanoseconds())); + EXPECT_THAT(replay_nanos, Lt(upper_expected_duration.nanoseconds())); + EXPECT_THAT(player->num_paused, 1); + EXPECT_THAT(player->num_resumed, 1); + EXPECT_THAT(player->num_played_next, 1); +} + +TEST_F(RosBag2PlayTestFixture, test_disable_keyboard_controls) +{ + auto primitive_message1 = get_messages_strings()[0]; + primitive_message1->string_value = "Hello World 1"; + + auto primitive_message2 = get_messages_strings()[0]; + primitive_message2->string_value = "Hello World 2"; + + auto primitive_message3 = get_messages_strings()[0]; + primitive_message3->string_value = "Hello World 3"; + + auto message_time_difference = rclcpp::Duration(1, 0); + auto topics_and_types = + std::vector{{"topic1", "test_msgs/Strings", "", ""}}; + std::vector> messages = + {serialize_test_message("topic1", 0, primitive_message1), + serialize_test_message("topic1", 0, primitive_message2), + serialize_test_message("topic1", 0, primitive_message3)}; + + messages[0]->time_stamp = 100; + messages[1]->time_stamp = messages[0]->time_stamp + message_time_difference.nanoseconds(); + messages[2]->time_stamp = messages[1]->time_stamp + message_time_difference.nanoseconds(); + + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, topics_and_types); + auto reader = std::make_unique(std::move(prepared_mock_reader)); + auto keyboard_handler = std::shared_ptr( + new MockKeyboardHandlerImpl( + play_options_.pause_resume_toggle_key, + play_options_.play_next_key)); + + rclcpp::Duration error_margin(0, 100000000); + auto lower_expected_duration = message_time_difference * 2 - error_margin; + auto upper_expected_duration = message_time_difference * 2 + error_margin; + + play_options_.disable_keyboard_controls = true; + + auto player = std::make_shared( + std::move(reader), keyboard_handler, storage_options_, play_options_); + auto start = std::chrono::steady_clock::now(); + player->play(); + + // no pause + auto replay_time = std::chrono::steady_clock::now() - start; + auto replay_nanos = std::chrono::nanoseconds(replay_time).count(); + EXPECT_THAT(replay_nanos, Gt(lower_expected_duration.nanoseconds())); + EXPECT_THAT(replay_nanos, Lt(upper_expected_duration.nanoseconds())); + EXPECT_THAT(player->num_paused, 0); + EXPECT_THAT(player->num_resumed, 0); + EXPECT_THAT(player->num_played_next, 0); +}