added compress_bag and fixed some bugs

Author: berndpfrommer

CMakeLists.txt

@@ -28,9 +28,9 @@ find_package(rosbag2_cpp REQUIRED)
 find_package(rosbag2_storage REQUIRED)
 find_package(sensor_msgs REQUIRED)
 
-foreach(pkg ${ament_dependencies})
-  find_package(${pkg} REQUIRED)
-endforeach()
+if(${rosbag2_cpp_VERSION_MAJOR} GREATER 0 OR ${rosbag2_cpp_VERSION_MINOR} GREATER 9)
+  add_definitions(-DUSE_NEW_ROSBAG_WRITE_INTERFACE)
+endif()
 
 if(${cv_bridge_VERSION} VERSION_GREATER "3.3.0")
   add_definitions(-DUSE_CV_BRIDGE_HPP)
@@ -73,9 +73,25 @@ target_link_libraries(bag_to_frames
 target_compile_features(bag_to_frames PRIVATE cxx_std_17)
 target_include_directories(bag_to_frames PRIVATE include)
 
+#
+# -------- compress_bag
+#
+add_executable(compress_bag  src/compress_bag.cpp)
+target_link_libraries(compress_bag
+  ffmpeg_encoder_decoder::ffmpeg_encoder_decoder
+  ${ffmpeg_image_transport_msgs_TARGETS}
+  rclcpp::rclcpp
+  rosbag2_cpp::rosbag2_cpp
+  rosbag2_storage::rosbag2_storage
+  ${sensor_msgs_TARGETS})
+target_compile_features(compress_bag PRIVATE cxx_std_17)
+target_include_directories(compress_bag PRIVATE include)
+
+
 install(TARGETS
   bag_to_file
   bag_to_frames
+  compress_bag
   DESTINATION lib/${PROJECT_NAME}/)
 
 if(BUILD_TESTING)

include/ffmpeg_image_transport_tools/bag_processor.hpp

@@ -25,6 +25,8 @@
 #include <rclcpp/serialized_message.hpp>
 #include <rosbag2_cpp/reader.hpp>
 #include <rosbag2_cpp/readers/sequential_reader.hpp>
+#include <set>
+#include <vector>
 
 using rclcpp::Time;
 using bag_time_t = rcutils_time_point_value_t;
@@ -36,24 +38,26 @@ class BagProcessor
 {
 public:
   BagProcessor(
-    const std::string & bag_name, const std::string & topic, const std::string & topic_type,
-    bag_time_t start_time, bag_time_t end_time)
-  : topic_(topic), start_time_(start_time), end_time_(end_time)
+    const std::string & bag_name, const std::vector<std::string> & topics,
+    const std::string & topic_type, bag_time_t start_time, bag_time_t end_time)
+  : start_time_(start_time), end_time_(end_time)
   {
     std::cout << "opening bag: " << bag_name << std::endl;
-    std::cout << "topic:       " << topic << std::endl;
     reader_.open(bag_name);
     const auto meta = reader_.get_all_topics_and_types();
-    const auto it = std::find_if(
-      meta.begin(), meta.end(), [topic, topic_type](const rosbag2_storage::TopicMetadata & m) {
-        return (m.name == topic && m.type == topic_type);
-      });
-    if (it == meta.end()) {
-      std::cerr << "topic " << topic << " with type " << topic_type << " not found!" << std::endl;
-      throw(std::runtime_error("topic not found!"));
+    for (const auto & topic : topics) {
+      topics_.insert(topic);
+      const auto it = std::find_if(
+        meta.begin(), meta.end(), [topic, topic_type](const rosbag2_storage::TopicMetadata & m) {
+          return (m.name == topic && m.type == topic_type);
+        });
+      if (it == meta.end()) {
+        std::cerr << "topic " << topic << " with type " << topic_type << " not found!" << std::endl;
+        throw(std::runtime_error("topic not found!"));
+      }
+      std::cout << "found topic: " << it->name << " with type: " << it->type << std::endl;
+      filter_.topics.push_back(topic);
     }
-
-    filter_.topics.push_back(topic);
     reader_.set_filter(filter_);
   }
 
@@ -68,7 +72,7 @@ class BagProcessor
 
     while (reader_.has_next()) {
       auto msg = reader_.read_next();
-      if (!msg || topic_ != msg->topic_name) {
+      if (!msg || (topics_.find(msg->topic_name) == topics_.end())) {
         continue;
       }
       rclcpp::SerializedMessage serialized_msg(*msg->serialized_data);
@@ -82,7 +86,7 @@ class BagProcessor
       if (t > end_time_) {
         break;
       }
-      mp->process(t, m);
+      mp->process(t, msg->topic_name, m);
       message_number++;
     }
     const auto stop = std::chrono::high_resolution_clock::now();
@@ -95,7 +99,7 @@ class BagProcessor
 private:
   rosbag2_cpp::Reader reader_;
   rosbag2_storage::StorageFilter filter_;
-  std::string topic_;
+  std::set<std::string> topics_;
   bag_time_t start_time_;
   bag_time_t end_time_;
   rclcpp::Serialization<T> serialization_;

include/ffmpeg_image_transport_tools/message_processor.hpp

@@ -23,7 +23,8 @@ class MessageProcessor
 {
 public:
   virtual ~MessageProcessor() {}
-  virtual void process(uint64_t t, const typename T::ConstSharedPtr & msg) = 0;
+  virtual void process(
+    uint64_t t, const std::string & topic, const typename T::ConstSharedPtr & msg) = 0;
 };
 }  // namespace ffmpeg_image_transport_tools
 

src/bag_to_file.cpp

@@ -58,7 +58,7 @@ class FileWriter : public ffmpeg_image_transport_tools::MessageProcessor<FFMPEGP
     ts_file_.open(ts_file);
   }
 
-  void process(uint64_t t, const FFMPEGPacket::ConstSharedPtr & m) final
+  void process(uint64_t t, const std::string &, const FFMPEGPacket::ConstSharedPtr & m) final
   {
     const auto t_header = Time(m->header.stamp).nanoseconds();
     raw_file_.write(reinterpret_cast<const char *>(m->data.data()), m->data.size());
@@ -98,7 +98,7 @@ int main(int argc, char ** argv)
         }
         break;
       case 'o':
-        out_file = atof(optarg);
+        out_file = optarg;
         break;
       case 'r':
         rate = atof(optarg);
@@ -144,9 +144,9 @@ int main(int argc, char ** argv)
 
   const std::string raw_file = out_file + ".h264";
   const std::string topic_type = "ffmpeg_image_transport_msgs/msg/FFMPEGPacket";
+  std::vector<std::string> topics{topic};
   ffmpeg_image_transport_tools::BagProcessor<FFMPEGPacket> bproc(
-    bag, topic, topic_type, start_time, end_time);
+    bag, topics, topic_type, start_time, end_time);
   FileWriter fw(raw_file, time_stamp_file);
   bproc.process(&fw);
