Skip to content

Commit

Permalink
keyboard controls for pause/resume toggle and play-next:
Browse files Browse the repository at this point in the history
- supports custom keyboard bindings set through commandline into play_options_

Signed-off-by: Sonia Jin <[email protected]>
  • Loading branch information
lihui815 committed Aug 14, 2021
1 parent 44e6422 commit fee38a6
Show file tree
Hide file tree
Showing 9 changed files with 413 additions and 3 deletions.
4 changes: 4 additions & 0 deletions ros2bag/ros2bag/verb/play.py
Original file line number Diff line number Diff line change
Expand Up @@ -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 pause/resume/play-next')

def main(self, *, args): # noqa: D102
qos_profile_overrides = {} # Specify a valid default
Expand Down Expand Up @@ -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)
1 change: 1 addition & 0 deletions rosbag2_py/src/rosbag2_py/_transport.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -239,6 +239,7 @@ PYBIND11_MODULE(_transport, m) {
"delay",
&PlayOptions::getDelay,
&PlayOptions::setDelay)
.def_readwrite("disable_keyboard_controls", &PlayOptions::disable_keyboard_controls)
;

py::class_<RecordOptions>(m, "RecordOptions")
Expand Down
8 changes: 8 additions & 0 deletions rosbag2_transport/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
6 changes: 6 additions & 0 deletions rosbag2_transport/include/rosbag2_transport/play_options.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#include <unordered_map>
#include <vector>

#include "keyboard_handler/keyboard_handler.hpp"
#include "rclcpp/duration.hpp"
#include "rclcpp/qos.hpp"

Expand Down Expand Up @@ -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::S;
};

} // namespace rosbag2_transport
Expand Down
40 changes: 40 additions & 0 deletions rosbag2_transport/include/rosbag2_transport/player.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -75,6 +77,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<rosbag2_cpp::Reader> reader,
std::shared_ptr<KeyboardHandlerBase> 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();

Expand Down Expand Up @@ -122,6 +133,28 @@ class Player : public rclcpp::Node
ROSBAG2_TRANSPORT_PUBLIC
bool play_next();

// \brief add keyboard callbacks
ROSBAG2_TRANSPORT_PUBLIC
void add_keyboard_callbacks();

ROSBAG2_TRANSPORT_PUBLIC
int get_num_paused()
{
return num_paused_;
}

ROSBAG2_TRANSPORT_PUBLIC
int get_num_resumed()
{
return num_resumed_;
}

ROSBAG2_TRANSPORT_PUBLIC
int get_num_play_next()
{
return num_play_next_;
}

protected:
bool is_ready_to_play_from_queue_{false};
std::mutex ready_to_play_from_queue_mutex_;
Expand Down Expand Up @@ -160,6 +193,13 @@ 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
std::shared_ptr<KeyboardHandlerBase> keyboard_handler_;

int num_paused_ = 0;
int num_resumed_ = 0;
int num_play_next_ = 0;
};

} // namespace rosbag2_transport
Expand Down
1 change: 1 addition & 0 deletions rosbag2_transport/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
82 changes: 79 additions & 3 deletions rosbag2_transport/src/rosbag2_transport/player.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<KeyboardHandlerBase>(new KeyboardHandler()),
storage_options, play_options,
node_name, node_options)
{}

Player::Player(
std::unique_ptr<rosbag2_cpp::Reader> reader,
std::shared_ptr<KeyboardHandlerBase> 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()});
Expand All @@ -118,7 +133,7 @@ Player::Player(

reader_->close();
}

// service callbacks
srv_pause_ = create_service<rosbag2_interfaces::srv::Pause>(
"~/pause",
[this](
Expand Down Expand Up @@ -182,6 +197,8 @@ Player::Player(
{
response->success = play_next();
});
// keyboard callbacks
add_keyboard_callbacks();
}

Player::~Player()
Expand Down Expand Up @@ -255,16 +272,20 @@ void Player::play()
void Player::pause()
{
clock_->pause();
RCLCPP_INFO_STREAM(this->get_logger(), "Pausing play.");
num_paused_++;
}

void Player::resume()
{
clock_->resume();
RCLCPP_INFO_STREAM(this->get_logger(), "Resuming play.");
num_resumed_++;
}

void Player::toggle_paused()
{
clock_->is_paused() ? clock_->resume() : clock_->pause();
this->is_paused() ? this->resume() : this->pause();
}

bool Player::is_paused() const
Expand Down Expand Up @@ -304,6 +325,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;
Expand All @@ -325,6 +348,9 @@ bool Player::play_next()
message_queue_.pop();
message_ptr = peek_next_message_from_queue();
}
if (next_message_published) {
num_play_next_++;
}
return next_message_published;
}

Expand Down Expand Up @@ -466,4 +492,54 @@ bool Player::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr mess
return message_published;
}

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
if (play_options_.pause_resume_toggle_key == KeyboardHandlerBase::KeyCode::UNKNOWN) {
RCLCPP_ERROR_STREAM(
this->get_logger(),
"Invalid key binding " <<
enum_key_code_to_str(play_options_.pause_resume_toggle_key) <<
" for pause/resume toggle!");
throw std::invalid_argument("Invalid key binding.");
}
if (play_options_.play_next_key == KeyboardHandlerBase::KeyCode::UNKNOWN) {
RCLCPP_ERROR_STREAM(
this->get_logger(),
"Invalid key binding " <<
enum_key_code_to_str(play_options_.play_next_key) <<
" for play-next!");
throw std::invalid_argument("Invalid key binding.");
}
// register callbacks on key_codes
keyboard_handler_->add_key_press_callback(
[this](KeyboardHandlerBase::KeyCode /*key_code*/,
KeyboardHandlerBase::KeyModifiers /*key_modifiers*/)
{
this->toggle_paused();
}, play_options_.pause_resume_toggle_key);
keyboard_handler_->add_key_press_callback(
[this](KeyboardHandlerBase::KeyCode /*key_code*/,
KeyboardHandlerBase::KeyModifiers /*key_modifiers*/)
{
this->play_next();
}, play_options_.play_next_key);
// show instructions
RCLCPP_INFO_STREAM(
this->get_logger(),
"Press " <<
enum_key_code_to_str(play_options_.pause_resume_toggle_key) <<
" to toggle Pause/Resume.");
RCLCPP_INFO_STREAM(
this->get_logger(),
"Press " <<
enum_key_code_to_str(play_options_.play_next_key) <<
" to play the next message if you are in PAUSED state.");
}

} // namespace rosbag2_transport
98 changes: 98 additions & 0 deletions rosbag2_transport/test/rosbag2_transport/mock_keyboard_handler.hpp
Original file line number Diff line number Diff line change
@@ -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 <memory>
#include <string>
#include <unordered_map>
#include <vector>

#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<float>(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<std::mutex> 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<float>(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_
Loading

0 comments on commit fee38a6

Please sign in to comment.