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 #include -#include #include 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 @@ libqt5-core libqt5-gui libqt5-widgets - + rosbag2_transport rclcpp rcl_interfaces 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 #include "swri_console/bag_reader.h" - -// #include -// #include -#include +#include using namespace swri_console; void BagReader::readBagFile(const QString& filename) { - /* - rosbag::Bag bag; - bag.open(filename.toStdString(), rosbag::bagmode::Read); - - std::vector 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(); - 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(); + rclcpp::Serialization 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); } - */ }