support compressed images in glim_rosbag

koide3 · Oct 30, 2024 · 0c85bf0 · 0c85bf0
1 parent 28ba890
commit 0c85bf0
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 2 deletions.
diff --git a/src/glim_rosbag.cpp b/src/glim_rosbag.cpp
@@ -4,6 +4,7 @@
 #include <spdlog/spdlog.h>
 #include <boost/format.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <cv_bridge/cv_bridge.hpp>
 #include <sensor_msgs/msg/compressed_image.hpp>
 #include <rosbag2_cpp/reader.hpp>
 #include <rosbag2_cpp/readers/sequential_reader.hpp>
@@ -224,6 +225,10 @@ int main(int argc, char** argv) {
       } else if (msg->topic_name == image_topic && topic_type == "sensor_msgs/msg/CompressedImage") {
         auto compressed_image_msg = std::make_shared<sensor_msgs::msg::CompressedImage>();
         compressed_image_serialization.deserialize_message(&serialized_msg, compressed_image_msg.get());
+
+        auto image_msg = std::make_shared<sensor_msgs::msg::Image>();
+        cv_bridge::toCvCopy(*compressed_image_msg, "bgr8")->toImageMsg(*image_msg);
+        glim->image_callback(image_msg);
       }
 
       auto found = subscription_map.find(msg->topic_name);

diff --git a/src/offline_viewer.cpp b/src/offline_viewer.cpp
@@ -1,4 +1,5 @@
 #include <iostream>
+#include <spdlog/spdlog.h>
 #include <ament_index_cpp/get_package_share_directory.hpp>
 
 #include <glim/util/config.hpp>
@@ -11,13 +12,13 @@ int main(int argc, char** argv) {
     config_path = ament_index_cpp::get_package_share_directory("glim") + "/" + config_path;
   }
 
-  std::cout << "config_path: " << config_path << std::endl;
+  spdlog::info("config_path: {}", config_path);
   glim::GlobalConfig::instance(config_path);
 
   std::string init_map_path;
   if (argc >= 2) {
     init_map_path = argv[1];
-    std::cout << "map_path=" << init_map_path << std::endl;
+    spdlog::info("map_path={}", init_map_path);
   }
 
   glim::OfflineViewer viewer(init_map_path);