Skip to content

Commit

Permalink
Do not multiply queue boundaries by number of bags
Browse files Browse the repository at this point in the history
Signed-off-by: Christophe Bedard <[email protected]>
  • Loading branch information
christophebedard committed Nov 23, 2024
1 parent bcd447a commit b481c33
Showing 1 changed file with 4 additions and 6 deletions.
10 changes: 4 additions & 6 deletions rosbag2_transport/src/rosbag2_transport/player.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -908,7 +908,7 @@ Player::callback_handle_t PlayerImpl::get_new_on_play_msg_callback_handle()
void PlayerImpl::wait_for_filled_queue() const
{
while (
message_queue_.size() < (input_bags_.size() * play_options_.read_ahead_queue_size) &&
message_queue_.size() < play_options_.read_ahead_queue_size &&
!is_storage_completely_loaded() && rclcpp::ok() && !stop_playback_)
{
std::this_thread::sleep_for(queue_read_wait_period_);
Expand All @@ -917,11 +917,9 @@ void PlayerImpl::wait_for_filled_queue() const

void PlayerImpl::load_storage_content()
{
// Multiply the boundary parameters by the number of input bags, since we want enough messages
// from each bags, not just enough messages overall
auto queue_lower_boundary = static_cast<size_t>(
input_bags_.size() * play_options_.read_ahead_queue_size * read_ahead_lower_bound_percentage_);
auto queue_upper_boundary = input_bags_.size() * play_options_.read_ahead_queue_size;
auto queue_lower_boundary =
static_cast<size_t>(play_options_.read_ahead_queue_size * read_ahead_lower_bound_percentage_);
auto queue_upper_boundary = play_options_.read_ahead_queue_size;

while (rclcpp::ok() && load_storage_content_ && !stop_playback_) {
rcpputils::unique_lock lk(reader_mutex_);
Expand Down

0 comments on commit b481c33

Please sign in to comment.