00001 #include <opencv/cv.h>
00002 #include <opencv/highgui.h>
00003
00004 #include <ros/ros.h>
00005 #include <cv_bridge/CvBridge.h>
00006 #include <image_transport/image_transport.h>
00007 #include <camera_calibration_parsers/parse.h>
00008 #include <boost/format.hpp>
00009
00010 sensor_msgs::CvBridge g_bridge;
00011 int g_count = 0;
00012 boost::format g_format;
00013
00014 void callback(const sensor_msgs::ImageConstPtr& image, const sensor_msgs::CameraInfoConstPtr& info)
00015 {
00016 if (g_bridge.fromImage(*image, "bgr8")) {
00017 IplImage *image = g_bridge.toIpl();
00018 if (image) {
00019 std::string filename = (g_format % g_count % "jpg").str();
00020 cvSaveImage(filename.c_str(), image);
00021 ROS_INFO("Saved image %s", filename.c_str());
00022 filename = (g_format % g_count % "ini").str();
00023 camera_calibration_parsers::writeCalibration(filename, "camera", *info);
00024
00025 g_count++;
00026 } else {
00027 ROS_WARN("Couldn't save image, no data!");
00028 }
00029 }
00030 else
00031 ROS_ERROR("Unable to convert %s image to bgr8", image->encoding.c_str());
00032 }
00033
00034 int main(int argc, char** argv)
00035 {
00036 ros::init(argc, argv, "image_saver", ros::init_options::AnonymousName);
00037 ros::NodeHandle nh;
00038 g_format.parse("left%04i.%s");
00039 image_transport::ImageTransport it(nh);
00040 std::string topic = nh.resolveName("image");
00041 image_transport::CameraSubscriber sub = it.subscribeCamera(topic, 1, &callback);
00042
00043 ros::spin();
00044 }
