[humble] Bugfix for incorrect MCAPStorage::seek(timestamp) when timestamp is current (backport #2157) (#2161)

* Bugfix for MCAPStorage::seek(time) advance in time is current (#2157)

- The seek(timestamp) shall adhere >= condition even when target
 timestamp is equal to the current timestamp in the storage.

Signed-off-by: Michael Orlov <[email protected]>
(cherry picked from commit 2738d77)

# Conflicts:
#	rosbag2_transport/test/rosbag2_transport/test_play_seek.cpp

* Address merge conflicts

Signed-off-by: Michael Orlov <[email protected]>

---------

Signed-off-by: Michael Orlov <[email protected]>
Co-authored-by: Michael Orlov <[email protected]>

Author: mergify[bot]

rosbag2_storage_mcap/src/mcap_storage.cpp

@@ -654,9 +654,7 @@ void MCAPStorage::reset_filter()
 
 void MCAPStorage::seek(const rcutils_time_point_value_t & time_stamp)
 {
-  if (time_stamp != last_read_time_point_) {
-    last_read_message_offset_ = std::nullopt;
-  }
+  last_read_message_offset_ = std::nullopt;
   last_read_time_point_ = time_stamp;
   reset_iterator();
 }

rosbag2_transport/test/rosbag2_transport/test_play_seek.cpp

@@ -180,6 +180,37 @@ TEST_P(RosBag2PlaySeekTestFixture, seek_with_timestamp_later_than_in_last_messag
   ASSERT_THAT(replayed_topic1, SizeIs(expected_number_of_messages));
 }
 
+TEST_P(RosBag2PlaySeekTestFixture, reader_can_correctly_do_seek) {
+  reader_->open(storage_options_);
+  const auto first_msg_timestamp = start_time_ms_ * 1000000;
+  const auto second_msg_timestamp = (start_time_ms_ + message_spacing_ms_) * 1000000;
+  const auto third_msg_timestamp = (start_time_ms_ + message_spacing_ms_ * 2) * 1000000;
+  ASSERT_TRUE(reader_->has_next());
+  auto msg = reader_->read_next();
+  EXPECT_EQ(msg->time_stamp, first_msg_timestamp);  // First message timestamp
+
+  // Jump on third message (1200 ms)
+  reader_->seek(third_msg_timestamp);
+  ASSERT_TRUE(reader_->has_next());
+  msg = reader_->read_next();
+  EXPECT_EQ(msg->time_stamp, third_msg_timestamp);
+
+  // Jump back on second message (1100 ms)
+  reader_->seek(second_msg_timestamp);
+  ASSERT_TRUE(reader_->has_next());
+  msg = reader_->read_next();
+  EXPECT_EQ(msg->time_stamp, second_msg_timestamp);
+  ASSERT_TRUE(reader_->has_next());
+  msg = reader_->read_next();
+  EXPECT_EQ(msg->time_stamp, third_msg_timestamp);
+
+  // Jump on third message (1200 ms) where timestamp is exactly the same as previous read message
+  reader_->seek(third_msg_timestamp);
+  ASSERT_TRUE(reader_->has_next());
+  msg = reader_->read_next();
+  EXPECT_EQ(msg->time_stamp, third_msg_timestamp);
+}
+
 TEST_P(RosBag2PlaySeekTestFixture, seek_forward) {
   const size_t expected_number_of_messages = num_msgs_in_bag_ - 1;
   auto player = std::make_shared<MockPlayer>(std::move(reader_), storage_options_, play_options_);