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

image_view
Author(s): Patrick Mihelich
autogenerated on Wed Jan 24 2024 03:57:22