image_saver.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 #include <opencv2/highgui/highgui.hpp>
00035 
00036 #include <ros/ros.h>
00037 #include <cv_bridge/cv_bridge.h>
00038 #include <image_transport/image_transport.h>
00039 #include <camera_calibration_parsers/parse.h>
00040 #include <boost/format.hpp>
00041 
00042 #include <std_srvs/Empty.h>
00043 #include <std_srvs/Trigger.h>
00044 
00045 boost::format g_format;
00046 bool save_all_image, save_image_service;
00047 std::string encoding;
00048 bool request_start_end;
00049 
00050 
00051 bool service(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
00052   save_image_service = true;
00053   return true;
00054 }
00055 
00058 class Callbacks {
00059 public:
00060   Callbacks() : is_first_image_(true), has_camera_info_(false), count_(0) {
00061   }
00062 
00063   bool callbackStartSave(std_srvs::Trigger::Request &req,
00064                          std_srvs::Trigger::Response &res)
00065   {
00066     ROS_INFO("Received start saving request");
00067     start_time_ = ros::Time::now();
00068     end_time_ = ros::Time(0);
00069 
00070     res.success = true;
00071     return true;
00072   }
00073 
00074   bool callbackEndSave(std_srvs::Trigger::Request &req,
00075                        std_srvs::Trigger::Response &res)
00076   {
00077     ROS_INFO("Received end saving request");
00078     end_time_ = ros::Time::now();
00079 
00080     res.success = true;
00081     return true;
00082   }
00083 
00084   void callbackWithoutCameraInfo(const sensor_msgs::ImageConstPtr& image_msg)
00085   {
00086     if (is_first_image_) {
00087       is_first_image_ = false;
00088 
00089       // Wait a tiny bit to see whether callbackWithCameraInfo is called
00090       ros::Duration(0.001).sleep();
00091     }
00092 
00093     if (has_camera_info_)
00094       return;
00095 
00096     // saving flag priority:
00097     //  1. request by service.
00098     //  2. request by topic about start and end.
00099     //  3. flag 'save_all_image'.
00100     if (!save_image_service && request_start_end) {
00101       if (start_time_ == ros::Time(0))
00102         return;
00103       else if (start_time_ > image_msg->header.stamp)
00104         return;  // wait for message which comes after start_time
00105       else if ((end_time_ != ros::Time(0)) && (end_time_ < image_msg->header.stamp))
00106         return;  // skip message which comes after end_time
00107     }
00108 
00109     // save the image
00110     std::string filename;
00111     if (!saveImage(image_msg, filename))
00112       return;
00113 
00114     count_++;
00115   }
00116 
00117   void callbackWithCameraInfo(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info)
00118   {
00119     has_camera_info_ = true;
00120 
00121     if (!save_image_service && request_start_end) {
00122       if (start_time_ == ros::Time(0))
00123         return;
00124       else if (start_time_ > image_msg->header.stamp)
00125         return;  // wait for message which comes after start_time
00126       else if ((end_time_ != ros::Time(0)) && (end_time_ < image_msg->header.stamp))
00127         return;  // skip message which comes after end_time
00128     }
00129 
00130     // save the image
00131     std::string filename;
00132     if (!saveImage(image_msg, filename))
00133       return;
00134 
00135     // save the CameraInfo
00136     if (info) {
00137       filename = filename.replace(filename.rfind("."), filename.length(), ".ini");
00138       camera_calibration_parsers::writeCalibration(filename, "camera", *info);
00139     }
00140 
00141     count_++;
00142   }
00143 private:
00144   bool saveImage(const sensor_msgs::ImageConstPtr& image_msg, std::string &filename) {
00145     cv::Mat image;
00146     try
00147     {
00148       image = cv_bridge::toCvShare(image_msg, encoding)->image;
00149     } catch(cv_bridge::Exception)
00150     {
00151       ROS_ERROR("Unable to convert %s image to %s", image_msg->encoding.c_str(), encoding.c_str());
00152       return false;
00153     }
00154 
00155     if (!image.empty()) {
00156       try {
00157         filename = (g_format).str();
00158       } catch (...) { g_format.clear(); }
00159       try {
00160         filename = (g_format % count_).str();
00161       } catch (...) { g_format.clear(); }
00162       try { 
00163         filename = (g_format % count_ % "jpg").str();
00164       } catch (...) { g_format.clear(); }
00165 
00166       if ( save_all_image || save_image_service ) {
00167         cv::imwrite(filename, image);
00168         ROS_INFO("Saved image %s", filename.c_str());
00169 
00170         save_image_service = false;
00171       } else {
00172         return false;
00173       }
00174     } else {
00175       ROS_WARN("Couldn't save image, no data!");
00176       return false;
00177     }
00178     return true;
00179   }
00180 
00181 private:
00182   bool is_first_image_;
00183   bool has_camera_info_;
00184   size_t count_;
00185   ros::Time start_time_;
00186   ros::Time end_time_;
00187 };
00188 
00189 int main(int argc, char** argv)
00190 {
00191   ros::init(argc, argv, "image_saver", ros::init_options::AnonymousName);
00192   ros::NodeHandle nh;
00193   image_transport::ImageTransport it(nh);
00194   std::string topic = nh.resolveName("image");
00195 
00196   Callbacks callbacks;
00197   // Useful when CameraInfo is being published
00198   image_transport::CameraSubscriber sub_image_and_camera = it.subscribeCamera(topic, 1,
00199                                                                               &Callbacks::callbackWithCameraInfo,
00200                                                                               &callbacks);
00201   // Useful when CameraInfo is not being published
00202   image_transport::Subscriber sub_image = it.subscribe(
00203       topic, 1, boost::bind(&Callbacks::callbackWithoutCameraInfo, &callbacks, _1));
00204 
00205   ros::NodeHandle local_nh("~");
00206   std::string format_string;
00207   local_nh.param("filename_format", format_string, std::string("left%04i.%s"));
00208   local_nh.param("encoding", encoding, std::string("bgr8"));
00209   local_nh.param("save_all_image", save_all_image, true);
00210   local_nh.param("request_start_end", request_start_end, false);
00211   g_format.parse(format_string);
00212   ros::ServiceServer save = local_nh.advertiseService ("save", service);
00213 
00214   if (request_start_end && !save_all_image)
00215     ROS_WARN("'request_start_end' is true, so overwriting 'save_all_image' as true");
00216 
00217   // FIXME(unkown): This does not make services appear
00218   // if (request_start_end) {
00219     ros::ServiceServer srv_start = local_nh.advertiseService(
00220       "start", &Callbacks::callbackStartSave, &callbacks);
00221     ros::ServiceServer srv_end = local_nh.advertiseService(
00222       "end", &Callbacks::callbackEndSave, &callbacks);
00223   // }
00224 
00225   ros::spin();
00226 }


image_view
Author(s): Patrick Mihelich
autogenerated on Wed May 1 2019 02:51:52