play: refuse to play non-monotonic bags

xqms · Mar 19, 2024 · dcf1fff · dcf1fff
1 parent 5216a83
commit dcf1fff
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 0 deletions.
diff --git a/rosbag_fancy/src/bag_reader.cpp b/rosbag_fancy/src/bag_reader.cpp
@@ -257,6 +257,8 @@ class BagReader::Private
 
 	ros::Time startTime;
 	ros::Time endTime;
+
+	bool isMonotonic = true;
 };
 
 class BagReader::ChunkIterator::Private
@@ -713,6 +715,9 @@ BagReader::BagReader(const std::string& filename)
 		chunk.startTime = ros::Time(startTime & 0xFFFFFFFF, startTime >> 32);
 		chunk.endTime = ros::Time(endTime & 0xFFFFFFFF, endTime >> 32);
 
+		if(lastChunk && lastChunk->endTime > chunk.startTime)
+			m_d->isMonotonic = false;
+
 		auto numConnections = rec.integralHeader<std::uint32_t>("count");
 
 		if(rec.dataSize < numConnections * sizeof(ConnectionInfo))
@@ -825,6 +830,11 @@ std::size_t BagReader::numChunks() const
 	return m_d->chunks.size();
 }
 
+bool BagReader::isMonotonic() const
+{
+	return m_d->isMonotonic;
+}
+
 
 // TESTS
 

diff --git a/rosbag_fancy/src/bag_reader.h b/rosbag_fancy/src/bag_reader.h
@@ -186,6 +186,8 @@ class BagReader
 
 	int findChunk(const ros::Time& time) const;
 	Iterator chunkBegin(int chunk) const;
+
+	bool isMonotonic() const;
 private:
 	class Private;
 	std::unique_ptr<Private> m_d;

diff --git a/rosbag_fancy/src/cmd_play.cpp b/rosbag_fancy/src/cmd_play.cpp
@@ -111,6 +111,15 @@ int play(const std::vector<std::string>& options)
 	{
 		auto& bag = bags.emplace_back(filename);
 
+		if(!bag.reader.isMonotonic())
+		{
+			throw std::runtime_error{fmt::format(
+				"Bag '{}' contains non-monotonic chunks. This is not supported by rosbag_fancy play at the moment. "
+				"You can convert a non-monotonic bag to a monotonic one by running 'rosbag filter in.bag out.bag True'.",
+				filename
+			)};
+		}
+
 		if(startTime == ros::Time(0) || bag.reader.startTime() < startTime)
 			startTime = bag.reader.startTime();