Skip to content

Commit

Permalink
Adresses reviewer's comments.
Browse files Browse the repository at this point in the history
  • Loading branch information
agalbachicar committed Mar 18, 2022
1 parent f71ea9c commit 1453615
Show file tree
Hide file tree
Showing 2 changed files with 19 additions and 28 deletions.
3 changes: 1 addition & 2 deletions rosbag2_transport/src/rosbag2_transport/player.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -193,11 +193,10 @@ bool Player::is_storage_completely_loaded() const

bool Player::play()
{
if (is_in_playback_) {
if (is_in_playback_.exchange(true)) {
RCLCPP_WARN_STREAM(get_logger(), "Trying to play() while in playback, dismissing request.");
return false;
}
is_in_playback_ = true;

rclcpp::Duration delay(0, 0);
if (play_options_.delay >= rclcpp::Duration(0, 0)) {
Expand Down
44 changes: 18 additions & 26 deletions rosbag2_transport/test/rosbag2_transport/test_play_for.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -271,9 +271,9 @@ class RosBag2PlayForInterruptedTestFixture : public RosBag2PlayTestFixture
// around without any knowledge about message chronology. It just picks
// the next one Make sure to keep the list in order or sort it!
std::vector<std::shared_ptr<rosbag2_storage::SerializedBagMessage>> messages =
{serialize_test_message(kTopic1Name, 500, primitive_message),
serialize_test_message(kTopic1Name, 700, primitive_message),
serialize_test_message(kTopic1Name, 900, primitive_message)};
{serialize_test_message(kTopic1Name, 50, primitive_message),
serialize_test_message(kTopic1Name, 100, primitive_message),
serialize_test_message(kTopic1Name, 150, primitive_message)};
// @}
return messages;
}
Expand All @@ -283,42 +283,34 @@ TEST_F(
RosBag2PlayForInterruptedTestFixture,
play_should_return_false_when_interrupted)
{

auto topic_types = get_topic_types();
auto messages = get_serialized_messages();

auto prepared_mock_reader = std::make_unique<MockSequentialReader>();
prepared_mock_reader->prepare(messages, topic_types);
auto reader = std::make_unique<rosbag2_cpp::Reader>(std::move(prepared_mock_reader));

sub_->add_subscription<test_msgs::msg::BasicTypes>(kTopic1, 3);
// Let the player only reproduce one message.
sub_->add_subscription<test_msgs::msg::BasicTypes>(kTopic1, 1);
play_options_.playback_duration = rclcpp::Duration(std::chrono::milliseconds(75));

play_options_.playback_duration = rclcpp::Duration(std::chrono::milliseconds(1000));
std::shared_ptr<MockPlayer> player_ = std::make_shared<MockPlayer>(
std::move(
reader), storage_options_, play_options_);
std::move(reader), storage_options_, play_options_);

// Wait for discovery to match publishers with subscribers
ASSERT_TRUE(
sub_->spin_and_wait_for_matched(player_->get_list_of_publishers(), std::chrono::seconds(5)));

auto await_received_messages = sub_->spin_subscriptions();

auto play_execution_1 = std::async(std::launch::async, [player_]() {return player_->play();});
auto play_execution_2 = std::async(std::launch::async, [player_]() {return player_->play();});

player_->pause();
auto player_future = std::async(std::launch::async, [player_]() {return player_->play();});
player_->wait_for_playback_to_start();
ASSERT_TRUE(player_->is_paused());
ASSERT_FALSE(player_->play());

player_->resume();
player_future.get();
await_received_messages.get();

const auto play_execution_1_wait_result = play_execution_1.wait_for(
std::chrono::milliseconds(
1500));
const auto play_execution_2_wait_result = play_execution_2.wait_for(
std::chrono::milliseconds(
1500));

ASSERT_EQ(std::future_status::ready, play_execution_1_wait_result);
ASSERT_EQ(std::future_status::ready, play_execution_2_wait_result);

ASSERT_TRUE(
(play_execution_1.get() && !play_execution_2.get()) ||
(!play_execution_1.get() && play_execution_2.get()));
auto replayed_topic1 = sub_->get_received_messages<test_msgs::msg::BasicTypes>(kTopic1);
EXPECT_THAT(replayed_topic1, SizeIs(1));
}

0 comments on commit 1453615

Please sign in to comment.