20 #include <opencv2/highgui/highgui.hpp>
26 #if CV_MAJOR_VERSION == 3
27 #include <opencv2/videoio.hpp>
44 void callback(
const sensor_msgs::ImageConstPtr& image_msg)
48 cv::Size size(image_msg->width, image_msg->height);
51 #
if CV_MAJOR_VERSION >= 3
52 cv::VideoWriter::fourcc(
codec.c_str()[0],
54 CV_FOURCC(
codec.c_str()[0],
65 ROS_ERROR(
"Could not create the output video! Check filename and/or support for codec.");
69 ROS_INFO_STREAM(
"Starting to record " <<
codec <<
" video at " << size <<
"@" <<
fps <<
"fps. Press Ctrl+C to stop recording." );
81 cv_bridge::CvtColorForDisplayOptions options;
82 options.do_dynamic_scaling = use_dynamic_range;
83 options.min_image_value = min_depth_range;
84 options.max_image_value = max_depth_range;
85 options.colormap = colormap;
86 const cv::Mat image = cv_bridge::cvtColorForDisplay(cv_bridge::toCvShare(image_msg), encoding, options)->image;
89 ROS_INFO_STREAM(
"Recording frame " << g_count <<
"\x1b[1F");
91 g_last_wrote_time = image_msg->header.stamp;
93 ROS_WARN(
"Frame skipped, no data!");
97 ROS_ERROR(
"Unable to convert %s image to %s", image_msg->encoding.c_str(),
encoding.c_str());
102 int main(
int argc,
char** argv)
107 local_nh.
param(
"filename",
filename, std::string(
"output.avi"));
111 local_nh.
param(
"codec",
codec, std::string(
"MJPG"));
120 std::size_t found =
filename.find_last_of(
"/\\");
121 std::string path =
filename.substr(0, found + 1);
122 std::string basename =
filename.substr(found + 1);
123 std::stringstream ss;
129 if (
codec.size() != 4) {
130 ROS_ERROR(
"The video codec must be a FOURCC identifier (4 chars)");
140 std::cout <<
"\nVideo saved as " <<
filename << std::endl;