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." );
73 if ((image_msg->header.stamp - g_last_wrote_time) <
ros::Duration(1.0 /
fps))
91 g_last_wrote_time = image_msg->header.stamp;
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"));
108 bool stamped_filename;
109 local_nh.
param(
"stamped_filename", stamped_filename,
false);
111 local_nh.
param(
"codec",
codec, std::string(
"MJPG"));
119 if (stamped_filename) {
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;
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
Subscriber subscribe(const std::string &base_topic, uint32_t queue_size, const boost::function< void(const sensor_msgs::ImageConstPtr &)> &callback, const ros::VoidPtr &tracked_object=ros::VoidPtr(), const TransportHints &transport_hints=TransportHints())
void callback(const sensor_msgs::ImageConstPtr &image_msg)
ros::Time g_last_wrote_time
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
int main(int argc, char **argv)
cv::VideoWriter outputVideo
std::string resolveName(const std::string &name, bool remap=true) const
#define ROS_INFO_STREAM(args)
CvImageConstPtr cvtColorForDisplay(const CvImageConstPtr &source, const std::string &encoding=std::string(), const CvtColorForDisplayOptions options=CvtColorForDisplayOptions())