initial version

src/video_to_rosbag.cc

@@ -0,0 +1,144 @@
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/image.hpp>
+
+#include <rosbag2_cpp/writer.hpp>
+#include <rosbag2_storage/storage_options.hpp>
+
+#include <opencv2/opencv.hpp>
+#include <opencv2/videoio.hpp>
+
+#include <string>
+#include <memory>
+#include <chrono>
+
+class VideoToRosbag : public rclcpp::Node
+{
+public:
+  VideoToRosbag()
+      : Node("video_to_rosbag")
+  {
+    // Parameters
+    this->declare_parameter<std::string>("input_video", "input.mp4");
+    this->declare_parameter<std::string>("output_bag", "output_bag");
+    this->declare_parameter<std::string>("topic", "/camera/image_raw");
+    this->declare_parameter<std::string>("storage_id", "mcap"); // mcap is default on Jazzy [web:32]
+    this->declare_parameter<std::string>("frame_id", "camera_frame");
+    this->declare_parameter<double>("fps_override", 0.0);
+
+    const std::string input_video = this->get_parameter("input_video").as_string();
+    const std::string output_bag = this->get_parameter("output_bag").as_string();
+    const std::string topic = this->get_parameter("topic").as_string();
+    const std::string storage_id = this->get_parameter("storage_id").as_string();
+    const std::string frame_id = this->get_parameter("frame_id").as_string();
+    double fps_override = this->get_parameter("fps_override").as_double();
+
+    RCLCPP_INFO(get_logger(), "Input video: %s", input_video.c_str());
+    RCLCPP_INFO(get_logger(), "Output bag:  %s (storage_id=%s)", output_bag.c_str(), storage_id.c_str());
+    RCLCPP_INFO(get_logger(), "Topic:       %s", topic.c_str());
+
+    // Open video
+    cv::VideoCapture cap(input_video);
+    if (!cap.isOpened())
+    {
+      RCLCPP_ERROR(get_logger(), "Failed to open video file: %s", input_video.c_str());
+      rclcpp::shutdown();
+      return;
+    }
+
+    // Video properties
+    double video_fps = cap.get(cv::CAP_PROP_FPS);
+    int total_frames = static_cast<int>(cap.get(cv::CAP_PROP_FRAME_COUNT));
+    int width = static_cast<int>(cap.get(cv::CAP_PROP_FRAME_WIDTH));
+    int height = static_cast<int>(cap.get(cv::CAP_PROP_FRAME_HEIGHT));
+
+    double fps;
+    if (fps_override > 0.0)
+    {
+      fps = fps_override;
+      RCLCPP_WARN(get_logger(), "Using FPS override: %.3f (video reports %.3f)", fps, video_fps);
+    }
+    else if (video_fps > 0.0)
+    {
+      fps = video_fps;
+      RCLCPP_INFO(get_logger(), "Detected FPS from video: %.3f", fps);
+    }
+    else
+    {
+      fps = 30.0;
+      RCLCPP_WARN(get_logger(), "Video FPS unknown, falling back to %.3f", fps);
+    }
+
+    RCLCPP_INFO(get_logger(), "Video size: %dx%d, frames: %d, fps used: %.3f",
+                width, height, total_frames, fps);
+
+    // rosbag2 storage options (FIXED line: assign string, not struct)
+    rosbag2_storage::StorageOptions storage_options;
+    storage_options.uri = output_bag;
+    storage_options.storage_id = storage_id; // <- was 'storage_options' before (compile error)
+
+    rosbag2_cpp::ConverterOptions converter_options;
+    converter_options.input_serialization_format = rmw_get_serialization_format();
+    converter_options.output_serialization_format = rmw_get_serialization_format();
+    auto writer = std::make_unique<rosbag2_cpp::Writer>();
+    writer->open(storage_options, converter_options); // Jazzy API, mirrors tutorial [web:32]
+
+    // Time base for frame stamps
+    rclcpp::Time start_time = this->now();
+
+    cv::Mat frame;
+    int frame_count = 0;
+
+    RCLCPP_INFO(get_logger(), "Starting frame loop...");
+
+    while (cap.read(frame))
+    {
+      if (frame.empty())
+      {
+        break;
+      }
+
+      sensor_msgs::msg::Image msg;
+
+      // Timestamp based on frame index and FPS
+      rclcpp::Time stamp = start_time +
+                           rclcpp::Duration::from_seconds(static_cast<double>(frame_count) / fps);
+
+      msg.header.stamp = stamp;
+      msg.header.frame_id = frame_id;
+      msg.height = static_cast<uint32_t>(frame.rows);
+      msg.width = static_cast<uint32_t>(frame.cols);
+      msg.encoding = "bgr8"; // OpenCV default color layout
+      msg.is_bigendian = false;
+      msg.step = static_cast<sensor_msgs::msg::Image::_step_type>(frame.step);
+
+      const size_t size_in_bytes = static_cast<size_t>(frame.step) * frame.rows;
+      msg.data.assign(frame.data, frame.data + size_in_bytes);
+
+      // Let rosbag2_cpp::Writer serialize and auto‑create the topic [web:29][web:32]
+      writer->write(msg, topic, stamp);
+
+      ++frame_count;
+      if (total_frames > 0 && frame_count % 100 == 0)
+      {
+        double progress = 100.0 * static_cast<double>(frame_count) /
+                          static_cast<double>(total_frames);
+        RCLCPP_INFO(get_logger(), "Frames: %d / %d (%.1f%%)",
+                    frame_count, total_frames, progress);
+      }
+    }
+
+    cap.release();
+    writer->close();
+
+    RCLCPP_INFO(get_logger(), "Done. Wrote %d frames to bag '%s'", frame_count, output_bag.c_str());
+  }
+};
+
+int main(int argc, char **argv)
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<VideoToRosbag>();
+  // All work is done in the constructor; we don't need to spin.
+  rclcpp::shutdown();
+  return 0;
+}