pointcloud2_plugin.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2015, Southwest Research Institute® (SwRI®)
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
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 Southwest Research Institute® (SwRI®) nor the
14 // names of its contributors may be used to endorse or promote products
15 // derived from 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 <COPYRIGHT HOLDER> BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28 // *****************************************************************************
29 
30 #include <GL/glew.h>
32 
33 // C++ standard libraries
34 #include <cmath>
35 #include <cstdio>
36 #include <algorithm>
37 #include <vector>
38 #include <map>
39 
40 // Boost libraries
41 #include <boost/algorithm/string.hpp>
42 
43 // QT libraries
44 #include <QColorDialog>
45 #include <QDialog>
46 #include <QGLWidget>
47 
48 // QT Autogenerated
49 #include "ui_topic_select.h"
50 
51 // ROS libraries
52 #include <ros/master.h>
55 
57 
58 // Declare plugin
60 
62 
63 namespace mapviz_plugins
64 {
66  config_widget_(new QWidget()),
67  topic_(""),
68  alpha_(1.0),
69  max_value_(100.0),
70  min_value_(0.0),
71  point_size_(3),
72  buffer_size_(1),
73  new_topic_(true),
74  has_message_(false),
75  num_of_feats_(0),
76  need_new_list_(true),
77  need_minmax_(false)
78  {
79  ui_.setupUi(config_widget_);
80 
81  // Set background white
82  QPalette p(config_widget_->palette());
83  p.setColor(QPalette::Background, Qt::white);
84  config_widget_->setPalette(p);
85 
86  // Set status text red
87  QPalette p3(ui_.status->palette());
88  p3.setColor(QPalette::Text, Qt::red);
89  ui_.status->setPalette(p3);
90 
91  // Initialize color selector colors
92  ui_.min_color->setColor(Qt::white);
93  ui_.max_color->setColor(Qt::black);
94  // Set color transformer choices
95  ui_.color_transformer->addItem(QString("Flat Color"), QVariant(0));
96 
97  QObject::connect(ui_.selecttopic,
98  SIGNAL(clicked()),
99  this,
100  SLOT(SelectTopic()));
101  QObject::connect(ui_.buttonResetBuffer,
102  SIGNAL(clicked()),
103  this,
104  SLOT(ClearPointClouds()));
105  QObject::connect(ui_.topic,
106  SIGNAL(editingFinished()),
107  this,
108  SLOT(TopicEdited()));
109  QObject::connect(ui_.alpha,
110  SIGNAL(valueChanged(double)),
111  this,
112  SLOT(AlphaEdited(double)));
113  QObject::connect(ui_.color_transformer,
114  SIGNAL(currentIndexChanged(int)),
115  this,
116  SLOT(ColorTransformerChanged(int)));
117  QObject::connect(ui_.max_color,
118  SIGNAL(colorEdited(const QColor &)),
119  this,
120  SLOT(UpdateColors()));
121  QObject::connect(ui_.min_color,
122  SIGNAL(colorEdited(const QColor &)),
123  this,
124  SLOT(UpdateColors()));
125  QObject::connect(ui_.minValue,
126  SIGNAL(valueChanged(double)),
127  this,
128  SLOT(MinValueChanged(double)));
129  QObject::connect(ui_.maxValue,
130  SIGNAL(valueChanged(double)),
131  this,
132  SLOT(MaxValueChanged(double)));
133  QObject::connect(ui_.bufferSize,
134  SIGNAL(valueChanged(int)),
135  this,
136  SLOT(BufferSizeChanged(int)));
137  QObject::connect(ui_.pointSize,
138  SIGNAL(valueChanged(int)),
139  this,
140  SLOT(PointSizeChanged(int)));
141  QObject::connect(ui_.use_rainbow,
142  SIGNAL(stateChanged(int)),
143  this,
144  SLOT(UseRainbowChanged(int)));
145  QObject::connect(ui_.unpack_rgb,
146  SIGNAL(stateChanged(int)),
147  this,
148  SLOT(UseRainbowChanged(int)));
149  QObject::connect(ui_.use_automaxmin,
150  SIGNAL(stateChanged(int)),
151  this,
152  SLOT(UseAutomaxminChanged(int)));
153  QObject::connect(ui_.max_color,
154  SIGNAL(colorEdited(const QColor &)),
155  this,
156  SLOT(DrawIcon()));
157  QObject::connect(ui_.min_color,
158  SIGNAL(colorEdited( const QColor &)),
159  this,
160  SLOT(DrawIcon()));
161  QObject::connect(this,
162  SIGNAL(TargetFrameChanged(const std::string&)),
163  this,
165  QObject::connect(this,
166  SIGNAL(VisibleChanged(bool)),
167  this,
168  SLOT(SetSubscription(bool)));
169 
170  PrintInfo("Constructed PointCloud2Plugin");
171  }
172 
174  {
175  }
176 
178  {
179  ROS_DEBUG("PointCloud2Plugin::ClearHistory()");
180  scans_.clear();
181  }
182 
184  {
185  if (icon_)
186  {
187  QPixmap icon(16, 16);
188  icon.fill(Qt::transparent);
189 
190  QPainter painter(&icon);
191  painter.setRenderHint(QPainter::Antialiasing, true);
192 
193  QPen pen;
194  pen.setWidth(4);
195  pen.setCapStyle(Qt::RoundCap);
196 
197  pen.setColor(ui_.min_color->color());
198  painter.setPen(pen);
199  painter.drawPoint(2, 13);
200 
201  pen.setColor(ui_.min_color->color());
202  painter.setPen(pen);
203  painter.drawPoint(4, 6);
204 
205  pen.setColor(ui_.max_color->color());
206  painter.setPen(pen);
207  painter.drawPoint(12, 9);
208 
209  pen.setColor(ui_.max_color->color());
210  painter.setPen(pen);
211  painter.drawPoint(13, 2);
212 
213  icon_->SetPixmap(icon);
214  }
215  }
216 
218  {
219  QMutexLocker locker(&scan_mutex_);
220  for (Scan& scan: scans_)
221  {
222  scan.transformed = false;
223  scan.gl_color.clear();
224  scan.gl_point.clear();
225  }
226  }
227 
229  {
230  QMutexLocker locker(&scan_mutex_);
231  scans_.clear();
232  }
233 
235  {
236  pc2_sub_.shutdown();
237 
238  if (subscribe && !topic_.empty())
239  {
241  new_topic_ = true;
242  need_new_list_ = true;
243  max_.clear();
244  min_.clear();
245  }
246  }
247 
249  {
250  float val;
251  unsigned int color_transformer = static_cast<unsigned int>(ui_.color_transformer->currentIndex());
252  unsigned int transformer_index = color_transformer -1;
253  if (num_of_feats_ > 0 && color_transformer > 0)
254  {
255  val = point.features[transformer_index];
256  if (need_minmax_)
257  {
258  if (val > max_[transformer_index])
259  {
260  max_[transformer_index] = val;
261  }
262 
263  if (val < min_[transformer_index])
264  {
265  min_[transformer_index] = val;
266  }
267  }
268  }
269  else // No intensity or (color_transformer == COLOR_FLAT)
270  {
271  return ui_.min_color->color();
272  }
273 
274  if (ui_.unpack_rgb->isChecked())
275  {
276  uint8_t* pixelColor = reinterpret_cast<uint8_t*>(&val);
277  return QColor(pixelColor[2], pixelColor[1], pixelColor[0], 255);
278  }
279 
280  if (max_value_ > min_value_)
281  {
282  val = (val - min_value_) / (max_value_ - min_value_);
283  }
284  val = std::max(0.0f, std::min(val, 1.0f));
285 
286  if (ui_.use_automaxmin->isChecked())
287  {
288  max_value_ = max_[transformer_index];
289  min_value_ = min_[transformer_index];
290  }
291 
292  if (ui_.use_rainbow->isChecked())
293  { // Hue Interpolation
294 
295  int hue = (int)(val * 255.0);
296  return QColor::fromHsl(hue, 255, 127, 255);
297  }
298  else
299  {
300  const QColor min_color = ui_.min_color->color();
301  const QColor max_color = ui_.max_color->color();
302  // RGB Interpolation
303  int red, green, blue;
304  red = (int)(val * max_color.red() + ((1.0 - val) * min_color.red()));
305  green = (int)(val * max_color.green() + ((1.0 - val) * min_color.green()));
306  blue = (int)(val * max_color.blue() + ((1.0 - val) * min_color.blue()));
307  return QColor(red, green, blue, 255);
308  }
309  }
310 
311  inline int32_t findChannelIndex(const sensor_msgs::PointCloud2ConstPtr& cloud, const std::string& channel)
312  {
313  for (int32_t i = 0; static_cast<size_t>(i) < cloud->fields.size(); ++i)
314  {
315  if (cloud->fields[i].name == channel)
316  {
317  return i;
318  }
319  }
320 
321  return -1;
322  }
323 
325  {
326  {
327  QMutexLocker locker(&scan_mutex_);
328  for (Scan& scan: scans_)
329  {
330  scan.gl_color.clear();
331  scan.gl_color.reserve(scan.points.size()*4);
332  for (const StampedPoint& point: scan.points)
333  {
334  const QColor color = CalculateColor(point);
335  scan.gl_color.push_back( color.red());
336  scan.gl_color.push_back( color.green());
337  scan.gl_color.push_back( color.blue());
338  scan.gl_color.push_back( static_cast<uint8_t>(alpha_ * 255.0 ) );
339  }
340  }
341  }
342  canvas_->update();
343  }
344 
346  {
348  "sensor_msgs/PointCloud2");
349 
350  if (!topic.name.empty())
351  {
352  ui_.topic->setText(QString::fromStdString(topic.name));
353  TopicEdited();
354  }
355  }
356 
357 
359  {
360  std::string topic = ui_.topic->text().trimmed().toStdString();
361  if (topic != topic_)
362  {
363  initialized_ = false;
364  {
365  QMutexLocker locker(&scan_mutex_);
366  scans_.clear();
367  }
368  has_message_ = false;
369  PrintWarning("No messages received.");
370 
371  topic_ = topic;
372  SetSubscription(this->Visible());
373  }
374  }
375 
377  {
378  min_value_ = value;
379  UpdateColors();
380  }
381 
383  {
384  max_value_ = value;
385  UpdateColors();
386  }
387 
389  {
390  buffer_size_ = (size_t)value;
391 
392  if (buffer_size_ > 0)
393  {
394  QMutexLocker locker(&scan_mutex_);
395  while (scans_.size() > buffer_size_)
396  {
397  scans_.pop_front();
398  }
399  }
400 
401  canvas_->update();
402  }
403 
405  {
406  point_size_ = (size_t)value;
407 
408  canvas_->update();
409  }
410 
411  void PointCloud2Plugin::PointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& msg)
412  {
413  if (!has_message_)
414  {
415  initialized_ = true;
416  has_message_ = true;
417  }
418 
419  // Note that unlike some plugins, this one does not store nor rely on the
420  // source_frame_ member variable. This one can potentially store many
421  // messages with different source frames, so we need to store and transform
422  // them individually.
423 
424  Scan scan;
425  {
426  // recycle already allocated memory, reusing an old scan
427  QMutexLocker locker(&scan_mutex_);
428  if (buffer_size_ > 0 )
429  {
430  if( scans_.size() >= buffer_size_)
431  {
432  scan = std::move( scans_.front() );
433  }
434  else{
435  glGenBuffers(1, &scan.color_vbo);
436  glGenBuffers(1, &scan.point_vbo);
437  }
438  while (scans_.size() >= buffer_size_)
439  {
440  scans_.pop_front();
441  }
442  }
443  }
444 
445  scan.stamp = msg->header.stamp;
446  scan.color = QColor::fromRgbF(1.0f, 0.0f, 0.0f, 1.0f);
447  scan.source_frame = msg->header.frame_id;
448  scan.transformed = true;
449 
451  if (!GetTransform(scan.source_frame, msg->header.stamp, transform))
452  {
453  scan.transformed = false;
454  PrintError("No transform between " + scan.source_frame + " and " + target_frame_);
455  }
456 
457  int32_t xi = findChannelIndex(msg, "x");
458  int32_t yi = findChannelIndex(msg, "y");
459  int32_t zi = findChannelIndex(msg, "z");
460 
461  if (xi == -1 || yi == -1 || zi == -1)
462  {
463  return;
464  }
465 
466  if (new_topic_)
467  {
468  for (size_t i = 0; i < msg->fields.size(); ++i)
469  {
470  FieldInfo input;
471  std::string name = msg->fields[i].name;
472 
473  uint32_t offset_value = msg->fields[i].offset;
474  uint8_t datatype_value = msg->fields[i].datatype;
475  input.offset = offset_value;
476  input.datatype = datatype_value;
477  scan.new_features.insert(std::pair<std::string, FieldInfo>(name, input));
478  }
479 
480  new_topic_ = false;
481  num_of_feats_ = scan.new_features.size();
482 
483  max_.resize(num_of_feats_);
484  min_.resize(num_of_feats_);
485 
486  int label = 1;
487  if (need_new_list_)
488  {
489  int new_feature_index = ui_.color_transformer->currentIndex();
490  std::map<std::string, FieldInfo>::const_iterator it;
491  for (it = scan.new_features.begin(); it != scan.new_features.end(); ++it)
492  {
493  ui_.color_transformer->removeItem(static_cast<int>(num_of_feats_));
494  num_of_feats_--;
495  }
496 
497  for (it = scan.new_features.begin(); it != scan.new_features.end(); ++it)
498  {
499  std::string const field = it->first;
500  if (field == saved_color_transformer_)
501  {
502  // The very first time we see a new set of features, that means the
503  // plugin was just created; if we have a saved value, set the current
504  // index to that and clear the saved value.
505  new_feature_index = label;
507  }
508 
509  ui_.color_transformer->addItem(QString::fromStdString(field), QVariant(label));
510  num_of_feats_++;
511  label++;
512 
513  }
514  ui_.color_transformer->setCurrentIndex(new_feature_index);
515  need_new_list_ = false;
516  }
517  }
518 
519  if (!msg->data.empty())
520  {
521  const uint8_t* ptr = &msg->data.front();
522  const uint32_t point_step = msg->point_step;
523  const uint32_t xoff = msg->fields[xi].offset;
524  const uint32_t yoff = msg->fields[yi].offset;
525  const uint32_t zoff = msg->fields[zi].offset;
526  const size_t num_points = msg->data.size() / point_step;
527  const size_t num_features = scan.new_features.size();
528  scan.points.resize(num_points);
529 
530  std::vector<FieldInfo> field_infos;
531  field_infos.reserve(num_features);
532  for (auto it = scan.new_features.begin(); it != scan.new_features.end(); ++it)
533  {
534  field_infos.push_back(it->second);
535  }
536 
537  scan.gl_point.clear();
538  scan.gl_point.reserve(num_points*2);
539  scan.gl_color.clear();
540  scan.gl_color.reserve(num_points*4);
541 
542  for (size_t i = 0; i < num_points; i++, ptr += point_step)
543  {
544  float x = *reinterpret_cast<const float*>(ptr + xoff);
545  float y = *reinterpret_cast<const float*>(ptr + yoff);
546  float z = *reinterpret_cast<const float*>(ptr + zoff);
547 
548  StampedPoint& point = scan.points[i];
549  point.point = tf::Point(x, y, z);
550 
551  point.features.resize(num_features);
552 
553  for (int count=0; count < field_infos.size(); count++)
554  {
555  point.features[count] = PointFeature(ptr, field_infos[count]);
556  }
557  if (scan.transformed)
558  {
559  const tf::Point transformed_point = transform * point.point;
560  scan.gl_point.push_back( transformed_point.getX() );
561  scan.gl_point.push_back( transformed_point.getY() );
562  }
563  const QColor color = CalculateColor(point);
564  scan.gl_color.push_back( color.red());
565  scan.gl_color.push_back( color.green());
566  scan.gl_color.push_back( color.blue());
567  scan.gl_color.push_back( static_cast<uint8_t>(alpha_ * 255.0 ) );
568  }
569  }
570 
571  {
572  QMutexLocker locker(&scan_mutex_);
573  scans_.push_back( std::move(scan) );
574  }
575  new_topic_ = true;
576  canvas_->update();
577  }
578 
579  float PointCloud2Plugin::PointFeature(const uint8_t* data, const FieldInfo& feature_info)
580  {
581  switch (feature_info.datatype)
582  {
583  case 1:
584  return *reinterpret_cast<const int8_t*>(data + feature_info.offset);
585  case 2:
586  return *(data + feature_info.offset);
587  case 3:
588  return *reinterpret_cast<const int16_t*>(data + feature_info.offset);
589  case 4:
590  return *reinterpret_cast<const uint16_t*>(data + feature_info.offset);
591  case 5:
592  return *reinterpret_cast<const int32_t*>(data + feature_info.offset);
593  case 6:
594  return *reinterpret_cast<const uint32_t*>(data + feature_info.offset);
595  case 7:
596  return *reinterpret_cast<const float*>(data + feature_info.offset);
597  case 8:
598  return *reinterpret_cast<const double*>(data + feature_info.offset);
599  default:
600  ROS_WARN("Unknown data type in point: %d", feature_info.datatype);
601  return 0.0;
602  }
603  }
604 
605  void PointCloud2Plugin::PrintError(const std::string& message)
606  {
607  PrintErrorHelper(ui_.status, message);
608  }
609 
610  void PointCloud2Plugin::PrintInfo(const std::string& message)
611  {
612  PrintInfoHelper(ui_.status, message);
613  }
614 
615  void PointCloud2Plugin::PrintWarning(const std::string& message)
616  {
617  PrintWarningHelper(ui_.status, message);
618  }
619 
620  QWidget* PointCloud2Plugin::GetConfigWidget(QWidget* parent)
621  {
622  config_widget_->setParent(parent);
623 
624  return config_widget_;
625  }
626 
627  bool PointCloud2Plugin::Initialize(QGLWidget* canvas)
628  {
629  canvas_ = canvas;
630 
631  DrawIcon();
632 
633  return true;
634  }
635 
636  void PointCloud2Plugin::Draw(double x, double y, double scale)
637  {
638  glPointSize(point_size_);
639 
640  glEnableClientState(GL_VERTEX_ARRAY);
641  glEnableClientState(GL_COLOR_ARRAY);
642 
643  {
644  QMutexLocker locker(&scan_mutex_);
645 
646  for (Scan& scan: scans_)
647  {
648  if (scan.transformed && !scan.gl_color.empty())
649  {
650  glBindBuffer(GL_ARRAY_BUFFER, scan.point_vbo); // coordinates
651  glBufferData(GL_ARRAY_BUFFER, scan.gl_point.size() * sizeof(float), scan.gl_point.data(), GL_STATIC_DRAW);
652  glVertexPointer( 2, GL_FLOAT, 0, 0);
653 
654  glBindBuffer(GL_ARRAY_BUFFER, scan.color_vbo); // color
655  glBufferData(GL_ARRAY_BUFFER, scan.gl_color.size() * sizeof(uint8_t), scan.gl_color.data(), GL_STATIC_DRAW);
656  glColorPointer( 4, GL_UNSIGNED_BYTE, 0, 0);
657 
658  glDrawArrays(GL_POINTS, 0, scan.gl_point.size() / 2 );
659  }
660  }
661  }
662  glDisableClientState(GL_VERTEX_ARRAY);
663  glDisableClientState(GL_COLOR_ARRAY);
664  glBindBuffer(GL_ARRAY_BUFFER, 0);
665 
666  PrintInfo("OK");
667  }
668 
670  {
672  UpdateColors();
673  }
674 
676  {
677  need_minmax_ = check_state == Qt::Checked;
678  if( !need_minmax_ )
679  {
680  min_value_ = ui_.minValue->value();
681  max_value_ = ui_.maxValue->value();
682  }
683 
685  UpdateColors();
686  }
687 
689  {
690  {
691  QMutexLocker locker(&scan_mutex_);
692 
693  bool was_using_latest_transforms = use_latest_transforms_;
694  use_latest_transforms_ = false;
695  for (Scan& scan: scans_)
696  {
697  if (!scan.transformed)
698  {
700  if (GetTransform(scan.source_frame, scan.stamp, transform))
701  {
702  scan.gl_point.clear();
703  scan.gl_point.reserve(scan.points.size()*2);
704 
705  scan.transformed = true;
706  for (StampedPoint& point: scan.points)
707  {
708  const tf::Point transformed_point = transform * point.point;
709  scan.gl_point.push_back( transformed_point.getX() );
710  scan.gl_point.push_back( transformed_point.getY() );
711  }
712  }
713  else
714  {
715  ROS_WARN("Unable to get transform.");
716  scan.transformed = false;
717  }
718  }
719  }
720  use_latest_transforms_ = was_using_latest_transforms;
721  }
722  // Z color is based on transformed color, so it is dependent on the
723  // transform
724  if (ui_.color_transformer->currentIndex() == COLOR_Z)
725  {
726  UpdateColors();
727  }
728  }
729 
730  void PointCloud2Plugin::LoadConfig(const YAML::Node& node,
731  const std::string& path)
732  {
733  if (node["topic"])
734  {
735  std::string topic;
736  node["topic"] >> topic;
737  ui_.topic->setText(boost::trim_copy(topic).c_str());
738  TopicEdited();
739  }
740 
741  if (node["size"])
742  {
743  node["size"] >> point_size_;
744  ui_.pointSize->setValue(static_cast<int>(point_size_));
745  }
746 
747  if (node["buffer_size"])
748  {
749  node["buffer_size"] >> buffer_size_;
750  ui_.bufferSize->setValue(static_cast<int>(buffer_size_));
751  }
752 
753  if (node["color_transformer"])
754  {
755  node["color_transformer"] >> saved_color_transformer_;
756  }
757 
758  if (node["min_color"])
759  {
760  std::string min_color_str;
761  node["min_color"] >> min_color_str;
762  ui_.min_color->setColor(QColor(min_color_str.c_str()));
763  }
764 
765  if (node["max_color"])
766  {
767  std::string max_color_str;
768  node["max_color"] >> max_color_str;
769  ui_.max_color->setColor(QColor(max_color_str.c_str()));
770  }
771 
772  if (node["value_min"])
773  {
774  node["value_min"] >> min_value_;
775  ui_.minValue->setValue(min_value_);
776  }
777 
778  if (node["value_max"])
779  {
780  node["value_max"] >> max_value_;
781  ui_.maxValue->setValue(max_value_);
782  }
783 
784  if (node["alpha"])
785  {
786  node["alpha"] >> alpha_;
787  ui_.alpha->setValue(alpha_);
788  }
789 
790  if (node["use_rainbow"])
791  {
792  bool use_rainbow;
793  node["use_rainbow"] >> use_rainbow;
794  ui_.use_rainbow->setChecked(use_rainbow);
795  }
796 
797  if (node["unpack_rgb"])
798  {
799  bool unpack_rgb;
800  node["unpack_rgb"] >> unpack_rgb;
801  ui_.unpack_rgb->setChecked(unpack_rgb);
802  }
803 
804  // UseRainbowChanged must be called *before* ColorTransformerChanged
805  UseRainbowChanged(ui_.use_rainbow->checkState());
806 
807  if (node["use_automaxmin"])
808  {
809  bool use_automaxmin;
810  node["use_automaxmin"] >> use_automaxmin;
811  ui_.use_automaxmin->setChecked(use_automaxmin);
812  }
813  // UseRainbowChanged must be called *before* ColorTransformerChanged
814  UseAutomaxminChanged(ui_.use_automaxmin->checkState());
815  // ColorTransformerChanged will also update colors of all points
816  ColorTransformerChanged(ui_.color_transformer->currentIndex());
817  }
818 
820  {
821  ROS_DEBUG("Color transformer changed to %d", index);
823  UpdateColors();
824  }
825 
827  {
828  bool color_is_flat = ui_.color_transformer->currentIndex() == COLOR_FLAT;
829 
830  if (color_is_flat)
831  {
832  ui_.maxColorLabel->hide();
833  ui_.max_color->hide();
834  ui_.minColorLabel->hide();
835  ui_.min_max_color_widget->show();
836  ui_.min_max_value_widget->hide();
837  ui_.use_automaxmin->hide();
838  ui_.use_rainbow->hide();
839  }
840  else
841  {
842  ui_.maxColorLabel->show();
843  ui_.max_color->show();
844  ui_.minColorLabel->show();
845  ui_.min_max_color_widget->setVisible(!ui_.use_rainbow->isChecked());
846  ui_.min_max_value_widget->setVisible(!ui_.use_automaxmin->isChecked());
847  ui_.use_automaxmin->show();
848  ui_.use_rainbow->show();
849  }
850 
851  config_widget_->updateGeometry();
852  config_widget_->adjustSize();
853 
854  Q_EMIT SizeChanged();
855  }
856 
861  {
862  alpha_ = std::max(0.0f, std::min((float)value, 1.0f));
863  }
864 
865  void PointCloud2Plugin::SaveConfig(YAML::Emitter& emitter,
866  const std::string& path)
867  {
868  emitter << YAML::Key << "topic" <<
869  YAML::Value << boost::trim_copy(ui_.topic->text().toStdString());
870  emitter << YAML::Key << "size" <<
871  YAML::Value << ui_.pointSize->value();
872  emitter << YAML::Key << "buffer_size" <<
873  YAML::Value << ui_.bufferSize->value();
874  emitter << YAML::Key << "alpha" <<
875  YAML::Value << alpha_;
876  emitter << YAML::Key << "color_transformer" <<
877  YAML::Value << ui_.color_transformer->currentText().toStdString();
878  emitter << YAML::Key << "min_color" <<
879  YAML::Value << ui_.min_color->color().name().toStdString();
880  emitter << YAML::Key << "max_color" <<
881  YAML::Value << ui_.max_color->color().name().toStdString();
882  emitter << YAML::Key << "value_min" <<
883  YAML::Value << ui_.minValue->value();
884  emitter << YAML::Key << "value_max" <<
885  YAML::Value << ui_.maxValue->value();
886  emitter << YAML::Key << "use_rainbow" <<
887  YAML::Value << ui_.use_rainbow->isChecked();
888  emitter << YAML::Key << "use_automaxmin" <<
889  YAML::Value << ui_.use_automaxmin->isChecked();
890  emitter << YAML::Key << "unpack_rgb" <<
891  YAML::Value << ui_.unpack_rgb->isChecked();
892  }
893 }
894 
895 
bool GetTransform(const ros::Time &stamp, swri_transform_util::Transform &transform, bool use_latest_transforms=true)
string name
static ros::master::TopicInfo selectTopic(const std::string &datatype, QWidget *parent=0)
IconWidget * icon_
ros::NodeHandle node_
f
QColor CalculateColor(const StampedPoint &point)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void Draw(double x, double y, double scale)
int32_t findChannelIndex(const sensor_msgs::PointCloud2ConstPtr &cloud, const std::string &channel)
void PointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr &scan)
static void PrintWarningHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
static void PrintErrorHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
std::string target_frame_
tf::Vector3 Point
#define ROS_WARN(...)
static void PrintInfoHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
void SetPixmap(QPixmap pixmap)
void PrintError(const std::string &message)
bool Visible() const
void SaveConfig(YAML::Emitter &emitter, const std::string &path)
void PrintInfo(const std::string &message)
void LoadConfig(const YAML::Node &node, const std::string &path)
void TargetFrameChanged(const std::string &target_frame)
QWidget * GetConfigWidget(QWidget *parent)
void VisibleChanged(bool visible)
void transform(marti_nav_msgs::Path &path, const swri_transform_util::Transform &transform, const std::string &target_frame)
std::map< std::string, FieldInfo > new_features
float PointFeature(const uint8_t *, const FieldInfo &)
void UseAutomaxminChanged(int check_state)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void PrintWarning(const std::string &message)
#define ROS_DEBUG(...)


mapviz_plugins
Author(s): Marc Alban
autogenerated on Fri Dec 16 2022 03:59:33