point_cloud_transformers.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2010, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <OgreColourValue.h>
31 #include <OgreMatrix4.h>
32 #include <OgreVector3.h>
33 
39 #include "rviz/validate_floats.h"
40 
42 
43 namespace rviz
44 {
45 static void getRainbowColor(float value, Ogre::ColourValue& color)
46 {
47  // this is HSV color palette with hue values going only from 0.0 to 0.833333.
48 
49  value = std::min(value, 1.0f);
50  value = std::max(value, 0.0f);
51 
52  float h = value * 5.0f + 1.0f;
53  int i = floor(h);
54  float f = h - i;
55  if (!(i & 1))
56  f = 1 - f; // if i is even
57  float n = 1 - f;
58 
59  if (i <= 1)
60  color[0] = n, color[1] = 0, color[2] = 1;
61  else if (i == 2)
62  color[0] = 0, color[1] = n, color[2] = 1;
63  else if (i == 3)
64  color[0] = 0, color[1] = 1, color[2] = n;
65  else if (i == 4)
66  color[0] = n, color[1] = 1, color[2] = 0;
67  else if (i >= 5)
68  color[0] = 1, color[1] = n, color[2] = 0;
69 }
70 
71 uint8_t IntensityPCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& cloud)
72 {
73  updateChannels(cloud);
74  return Support_Color;
75 }
76 
77 uint8_t IntensityPCTransformer::score(const sensor_msgs::PointCloud2ConstPtr& /*cloud*/)
78 {
79  return 255;
80 }
81 
82 bool IntensityPCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud,
83  uint32_t mask,
84  const Ogre::Matrix4& /*transform*/,
85  V_PointCloudPoint& points_out)
86 {
87  if (!(mask & Support_Color))
88  {
89  return false;
90  }
91 
92  int32_t index = findChannelIndex(cloud, channel_name_property_->getStdString());
93 
94  if (index == -1)
95  {
96  if (channel_name_property_->getStdString() == "intensity")
97  {
98  index = findChannelIndex(cloud, "intensities");
99  if (index == -1)
100  {
101  return false;
102  }
103  }
104  else
105  {
106  return false;
107  }
108  }
109 
110  const uint32_t offset = cloud->fields[index].offset;
111  const uint8_t type = cloud->fields[index].datatype;
112  const uint32_t point_step = cloud->point_step;
113  const uint32_t num_points = cloud->width * cloud->height;
114 
115  float min_intensity = 999999.0f;
116  float max_intensity = -999999.0f;
118  {
119  for (uint32_t i = 0; i < num_points; ++i)
120  {
121  float val = valueFromCloud<float>(cloud, offset, type, point_step, i);
122  min_intensity = std::min(val, min_intensity);
123  max_intensity = std::max(val, max_intensity);
124  }
125 
126  min_intensity = std::max(-999999.0f, min_intensity);
127  max_intensity = std::min(999999.0f, max_intensity);
128  min_intensity_property_->setFloat(min_intensity);
129  max_intensity_property_->setFloat(max_intensity);
130  }
131  else
132  {
133  min_intensity = min_intensity_property_->getFloat();
134  max_intensity = max_intensity_property_->getFloat();
135  }
136  float diff_intensity = max_intensity - min_intensity;
137  if (diff_intensity == 0)
138  {
139  // If min and max are equal, set the diff to something huge so
140  // when we divide by it, we effectively get zero. That way the
141  // point cloud coloring will be predictably uniform when min and
142  // max are equal.
143  diff_intensity = 1e20;
144  }
145  Ogre::ColourValue max_color = max_color_property_->getOgreColor();
146  Ogre::ColourValue min_color = min_color_property_->getOgreColor();
147 
149  {
150  for (uint32_t i = 0; i < num_points; ++i)
151  {
152  float val = valueFromCloud<float>(cloud, offset, type, point_step, i);
153  float value = 1.0 - (val - min_intensity) / diff_intensity;
155  {
156  value = 1.0 - value;
157  }
158  getRainbowColor(value, points_out[i].color);
159  }
160  }
161  else
162  {
163  for (uint32_t i = 0; i < num_points; ++i)
164  {
165  float val = valueFromCloud<float>(cloud, offset, type, point_step, i);
166  float normalized_intensity = (val - min_intensity) / diff_intensity;
167  normalized_intensity = std::min(1.0f, std::max(0.0f, normalized_intensity));
168  points_out[i].color.r =
169  max_color.r * normalized_intensity + min_color.r * (1.0f - normalized_intensity);
170  points_out[i].color.g =
171  max_color.g * normalized_intensity + min_color.g * (1.0f - normalized_intensity);
172  points_out[i].color.b =
173  max_color.b * normalized_intensity + min_color.b * (1.0f - normalized_intensity);
174  }
175  }
176 
177  return true;
178 }
179 
181  uint32_t mask,
182  QList<Property*>& out_props)
183 {
184  if (mask & Support_Color)
185  {
187  new EditableEnumProperty("Channel Name", "intensity",
188  "Select the channel to use to compute the intensity", parent_property,
189  SIGNAL(needRetransform()), this);
190 
192  new BoolProperty("Use rainbow", true,
193  "Whether to use a rainbow of colors or interpolate between two",
194  parent_property, SLOT(updateUseRainbow()), this);
196  new BoolProperty("Invert Rainbow", false, "Whether to invert rainbow colors", parent_property,
197  SLOT(updateUseRainbow()), this);
198 
199  min_color_property_ = new ColorProperty("Min Color", Qt::black,
200  "Color to assign the points with the minimum intensity. "
201  "Actual color is interpolated between this and Max Color.",
202  parent_property, SIGNAL(needRetransform()), this);
203 
204  max_color_property_ = new ColorProperty("Max Color", Qt::white,
205  "Color to assign the points with the maximum intensity. "
206  "Actual color is interpolated between this and Min Color.",
207  parent_property, SIGNAL(needRetransform()), this);
208 
210  new BoolProperty("Autocompute Intensity Bounds", true,
211  "Whether to automatically compute the intensity min/max values.",
212  parent_property, SLOT(updateAutoComputeIntensityBounds()), this);
213 
215  "Min Intensity", 0,
216  "Minimum possible intensity value, used to interpolate from Min Color to Max Color for a point.",
217  parent_property);
218 
220  "Max Intensity", 4096,
221  "Maximum possible intensity value, used to interpolate from Min Color to Max Color for a point.",
222  parent_property);
223 
224  out_props.push_back(channel_name_property_);
225  out_props.push_back(use_rainbow_property_);
226  out_props.push_back(invert_rainbow_property_);
227  out_props.push_back(min_color_property_);
228  out_props.push_back(max_color_property_);
229  out_props.push_back(auto_compute_intensity_bounds_property_);
230  out_props.push_back(min_intensity_property_);
231  out_props.push_back(max_intensity_property_);
232 
235  }
236 }
237 
238 void IntensityPCTransformer::updateChannels(const sensor_msgs::PointCloud2ConstPtr& cloud)
239 {
240  V_string channels;
241  for (size_t i = 0; i < cloud->fields.size(); ++i)
242  {
243  channels.push_back(cloud->fields[i].name);
244  }
245  std::sort(channels.begin(), channels.end());
246 
247  if (channels != available_channels_)
248  {
250  for (V_string::const_iterator it = channels.begin(); it != channels.end(); ++it)
251  {
252  const std::string& channel = *it;
253  if (channel.empty())
254  {
255  continue;
256  }
258  }
259  available_channels_ = channels;
260  }
261 }
262 
264 {
265  bool auto_compute = auto_compute_intensity_bounds_property_->getBool();
266  min_intensity_property_->setReadOnly(auto_compute);
267  max_intensity_property_->setReadOnly(auto_compute);
268  if (auto_compute)
269  {
270  disconnect(min_intensity_property_, SIGNAL(changed()), this, SIGNAL(needRetransform()));
271  disconnect(max_intensity_property_, SIGNAL(changed()), this, SIGNAL(needRetransform()));
272  }
273  else
274  {
275  connect(min_intensity_property_, SIGNAL(changed()), this, SIGNAL(needRetransform()));
276  connect(max_intensity_property_, SIGNAL(changed()), this, SIGNAL(needRetransform()));
277  }
278  Q_EMIT needRetransform();
279 }
280 
282 {
283  bool use_rainbow = use_rainbow_property_->getBool();
284  invert_rainbow_property_->setHidden(!use_rainbow);
285  min_color_property_->setHidden(use_rainbow);
286  max_color_property_->setHidden(use_rainbow);
287  Q_EMIT needRetransform();
288 }
289 
290 uint8_t XYZPCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& cloud)
291 {
292  int32_t xi = findChannelIndex(cloud, "x");
293  int32_t yi = findChannelIndex(cloud, "y");
294  int32_t zi = findChannelIndex(cloud, "z");
295 
296  if (xi == -1 || yi == -1 || zi == -1)
297  {
298  return Support_None;
299  }
300 
301  if (cloud->fields[xi].datatype == sensor_msgs::PointField::FLOAT32)
302  {
303  return Support_XYZ;
304  }
305 
306  return Support_None;
307 }
308 
309 bool XYZPCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud,
310  uint32_t mask,
311  const Ogre::Matrix4& /*transform*/,
312  V_PointCloudPoint& points_out)
313 {
314  if (!(mask & Support_XYZ))
315  {
316  return false;
317  }
318 
319  int32_t xi = findChannelIndex(cloud, "x");
320  int32_t yi = findChannelIndex(cloud, "y");
321  int32_t zi = findChannelIndex(cloud, "z");
322 
323  const uint32_t xoff = cloud->fields[xi].offset;
324  const uint32_t yoff = cloud->fields[yi].offset;
325  const uint32_t zoff = cloud->fields[zi].offset;
326  const uint32_t point_step = cloud->point_step;
327  uint8_t const* point_x = &cloud->data.front() + xoff;
328  uint8_t const* point_y = &cloud->data.front() + yoff;
329  uint8_t const* point_z = &cloud->data.front() + zoff;
330  for (V_PointCloudPoint::iterator iter = points_out.begin(); iter != points_out.end();
331  ++iter, point_x += point_step, point_y += point_step, point_z += point_step)
332  {
333  iter->position.x = *reinterpret_cast<const float*>(point_x);
334  iter->position.y = *reinterpret_cast<const float*>(point_y);
335  iter->position.z = *reinterpret_cast<const float*>(point_z);
336  }
337 
338  return true;
339 }
340 
341 uint8_t RGB8PCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& cloud)
342 {
343  int32_t index = std::max(findChannelIndex(cloud, "rgb"), findChannelIndex(cloud, "rgba"));
344  if (index == -1)
345  {
346  return Support_None;
347  }
348 
349  if (cloud->fields[index].datatype == sensor_msgs::PointField::INT32 ||
350  cloud->fields[index].datatype == sensor_msgs::PointField::UINT32 ||
351  cloud->fields[index].datatype == sensor_msgs::PointField::FLOAT32)
352  {
353  return Support_Color;
354  }
355 
356  return Support_None;
357 }
358 
359 bool RGB8PCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud,
360  uint32_t mask,
361  const Ogre::Matrix4& /*transform*/,
362  V_PointCloudPoint& points_out)
363 {
364  if (!(mask & Support_Color))
365  {
366  return false;
367  }
368 
369  const int32_t rgb = findChannelIndex(cloud, "rgb");
370  const int32_t rgba = findChannelIndex(cloud, "rgba");
371  const int32_t index = std::max(rgb, rgba);
372 
373  const uint32_t off = cloud->fields[index].offset;
374  uint8_t const* rgb_ptr = &cloud->data.front() + off;
375  const uint32_t point_step = cloud->point_step;
376 
377  // Create a look-up table for colors
378  float rgb_lut[256];
379  for (int i = 0; i < 256; ++i)
380  {
381  rgb_lut[i] = float(i) / 255.0f;
382  }
383  if (rgb != -1) // rgb
384  {
385  for (V_PointCloudPoint::iterator iter = points_out.begin(); iter != points_out.end();
386  ++iter, rgb_ptr += point_step)
387  {
388  uint32_t rgb = *reinterpret_cast<const uint32_t*>(rgb_ptr);
389  iter->color.r = rgb_lut[(rgb >> 16) & 0xff];
390  iter->color.g = rgb_lut[(rgb >> 8) & 0xff];
391  iter->color.b = rgb_lut[rgb & 0xff];
392  iter->color.a = 1.0f;
393  }
394  }
395  else // rgba
396  {
397  for (V_PointCloudPoint::iterator iter = points_out.begin(); iter != points_out.end();
398  ++iter, rgb_ptr += point_step)
399  {
400  uint32_t rgb = *reinterpret_cast<const uint32_t*>(rgb_ptr);
401  iter->color.r = rgb_lut[(rgb >> 16) & 0xff];
402  iter->color.g = rgb_lut[(rgb >> 8) & 0xff];
403  iter->color.b = rgb_lut[rgb & 0xff];
404  iter->color.a = rgb_lut[rgb >> 24];
405  }
406  }
407 
408  return true;
409 }
410 
411 bool MONO8PCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud,
412  uint32_t mask,
413  const Ogre::Matrix4& /*transform*/,
414  V_PointCloudPoint& points_out)
415 {
416  if (!(mask & Support_Color))
417  {
418  return false;
419  }
420 
421  const int32_t rgb = findChannelIndex(cloud, "rgb");
422  const int32_t rgba = findChannelIndex(cloud, "rgba");
423  const int32_t index = std::max(rgb, rgba);
424 
425  const uint32_t off = cloud->fields[index].offset;
426  uint8_t const* rgb_ptr = &cloud->data.front() + off;
427  const uint32_t point_step = cloud->point_step;
428 
429  // Create a look-up table for colors
430  float rgb_lut[256];
431  for (int i = 0; i < 256; ++i)
432  {
433  rgb_lut[i] = float(i) / 255.0f;
434  }
435  if (rgb != -1) // rgb
436  {
437  for (V_PointCloudPoint::iterator iter = points_out.begin(); iter != points_out.end();
438  ++iter, rgb_ptr += point_step)
439  {
440  uint32_t rgb = *reinterpret_cast<const uint32_t*>(rgb_ptr);
441  float r = rgb_lut[(rgb >> 16) & 0xff];
442  float g = rgb_lut[(rgb >> 8) & 0xff];
443  float b = rgb_lut[rgb & 0xff];
444  float mono = 0.2989 * r + 0.5870 * g + 0.1140 * b;
445  iter->color.r = iter->color.g = iter->color.b = mono;
446  iter->color.a = 1.0f;
447  }
448  }
449  else // rgba
450  {
451  for (V_PointCloudPoint::iterator iter = points_out.begin(); iter != points_out.end();
452  ++iter, rgb_ptr += point_step)
453  {
454  uint32_t rgb = *reinterpret_cast<const uint32_t*>(rgb_ptr);
455  float r = rgb_lut[(rgb >> 16) & 0xff];
456  float g = rgb_lut[(rgb >> 8) & 0xff];
457  float b = rgb_lut[rgb & 0xff];
458  float mono = 0.2989 * r + 0.5870 * g + 0.1140 * b;
459  iter->color.r = iter->color.g = iter->color.b = mono;
460  iter->color.a = rgb_lut[rgb >> 24];
461  }
462  }
463 
464  return true;
465 }
466 
467 uint8_t RGBF32PCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& cloud)
468 {
469  int32_t ri = findChannelIndex(cloud, "r");
470  int32_t gi = findChannelIndex(cloud, "g");
471  int32_t bi = findChannelIndex(cloud, "b");
472  if (ri == -1 || gi == -1 || bi == -1)
473  {
474  return Support_None;
475  }
476 
477  if (cloud->fields[ri].datatype == sensor_msgs::PointField::FLOAT32)
478  {
479  return Support_Color;
480  }
481 
482  return Support_None;
483 }
484 
485 bool RGBF32PCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud,
486  uint32_t mask,
487  const Ogre::Matrix4& /*transform*/,
488  V_PointCloudPoint& points_out)
489 {
490  if (!(mask & Support_Color))
491  {
492  return false;
493  }
494 
495  int32_t ri = findChannelIndex(cloud, "r");
496  int32_t gi = findChannelIndex(cloud, "g");
497  int32_t bi = findChannelIndex(cloud, "b");
498 
499  const uint32_t roff = cloud->fields[ri].offset;
500  const uint32_t goff = cloud->fields[gi].offset;
501  const uint32_t boff = cloud->fields[bi].offset;
502  const uint32_t point_step = cloud->point_step;
503  const uint32_t num_points = cloud->width * cloud->height;
504  uint8_t const* point = &cloud->data.front();
505  for (uint32_t i = 0; i < num_points; ++i, point += point_step)
506  {
507  float r = *reinterpret_cast<const float*>(point + roff);
508  float g = *reinterpret_cast<const float*>(point + goff);
509  float b = *reinterpret_cast<const float*>(point + boff);
510  points_out[i].color = Ogre::ColourValue(r, g, b);
511  }
512 
513  return true;
514 }
515 
516 uint8_t FlatColorPCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& /*cloud*/)
517 {
518  return Support_Color;
519 }
520 
521 uint8_t FlatColorPCTransformer::score(const sensor_msgs::PointCloud2ConstPtr& /*cloud*/)
522 {
523  return 0;
524 }
525 
526 bool FlatColorPCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud,
527  uint32_t mask,
528  const Ogre::Matrix4& /*transform*/,
529  V_PointCloudPoint& points_out)
530 {
531  if (!(mask & Support_Color))
532  {
533  return false;
534  }
535 
536  Ogre::ColourValue color = color_property_->getOgreColor();
537 
538  const uint32_t num_points = cloud->width * cloud->height;
539  for (uint32_t i = 0; i < num_points; ++i)
540  {
541  points_out[i].color = color;
542  }
543 
544  return true;
545 }
546 
548  uint32_t mask,
549  QList<Property*>& out_props)
550 {
551  if (mask & Support_Color)
552  {
553  color_property_ = new ColorProperty("Color", Qt::white, "Color to assign to every point.",
554  parent_property, SIGNAL(needRetransform()), this);
555  out_props.push_back(color_property_);
556  }
557 }
558 
559 uint8_t AxisColorPCTransformer::supports(const sensor_msgs::PointCloud2ConstPtr& /*cloud*/)
560 {
561  return Support_Color;
562 }
563 
564 uint8_t AxisColorPCTransformer::score(const sensor_msgs::PointCloud2ConstPtr& /*cloud*/)
565 {
566  return 255;
567 }
568 
569 bool AxisColorPCTransformer::transform(const sensor_msgs::PointCloud2ConstPtr& cloud,
570  uint32_t mask,
571  const Ogre::Matrix4& transform,
572  V_PointCloudPoint& points_out)
573 {
574  if (!(mask & Support_Color))
575  {
576  return false;
577  }
578 
579  int32_t xi = findChannelIndex(cloud, "x");
580  int32_t yi = findChannelIndex(cloud, "y");
581  int32_t zi = findChannelIndex(cloud, "z");
582 
583  const uint32_t xoff = cloud->fields[xi].offset;
584  const uint32_t yoff = cloud->fields[yi].offset;
585  const uint32_t zoff = cloud->fields[zi].offset;
586  const uint32_t point_step = cloud->point_step;
587  const uint32_t num_points = cloud->width * cloud->height;
588  uint8_t const* point = &cloud->data.front();
589 
590  // Fill a vector of floats with values based on the chosen axis.
591  int axis = axis_property_->getOptionInt();
592  std::vector<float> values;
593  values.reserve(num_points);
594  Ogre::Vector3 pos;
595  if (use_fixed_frame_property_->getBool())
596  {
597  for (uint32_t i = 0; i < num_points; ++i, point += point_step)
598  {
599  // TODO: optimize this by only doing the multiplication needed
600  // for the desired output value, instead of doing all of them
601  // and then throwing most away.
602  pos.x = *reinterpret_cast<const float*>(point + xoff);
603  pos.y = *reinterpret_cast<const float*>(point + yoff);
604  pos.z = *reinterpret_cast<const float*>(point + zoff);
605 
606  pos = transform * pos;
607  values.push_back(pos[axis]);
608  }
609  }
610  else
611  {
612  const uint32_t offsets[3] = {xoff, yoff, zoff};
613  const uint32_t off = offsets[axis];
614  for (uint32_t i = 0; i < num_points; ++i, point += point_step)
615  {
616  values.push_back(*reinterpret_cast<const float*>(point + off));
617  }
618  }
619  float min_value_current = 9999.0f;
620  float max_value_current = -9999.0f;
621  if (auto_compute_bounds_property_->getBool())
622  {
623  for (uint32_t i = 0; i < num_points; i++)
624  {
625  float val = values[i];
626  min_value_current = std::min(min_value_current, val);
627  max_value_current = std::max(max_value_current, val);
628  }
629  min_value_property_->setFloat(min_value_current);
630  max_value_property_->setFloat(max_value_current);
631  }
632  else
633  {
634  min_value_current = min_value_property_->getFloat();
635  max_value_current = max_value_property_->getFloat();
636  }
637 
638  float range = max_value_current - min_value_current;
639  if (range == 0)
640  {
641  range = 0.001f;
642  }
643  for (uint32_t i = 0; i < num_points; ++i)
644  {
645  float value = 1.0 - (values[i] - min_value_current) / range;
646  getRainbowColor(value, points_out[i].color);
647  }
648 
649  return true;
650 }
651 
653  uint32_t mask,
654  QList<Property*>& out_props)
655 {
656  if (mask & Support_Color)
657  {
658  axis_property_ = new EnumProperty("Axis", "Z", "The axis to interpolate the color along.",
659  parent_property, SIGNAL(needRetransform()), this);
660  axis_property_->addOption("X", AXIS_X);
661  axis_property_->addOption("Y", AXIS_Y);
662  axis_property_->addOption("Z", AXIS_Z);
663 
664  auto_compute_bounds_property_ =
665  new BoolProperty("Autocompute Value Bounds", true,
666  "Whether to automatically compute the value min/max values.", parent_property,
667  SLOT(updateAutoComputeBounds()), this);
668 
669  min_value_property_ =
670  new FloatProperty("Min Value", -10,
671  "Minimum value value, used to interpolate the color of a point.",
672  auto_compute_bounds_property_);
673 
674  max_value_property_ =
675  new FloatProperty("Max Value", 10,
676  "Maximum value value, used to interpolate the color of a point.",
677  auto_compute_bounds_property_);
678 
679  use_fixed_frame_property_ = new BoolProperty(
680  "Use Fixed Frame", true,
681  "Whether to color the cloud based on its fixed frame position or its local frame position.",
682  parent_property, SIGNAL(needRetransform()), this);
683 
684  out_props.push_back(axis_property_);
685  out_props.push_back(auto_compute_bounds_property_);
686  out_props.push_back(use_fixed_frame_property_);
687 
688  updateAutoComputeBounds();
689  }
690 }
691 
693 {
694  bool auto_compute = auto_compute_bounds_property_->getBool();
695  min_value_property_->setHidden(auto_compute);
696  max_value_property_->setHidden(auto_compute);
697  if (auto_compute)
698  {
699  disconnect(min_value_property_, SIGNAL(changed()), this, SIGNAL(needRetransform()));
700  disconnect(max_value_property_, SIGNAL(changed()), this, SIGNAL(needRetransform()));
701  }
702  else
703  {
704  connect(min_value_property_, SIGNAL(changed()), this, SIGNAL(needRetransform()));
705  connect(max_value_property_, SIGNAL(changed()), this, SIGNAL(needRetransform()));
706  auto_compute_bounds_property_->expand();
707  }
708  Q_EMIT needRetransform();
709 }
710 
711 } // end namespace rviz
712 
bool transform(const sensor_msgs::PointCloud2ConstPtr &cloud, uint32_t mask, const Ogre::Matrix4 &transform, V_PointCloudPoint &points_out) override
Transforms a PointCloud2 into an rviz::PointCloud. The rviz::PointCloud is assumed to have been preal...
bool transform(const sensor_msgs::PointCloud2ConstPtr &cloud, uint32_t mask, const Ogre::Matrix4 &transform, V_PointCloudPoint &points_out) override
Transforms a PointCloud2 into an rviz::PointCloud. The rviz::PointCloud is assumed to have been preal...
bool transform(const sensor_msgs::PointCloud2ConstPtr &cloud, uint32_t mask, const Ogre::Matrix4 &transform, V_PointCloudPoint &points_out) override
Transforms a PointCloud2 into an rviz::PointCloud. The rviz::PointCloud is assumed to have been preal...
f
std::vector< double > values
uint8_t score(const sensor_msgs::PointCloud2ConstPtr &cloud) override
"Score" a message for how well supported the message is. For example, a "flat color" transformer can ...
uint8_t supports(const sensor_msgs::PointCloud2ConstPtr &cloud) override
Returns a level of support for a specific cloud. This level of support is a mask using the SupportLev...
A single element of a property tree, with a name, value, description, and possibly children...
Definition: property.h:100
EditableEnumProperty * channel_name_property_
void needRetransform()
Subclasses should emit this signal whenever they think the points should be re-transformed.
bool transform(const sensor_msgs::PointCloud2ConstPtr &cloud, uint32_t mask, const Ogre::Matrix4 &transform, V_PointCloudPoint &points_out) override
Transforms a PointCloud2 into an rviz::PointCloud. The rviz::PointCloud is assumed to have been preal...
Ogre::ColourValue getOgreColor() const
Editable Enum property.
void addOptionStd(const std::string &option)
Property specialized to enforce floating point max/min.
std::string getStdString()
bool setFloat(float new_value)
Float-typed "SLOT" version of setValue().
void createProperties(Property *parent_property, uint32_t mask, QList< Property *> &out_props) override
Create any properties necessary for this transformer. Will be called once when the transformer is loa...
void updateChannels(const sensor_msgs::PointCloud2ConstPtr &cloud)
std::vector< PointCloud::Point > V_PointCloudPoint
uint8_t supports(const sensor_msgs::PointCloud2ConstPtr &cloud) override
Returns a level of support for a specific cloud. This level of support is a mask using the SupportLev...
uint8_t score(const sensor_msgs::PointCloud2ConstPtr &cloud) override
"Score" a message for how well supported the message is. For example, a "flat color" transformer can ...
void createProperties(Property *parent_property, uint32_t mask, QList< Property *> &out_props) override
Create any properties necessary for this transformer. Will be called once when the transformer is loa...
std::vector< std::string > V_string
uint8_t score(const sensor_msgs::PointCloud2ConstPtr &cloud) override
"Score" a message for how well supported the message is. For example, a "flat color" transformer can ...
virtual void setHidden(bool hidden)
Hide or show this property in any PropertyTreeWidget viewing its parent.
Definition: property.cpp:552
bool transform(const sensor_msgs::PointCloud2ConstPtr &cloud, uint32_t mask, const Ogre::Matrix4 &transform, V_PointCloudPoint &points_out) override
Transforms a PointCloud2 into an rviz::PointCloud. The rviz::PointCloud is assumed to have been preal...
int32_t findChannelIndex(const sensor_msgs::PointCloud2ConstPtr &cloud, const std::string &channel)
Property specialized to provide getter for booleans.
Definition: bool_property.h:38
virtual float getFloat() const
BoolProperty * auto_compute_intensity_bounds_property_
uint8_t supports(const sensor_msgs::PointCloud2ConstPtr &cloud) override
Returns a level of support for a specific cloud. This level of support is a mask using the SupportLev...
void createProperties(Property *parent_property, uint32_t mask, QList< Property *> &out_props) override
Create any properties necessary for this transformer. Will be called once when the transformer is loa...
bool transform(const sensor_msgs::PointCloud2ConstPtr &cloud, uint32_t mask, const Ogre::Matrix4 &transform, V_PointCloudPoint &points_out) override
Transforms a PointCloud2 into an rviz::PointCloud. The rviz::PointCloud is assumed to have been preal...
virtual bool getBool() const
virtual void setReadOnly(bool read_only)
Prevent or allow users to edit this property from a PropertyTreeWidget.
Definition: property.h:436
r
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
static void getRainbowColor(float value, Ogre::ColourValue &color)
Enum property.
Definition: enum_property.h:46
uint8_t supports(const sensor_msgs::PointCloud2ConstPtr &cloud) override
Returns a level of support for a specific cloud. This level of support is a mask using the SupportLev...
uint8_t supports(const sensor_msgs::PointCloud2ConstPtr &cloud) override
Returns a level of support for a specific cloud. This level of support is a mask using the SupportLev...
uint8_t supports(const sensor_msgs::PointCloud2ConstPtr &cloud) override
Returns a level of support for a specific cloud. This level of support is a mask using the SupportLev...
bool transform(const sensor_msgs::PointCloud2ConstPtr &cloud, uint32_t mask, const Ogre::Matrix4 &transform, V_PointCloudPoint &points_out) override
Transforms a PointCloud2 into an rviz::PointCloud. The rviz::PointCloud is assumed to have been preal...


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Sat May 27 2023 02:06:25