image_saver.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 #include <opencv2/highgui/highgui.hpp>
35 
36 #include <ros/ros.h>
37 #include <cv_bridge/cv_bridge.h>
40 #include <boost/format.hpp>
41 
42 #include <std_srvs/Empty.h>
43 #include <std_srvs/Trigger.h>
44 
45 boost::format g_format;
47 std::string encoding;
49 
50 
51 bool service(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
52  save_image_service = true;
53  return true;
54 }
55 
58 class Callbacks {
59 public:
61  }
62 
63  bool callbackStartSave(std_srvs::Trigger::Request &req,
64  std_srvs::Trigger::Response &res)
65  {
66  ROS_INFO("Received start saving request");
68  end_time_ = ros::Time(0);
69 
70  res.success = true;
71  return true;
72  }
73 
74  bool callbackEndSave(std_srvs::Trigger::Request &req,
75  std_srvs::Trigger::Response &res)
76  {
77  ROS_INFO("Received end saving request");
79 
80  res.success = true;
81  return true;
82  }
83 
84  void callbackWithoutCameraInfo(const sensor_msgs::ImageConstPtr& image_msg)
85  {
86  if (is_first_image_) {
87  is_first_image_ = false;
88 
89  // Wait a tiny bit to see whether callbackWithCameraInfo is called
90  ros::Duration(0.001).sleep();
91  }
92 
93  if (has_camera_info_)
94  return;
95 
96  // saving flag priority:
97  // 1. request by service.
98  // 2. request by topic about start and end.
99  // 3. flag 'save_all_image'.
101  if (start_time_ == ros::Time(0))
102  return;
103  else if (start_time_ > image_msg->header.stamp)
104  return; // wait for message which comes after start_time
105  else if ((end_time_ != ros::Time(0)) && (end_time_ < image_msg->header.stamp))
106  return; // skip message which comes after end_time
107  }
108 
109  // save the image
110  std::string filename;
111  if (!saveImage(image_msg, filename))
112  return;
113 
114  count_++;
115  }
116 
117  void callbackWithCameraInfo(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info)
118  {
119  has_camera_info_ = true;
120 
122  if (start_time_ == ros::Time(0))
123  return;
124  else if (start_time_ > image_msg->header.stamp)
125  return; // wait for message which comes after start_time
126  else if ((end_time_ != ros::Time(0)) && (end_time_ < image_msg->header.stamp))
127  return; // skip message which comes after end_time
128  }
129 
130  // save the image
131  std::string filename;
132  if (!saveImage(image_msg, filename))
133  return;
134 
135  // save the CameraInfo
136  if (info) {
137  filename = filename.replace(filename.rfind("."), filename.length(), ".ini");
138  camera_calibration_parsers::writeCalibration(filename, "camera", *info);
139  }
140 
141  count_++;
142  }
143 private:
144  bool saveImage(const sensor_msgs::ImageConstPtr& image_msg, std::string &filename) {
145  cv::Mat image;
146  try
147  {
148  image = cv_bridge::toCvShare(image_msg, encoding)->image;
149  } catch(cv_bridge::Exception)
150  {
151  ROS_ERROR("Unable to convert %s image to %s", image_msg->encoding.c_str(), encoding.c_str());
152  return false;
153  }
154 
155  if (!image.empty()) {
156  try {
157  filename = (g_format).str();
158  } catch (...) { g_format.clear(); }
159  try {
160  filename = (g_format % count_).str();
161  } catch (...) { g_format.clear(); }
162  try {
163  filename = (g_format % count_ % "jpg").str();
164  } catch (...) { g_format.clear(); }
165 
167  cv::imwrite(filename, image);
168  ROS_INFO("Saved image %s", filename.c_str());
169 
170  save_image_service = false;
171  } else {
172  return false;
173  }
174  } else {
175  ROS_WARN("Couldn't save image, no data!");
176  return false;
177  }
178  return true;
179  }
180 
181 private:
184  size_t count_;
187 };
188 
189 int main(int argc, char** argv)
190 {
191  ros::init(argc, argv, "image_saver", ros::init_options::AnonymousName);
192  ros::NodeHandle nh;
194  std::string topic = nh.resolveName("image");
195 
196  Callbacks callbacks;
197  // Useful when CameraInfo is being published
198  image_transport::CameraSubscriber sub_image_and_camera = it.subscribeCamera(topic, 1,
200  &callbacks);
201  // Useful when CameraInfo is not being published
202  image_transport::Subscriber sub_image = it.subscribe(
203  topic, 1, boost::bind(&Callbacks::callbackWithoutCameraInfo, &callbacks, _1));
204 
205  ros::NodeHandle local_nh("~");
206  std::string format_string;
207  local_nh.param("filename_format", format_string, std::string("left%04i.%s"));
208  local_nh.param("encoding", encoding, std::string("bgr8"));
209  local_nh.param("save_all_image", save_all_image, true);
210  local_nh.param("request_start_end", request_start_end, false);
211  g_format.parse(format_string);
212  ros::ServiceServer save = local_nh.advertiseService ("save", service);
213 
215  ROS_WARN("'request_start_end' is true, so overwriting 'save_all_image' as true");
216 
217  // FIXME(unkown): This does not make services appear
218  // if (request_start_end) {
219  ros::ServiceServer srv_start = local_nh.advertiseService(
220  "start", &Callbacks::callbackStartSave, &callbacks);
221  ros::ServiceServer srv_end = local_nh.advertiseService(
222  "end", &Callbacks::callbackEndSave, &callbacks);
223  // }
224 
225  ros::spin();
226 }
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
Subscriber subscribe(const std::string &base_topic, uint32_t queue_size, const boost::function< void(const sensor_msgs::ImageConstPtr &)> &callback, const ros::VoidPtr &tracked_object=ros::VoidPtr(), const TransportHints &transport_hints=TransportHints())
std::string filename
bool service(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
Definition: image_saver.cpp:51
CameraSubscriber subscribeCamera(const std::string &base_topic, uint32_t queue_size, const CameraSubscriber::Callback &callback, const ros::VoidPtr &tracked_object=ros::VoidPtr(), const TransportHints &transport_hints=TransportHints())
std::string resolveName(const std::string &name, bool remap=true) const
void callbackWithCameraInfo(const sensor_msgs::ImageConstPtr &image_msg, const sensor_msgs::CameraInfoConstPtr &info)
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool saveImage(const sensor_msgs::ImageConstPtr &image_msg, std::string &filename)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
bool callbackStartSave(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
Definition: image_saver.cpp:63
#define ROS_WARN(...)
ROSCPP_DECL void spin(Spinner &spinner)
bool is_first_image_
bool save_image_service
Definition: image_saver.cpp:46
boost::format g_format
Definition: image_saver.cpp:45
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
size_t count_
bool writeCalibration(const std::string &file_name, const std::string &camera_name, const sensor_msgs::CameraInfo &cam_info)
ros::Time start_time_
bool save_all_image
Definition: image_saver.cpp:46
int main(int argc, char **argv)
bool callbackEndSave(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
Definition: image_saver.cpp:74
bool has_camera_info_
static Time now()
ros::Time end_time_
const std::string header
bool request_start_end
Definition: image_saver.cpp:48
#define ROS_ERROR(...)
std::string encoding
Definition: image_saver.cpp:47
void callbackWithoutCameraInfo(const sensor_msgs::ImageConstPtr &image_msg)
Definition: image_saver.cpp:84


image_view
Author(s): Patrick Mihelich
autogenerated on Fri May 3 2019 02:59:57