From 2c675f5e4061f1c61d29c616f5856f472cd24efc Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Sat, 19 Sep 2020 00:08:11 +0900 Subject: [PATCH 1/3] [video_recorder] Add timer to record low fps video at the correct speed --- image_view/src/nodes/video_recorder.cpp | 32 +++++++++++++++---------- 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/image_view/src/nodes/video_recorder.cpp b/image_view/src/nodes/video_recorder.cpp index 3b3420eeb..245f6abde 100644 --- a/image_view/src/nodes/video_recorder.cpp +++ b/image_view/src/nodes/video_recorder.cpp @@ -39,6 +39,8 @@ double min_depth_range; double max_depth_range; bool use_dynamic_range; int colormap; +cv::Mat image; +ros::Time stamp; void callback(const sensor_msgs::ImageConstPtr& image_msg) @@ -47,7 +49,7 @@ void callback(const sensor_msgs::ImageConstPtr& image_msg) cv::Size size(image_msg->width, image_msg->height); - outputVideo.open(filename, + outputVideo.open(filename, #if CV_MAJOR_VERSION >= 3 cv::VideoWriter::fourcc(codec.c_str()[0], #else @@ -55,7 +57,7 @@ void callback(const sensor_msgs::ImageConstPtr& image_msg) #endif codec.c_str()[1], codec.c_str()[2], - codec.c_str()[3]), + codec.c_str()[3]), fps, size, true); @@ -70,7 +72,8 @@ void callback(const sensor_msgs::ImageConstPtr& image_msg) } - if ((image_msg->header.stamp - g_last_wrote_time) < ros::Duration(1.0 / fps)) + stamp = image_msg->header.stamp; + if ((stamp - g_last_wrote_time) < ros::Duration(1.0 / fps)) { // Skip to get video with correct fps return; @@ -83,15 +86,7 @@ void callback(const sensor_msgs::ImageConstPtr& image_msg) options.min_image_value = min_depth_range; options.max_image_value = max_depth_range; options.colormap = colormap; - const cv::Mat image = cv_bridge::cvtColorForDisplay(cv_bridge::toCvShare(image_msg), encoding, options)->image; - if (!image.empty()) { - outputVideo << image; - ROS_INFO_STREAM("Recording frame " << g_count << "\x1b[1F"); - g_count++; - g_last_wrote_time = image_msg->header.stamp; - } else { - ROS_WARN("Frame skipped, no data!"); - } + image = cv_bridge::cvtColorForDisplay(cv_bridge::toCvShare(image_msg), encoding, options)->image; } catch(cv_bridge::Exception) { ROS_ERROR("Unable to convert %s image to %s", image_msg->encoding.c_str(), encoding.c_str()); @@ -99,6 +94,18 @@ void callback(const sensor_msgs::ImageConstPtr& image_msg) } } +void timercallback(const ros::TimerEvent&) +{ + if (!image.empty()) { + outputVideo << image; + ROS_INFO_STREAM("Recording frame " << g_count << "\x1b[1F"); + g_count++; + g_last_wrote_time = stamp; + } else { + ROS_WARN("Frame skipped, no data!"); + } +} + int main(int argc, char** argv) { ros::init(argc, argv, "video_recorder", ros::init_options::AnonymousName); @@ -134,6 +141,7 @@ int main(int argc, char** argv) image_transport::ImageTransport it(nh); std::string topic = nh.resolveName("image"); image_transport::Subscriber sub_image = it.subscribe(topic, 1, callback); + ros::Timer timer = nh.createTimer(ros::Duration(1.0 / fps), timercallback); ROS_INFO_STREAM("Waiting for topic " << topic << "..."); ros::spin(); From d6b8e626fe4876afb89203ea4169575f66fad553 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Sat, 19 Sep 2020 01:27:15 +0900 Subject: [PATCH 2/3] Remove trailing whitespace --- image_view/src/nodes/video_recorder.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/image_view/src/nodes/video_recorder.cpp b/image_view/src/nodes/video_recorder.cpp index 245f6abde..07cd9fe75 100644 --- a/image_view/src/nodes/video_recorder.cpp +++ b/image_view/src/nodes/video_recorder.cpp @@ -49,7 +49,7 @@ void callback(const sensor_msgs::ImageConstPtr& image_msg) cv::Size size(image_msg->width, image_msg->height); - outputVideo.open(filename, + outputVideo.open(filename, #if CV_MAJOR_VERSION >= 3 cv::VideoWriter::fourcc(codec.c_str()[0], #else @@ -57,7 +57,7 @@ void callback(const sensor_msgs::ImageConstPtr& image_msg) #endif codec.c_str()[1], codec.c_str()[2], - codec.c_str()[3]), + codec.c_str()[3]), fps, size, true); From 0acb2515456c64e0a1836b3775c17498b7eeaa50 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Fri, 9 Oct 2020 09:30:48 +0900 Subject: [PATCH 3/3] Use ros::Time::now() for the last image writing --- image_view/src/nodes/video_recorder.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/image_view/src/nodes/video_recorder.cpp b/image_view/src/nodes/video_recorder.cpp index 07cd9fe75..708f90057 100644 --- a/image_view/src/nodes/video_recorder.cpp +++ b/image_view/src/nodes/video_recorder.cpp @@ -40,7 +40,6 @@ double max_depth_range; bool use_dynamic_range; int colormap; cv::Mat image; -ros::Time stamp; void callback(const sensor_msgs::ImageConstPtr& image_msg) @@ -72,8 +71,7 @@ void callback(const sensor_msgs::ImageConstPtr& image_msg) } - stamp = image_msg->header.stamp; - if ((stamp - g_last_wrote_time) < ros::Duration(1.0 / fps)) + if ((image_msg->header.stamp - g_last_wrote_time) < ros::Duration(1.0 / fps)) { // Skip to get video with correct fps return; @@ -100,7 +98,7 @@ void timercallback(const ros::TimerEvent&) outputVideo << image; ROS_INFO_STREAM("Recording frame " << g_count << "\x1b[1F"); g_count++; - g_last_wrote_time = stamp; + g_last_wrote_time = ros::Time::now(); } else { ROS_WARN("Frame skipped, no data!"); }