Skip to content

Commit

Permalink
Address minor feedback
Browse files Browse the repository at this point in the history
Signed-off-by: Christophe Bedard <[email protected]>
  • Loading branch information
christophebedard committed Nov 22, 2024
1 parent 5fa1031 commit e025055
Show file tree
Hide file tree
Showing 4 changed files with 25 additions and 23 deletions.
27 changes: 14 additions & 13 deletions ros2bag/ros2bag/api/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -158,35 +158,36 @@ def check_not_negative_int(arg: str) -> int:
raise ArgumentTypeError('{} is not the valid type (int)'.format(value))


def add_standard_reader_args(parser: ArgumentParser, multi: bool = False) -> None:
def add_standard_reader_args(parser: ArgumentParser, multiple_readers: bool = False) -> None:
"""
Add arguments for one of multiple input bags.
:param parser: the parser
:param multi: whether this is part of a verb that takes in multiple input bags
:param multiple_readers: whether this is part of a verb that takes in multiple input bags
"""
# If multi, let user provide an input bag path using an optional positional arg, but require
# them to use --input to provide an input bag with a specific storage ID
# If multiple readers, let user provide an input bag path using an optional positional arg, but
# require them to use --input to provide an input bag with a specific storage ID

reader_choices = rosbag2_py.get_registered_readers()
bag_help_multi_str = ''
storage_help_multi_str = ''
if multi:
bag_help_multi_str = ' Use --input to provide an input bag with a specific storage ID.'
storage_help_multi_str = \
bag_help_multiple_readers_str = ''
storage_help_multiple_readers_str = ''
if multiple_readers:
bag_help_multiple_readers_str = \
' Use --input to provide an input bag with a specific storage ID.'
storage_help_multiple_readers_str = \
' (deprecated: use --input to provide an input bag with a specific storage ID)'
parser.add_argument(
'bag_path',
nargs='?' if multi else None,
nargs='?' if multiple_readers else None,
type=check_path_exists,
help='Bag to open.' + bag_help_multi_str)
help='Bag to open.' + bag_help_multiple_readers_str)
parser.add_argument(
'-s', '--storage', default='', choices=reader_choices,
help='Storage implementation of bag. '
'By default attempts to detect automatically - use this argument to override.'
+ storage_help_multi_str)
+ storage_help_multiple_readers_str)

if multi:
if multiple_readers:
add_multi_bag_input_arg(parser, required=False)


Expand Down
2 changes: 1 addition & 1 deletion ros2bag/ros2bag/verb/play.py
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ class PlayVerb(VerbExtension):
"""Play back ROS data from a bag."""

def add_arguments(self, parser, cli_name): # noqa: D102
add_standard_reader_args(parser, multi=True)
add_standard_reader_args(parser, multiple_readers=True)
parser.add_argument(
'--read-ahead-queue-size', type=int, default=1000,
help='size of message queue rosbag tries to hold in memory to help deterministic '
Expand Down
6 changes: 3 additions & 3 deletions rosbag2_py/src/rosbag2_py/_transport.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -254,9 +254,9 @@ class Player
{
install_signal_handlers();
try {
std::vector<rosbag2_transport::Player::ReaderStorageOptionsPair> bags{};
std::vector<rosbag2_transport::Player::ReaderStorageOptionsPair> readers{};
for (const auto & options : storage_options) {
bags.emplace_back(rosbag2_transport::ReaderWriterFactory::make_reader(options), options);
readers.emplace_back(rosbag2_transport::ReaderWriterFactory::make_reader(options), options);
}
std::shared_ptr<KeyboardHandler> keyboard_handler;
if (!play_options.disable_keyboard_controls) {
Expand All @@ -270,7 +270,7 @@ class Player
#endif
}
auto player = std::make_shared<rosbag2_transport::Player>(
std::move(bags), std::move(keyboard_handler), play_options);
std::move(readers), std::move(keyboard_handler), play_options);

rclcpp::executors::SingleThreadedExecutor exec;
exec.add_node(player);
Expand Down
13 changes: 7 additions & 6 deletions rosbag2_transport/src/rosbag2_transport/player.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -904,12 +904,13 @@ Player::callback_handle_t PlayerImpl::get_new_on_play_msg_callback_handle()

void PlayerImpl::wait_for_filled_queue() const
{
const auto read_ahead_queues_filled = [this]() {
bool filled = true;
const auto read_ahead_queues_filled = [this]() -> bool {
for (std::size_t i = 0; i < input_bags_.size(); i++) {
filled &= message_queue_.size_approx(i) >= play_options_.read_ahead_queue_size;
if (message_queue_.size_approx(i) < play_options_.read_ahead_queue_size) {
return false;
}
}
return filled;
return true;
};
while (
!read_ahead_queues_filled() &&
Expand Down Expand Up @@ -943,9 +944,9 @@ void PlayerImpl::load_storage_content()
enqueue_up_to_boundary(queue_upper_boundary, bag_index);
} else {
lk.unlock();
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
}
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
}

Expand Down Expand Up @@ -1134,7 +1135,7 @@ void PlayerImpl::prepare_publishers()
std::vector<rosbag2_storage::TopicMetadata> topics{};
for (const auto & [reader, _] : input_bags_) {
auto bag_topics = reader->get_all_topics_and_types();
topics.insert(topics.begin(), bag_topics.begin(), bag_topics.end());
topics.insert(topics.end(), bag_topics.begin(), bag_topics.end());
}
std::string topic_without_support_acked;
for (const auto & topic : topics) {
Expand Down

0 comments on commit e025055

Please sign in to comment.