Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[video_recorder] Add timer to record low fps video at the correct speed #600

Open
wants to merge 3 commits into
base: melodic
Choose a base branch
from
Open
Changes from 2 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 18 additions & 10 deletions image_view/src/nodes/video_recorder.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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))
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This change is unnecessary given my other comment.

{
// Skip to get video with correct fps
return;
Expand All @@ -83,22 +86,26 @@ 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());
return;
}
}

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;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is no longer true. The timestamp of the last-received image can no longer be considered close enough to the actual time that the frame was written. Use ros::Time::now() instead.

} else {
ROS_WARN("Frame skipped, no data!");
}
}

int main(int argc, char** argv)
{
ros::init(argc, argv, "video_recorder", ros::init_options::AnonymousName);
Expand Down Expand Up @@ -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();
Expand Down