synchronizedcamerathrottler.cc

Go to the documentation of this file.

/*
 * synchronizedcamerathrottler.cc
 * Mac Mason <mmason@willowgarage.com>
 *
 * Like topic_tools/throttler, but uses a camera subscriber to smartly
 * throttle pairs of messages.
 */

#include <boost/thread.hpp>
#include "ros/ros.h"
#include "image_transport/image_transport.h"

// The fancy subscriber handles synchronization for us.
image_transport::CameraSubscriber camera_sub;
image_transport::CameraPublisher camera_pub;

// (Not) off to the races.
boost::mutex mutex;

sensor_msgs::Image current_image;
sensor_msgs::CameraInfo current_info;

void camera_cb(const sensor_msgs::ImageConstPtr& img,
               const sensor_msgs::CameraInfoConstPtr& info)
{
  boost::mutex::scoped_lock lock(mutex);
  current_image = *img;
  current_info = *info;
}

void timer_cb(const ros::TimerEvent& e)
{
  boost::mutex::scoped_lock lock(mutex);
  camera_pub.publish(current_image, current_info);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "synchronizedcamerathrottler");
  if (argc != 2)
  {
    ROS_FATAL("Need to provide a frequency!");
    return 1;
  }
  double frequency = atof(argv[1]);

  ros::NodeHandle node;
  image_transport::ImageTransport transport(node);
  camera_sub = transport.subscribeCamera("cam_input", 100, camera_cb);
  camera_pub = transport.advertiseCamera("cam_output", 100);

  ros::Timer timer = 
    node.createTimer(ros::Duration(1.0 / frequency), timer_cb);

  ros::spin();
  return 0;
}