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