-  convertToMP4(raw_file, out_file, rate);
 }

src/bag_to_frames.cpp

@@ -61,15 +61,16 @@ class FrameWriter : public ffmpeg_image_transport_tools::MessageProcessor<FFMPEG
     ts_file_.open(Path(base_dir_) / Path(ts_file));
   }
 
-  void process(uint64_t t, const FFMPEGPacket::ConstSharedPtr & m) final
+  void process(uint64_t t, const std::string &, const FFMPEGPacket::ConstSharedPtr & m) final
   {
     if (!decoder_.isInitialized()) {
       std::string dtype = decoder_type_;
       if (dtype.empty()) {
         const auto & decoderMap = Decoder::getDefaultEncoderToDecoderMap();
         auto decTypeIt = decoderMap.find(m->encoding);
         if (decTypeIt == decoderMap.end()) {
-          std::cerr << "unknown encoding: " << m->encoding << std::endl;
+          std::cerr << "unknown encoder: " << m->encoding << std::endl;
+          std::cerr << "must specify decoder!" << std::endl;
           throw(std::runtime_error("unknown encoding: " + m->encoding));
         }
         dtype = decTypeIt->second;
@@ -152,7 +153,7 @@ int main(int argc, char ** argv)
         }
         break;
       case 'o':
-        out_dir = atof(optarg);
+        out_dir = optarg;
         break;
       case 's':
         start_time = static_cast<bag_time_t>(atof(optarg) * 1e9);
@@ -187,10 +188,10 @@ int main(int argc, char ** argv)
     usage();
     return (-1);
   }
-
+  const std::vector<std::string> topics{topic};
   const std::string topic_type = "ffmpeg_image_transport_msgs/msg/FFMPEGPacket";
   ffmpeg_image_transport_tools::BagProcessor<FFMPEGPacket> bproc(
-    bag, topic, topic_type, start_time, end_time);
+    bag, topics, topic_type, start_time, end_time);
   FrameWriter fw(decoder, out_dir, time_stamp_file);
   bproc.process(&fw);
 }