Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #include <opencv2/highgui/highgui.hpp>
00021 #include <ros/ros.h>
00022 #include <sensor_msgs/image_encodings.h>
00023 #include <cv_bridge/cv_bridge.h>
00024 #include <image_transport/image_transport.h>
00025 #include <camera_calibration_parsers/parse.h>
00026 #if CV_MAJOR_VERSION == 3
00027 #include <opencv2/videoio.hpp>
00028 #endif
00029
00030 cv::VideoWriter outputVideo;
00031
00032 int g_count = 0;
00033 ros::Time g_last_wrote_time = ros::Time(0);
00034 std::string encoding;
00035 std::string codec;
00036 int fps;
00037 std::string filename;
00038 double min_depth_range;
00039 double max_depth_range;
00040 bool use_dynamic_range;
00041 int colormap;
00042
00043
00044 void callback(const sensor_msgs::ImageConstPtr& image_msg)
00045 {
00046 if (!outputVideo.isOpened()) {
00047
00048 cv::Size size(image_msg->width, image_msg->height);
00049
00050 outputVideo.open(filename,
00051 #if CV_MAJOR_VERSION == 3
00052 cv::VideoWriter::fourcc(codec.c_str()[0],
00053 #else
00054 CV_FOURCC(codec.c_str()[0],
00055 #endif
00056 codec.c_str()[1],
00057 codec.c_str()[2],
00058 codec.c_str()[3]),
00059 fps,
00060 size,
00061 true);
00062
00063 if (!outputVideo.isOpened())
00064 {
00065 ROS_ERROR("Could not create the output video! Check filename and/or support for codec.");
00066 exit(-1);
00067 }
00068
00069 ROS_INFO_STREAM("Starting to record " << codec << " video at " << size << "@" << fps << "fps. Press Ctrl+C to stop recording." );
00070
00071 }
00072
00073 if ((image_msg->header.stamp - g_last_wrote_time) < ros::Duration(1 / fps))
00074 {
00075
00076 return;
00077 }
00078
00079 try
00080 {
00081 cv_bridge::CvtColorForDisplayOptions options;
00082 options.do_dynamic_scaling = use_dynamic_range;
00083 options.min_image_value = min_depth_range;
00084 options.max_image_value = max_depth_range;
00085 options.colormap = colormap;
00086 const cv::Mat image = cv_bridge::cvtColorForDisplay(cv_bridge::toCvShare(image_msg), encoding, options)->image;
00087 if (!image.empty()) {
00088 outputVideo << image;
00089 ROS_INFO_STREAM("Recording frame " << g_count << "\x1b[1F");
00090 g_count++;
00091 g_last_wrote_time = image_msg->header.stamp;
00092 } else {
00093 ROS_WARN("Frame skipped, no data!");
00094 }
00095 } catch(cv_bridge::Exception)
00096 {
00097 ROS_ERROR("Unable to convert %s image to %s", image_msg->encoding.c_str(), encoding.c_str());
00098 return;
00099 }
00100 }
00101
00102 int main(int argc, char** argv)
00103 {
00104 ros::init(argc, argv, "video_recorder", ros::init_options::AnonymousName);
00105 ros::NodeHandle nh;
00106 ros::NodeHandle local_nh("~");
00107 local_nh.param("filename", filename, std::string("output.avi"));
00108 bool stamped_filename;
00109 local_nh.param("stamped_filename", stamped_filename, false);
00110 local_nh.param("fps", fps, 15);
00111 local_nh.param("codec", codec, std::string("MJPG"));
00112 local_nh.param("encoding", encoding, std::string("bgr8"));
00113
00114 local_nh.param("min_depth_range", min_depth_range, 0.0);
00115 local_nh.param("max_depth_range", max_depth_range, 0.0);
00116 local_nh.param("use_dynamic_depth_range", use_dynamic_range, false);
00117 local_nh.param("colormap", colormap, -1);
00118
00119 if (stamped_filename) {
00120 std::size_t found = filename.find_last_of("/\\");
00121 std::string path = filename.substr(0, found + 1);
00122 std::string basename = filename.substr(found + 1);
00123 std::stringstream ss;
00124 ss << ros::Time::now().toNSec() << basename;
00125 filename = path + ss.str();
00126 ROS_INFO("Video recording to %s", filename.c_str());
00127 }
00128
00129 if (codec.size() != 4) {
00130 ROS_ERROR("The video codec must be a FOURCC identifier (4 chars)");
00131 exit(-1);
00132 }
00133
00134 image_transport::ImageTransport it(nh);
00135 std::string topic = nh.resolveName("image");
00136 image_transport::Subscriber sub_image = it.subscribe(topic, 1, callback);
00137
00138 ROS_INFO_STREAM("Waiting for topic " << topic << "...");
00139 ros::spin();
00140 std::cout << "\nVideo saved as " << filename << std::endl;
00141 }