image_nodelet.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 <ros/ros.h>
00035 #include <nodelet/nodelet.h>
00036 #include <image_transport/image_transport.h>
00037 
00038 #include <cv_bridge/cv_bridge.h>
00039 #include <opencv2/highgui/highgui.hpp>
00040 #include "window_thread.h"
00041 
00042 #include <boost/thread.hpp>
00043 #include <boost/format.hpp>
00044 
00045 #ifdef HAVE_GTK
00046 #include <gtk/gtk.h>
00047 
00048 // Platform-specific workaround for #3026: image_view doesn't close when
00049 // closing image window. On platforms using GTK+ we connect this to the
00050 // window's "destroy" event so that image_view exits.
00051 static void destroyNode(GtkWidget *widget, gpointer data)
00052 {
00054   //ros::shutdown();
00055   exit(0); // brute force solution
00056 }
00057 
00058 static void destroyNodelet(GtkWidget *widget, gpointer data)
00059 {
00060   // We can't actually unload the nodelet from here, but we can at least
00061   // unsubscribe from the image topic.
00062   reinterpret_cast<image_transport::Subscriber*>(data)->shutdown();
00063 }
00064 #endif
00065 
00066 
00067 namespace image_view {
00068 
00069 class ImageNodelet : public nodelet::Nodelet
00070 {
00071   image_transport::Subscriber sub_;
00072 
00073   boost::mutex image_mutex_;
00074   sensor_msgs::ImageConstPtr last_msg_;
00075   cv::Mat last_image_;
00076   
00077   std::string window_name_;
00078   boost::format filename_format_;
00079   int count_;
00080 
00081   virtual void onInit();
00082   
00083   void imageCb(const sensor_msgs::ImageConstPtr& msg);
00084 
00085   static void mouseCb(int event, int x, int y, int flags, void* param);
00086 
00087 public:
00088   ImageNodelet();
00089 
00090   ~ImageNodelet();
00091 };
00092 
00093 ImageNodelet::ImageNodelet()
00094   : filename_format_(""), count_(0)
00095 {
00096 }
00097 
00098 ImageNodelet::~ImageNodelet()
00099 {
00100   cv::destroyWindow(window_name_);
00101 }
00102 
00103 void ImageNodelet::onInit()
00104 {
00105   ros::NodeHandle nh = getNodeHandle();
00106   ros::NodeHandle local_nh = getPrivateNodeHandle();
00107 
00108   // Command line argument parsing
00109   const std::vector<std::string>& argv = getMyArgv();
00110   // First positional argument is the transport type
00111   std::string transport = "raw";
00112   for (int i = 0; i < (int)argv.size(); ++i)
00113   {
00114     if (argv[i][0] != '-')
00115     {
00116       transport = argv[i];
00117       break;
00118     }
00119   }
00120   // Internal option, should be used only by the image_view node
00121   bool shutdown_on_close = std::find(argv.begin(), argv.end(),
00122                                      "--shutdown-on-close") != argv.end();
00123 
00124   // Default window name is the resolved topic name
00125   std::string topic = nh.resolveName("image");
00126   local_nh.param("window_name", window_name_, topic);
00127 
00128   bool autosize;
00129   local_nh.param("autosize", autosize, false);
00130   
00131   std::string format_string;
00132   local_nh.param("filename_format", format_string, std::string("frame%04i.jpg"));
00133   filename_format_.parse(format_string);
00134 
00135   cv::namedWindow(window_name_, autosize ? CV_WINDOW_AUTOSIZE : 0);
00136   cv::setMouseCallback(window_name_, &ImageNodelet::mouseCb, this);
00137   
00138 #ifdef HAVE_GTK
00139   // Register appropriate handler for when user closes the display window
00140   GtkWidget *widget = GTK_WIDGET( cvGetWindowHandle(window_name_.c_str()) );
00141   if (shutdown_on_close)
00142     g_signal_connect(widget, "destroy", G_CALLBACK(destroyNode), NULL);
00143   else
00144     g_signal_connect(widget, "destroy", G_CALLBACK(destroyNodelet), &sub_);
00145 #endif
00146 
00147   // Start the OpenCV window thread so we don't have to waitKey() somewhere
00148   startWindowThread();
00149 
00150   image_transport::ImageTransport it(nh);
00151   image_transport::TransportHints hints(transport, ros::TransportHints(), getPrivateNodeHandle());
00152   sub_ = it.subscribe(topic, 1, &ImageNodelet::imageCb, this, hints);
00153 }
00154 
00155 void ImageNodelet::imageCb(const sensor_msgs::ImageConstPtr& msg)
00156 {
00157   image_mutex_.lock();
00158 
00159   // May want to view raw bayer data, which CvBridge doesn't know about
00160   if (msg->encoding.find("bayer") != std::string::npos)
00161   {
00162     last_image_ = cv::Mat(msg->height, msg->width, CV_8UC1,
00163                           const_cast<uint8_t*>(&msg->data[0]), msg->step);
00164   }
00165   // We want to scale floating point images so that they display nicely
00166   else if(msg->encoding.find("F") != std::string::npos)
00167   {
00168     cv::Mat float_image_bridge = cv_bridge::toCvShare(msg, msg->encoding)->image;
00169     cv::Mat_<float> float_image = float_image_bridge;
00170     float max_val = 0;
00171     for(int i = 0; i < float_image.rows; ++i)
00172     {
00173       for(int j = 0; j < float_image.cols; ++j)
00174       {
00175         max_val = std::max(max_val, float_image(i, j));
00176       }
00177     }
00178 
00179     if(max_val > 0)
00180     {
00181       float_image /= max_val;
00182     }
00183     last_image_ = float_image;
00184   }
00185   else
00186   {
00187     // Convert to OpenCV native BGR color
00188     try {
00189       last_image_ = cv_bridge::toCvShare(msg, "bgr8")->image;
00190     }
00191     catch (cv_bridge::Exception& e) {
00192       NODELET_ERROR_THROTTLE(30, "Unable to convert '%s' image to bgr8: '%s'",
00193                              msg->encoding.c_str(), e.what());
00194     }
00195   }
00196 
00197   // last_image_ may point to data owned by last_msg_, so we hang onto it for
00198   // the sake of mouseCb.
00199   last_msg_ = msg;
00200 
00201   // Must release the mutex before calling cv::imshow, or can deadlock against
00202   // OpenCV's window mutex.
00203   image_mutex_.unlock();
00204   if (!last_image_.empty())
00205     cv::imshow(window_name_, last_image_);
00206 }
00207 
00208 void ImageNodelet::mouseCb(int event, int x, int y, int flags, void* param)
00209 {
00210   ImageNodelet *this_ = reinterpret_cast<ImageNodelet*>(param);
00211   // Trick to use NODELET_* logging macros in static function
00212   boost::function<const std::string&()> getName =
00213     boost::bind(&ImageNodelet::getName, this_);
00214   
00215   if (event == CV_EVENT_LBUTTONDOWN)
00216   {
00217     NODELET_WARN_ONCE("Left-clicking no longer saves images. Right-click instead.");
00218     return;
00219   }
00220   if (event != CV_EVENT_RBUTTONDOWN)
00221     return;
00222   
00223   boost::lock_guard<boost::mutex> guard(this_->image_mutex_);
00224 
00225   const cv::Mat image = this_->last_image_;
00226   if (image.empty())
00227   {
00228     NODELET_WARN("Couldn't save image, no data!");
00229     return;
00230   }
00231 
00232   std::string filename = (this_->filename_format_ % this_->count_).str();
00233   if (cv::imwrite(filename, image))
00234   {
00235     NODELET_INFO("Saved image %s", filename.c_str());
00236     this_->count_++;
00237   }
00238   else
00239   {
00241     NODELET_ERROR("Failed to save image.");
00242   }
00243 }
00244 
00245 } // namespace image_view
00246 
00247 // Register the nodelet
00248 #include <pluginlib/class_list_macros.h>
00249 PLUGINLIB_EXPORT_CLASS( image_view::ImageNodelet, nodelet::Nodelet)


image_view
Author(s): Patrick Mihelich
autogenerated on Mon Oct 6 2014 00:45:31