Implement reading from rosbag2 files

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}

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

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>
 

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);
   }
-   */
 }