Skip to content
Open
Changes from all 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
24 changes: 15 additions & 9 deletions image_view/src/nodes/video_recorder.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@ double min_depth_range;
double max_depth_range;
bool use_dynamic_range;
int colormap;
cv::Mat image;


void callback(const sensor_msgs::ImageConstPtr& image_msg)
Expand Down Expand Up @@ -83,22 +84,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 = ros::Time::now();
} 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 +139,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