From 954e5bf8cd26ebae8ca023c8cf0f641505129f86 Mon Sep 17 00:00:00 2001
From: Ramon Wijnands <ramon.wijnands@nobleo.nl>
Date: Fri, 7 Jun 2024 14:33:36 +0200
Subject: [PATCH] Implement reading from rosbag2 files

I've implemented reading from rosbag2 files. I've used
`rosbag2_transport::ReaderWriterFactory` so that both compressed and
uncompressed bagfiles can be handled.
---
 CMakeLists.txt                    |  2 ++
 include/swri_console/bag_reader.h |  1 -
 package.xml                       |  2 +-
 src/bag_reader.cpp                | 45 +++++++++++++------------------
 4 files changed, 21 insertions(+), 29 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 78579f8..700a83b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -14,6 +14,7 @@ project(swri_console)
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rcl_interfaces REQUIRED)
+find_package(rosbag2_transport REQUIRED)
 
 ### QT PACKAGES ###
 find_package(Qt5Core REQUIRED)
@@ -68,6 +69,7 @@ ament_target_dependencies(${PROJECT_NAME}
   ament_cmake
   rclcpp
   rcl_interfaces
+  rosbag2_transport
 )
 
 install(TARGETS ${PROJECT_NAME}
diff --git a/include/swri_console/bag_reader.h b/include/swri_console/bag_reader.h
index 15f4465..ee3ba7e 100644
--- a/include/swri_console/bag_reader.h
+++ b/include/swri_console/bag_reader.h
@@ -35,7 +35,6 @@
 #include <QString>
 #include <QMetaType>
 
-#include <rclcpp/rclcpp.hpp>
 #include <rcl_interfaces/msg/log.hpp>
 
 namespace swri_console
diff --git a/package.xml b/package.xml
index 6ec7e5a..467e9bd 100644
--- a/package.xml
+++ b/package.xml
@@ -22,7 +22,7 @@
   <depend>libqt5-core</depend>
   <depend>libqt5-gui</depend>
   <depend>libqt5-widgets</depend>
-  <!-- <depend>rosbag_storage</depend> -->
+  <depend>rosbag2_transport</depend>
   <depend>rclcpp</depend>
   <depend>rcl_interfaces</depend>
 
diff --git a/src/bag_reader.cpp b/src/bag_reader.cpp
index 59cf22b..fb1c2f7 100644
--- a/src/bag_reader.cpp
+++ b/src/bag_reader.cpp
@@ -33,54 +33,45 @@
 #include <QMessageBox>
 
 #include "swri_console/bag_reader.h"
-
-// #include <rosbag/bag.h>
-// #include <rosbag/view.h>
-#include <rclcpp/rclcpp.hpp>
+#include <rosbag2_transport/reader_writer_factory.hpp>
 
 using namespace swri_console;
 
 void BagReader::readBagFile(const QString& filename)
 {
-  /*
-  rosbag::Bag bag;
-  bag.open(filename.toStdString(), rosbag::bagmode::Read);
-
-  std::vector<std::string> topics;
-  topics.push_back(std::string("/rosout"));
+  rosbag2_storage::StorageOptions storage_options;
+  storage_options.uri = filename.toStdString();
+  auto reader = rosbag2_transport::ReaderWriterFactory::make_reader(storage_options);
+  reader->open(storage_options);
 
-  rosbag::View view(bag, rosbag::TopicQuery(topics));
-  rosbag::View::const_iterator iter;
+  while (reader->has_next()) {
+    auto msg = reader->read_next();
 
-  for(iter = view.begin(); iter != view.end(); ++iter)
-  {
-    rosgraph_msgs::LogConstPtr log = iter->instantiate<rosgraph_msgs::Log>();
-    if (log != NULL ) {
-      emit logReceived(log);
-    }
-    else {
-      qWarning("Got a message that was not a log message but a: %s", iter->getDataType().c_str());
+    if (msg->topic_name != "/rosout") {
+      continue;
     }
+
+    // Deserialize the message
+    rclcpp::SerializedMessage serialized_msg(*msg->serialized_data);
+    auto log = std::make_shared<rcl_interfaces::msg::Log>();
+    rclcpp::Serialization<rcl_interfaces::msg::Log> serialization_;
+    serialization_.deserialize_message(&serialized_msg, log.get());
+
+    emit logReceived(log);
   }
-   */
 
   emit finishedReading();
 }
 
 void BagReader::promptForBagFile()
 {
-  QMessageBox::information(nullptr,
-                           tr("Bag files not supported"),
-                           tr("Reading and writing bag files is not yet supported in ROS 2."));
-  /*
   QString filename = QFileDialog::getOpenFileName(nullptr,
                                                   tr("Open Bag File"),
                                                   QDir::homePath(),
-                                                  tr("Bag Files (*.bag)"));
+                                                  tr("Bag Files (*.mcap)"));
 
   if (filename != nullptr)
   {
     readBagFile(filename);
   }
-   */
 }