Synchronised saver for two image streams coming from ROS, useful for rgb+depth image saving

synced_save.cpp

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>

#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>

#include <boost/format.hpp>
#include <boost/thread.hpp>
#include <boost/filesystem.hpp>

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>

int g_count;
boost::format g_filename_format;
boost::mutex g_image_mutex;

cv_bridge::CvImageConstPtr imageToOpenCV(const sensor_msgs::ImageConstPtr& msg) {
  // Convert to OpenCV native BGR color
  cv_bridge::CvImageConstPtr cv_ptr;
  try {
    cv_bridge::CvtColorForDisplayOptions options;
    options.do_dynamic_scaling = false;
    options.colormap = -1;
    // Set min/max value for scaling to visualize depth/float image.
    // Because of current sensor limitation, we use 10m as default of max range of depth
    // with consistency to the configuration in rqt_image_view.
    options.min_image_value = 0;
    if (msg->encoding == "32FC1") {
      options.max_image_value = 10;  // 10 [m]
    } else if (msg->encoding == "16UC1") {
      options.max_image_value = 10 * 1000;  // 10 * 1000 [mm]
    }

    cv_ptr = cv_bridge::cvtColorForDisplay(cv_bridge::toCvShare(msg), "", options);
  } catch (cv_bridge::Exception& e) {
    ROS_ERROR_THROTTLE(30, "Unable to convert '%s' image for display: '%s'",
		       msg->encoding.c_str(), e.what());
  }

  return cv_ptr;
}

void saveImage(cv_bridge::CvImageConstPtr& im, std::string& filename, int stream_num) {
  if (!(im->image).empty()) {
    filename = (g_filename_format % stream_num % g_count % "jpg").str();
    
    cv::imwrite(filename, im->image);
    ROS_INFO("Saved image %s", filename.c_str());
  } else {
    ROS_WARN("Couldn't save image, no data!");
  }
  
}

void callback(const sensor_msgs::ImageConstPtr& im_1, const sensor_msgs::ImageConstPtr& im_2) {
  boost::mutex::scoped_lock lock(g_image_mutex);

  cv_bridge::CvImageConstPtr im1_conv = imageToOpenCV(im_1);
  cv_bridge::CvImageConstPtr im2_conv = imageToOpenCV(im_2);

  std::string filename;
  saveImage(im1_conv, filename, 1);
  saveImage(im2_conv, filename, 2);
  g_count++;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "image_view", ros::init_options::AnonymousName);

  if (ros::names::remap("image_1") == "image_1") {
    ROS_WARN("Topic 'image_1' has not been remapped! Typical command-line usage:\n"
	     "\t$ rosrun image_view image_view image_1:=<image topic> [transport]");
  }

  if (ros::names::remap("image_2") == "image_2") {
    ROS_WARN("Topic 'image2' has not been remapped! Typical command-line usage:\n"
	     "\t$ rosrun image_view image_view image_2:=<image topic> [transport]");
  }

  ros::NodeHandle nh;
  ros::NodeHandle local_nh("~");

  g_filename_format.parse("stream_%d_%04i.%s");

  // Default window name is the resolved topic name
  std::string im_1_topic = nh.resolveName("image_1");
  std::string im_2_topic = nh.resolveName("image_2");

  // Handle transport
  // priority:
  //    1. command line argument
  //    2. rosparam '~image_transport'
  std::string transport;
  local_nh.param("image_transport", transport, std::string("raw"));
  ros::V_string myargv;
  ros::removeROSArgs(argc, argv, myargv);
  for (size_t i = 1; i < myargv.size(); ++i) {
    if (myargv[i][0] != '-') {
      transport = myargv[i];
      break;
    }
  }

  ROS_INFO_STREAM("Using transport \"" << transport << "\"");
  image_transport::ImageTransport it(nh);
  image_transport::TransportHints hints(transport, ros::TransportHints(), local_nh);

  image_transport::SubscriberFilter im_1_sub(it, im_1_topic, 1, hints);
  image_transport::SubscriberFilter im_2_sub(it, im_2_topic, 1, hints);

  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,
							  sensor_msgs::Image> SyncPolicy;

  message_filters::Synchronizer<SyncPolicy> sync(SyncPolicy(10), im_1_sub, im_2_sub);

  {
    sync.registerCallback(boost::bind(&callback, _1, _2 ));
  }

  ros::spin();

  return 0;
}