image_view: disparity_nodelet.cpp Source File

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 <sensor_msgs/image_encodings.h>
00037 #include <stereo_msgs/DisparityImage.h>
00038 #include <opencv2/highgui/highgui.hpp>
00039 #include "window_thread.h"
00040 
00041 #ifdef HAVE_GTK
00042 #include <gtk/gtk.h>
00043 
00044 // Platform-specific workaround for #3026: image_view doesn't close when
00045 // closing image window. On platforms using GTK+ we connect this to the
00046 // window's "destroy" event so that image_view exits.
00047 static void destroyNode(GtkWidget *widget, gpointer data)
00048 {
00049   exit(0);
00050 }
00051 
00052 static void destroyNodelet(GtkWidget *widget, gpointer data)
00053 {
00054   // We can't actually unload the nodelet from here, but we can at least
00055   // unsubscribe from the image topic.
00056   reinterpret_cast<ros::Subscriber*>(data)->shutdown();
00057 }
00058 #endif
00059 
00060 
00061 namespace image_view {
00062 
00063 class DisparityNodelet : public nodelet::Nodelet
00064 {
00065   // colormap for disparities, RGB order
00066   static unsigned char colormap[];
00067 
00068   std::string window_name_;
00069   ros::Subscriber sub_;
00070   cv::Mat_<cv::Vec3b> disparity_color_;
00071   bool initialized;
00072   
00073   virtual void onInit();
00074   
00075   void imageCb(const stereo_msgs::DisparityImageConstPtr& msg);
00076 
00077 public:
00078   ~DisparityNodelet();
00079 };
00080 
00081 DisparityNodelet::~DisparityNodelet()
00082 {
00083   cv::destroyWindow(window_name_);
00084 }
00085 
00086 void DisparityNodelet::onInit()
00087 {
00088   initialized = false;
00089   ros::NodeHandle nh = getNodeHandle();
00090   ros::NodeHandle local_nh = getPrivateNodeHandle();
00091   const std::vector<std::string>& argv = getMyArgv();
00092 
00093   // Internal option, should be used only by image_view nodes
00094   bool shutdown_on_close = std::find(argv.begin(), argv.end(),
00095                                      "--shutdown-on-close") != argv.end();
00096 
00097   // Default window name is the resolved topic name
00098   std::string topic = nh.resolveName("image");
00099   local_nh.param("window_name", window_name_, topic);
00100 
00101   bool autosize;
00102   local_nh.param("autosize", autosize, false);
00103 
00104   //cv::namedWindow(window_name_, autosize ? cv::WND_PROP_AUTOSIZE : 0);
00105 #if CV_MAJOR_VERSION ==2
00106 #ifdef HAVE_GTK
00107   // Register appropriate handler for when user closes the display window
00108   GtkWidget *widget = GTK_WIDGET( cvGetWindowHandle(window_name_.c_str()) );
00109   if (shutdown_on_close)
00110     g_signal_connect(widget, "destroy", G_CALLBACK(destroyNode), NULL);
00111   else
00112     g_signal_connect(widget, "destroy", G_CALLBACK(destroyNodelet), &sub_);
00113 #endif
00114   // Start the OpenCV window thread so we don't have to waitKey() somewhere
00115   startWindowThread();
00116 #endif
00117 
00118   sub_ = nh.subscribe<stereo_msgs::DisparityImage>(topic, 1, &DisparityNodelet::imageCb, this);
00119 }
00120 
00121 void DisparityNodelet::imageCb(const stereo_msgs::DisparityImageConstPtr& msg)
00122 {
00123   // Check for common errors in input
00124   if (msg->min_disparity == 0.0 && msg->max_disparity == 0.0)
00125   {
00126     NODELET_ERROR_THROTTLE(30, "Disparity image fields min_disparity and "
00127                            "max_disparity are not set");
00128     return;
00129   }
00130   if (msg->image.encoding != sensor_msgs::image_encodings::TYPE_32FC1)
00131   {
00132     NODELET_ERROR_THROTTLE(30, "Disparity image must be 32-bit floating point "
00133                            "(encoding '32FC1'), but has encoding '%s'",
00134                            msg->image.encoding.c_str());
00135     return;
00136   }
00137   
00138   if(!initialized) {
00139     cv::namedWindow(window_name_, false ? cv::WND_PROP_AUTOSIZE : 0);
00140     initialized = true;
00141   }
00142   // Colormap and display the disparity image
00143   float min_disparity = msg->min_disparity;
00144   float max_disparity = msg->max_disparity;
00145   float multiplier = 255.0f / (max_disparity - min_disparity);
00146 
00147   const cv::Mat_<float> dmat(msg->image.height, msg->image.width,
00148                              (float*)&msg->image.data[0], msg->image.step);
00149   disparity_color_.create(msg->image.height, msg->image.width);
00150     
00151   for (int row = 0; row < disparity_color_.rows; ++row) {
00152     const float* d = dmat[row];
00153     cv::Vec3b *disparity_color = disparity_color_[row],
00154               *disparity_color_end = disparity_color + disparity_color_.cols;
00155     for (; disparity_color < disparity_color_end; ++disparity_color, ++d) {
00156       int index = (*d - min_disparity) * multiplier + 0.5;
00157       index = std::min(255, std::max(0, index));
00158       // Fill as BGR
00159       (*disparity_color)[2] = colormap[3*index + 0];
00160       (*disparity_color)[1] = colormap[3*index + 1];
00161       (*disparity_color)[0] = colormap[3*index + 2];
00162     }
00163   }
00164 
00166 #if 0
00167   sensor_msgs::RegionOfInterest valid = msg->valid_window;
00168   cv::Point tl(valid.x_offset, valid.y_offset), br(valid.x_offset + valid.width, valid.y_offset + valid.height);
00169   cv::rectangle(disparity_color_, tl, br, CV_RGB(255,0,0), 1);
00170 #endif
00171 
00172   cv::imshow(window_name_, disparity_color_);
00173   cv::waitKey(10);
00174 }
00175 
00176 unsigned char DisparityNodelet::colormap[768] =
00177   { 150, 150, 150,
00178     107, 0, 12,
00179     106, 0, 18,
00180     105, 0, 24,
00181     103, 0, 30,
00182     102, 0, 36,
00183     101, 0, 42,
00184     99, 0, 48,
00185     98, 0, 54,
00186     97, 0, 60,
00187     96, 0, 66,
00188     94, 0, 72,
00189     93, 0, 78,
00190     92, 0, 84,
00191     91, 0, 90,
00192     89, 0, 96,
00193     88, 0, 102,
00194     87, 0, 108,
00195     85, 0, 114,
00196     84, 0, 120,
00197     83, 0, 126,
00198     82, 0, 131,
00199     80, 0, 137,
00200     79, 0, 143,
00201     78, 0, 149,
00202     77, 0, 155,
00203     75, 0, 161,
00204     74, 0, 167,
00205     73, 0, 173,
00206     71, 0, 179,
00207     70, 0, 185,
00208     69, 0, 191,
00209     68, 0, 197,
00210     66, 0, 203,
00211     65, 0, 209,
00212     64, 0, 215,
00213     62, 0, 221,
00214     61, 0, 227,
00215     60, 0, 233,
00216     59, 0, 239,
00217     57, 0, 245,
00218     56, 0, 251,
00219     55, 0, 255,
00220     54, 0, 255,
00221     52, 0, 255,
00222     51, 0, 255,
00223     50, 0, 255,
00224     48, 0, 255,
00225     47, 0, 255,
00226     46, 0, 255,
00227     45, 0, 255,
00228     43, 0, 255,
00229     42, 0, 255,
00230     41, 0, 255,
00231     40, 0, 255,
00232     38, 0, 255,
00233     37, 0, 255,
00234     36, 0, 255,
00235     34, 0, 255,
00236     33, 0, 255,
00237     32, 0, 255,
00238     31, 0, 255,
00239     29, 0, 255,
00240     28, 0, 255,
00241     27, 0, 255,
00242     26, 0, 255,
00243     24, 0, 255,
00244     23, 0, 255,
00245     22, 0, 255,
00246     20, 0, 255,
00247     19, 0, 255,
00248     18, 0, 255,
00249     17, 0, 255,
00250     15, 0, 255,
00251     14, 0, 255,
00252     13, 0, 255,
00253     11, 0, 255,
00254     10, 0, 255,
00255     9, 0, 255,
00256     8, 0, 255,
00257     6, 0, 255,
00258     5, 0, 255,
00259     4, 0, 255,
00260     3, 0, 255,
00261     1, 0, 255,
00262     0, 4, 255,
00263     0, 10, 255,
00264     0, 16, 255,
00265     0, 22, 255,
00266     0, 28, 255,
00267     0, 34, 255,
00268     0, 40, 255,
00269     0, 46, 255,
00270     0, 52, 255,
00271     0, 58, 255,
00272     0, 64, 255,
00273     0, 70, 255,
00274     0, 76, 255,
00275     0, 82, 255,
00276     0, 88, 255,
00277     0, 94, 255,
00278     0, 100, 255,
00279     0, 106, 255,
00280     0, 112, 255,
00281     0, 118, 255,
00282     0, 124, 255,
00283     0, 129, 255,
00284     0, 135, 255,
00285     0, 141, 255,
00286     0, 147, 255,
00287     0, 153, 255,
00288     0, 159, 255,
00289     0, 165, 255,
00290     0, 171, 255,
00291     0, 177, 255,
00292     0, 183, 255,
00293     0, 189, 255,
00294     0, 195, 255,
00295     0, 201, 255,
00296     0, 207, 255,
00297     0, 213, 255,
00298     0, 219, 255,
00299     0, 225, 255,
00300     0, 231, 255,
00301     0, 237, 255,
00302     0, 243, 255,
00303     0, 249, 255,
00304     0, 255, 255,
00305     0, 255, 249,
00306     0, 255, 243,
00307     0, 255, 237,
00308     0, 255, 231,
00309     0, 255, 225,
00310     0, 255, 219,
00311     0, 255, 213,
00312     0, 255, 207,
00313     0, 255, 201,
00314     0, 255, 195,
00315     0, 255, 189,
00316     0, 255, 183,
00317     0, 255, 177,
00318     0, 255, 171,
00319     0, 255, 165,
00320     0, 255, 159,
00321     0, 255, 153,
00322     0, 255, 147,
00323     0, 255, 141,
00324     0, 255, 135,
00325     0, 255, 129,
00326     0, 255, 124,
00327     0, 255, 118,
00328     0, 255, 112,
00329     0, 255, 106,
00330     0, 255, 100,
00331     0, 255, 94,
00332     0, 255, 88,
00333     0, 255, 82,
00334     0, 255, 76,
00335     0, 255, 70,
00336     0, 255, 64,
00337     0, 255, 58,
00338     0, 255, 52,
00339     0, 255, 46,
00340     0, 255, 40,
00341     0, 255, 34,
00342     0, 255, 28,
00343     0, 255, 22,
00344     0, 255, 16,
00345     0, 255, 10,
00346     0, 255, 4,
00347     2, 255, 0,
00348     8, 255, 0,
00349     14, 255, 0,
00350     20, 255, 0,
00351     26, 255, 0,
00352     32, 255, 0,
00353     38, 255, 0,
00354     44, 255, 0,
00355     50, 255, 0,
00356     56, 255, 0,
00357     62, 255, 0,
00358     68, 255, 0,
00359     74, 255, 0,
00360     80, 255, 0,
00361     86, 255, 0,
00362     92, 255, 0,
00363     98, 255, 0,
00364     104, 255, 0,
00365     110, 255, 0,
00366     116, 255, 0,
00367     122, 255, 0,
00368     128, 255, 0,
00369     133, 255, 0,
00370     139, 255, 0,
00371     145, 255, 0,
00372     151, 255, 0,
00373     157, 255, 0,
00374     163, 255, 0,
00375     169, 255, 0,
00376     175, 255, 0,
00377     181, 255, 0,
00378     187, 255, 0,
00379     193, 255, 0,
00380     199, 255, 0,
00381     205, 255, 0,
00382     211, 255, 0,
00383     217, 255, 0,
00384     223, 255, 0,
00385     229, 255, 0,
00386     235, 255, 0,
00387     241, 255, 0,
00388     247, 255, 0,
00389     253, 255, 0,
00390     255, 251, 0,
00391     255, 245, 0,
00392     255, 239, 0,
00393     255, 233, 0,
00394     255, 227, 0,
00395     255, 221, 0,
00396     255, 215, 0,
00397     255, 209, 0,
00398     255, 203, 0,
00399     255, 197, 0,
00400     255, 191, 0,
00401     255, 185, 0,
00402     255, 179, 0,
00403     255, 173, 0,
00404     255, 167, 0,
00405     255, 161, 0,
00406     255, 155, 0,
00407     255, 149, 0,
00408     255, 143, 0,
00409     255, 137, 0,
00410     255, 131, 0,
00411     255, 126, 0,
00412     255, 120, 0,
00413     255, 114, 0,
00414     255, 108, 0,
00415     255, 102, 0,
00416     255, 96, 0,
00417     255, 90, 0,
00418     255, 84, 0,
00419     255, 78, 0,
00420     255, 72, 0,
00421     255, 66, 0,
00422     255, 60, 0,
00423     255, 54, 0,
00424     255, 48, 0,
00425     255, 42, 0,
00426     255, 36, 0,
00427     255, 30, 0,
00428     255, 24, 0,
00429     255, 18, 0,
00430     255, 12, 0,
00431     255,  6, 0,
00432     255,  0, 0,
00433   };
00434 
00435 } // namespace image_view
00436 
00437 // Register the nodelet
00438 #include <pluginlib/class_list_macros.h>
00439 PLUGINLIB_EXPORT_CLASS( image_view::DisparityNodelet, nodelet::Nodelet)