如何将VideoWriter代码合并到ros image_publisher代码中

我正在设置一个 ros 系统，为我的无人机发布带有 ros、c++ 和 opencv-2 的图像。下面的代码正在发布原始图像。在发布时，我想以1280 x 720的分辨率逐帧编写灰度图像来录制视频。我找到了一个现成的 opencv 视频编写代码。但是，我无法将此代码合并到image_publisher代码中。这是image_publisher代码：

#include <ros/ros.h>
#include <camera_info_manager/camera_info_manager.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <opencv2/opencv.hpp>
int main(int argc, char **argv)
{
ROS_INFO("Starting image_pub ROS node...n");
ros::init(argc, argv, "image_pub");
ros::NodeHandle nh("~");
std::string camera_topic;
std::string camera_info_topic;
std::string camera_info_url;
std::string img_path;
std::string frame_id;
float       pub_rate;
int         start_sec;
bool        repeat;
nh.param<std::string>("camera_topic",      camera_topic,      "/camera/image_raw");
nh.param<std::string>("camera_info_topic", camera_info_topic, "/camera/camera_info");
nh.param<std::string>("camera_info_url",   camera_info_url,   "");
nh.param<std::string>("img_path", img_path, "");
nh.param<std::string>("frame_id", frame_id, "");
nh.param("pub_rate",  pub_rate, 30.0f);
nh.param("start_sec", start_sec, 0);
nh.param("repeat",    repeat, false);
ROS_INFO("CTopic : %s", camera_topic.c_str());
ROS_INFO("ITopic : %s", camera_info_topic.c_str());
ROS_INFO("CI URL : %s", camera_info_url.c_str());
ROS_INFO("Source : %s", img_path.c_str());
ROS_INFO("Rate   : %.1f", pub_rate);
ROS_INFO("Start  : %d", start_sec);
ROS_INFO("Repeat : %s", repeat ? "yes" : "no");
ROS_INFO("FrameID: %s", frame_id.c_str());
camera_info_manager::CameraInfoManager camera_info_manager(nh);
if (camera_info_manager.validateURL(camera_info_url))
    camera_info_manager.loadCameraInfo(camera_info_url);
ros::Publisher img_pub  = nh.advertise<sensor_msgs::Image>(camera_topic, 1);
ros::Publisher info_pub = nh.advertise<sensor_msgs::CameraInfo>(camera_info_topic, 1);
cv::VideoCapture vid_cap(img_path.c_str());
if (start_sec > 0)
    vid_cap.set(CV_CAP_PROP_POS_MSEC, 1000.0 * start_sec);
ros::Rate rate(pub_rate);
while (ros::ok())
{
    cv::Mat img;
    if (!vid_cap.read(img))
    {
        if (repeat)
        {
            vid_cap.open(img_path.c_str());
            if (start_sec > 0)
                vid_cap.set(CV_CAP_PROP_POS_MSEC, 1000.0 * start_sec);
            continue;
        }
        ROS_ERROR("Failed to capture frame.");
        ros::shutdown();
    }
    else
    {
        //ROS_DEBUG("Image: %dx%dx%d, %zu, %d", img.rows, img.cols, img.channels(), img.elemSize(), img.type() == CV_8UC3);
        if (img.type() != CV_8UC3)
            img.convertTo(img, CV_8UC3);
        // Convert image from BGR format used by OpenCV to RGB.
        cv::cvtColor(img, img, CV_BGR2RGB);
        auto img_msg = boost::make_shared<sensor_msgs::Image>();
        img_msg->header.stamp    = ros::Time::now();
        img_msg->header.frame_id = frame_id;
        img_msg->encoding = "rgb8";
        img_msg->width = img.cols;
        img_msg->height = img.rows;
        img_msg->step = img_msg->width * img.channels();
        auto ptr = img.ptr<unsigned char>(0);
        img_msg->data = std::vector<unsigned char>(ptr, ptr + img_msg->step * img_msg->height);
        img_pub.publish(img_msg);
        if (camera_info_manager.isCalibrated())
        {
            auto info = boost::make_shared<sensor_msgs::CameraInfo>(camera_info_manager.getCameraInfo());
            info->header = img_msg->header;
            info_pub.publish(info);
        }
    }
    ros::spinOnce();
    rate.sleep();
}
return 0;
}

视频编写代码（请参阅此链接）：

#include "opencv2/opencv.hpp"
#include <iostream>
using namespace std;
using namespace cv;
int main(){
// Create a VideoCapture object and use camera to capture the video
VideoCapture cap(0); 
// Check if camera opened successfully
if(!cap.isOpened())
{
  cout << "Error opening video stream" << endl; 
  return -1; 
} 
// Default resolution of the frame is obtained.The default resolution is system dependent. 
int frame_width = cap.get(CV_CAP_PROP_FRAME_WIDTH); 
int frame_height = cap.get(CV_CAP_PROP_FRAME_HEIGHT); 
// Define the codec and create VideoWriter object.The output is stored in 'outcpp.avi' file. 
VideoWriter video("outcpp.avi",CV_FOURCC('M','J','P','G'),10, 
Size(frame_width,frame_height)); 
while(1)
{ 
Mat frame; 
// Capture frame-by-frame 
cap >> frame;
// If the frame is empty, break immediately
if (frame.empty())
  break;
// Write the frame into the file 'outcpp.avi'
video.write(frame);
// Display the resulting frame    
imshow( "Frame", frame );
// Press  ESC on keyboard to  exit
char c = (char)waitKey(1);
if( c == 27 ) 
  break;
}
// When everything done, release the video capture and write object
cap.release();
video.release();
// Closes all the windows
destroyAllWindows();
return 0;
  }

首先，我尝试将视频编写代码（无灰度）合并到发布者代码中。但它未能运行。 总而言之，image_publisher代码应该在完成任务后生成视频。这样做的正确方法是什么？