disparity_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>
36 #include <sensor_msgs/image_encodings.h>
37 #include <stereo_msgs/DisparityImage.h>
38 #include <opencv2/highgui/highgui.hpp>
39 #include "window_thread.h"
40 
41 #ifdef HAVE_GTK
42 #include <gtk/gtk.h>
43 
44 // Platform-specific workaround for #3026: image_view doesn't close when
45 // closing image window. On platforms using GTK+ we connect this to the
46 // window's "destroy" event so that image_view exits.
47 static void destroyNode(GtkWidget *widget, gpointer data)
48 {
49  exit(0);
50 }
51 
52 static void destroyNodelet(GtkWidget *widget, gpointer data)
53 {
54  // We can't actually unload the nodelet from here, but we can at least
55  // unsubscribe from the image topic.
56  reinterpret_cast<ros::Subscriber*>(data)->shutdown();
57 }
58 #endif
59 
60 
61 namespace image_view {
62 
63 class DisparityNodelet : public nodelet::Nodelet
64 {
65  // colormap for disparities, RGB order
66  static unsigned char colormap[];
67 
68  std::string window_name_;
69  ros::Subscriber sub_;
70  cv::Mat_<cv::Vec3b> disparity_color_;
71  bool initialized;
72 
73  virtual void onInit();
74 
75  void imageCb(const stereo_msgs::DisparityImageConstPtr& msg);
76 
77 public:
78  ~DisparityNodelet();
79 };
80 
81 DisparityNodelet::~DisparityNodelet()
82 {
83  cv::destroyWindow(window_name_);
84 }
85 
86 void DisparityNodelet::onInit()
87 {
88  initialized = false;
89  ros::NodeHandle nh = getNodeHandle();
90  ros::NodeHandle local_nh = getPrivateNodeHandle();
91  const std::vector<std::string>& argv = getMyArgv();
92 
93  // Internal option, should be used only by image_view nodes
94  bool shutdown_on_close = std::find(argv.begin(), argv.end(),
95  "--shutdown-on-close") != argv.end();
96 
97  // Default window name is the resolved topic name
98  std::string topic = nh.resolveName("image");
99  local_nh.param("window_name", window_name_, topic);
100 
101  bool autosize;
102  local_nh.param("autosize", autosize, false);
103 
104  //cv::namedWindow(window_name_, autosize ? cv::WND_PROP_AUTOSIZE : 0);
105 #if CV_MAJOR_VERSION ==2
106 #ifdef HAVE_GTK
107  // Register appropriate handler for when user closes the display window
108  GtkWidget *widget = GTK_WIDGET( cvGetWindowHandle(window_name_.c_str()) );
109  if (shutdown_on_close)
110  g_signal_connect(widget, "destroy", G_CALLBACK(destroyNode), NULL);
111  else
112  g_signal_connect(widget, "destroy", G_CALLBACK(destroyNodelet), &sub_);
113 #endif
114  // Start the OpenCV window thread so we don't have to waitKey() somewhere
115  startWindowThread();
116 #endif
117 
118  sub_ = nh.subscribe<stereo_msgs::DisparityImage>(topic, 1, &DisparityNodelet::imageCb, this);
119 }
120 
121 void DisparityNodelet::imageCb(const stereo_msgs::DisparityImageConstPtr& msg)
122 {
123  // Check for common errors in input
124  if (msg->min_disparity == 0.0 && msg->max_disparity == 0.0)
125  {
126  NODELET_ERROR_THROTTLE(30, "Disparity image fields min_disparity and "
127  "max_disparity are not set");
128  return;
129  }
130  if (msg->image.encoding != sensor_msgs::image_encodings::TYPE_32FC1)
131  {
132  NODELET_ERROR_THROTTLE(30, "Disparity image must be 32-bit floating point "
133  "(encoding '32FC1'), but has encoding '%s'",
134  msg->image.encoding.c_str());
135  return;
136  }
137 
138  if(!initialized) {
139  cv::namedWindow(window_name_, false ? cv::WND_PROP_AUTOSIZE : 0);
140  initialized = true;
141  }
142  // Colormap and display the disparity image
143  float min_disparity = msg->min_disparity;
144  float max_disparity = msg->max_disparity;
145  float multiplier = 255.0f / (max_disparity - min_disparity);
146 
147  const cv::Mat_<float> dmat(msg->image.height, msg->image.width,
148  (float*)&msg->image.data[0], msg->image.step);
149  disparity_color_.create(msg->image.height, msg->image.width);
150 
151  for (int row = 0; row < disparity_color_.rows; ++row) {
152  const float* d = dmat[row];
153  cv::Vec3b *disparity_color = disparity_color_[row],
154  *disparity_color_end = disparity_color + disparity_color_.cols;
155  for (; disparity_color < disparity_color_end; ++disparity_color, ++d) {
156  int index = (*d - min_disparity) * multiplier + 0.5;
157  index = std::min(255, std::max(0, index));
158  // Fill as BGR
159  (*disparity_color)[2] = colormap[3*index + 0];
160  (*disparity_color)[1] = colormap[3*index + 1];
161  (*disparity_color)[0] = colormap[3*index + 2];
162  }
163  }
164 
166 #if 0
167  sensor_msgs::RegionOfInterest valid = msg->valid_window;
168  cv::Point tl(valid.x_offset, valid.y_offset), br(valid.x_offset + valid.width, valid.y_offset + valid.height);
169  cv::rectangle(disparity_color_, tl, br, CV_RGB(255,0,0), 1);
170 #endif
171 
172  cv::imshow(window_name_, disparity_color_);
173  cv::waitKey(10);
174 }
175 
176 unsigned char DisparityNodelet::colormap[768] =
177  { 150, 150, 150,
178  107, 0, 12,
179  106, 0, 18,
180  105, 0, 24,
181  103, 0, 30,
182  102, 0, 36,
183  101, 0, 42,
184  99, 0, 48,
185  98, 0, 54,
186  97, 0, 60,
187  96, 0, 66,
188  94, 0, 72,
189  93, 0, 78,
190  92, 0, 84,
191  91, 0, 90,
192  89, 0, 96,
193  88, 0, 102,
194  87, 0, 108,
195  85, 0, 114,
196  84, 0, 120,
197  83, 0, 126,
198  82, 0, 131,
199  80, 0, 137,
200  79, 0, 143,
201  78, 0, 149,
202  77, 0, 155,
203  75, 0, 161,
204  74, 0, 167,
205  73, 0, 173,
206  71, 0, 179,
207  70, 0, 185,
208  69, 0, 191,
209  68, 0, 197,
210  66, 0, 203,
211  65, 0, 209,
212  64, 0, 215,
213  62, 0, 221,
214  61, 0, 227,
215  60, 0, 233,
216  59, 0, 239,
217  57, 0, 245,
218  56, 0, 251,
219  55, 0, 255,
220  54, 0, 255,
221  52, 0, 255,
222  51, 0, 255,
223  50, 0, 255,
224  48, 0, 255,
225  47, 0, 255,
226  46, 0, 255,
227  45, 0, 255,
228  43, 0, 255,
229  42, 0, 255,
230  41, 0, 255,
231  40, 0, 255,
232  38, 0, 255,
233  37, 0, 255,
234  36, 0, 255,
235  34, 0, 255,
236  33, 0, 255,
237  32, 0, 255,
238  31, 0, 255,
239  29, 0, 255,
240  28, 0, 255,
241  27, 0, 255,
242  26, 0, 255,
243  24, 0, 255,
244  23, 0, 255,
245  22, 0, 255,
246  20, 0, 255,
247  19, 0, 255,
248  18, 0, 255,
249  17, 0, 255,
250  15, 0, 255,
251  14, 0, 255,
252  13, 0, 255,
253  11, 0, 255,
254  10, 0, 255,
255  9, 0, 255,
256  8, 0, 255,
257  6, 0, 255,
258  5, 0, 255,
259  4, 0, 255,
260  3, 0, 255,
261  1, 0, 255,
262  0, 4, 255,
263  0, 10, 255,
264  0, 16, 255,
265  0, 22, 255,
266  0, 28, 255,
267  0, 34, 255,
268  0, 40, 255,
269  0, 46, 255,
270  0, 52, 255,
271  0, 58, 255,
272  0, 64, 255,
273  0, 70, 255,
274  0, 76, 255,
275  0, 82, 255,
276  0, 88, 255,
277  0, 94, 255,
278  0, 100, 255,
279  0, 106, 255,
280  0, 112, 255,
281  0, 118, 255,
282  0, 124, 255,
283  0, 129, 255,
284  0, 135, 255,
285  0, 141, 255,
286  0, 147, 255,
287  0, 153, 255,
288  0, 159, 255,
289  0, 165, 255,
290  0, 171, 255,
291  0, 177, 255,
292  0, 183, 255,
293  0, 189, 255,
294  0, 195, 255,
295  0, 201, 255,
296  0, 207, 255,
297  0, 213, 255,
298  0, 219, 255,
299  0, 225, 255,
300  0, 231, 255,
301  0, 237, 255,
302  0, 243, 255,
303  0, 249, 255,
304  0, 255, 255,
305  0, 255, 249,
306  0, 255, 243,
307  0, 255, 237,
308  0, 255, 231,
309  0, 255, 225,
310  0, 255, 219,
311  0, 255, 213,
312  0, 255, 207,
313  0, 255, 201,
314  0, 255, 195,
315  0, 255, 189,
316  0, 255, 183,
317  0, 255, 177,
318  0, 255, 171,
319  0, 255, 165,
320  0, 255, 159,
321  0, 255, 153,
322  0, 255, 147,
323  0, 255, 141,
324  0, 255, 135,
325  0, 255, 129,
326  0, 255, 124,
327  0, 255, 118,
328  0, 255, 112,
329  0, 255, 106,
330  0, 255, 100,
331  0, 255, 94,
332  0, 255, 88,
333  0, 255, 82,
334  0, 255, 76,
335  0, 255, 70,
336  0, 255, 64,
337  0, 255, 58,
338  0, 255, 52,
339  0, 255, 46,
340  0, 255, 40,
341  0, 255, 34,
342  0, 255, 28,
343  0, 255, 22,
344  0, 255, 16,
345  0, 255, 10,
346  0, 255, 4,
347  2, 255, 0,
348  8, 255, 0,
349  14, 255, 0,
350  20, 255, 0,
351  26, 255, 0,
352  32, 255, 0,
353  38, 255, 0,
354  44, 255, 0,
355  50, 255, 0,
356  56, 255, 0,
357  62, 255, 0,
358  68, 255, 0,
359  74, 255, 0,
360  80, 255, 0,
361  86, 255, 0,
362  92, 255, 0,
363  98, 255, 0,
364  104, 255, 0,
365  110, 255, 0,
366  116, 255, 0,
367  122, 255, 0,
368  128, 255, 0,
369  133, 255, 0,
370  139, 255, 0,
371  145, 255, 0,
372  151, 255, 0,
373  157, 255, 0,
374  163, 255, 0,
375  169, 255, 0,
376  175, 255, 0,
377  181, 255, 0,
378  187, 255, 0,
379  193, 255, 0,
380  199, 255, 0,
381  205, 255, 0,
382  211, 255, 0,
383  217, 255, 0,
384  223, 255, 0,
385  229, 255, 0,
386  235, 255, 0,
387  241, 255, 0,
388  247, 255, 0,
389  253, 255, 0,
390  255, 251, 0,
391  255, 245, 0,
392  255, 239, 0,
393  255, 233, 0,
394  255, 227, 0,
395  255, 221, 0,
396  255, 215, 0,
397  255, 209, 0,
398  255, 203, 0,
399  255, 197, 0,
400  255, 191, 0,
401  255, 185, 0,
402  255, 179, 0,
403  255, 173, 0,
404  255, 167, 0,
405  255, 161, 0,
406  255, 155, 0,
407  255, 149, 0,
408  255, 143, 0,
409  255, 137, 0,
410  255, 131, 0,
411  255, 126, 0,
412  255, 120, 0,
413  255, 114, 0,
414  255, 108, 0,
415  255, 102, 0,
416  255, 96, 0,
417  255, 90, 0,
418  255, 84, 0,
419  255, 78, 0,
420  255, 72, 0,
421  255, 66, 0,
422  255, 60, 0,
423  255, 54, 0,
424  255, 48, 0,
425  255, 42, 0,
426  255, 36, 0,
427  255, 30, 0,
428  255, 24, 0,
429  255, 18, 0,
430  255, 12, 0,
431  255, 6, 0,
432  255, 0, 0,
433  };
434 
435 } // namespace image_view
436 
437 // Register the nodelet
438 #include <pluginlib/class_list_macros.h>
439 PLUGINLIB_EXPORT_CLASS( image_view::DisparityNodelet, nodelet::Nodelet)
d
d
image_view
Definition: disparity_nodelet.cpp:61
ros::NodeHandle
image_view::DisparityNodelet::onInit
virtual void onInit()
Definition: disparity_nodelet.cpp:86
image_view::DisparityNodelet::imageCb
void imageCb(const stereo_msgs::DisparityImageConstPtr &msg)
Definition: disparity_nodelet.cpp:121
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
nodelet::Nodelet::getNodeHandle
ros::NodeHandle & getNodeHandle() const
NODELET_ERROR_THROTTLE
#define NODELET_ERROR_THROTTLE(rate,...)
image_view::DisparityNodelet::sub_
ros::Subscriber sub_
Definition: disparity_nodelet.cpp:69
nodelet::Nodelet::getPrivateNodeHandle
ros::NodeHandle & getPrivateNodeHandle() const
image_view::DisparityNodelet::window_name_
std::string window_name_
Definition: disparity_nodelet.cpp:68
image_view::DisparityNodelet::disparity_color_
cv::Mat_< cv::Vec3b > disparity_color_
Definition: disparity_nodelet.cpp:70
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
image_view::DisparityNodelet::~DisparityNodelet
~DisparityNodelet()
Definition: disparity_nodelet.cpp:81
image_view::DisparityNodelet::colormap
static unsigned char colormap[]
Definition: disparity_nodelet.cpp:66
class_list_macros.h
image_view::DisparityNodelet::initialized
bool initialized
Definition: disparity_nodelet.cpp:71
sensor_msgs::image_encodings::TYPE_32FC1
const std::string TYPE_32FC1
image_view::DisparityNodelet
Definition: disparity_nodelet.cpp:63
ros::NodeHandle::resolveName
std::string resolveName(const std::string &name, bool remap=true) const
image_view::startWindowThread
void startWindowThread()
Definition: window_thread.cpp:46
nodelet::Nodelet::getMyArgv
const V_string & getMyArgv() const
shutdown
ROSCONSOLE_DECL void shutdown()
window_thread.h
image_encodings.h
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)

image_view
Author(s): Patrick Mihelich
autogenerated on Wed Dec 7 2022 03:25:28