image_view: disparity_nodelet.cpp Source File

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