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 <ros/ros.h>
35 #include <nodelet/nodelet.h>
37 
38 #include <cv_bridge/cv_bridge.h>
39 #include <opencv2/highgui/highgui.hpp>
40 #include "window_thread.h"
41 
42 #include <boost/thread.hpp>
43 #include <boost/format.hpp>
44 
45 #ifdef HAVE_GTK
46 #include <gtk/gtk.h>
47 
48 // Platform-specific workaround for #3026: image_view doesn't close when
49 // closing image window. On platforms using GTK+ we connect this to the
50 // window's "destroy" event so that image_view exits.
51 static void destroyNode(GtkWidget *widget, gpointer data)
52 {
54  //ros::shutdown();
55  exit(0); // brute force solution
56 }
57 
58 static void destroyNodelet(GtkWidget *widget, gpointer data)
59 {
60  // We can't actually unload the nodelet from here, but we can at least
61  // unsubscribe from the image topic.
62  reinterpret_cast<image_transport::Subscriber*>(data)->shutdown();
63 }
64 #endif
65 
66 
67 namespace image_view {
68 
70 {
72 
73  boost::mutex image_mutex_;
74  cv::Mat last_image_;
75 
76  std::string window_name_;
77  bool autosize_;
78  boost::format filename_format_;
79  int count_;
81 
82  virtual void onInit();
83 
84  void imageCb(const sensor_msgs::ImageConstPtr& msg);
85 
86  static void mouseCb(int event, int x, int y, int flags, void* param);
87 
88 public:
89  ImageNodelet();
90 
91  ~ImageNodelet();
92 };
93 
95  : filename_format_(""), count_(0), initialized_(false)
96 {
97 }
98 
100 {
101  cv::destroyWindow(window_name_);
102 }
103 
105 {
108 
109  // Command line argument parsing
110  const std::vector<std::string>& argv = getMyArgv();
111  // First positional argument is the transport type
112  std::string transport;
113  local_nh.param("image_transport", transport, std::string("raw"));
114  for (int i = 0; i < (int)argv.size(); ++i)
115  {
116  if (argv[i][0] != '-')
117  {
118  transport = argv[i];
119  break;
120  }
121  }
122  NODELET_INFO_STREAM("Using transport \"" << transport << "\"");
123  // Internal option, should be used only by the image_view node
124  bool shutdown_on_close = std::find(argv.begin(), argv.end(),
125  "--shutdown-on-close") != argv.end();
126 
127  // Default window name is the resolved topic name
128  std::string topic = nh.resolveName("image");
129  local_nh.param("window_name", window_name_, topic);
130 
131  local_nh.param("autosize", autosize_, false);
132 
133  std::string format_string;
134  local_nh.param("filename_format", format_string, std::string("frame%04i.jpg"));
135  filename_format_.parse(format_string);
136 
137 #ifdef HAVE_GTK
138  // Register appropriate handler for when user closes the display window
139  GtkWidget *widget = GTK_WIDGET( cvGetWindowHandle(window_name_.c_str()) );
140  if (shutdown_on_close)
141  g_signal_connect(widget, "destroy", G_CALLBACK(destroyNode), NULL);
142  else
143  g_signal_connect(widget, "destroy", G_CALLBACK(destroyNodelet), &sub_);
144 #endif
145 
146  // Start the OpenCV window thread so we don't have to waitKey() somewhere
148 
151  sub_ = it.subscribe(topic, 1, &ImageNodelet::imageCb, this, hints);
152 }
153 
154 void ImageNodelet::imageCb(const sensor_msgs::ImageConstPtr& msg)
155 {
156  if ( ! initialized_ ) {
157  cv::namedWindow(window_name_, autosize_ ? cv::WND_PROP_AUTOSIZE : 0);
158  cv::setMouseCallback(window_name_, &ImageNodelet::mouseCb, this);
159  initialized_ = true;
160  }
161 
162  image_mutex_.lock();
163 
164  // We want to scale floating point images so that they display nicely
165  bool do_dynamic_scaling = (msg->encoding.find("F") != std::string::npos);
166 
167  // Convert to OpenCV native BGR color
168  try {
170  options.do_dynamic_scaling = do_dynamic_scaling;
171  last_image_ = cvtColorForDisplay(cv_bridge::toCvShare(msg), "", options)->image;
172  }
173  catch (cv_bridge::Exception& e) {
174  NODELET_ERROR_THROTTLE(30, "Unable to convert '%s' image for display: '%s'",
175  msg->encoding.c_str(), e.what());
176  }
177 
178  // Must release the mutex before calling cv::imshow, or can deadlock against
179  // OpenCV's window mutex.
180  image_mutex_.unlock();
181  if (!last_image_.empty()) {
182  cv::imshow(window_name_, last_image_);
183  cv::waitKey(1);
184  }
185 }
186 
187 void ImageNodelet::mouseCb(int event, int x, int y, int flags, void* param)
188 {
189  ImageNodelet *this_ = reinterpret_cast<ImageNodelet*>(param);
190  // Trick to use NODELET_* logging macros in static function
191  boost::function<const std::string&()> getName =
192  boost::bind(&ImageNodelet::getName, this_);
193 
194  if (event == cv::EVENT_LBUTTONDOWN)
195  {
196  NODELET_WARN_ONCE("Left-clicking no longer saves images. Right-click instead.");
197  return;
198  }
199  if (event != cv::EVENT_RBUTTONDOWN)
200  return;
201 
202  boost::lock_guard<boost::mutex> guard(this_->image_mutex_);
203 
204  const cv::Mat &image = this_->last_image_;
205  if (image.empty())
206  {
207  NODELET_WARN("Couldn't save image, no data!");
208  return;
209  }
210 
211  std::string filename = (this_->filename_format_ % this_->count_).str();
212  if (cv::imwrite(filename, image))
213  {
214  NODELET_INFO("Saved image %s", filename.c_str());
215  this_->count_++;
216  }
217  else
218  {
220  NODELET_ERROR("Failed to save image.");
221  }
222 }
223 
224 } // namespace image_view
225 
226 // Register the nodelet
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())
#define NODELET_INFO_STREAM(...)
std::string filename
static void mouseCb(int event, int x, int y, int flags, void *param)
const V_string & getMyArgv() const
#define NODELET_ERROR(...)
#define NODELET_WARN(...)
std::string resolveName(const std::string &name, bool remap=true) const
#define NODELET_ERROR_THROTTLE(rate,...)
const std::string & getName() const
boost::format filename_format_
ros::NodeHandle & getPrivateNodeHandle() const
image_transport::Subscriber sub_
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ros::NodeHandle & getNodeHandle() const
#define NODELET_WARN_ONCE(...)
void startWindowThread()
#define NODELET_INFO(...)
CvImageConstPtr cvtColorForDisplay(const CvImageConstPtr &source, const std::string &encoding=std::string(), const CvtColorForDisplayOptions options=CvtColorForDisplayOptions())
ROSCPP_DECL void shutdown()
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void imageCb(const sensor_msgs::ImageConstPtr &msg)


image_view
Author(s): Patrick Mihelich
autogenerated on Thu Nov 7 2019 03:45:05