image_nodelet.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 <image_view/ImageViewConfig.h>
35 
36 #include <ros/ros.h>
37 #include <nodelet/nodelet.h>
39 #include <dynamic_reconfigure/server.h>
40 
41 #include <cv_bridge/cv_bridge.h>
42 #include <opencv2/highgui/highgui.hpp>
43 #include "window_thread.h"
44 
45 #include <boost/bind/bind.hpp>
46 #include <boost/thread.hpp>
47 #include <boost/format.hpp>
48 
49 
50 namespace image_view {
51 
52 class ThreadSafeImage
53 {
54  boost::mutex mutex_;
55  boost::condition_variable condition_;
56  cv::Mat image_;
57 
58 public:
59  void set(const cv::Mat& image);
60 
61  cv::Mat get();
62 
63  cv::Mat pop();
64 };
65 
66 void ThreadSafeImage::set(const cv::Mat& image)
67 {
68  boost::unique_lock<boost::mutex> lock(mutex_);
69  image_ = image;
70  condition_.notify_one();
71 }
72 
73 cv::Mat ThreadSafeImage::get()
74 {
75  boost::unique_lock<boost::mutex> lock(mutex_);
76  return image_;
77 }
78 
79 cv::Mat ThreadSafeImage::pop()
80 {
81  cv::Mat image;
82  {
83  boost::unique_lock<boost::mutex> lock(mutex_);
84  while (image_.empty())
85  {
86  condition_.wait(lock);
87  }
88  image = image_;
89  image_.release();
90  }
91  return image;
92 }
93 
94 class ImageNodelet : public nodelet::Nodelet
95 {
97 
98  boost::thread window_thread_;
99 
101 
102  std::string window_name_;
103  bool autosize_;
104  boost::format filename_format_;
105  int count_;
106 
108 
109  dynamic_reconfigure::Server<image_view::ImageViewConfig> srv_;
110  bool do_dynamic_scaling_;
112  double min_image_value_;
113  double max_image_value_;
114 
115  virtual void onInit();
116 
117  void reconfigureCb(image_view::ImageViewConfig &config, uint32_t level);
118 
119  void imageCb(const sensor_msgs::ImageConstPtr& msg);
120 
121  static void mouseCb(int event, int x, int y, int flags, void* param);
122 
123  void windowThread();
124 
125 public:
127 
129 };
130 
132  : filename_format_(""), count_(0)
133 {
134 }
135 
137 {
138  if (window_thread_.joinable())
139  {
140  window_thread_.interrupt();
142  }
143 }
144 
146 {
149 
150  // Command line argument parsing
151  const std::vector<std::string>& argv = getMyArgv();
152  // First positional argument is the transport type
153  std::string transport;
154  local_nh.param("image_transport", transport, std::string("raw"));
155  for (int i = 0; i < (int)argv.size(); ++i)
156  {
157  if (argv[i][0] != '-')
158  {
159  transport = argv[i];
160  break;
161  }
162  }
163  NODELET_INFO_STREAM("Using transport \"" << transport << "\"");
164  // Internal option, should be used only by the image_view node
165  bool shutdown_on_close = std::find(argv.begin(), argv.end(),
166  "--shutdown-on-close") != argv.end();
167 
168  // Default window name is the resolved topic name
169  std::string topic = nh.resolveName("image");
170  local_nh.param("window_name", window_name_, topic);
171 
172  local_nh.param("autosize", autosize_, false);
173 
174  std::string format_string;
175  local_nh.param("filename_format", format_string, std::string("frame%04i.jpg"));
176  filename_format_.parse(format_string);
177 
178  window_thread_ = boost::thread(&ImageNodelet::windowThread, this);
179 
182  sub_ = it.subscribe(topic, 1, &ImageNodelet::imageCb, this, hints);
183  pub_ = local_nh.advertise<sensor_msgs::Image>("output", 1);
184 
185  dynamic_reconfigure::Server<image_view::ImageViewConfig>::CallbackType f =
186  boost::bind(&ImageNodelet::reconfigureCb, this, boost::placeholders::_1, boost::placeholders::_2);
187  srv_.setCallback(f);
188 }
189 
190 void ImageNodelet::reconfigureCb(image_view::ImageViewConfig &config, uint32_t level)
191 {
192  do_dynamic_scaling_ = config.do_dynamic_scaling;
193  colormap_ = config.colormap;
194  min_image_value_ = config.min_image_value;
195  max_image_value_ = config.max_image_value;
196 }
197 
198 void ImageNodelet::imageCb(const sensor_msgs::ImageConstPtr& msg)
199 {
200  // We want to scale floating point images so that they display nicely
201  bool do_dynamic_scaling;
202  if (msg->encoding.find("F") != std::string::npos) {
203  do_dynamic_scaling = true;
204  } else {
205  do_dynamic_scaling = do_dynamic_scaling_;
206  }
207 
208  // Convert to OpenCV native BGR color
210  try {
212  options.do_dynamic_scaling = do_dynamic_scaling;
213  options.colormap = colormap_;
214  // Set min/max value for scaling to visualize depth/float image.
216  // Not specified by rosparam, then set default value.
217  // Because of current sensor limitation, we use 10m as default of max range of depth
218  // with consistency to the configuration in rqt_image_view.
219  options.min_image_value = 0;
220  if (msg->encoding == "32FC1") {
221  options.max_image_value = 10; // 10 [m]
222  } else if (msg->encoding == "16UC1") {
223  options.max_image_value = 10 * 1000; // 10 * 1000 [mm]
224  }
225  } else {
228  }
229  cv_ptr = cvtColorForDisplay(cv_bridge::toCvShare(msg), "", options);
230  queued_image_.set(cv_ptr->image.clone());
231  }
232  catch (cv_bridge::Exception& e) {
233  NODELET_ERROR_THROTTLE(30, "Unable to convert '%s' image for display: '%s'",
234  msg->encoding.c_str(), e.what());
235  }
236  if (pub_.getNumSubscribers() > 0) {
237  pub_.publish(cv_ptr);
238  }
239 }
240 
241 void ImageNodelet::mouseCb(int event, int x, int y, int flags, void* param)
242 {
243  ImageNodelet *this_ = reinterpret_cast<ImageNodelet*>(param);
244  // Trick to use NODELET_* logging macros in static function
245  boost::function<const std::string&()> getName =
246  boost::bind(&ImageNodelet::getName, this_);
247 
248  if (event == cv::EVENT_LBUTTONDOWN)
249  {
250  NODELET_WARN_ONCE("Left-clicking no longer saves images. Right-click instead.");
251  return;
252  }
253  if (event != cv::EVENT_RBUTTONDOWN)
254  return;
255 
256  cv::Mat image(this_->shown_image_.get());
257  if (image.empty())
258  {
259  NODELET_WARN("Couldn't save image, no data!");
260  return;
261  }
262 
263  std::string filename;
264  try
265  {
266  filename = (this_->filename_format_ % this_->count_).str();
267  }
268  catch (const boost::io::too_many_args&)
269  {
270  NODELET_WARN_ONCE("Couldn't save image, filename_format is invalid.");
271  return;
272  }
273  if (cv::imwrite(filename, image))
274  {
275  NODELET_INFO("Saved image %s", filename.c_str());
276  this_->count_++;
277  }
278  else
279  {
281  NODELET_ERROR("Failed to save image.");
282  }
283 }
284 
286 {
287  cv::namedWindow(window_name_, autosize_ ? cv::WND_PROP_AUTOSIZE : 0);
288  cv::setMouseCallback(window_name_, &ImageNodelet::mouseCb, this);
289 
290  try
291  {
292  while (ros::ok())
293  {
294  cv::Mat image(queued_image_.pop());
295  cv::imshow(window_name_, image);
296  shown_image_.set(image);
297  cv::waitKey(1);
298 
299  if (cv::getWindowProperty(window_name_, 1) < 0)
300  {
301  break;
302  }
303  }
304  }
305  catch (const boost::thread_interrupted&)
306  {
307  }
308 
309  cv::destroyAllWindows();
310 
311  pub_.shutdown();
312 
313  if (ros::ok())
314  {
315  ros::shutdown();
316  }
317 }
318 
319 } // namespace image_view
320 
321 // Register the nodelet
image_view::ThreadSafeImage::pop
cv::Mat pop()
Definition: image_nodelet.cpp:111
cv_bridge::CvtColorForDisplayOptions::do_dynamic_scaling
bool do_dynamic_scaling
image_view::ImageNodelet::mouseCb
static void mouseCb(int event, int x, int y, int flags, void *param)
Definition: image_nodelet.cpp:273
NODELET_ERROR
#define NODELET_ERROR(...)
ros::Publisher
nodelet::Nodelet::getNodeHandle
ros::NodeHandle & getNodeHandle() const
image_transport::ImageTransport
image_view::ImageNodelet::pub_
ros::Publisher pub_
Definition: image_nodelet.cpp:139
boost::shared_ptr
cv_bridge::CvtColorForDisplayOptions
NODELET_ERROR_THROTTLE
#define NODELET_ERROR_THROTTLE(rate,...)
ros.h
image_view::ImageNodelet::reconfigureCb
void reconfigureCb(image_view::ImageViewConfig &config, uint32_t level)
Definition: image_nodelet.cpp:222
image_view::ImageNodelet::window_thread_
boost::thread window_thread_
Definition: image_nodelet.cpp:130
image_view::ImageNodelet::autosize_
bool autosize_
Definition: image_nodelet.cpp:135
image_view::ImageNodelet::filename_format_
boost::format filename_format_
Definition: image_nodelet.cpp:136
ros::TransportHints
NODELET_WARN
#define NODELET_WARN(...)
ros::shutdown
ROSCPP_DECL void shutdown()
cv_bridge::Exception
nodelet::Nodelet::getMyArgv
const V_string & getMyArgv() const
image_view::ImageNodelet::do_dynamic_scaling_
bool do_dynamic_scaling_
Definition: image_nodelet.cpp:142
nodelet::Nodelet::getPrivateNodeHandle
ros::NodeHandle & getPrivateNodeHandle() const
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
ros::ok
ROSCPP_DECL bool ok()
f
f
window_thread.h
image_view::ImageNodelet::max_image_value_
double max_image_value_
Definition: image_nodelet.cpp:145
image_transport::Subscriber
cv_bridge::CvtColorForDisplayOptions::colormap
int colormap
ros::NodeHandle::resolveName
std::string resolveName(const std::string &name, bool remap=true) const
cvtColorForDisplay
CvImageConstPtr cvtColorForDisplay(const CvImageConstPtr &source, const std::string &encoding=std::string(), const CvtColorForDisplayOptions options=CvtColorForDisplayOptions())
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
ros::Publisher::shutdown
void shutdown()
image_view::ImageNodelet::ImageNodelet
ImageNodelet()
Definition: image_nodelet.cpp:163
filename
std::string filename
Definition: video_recorder.cpp:37
image_view::ImageNodelet::colormap_
int colormap_
Definition: image_nodelet.cpp:143
NODELET_WARN_ONCE
#define NODELET_WARN_ONCE(...)
image_view::ImageNodelet::imageCb
void imageCb(const sensor_msgs::ImageConstPtr &msg)
Definition: image_nodelet.cpp:230
image_view::ImageNodelet::sub_
image_transport::Subscriber sub_
Definition: image_nodelet.cpp:128
image_view::ThreadSafeImage::set
void set(const cv::Mat &image)
Definition: image_nodelet.cpp:98
image_view::ImageNodelet::srv_
dynamic_reconfigure::Server< image_view::ImageViewConfig > srv_
Definition: image_nodelet.cpp:141
image_view::ImageNodelet::onInit
virtual void onInit()
Definition: image_nodelet.cpp:177
image_view::ThreadSafeImage::image_
cv::Mat image_
Definition: image_nodelet.cpp:120
cv_bridge::CvtColorForDisplayOptions::max_image_value
double max_image_value
image_transport.h
NODELET_INFO
#define NODELET_INFO(...)
nodelet::Nodelet
image_view::ImageNodelet::shown_image_
ThreadSafeImage shown_image_
Definition: image_nodelet.cpp:132
image_view::ImageNodelet
Definition: image_nodelet.cpp:126
image_view::ImageNodelet::min_image_value_
double min_image_value_
Definition: image_nodelet.cpp:144
nodelet.h
image_view::ImageNodelet::window_name_
std::string window_name_
Definition: image_nodelet.cpp:134
cv_bridge.h
cv_bridge::CvtColorForDisplayOptions::min_image_value
double min_image_value
class_list_macros.hpp
nodelet::Nodelet::getName
const std::string & getName() const
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
param
T param(const std::string &param_name, const T &default_val)
image_view
Definition: disparity_nodelet.cpp:42
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
image_transport::TransportHints
image_view::ImageNodelet::windowThread
void windowThread()
Definition: image_nodelet.cpp:317
cv_bridge::toCvShare
CvImageConstPtr toCvShare(const sensor_msgs::Image &source, const boost::shared_ptr< void const > &tracked_object, const std::string &encoding=std::string())
image_view::ThreadSafeImage::mutex_
boost::mutex mutex_
Definition: image_nodelet.cpp:118
NODELET_INFO_STREAM
#define NODELET_INFO_STREAM(...)
image_view::ThreadSafeImage
Definition: image_nodelet.cpp:84
image_view::ThreadSafeImage::get
cv::Mat get()
Definition: image_nodelet.cpp:105
image_view::ImageNodelet::count_
int count_
Definition: image_nodelet.cpp:137
image_view::ImageNodelet::queued_image_
ThreadSafeImage queued_image_
Definition: image_nodelet.cpp:132
ros::NodeHandle
image_view::ImageNodelet::~ImageNodelet
~ImageNodelet()
Definition: image_nodelet.cpp:168
image_view::ThreadSafeImage::condition_
boost::condition_variable condition_
Definition: image_nodelet.cpp:119


image_view
Author(s): Patrick Mihelich
autogenerated on Wed Jan 24 2024 03:57:22