point_cloud_common.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, 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 <QColor>
31 
32 #include <OgreSceneManager.h>
33 #include <OgreSceneNode.h>
34 #include <OgreWireBoundingBox.h>
35 
36 #include <ros/time.h>
37 
39 
42 #include <rviz/display.h>
43 #include <rviz/display_context.h>
44 #include <rviz/frame_manager.h>
51 #include <rviz/validate_floats.h>
52 
54 
55 namespace rviz
56 {
58 {
59  IndexAndMessage(int _index, const void* _message) : index(_index), message((uint64_t)_message)
60  {
61  }
62 
63  int index;
64  uint64_t message;
65 };
66 
68 {
69  return ((uint)iam.index) + ((uint)(iam.message >> 32)) + ((uint)(iam.message & 0xffffffff));
70 }
71 
73 {
74  return a.index == b.index && a.message == b.message;
75 }
76 
78  PointCloudCommon::CloudInfo* cloud_info,
79  DisplayContext* context)
80  : SelectionHandler(context), cloud_info_(cloud_info), box_size_(box_size)
81 {
82 }
83 
85 {
86  // delete all the Property objects on our way out.
87  QHash<IndexAndMessage, Property*>::const_iterator iter;
88  for (iter = property_hash_.begin(); iter != property_hash_.end(); iter++)
89  {
90  delete iter.value();
91  }
92 }
93 
95 {
97 
98  switch (pass)
99  {
100  case 0:
102  break;
103  case 1:
104  cloud_info_->cloud_->setColorByIndex(true);
105  break;
106  default:
107  break;
108  }
109 }
110 
112 {
114 
115  if (pass == 1)
116  {
117  cloud_info_->cloud_->setColorByIndex(false);
118  }
119 }
120 
121 Ogre::Vector3 pointFromCloud(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t index)
122 {
123  int32_t xi = findChannelIndex(cloud, "x");
124  int32_t yi = findChannelIndex(cloud, "y");
125  int32_t zi = findChannelIndex(cloud, "z");
126 
127  const uint32_t xoff = cloud->fields[xi].offset;
128  const uint32_t yoff = cloud->fields[yi].offset;
129  const uint32_t zoff = cloud->fields[zi].offset;
130  const uint8_t type = cloud->fields[xi].datatype;
131  const uint32_t point_step = cloud->point_step;
132  float x = valueFromCloud<float>(cloud, xoff, type, point_step, index);
133  float y = valueFromCloud<float>(cloud, yoff, type, point_step, index);
134  float z = valueFromCloud<float>(cloud, zoff, type, point_step, index);
135  return Ogre::Vector3(x, y, z);
136 }
137 
139 {
140  typedef std::set<int> S_int;
141  S_int indices;
142  {
143  S_uint64::const_iterator it = obj.extra_handles.begin();
144  S_uint64::const_iterator end = obj.extra_handles.end();
145  for (; it != end; ++it)
146  {
147  uint64_t handle = *it;
148  indices.insert((handle & 0xffffffff) - 1);
149  }
150  }
151 
152  {
153  S_int::iterator it = indices.begin();
154  S_int::iterator end = indices.end();
155  for (; it != end; ++it)
156  {
157  int index = *it;
158  const sensor_msgs::PointCloud2ConstPtr& message = cloud_info_->message_;
159 
160  IndexAndMessage hash_key(index, message.get());
161  if (!property_hash_.contains(hash_key))
162  {
163  Property* cat =
164  new Property(QString("Point %1 [cloud 0x%2]").arg(index).arg((uint64_t)message.get()),
165  QVariant(), "", parent_property);
166  property_hash_.insert(hash_key, cat);
167 
168  // First add the position.
169  VectorProperty* pos_prop =
170  new VectorProperty("Position", cloud_info_->transformed_points_[index].position, "", cat);
171  pos_prop->setReadOnly(true);
172 
173  // Then add all other fields as well.
174  for (size_t field = 0; field < message->fields.size(); ++field)
175  {
176  const sensor_msgs::PointField& f = message->fields[field];
177  const std::string& name = f.name;
178 
179  if (name == "x" || name == "y" || name == "z" || name == "X" || name == "Y" || name == "Z")
180  {
181  continue;
182  }
183  if (name == "rgb" || name == "rgba")
184  {
185  float float_val =
186  valueFromCloud<float>(message, f.offset, f.datatype, message->point_step, index);
187  // Convertion hack because rgb are stored int float (datatype=7) and valueFromCloud can't
188  // cast float to uint32_t
189  uint32_t val = *((uint32_t*)&float_val);
190  ColorProperty* prop =
191  new ColorProperty(QString("%1: %2").arg(field).arg(QString::fromStdString(name)),
192  QColor((val >> 16) & 0xff, (val >> 8) & 0xff, val & 0xff), "", cat);
193  prop->setReadOnly(true);
194 
195  FloatProperty* aprop = new FloatProperty(QString("alpha"), ((val >> 24) / 255.0), "", cat);
196  aprop->setReadOnly(true);
197  }
198  else
199  {
200  float val = valueFromCloud<float>(message, f.offset, f.datatype, message->point_step, index);
201  FloatProperty* prop = new FloatProperty(
202  QString("%1: %2").arg(field).arg(QString::fromStdString(name)), val, "", cat);
203  prop->setReadOnly(true);
204  }
205  }
206  }
207  }
208  }
209 }
210 
211 void PointCloudSelectionHandler::destroyProperties(const Picked& obj, Property* /*parent_property*/)
212 {
213  typedef std::set<int> S_int;
214  S_int indices;
215  {
216  S_uint64::const_iterator it = obj.extra_handles.begin();
217  S_uint64::const_iterator end = obj.extra_handles.end();
218  for (; it != end; ++it)
219  {
220  uint64_t handle = *it;
221  indices.insert((handle & 0xffffffff) - 1);
222  }
223  }
224 
225  {
226  S_int::iterator it = indices.begin();
227  S_int::iterator end = indices.end();
228  for (; it != end; ++it)
229  {
230  int index = *it;
231  const sensor_msgs::PointCloud2ConstPtr& message = cloud_info_->message_;
232 
233  IndexAndMessage hash_key(index, message.get());
234 
235  Property* prop = property_hash_.take(hash_key);
236  delete prop;
237  }
238  }
239 }
240 
242 {
243  S_uint64::iterator it = obj.extra_handles.begin();
244  S_uint64::iterator end = obj.extra_handles.end();
245  for (; it != end; ++it)
246  {
247  M_HandleToBox::iterator find_it = boxes_.find(std::make_pair(obj.handle, *it - 1));
248  if (find_it != boxes_.end())
249  {
250  Ogre::WireBoundingBox* box = find_it->second.second;
251 
252  aabbs.push_back(box->getWorldBoundingBox());
253  }
254  }
255 }
256 
258 {
259  S_uint64::iterator it = obj.extra_handles.begin();
260  S_uint64::iterator end = obj.extra_handles.end();
261  for (; it != end; ++it)
262  {
263  int index = (*it & 0xffffffff) - 1;
264 
265  sensor_msgs::PointCloud2ConstPtr message = cloud_info_->message_;
266 
267  Ogre::Vector3 pos = cloud_info_->transformed_points_[index].position;
268  pos = cloud_info_->scene_node_->convertLocalToWorldPosition(pos);
269 
270  float size = box_size_ * 0.5f;
271 
272  Ogre::AxisAlignedBox aabb(pos - size, pos + size);
273 
274  createBox(std::make_pair(obj.handle, index), aabb, "RVIZ/Cyan");
275  }
276 }
277 
279 {
280  S_uint64::iterator it = obj.extra_handles.begin();
281  S_uint64::iterator end = obj.extra_handles.end();
282  for (; it != end; ++it)
283  {
284  int global_index = (*it & 0xffffffff) - 1;
285 
286  destroyBox(std::make_pair(obj.handle, global_index));
287  }
288 }
289 
290 PointCloudCommon::CloudInfo::CloudInfo() : manager_(nullptr), scene_node_(nullptr)
291 {
292 }
293 
295 {
296  clear();
297 }
298 
300 {
301  if (scene_node_)
302  {
303  manager_->destroySceneNode(scene_node_);
304  scene_node_ = nullptr;
305  }
306 }
307 
309  : auto_size_(false)
310  , new_xyz_transformer_(false)
311  , new_color_transformer_(false)
312  , needs_retransform_(false)
313  , transformer_class_loader_(nullptr)
314  , display_(display)
315 {
317  new BoolProperty("Selectable", true,
318  "Whether or not the points in this point cloud are selectable.", display_,
320 
321  style_property_ = new EnumProperty("Style", "Flat Squares",
322  "Rendering mode to use, in order of computational complexity.",
329 
330  point_world_size_property_ = new FloatProperty("Size (m)", 0.01, "Point size in meters.", display_,
333 
334  point_pixel_size_property_ = new FloatProperty("Size (Pixels)", 3, "Point size in pixels.", display_,
337 
338  alpha_property_ = new FloatProperty("Alpha", 1.0,
339  "Amount of transparency to apply to the points. "
340  "Note that this is experimental and does not always look correct.",
344 
346  "Decay Time", 0,
347  "Duration, in seconds, to keep the incoming points. 0 means only show the latest points.",
350 
352  new EnumProperty("Position Transformer", "",
353  "Set the transformer to use to set the position of the points.", display_,
357 
359  new EnumProperty("Color Transformer", "",
360  "Set the transformer to use to set the color of the points.", display_,
364 }
365 
366 void PointCloudCommon::initialize(DisplayContext* context, Ogre::SceneNode* scene_node)
367 {
369  new pluginlib::ClassLoader<PointCloudTransformer>("rviz", "rviz::PointCloudTransformer");
371 
372  context_ = context;
373  scene_node_ = scene_node;
374 
375  updateStyle();
377  updateAlpha();
379 }
380 
382 {
383  // Ensure any threads holding the mutexes have finished
384  boost::recursive_mutex::scoped_lock lock1(transformers_mutex_);
385  boost::mutex::scoped_lock lock2(new_clouds_mutex_);
387 }
388 
390 {
391  std::vector<std::string> classes = transformer_class_loader_->getDeclaredClasses();
392  std::vector<std::string>::iterator ci;
393 
394  for (ci = classes.begin(); ci != classes.end(); ci++)
395  {
396  const std::string& lookup_name = *ci;
397  std::string name = transformer_class_loader_->getName(lookup_name);
398 
399  if (transformers_.count(name) > 0)
400  {
401  ROS_ERROR("Transformer type [%s] is already loaded.", name.c_str());
402  continue;
403  }
404 
405  PointCloudTransformerPtr trans(transformer_class_loader_->createUnmanagedInstance(lookup_name));
406  trans->init();
407  connect(trans.get(), &PointCloudTransformer::needRetransform, this,
409 
410  TransformerInfo info;
411  info.transformer = trans;
412  info.readable_name = name;
413  info.lookup_name = lookup_name;
414 
415  info.transformer->createProperties(display_, PointCloudTransformer::Support_XYZ, info.xyz_props);
416  setPropertiesHidden(info.xyz_props, true);
417 
419  setPropertiesHidden(info.color_props, true);
420 
421  transformers_[name] = info;
422  }
423 }
424 
425 void PointCloudCommon::setAutoSize(bool auto_size)
426 {
427  auto_size_ = auto_size;
428  for (unsigned i = 0; i < cloud_infos_.size(); i++)
429  {
430  cloud_infos_[i]->cloud_->setAutoSize(auto_size);
431  }
432 }
433 
434 
436 {
437  for (unsigned i = 0; i < cloud_infos_.size(); i++)
438  {
439  bool per_point_alpha = findChannelIndex(cloud_infos_[i]->message_, "rgba") != -1;
440  cloud_infos_[i]->cloud_->setAlpha(alpha_property_->getFloat(), per_point_alpha);
441  }
442 }
443 
445 {
446  bool selectable = selectable_property_->getBool();
447 
448  if (selectable)
449  {
450  for (unsigned i = 0; i < cloud_infos_.size(); i++)
451  {
452  cloud_infos_[i]->selection_handler_.reset(
454  cloud_infos_[i]->cloud_->setPickColor(
455  SelectionManager::handleToColor(cloud_infos_[i]->selection_handler_->getHandle()));
456  }
457  }
458  else
459  {
460  for (unsigned i = 0; i < cloud_infos_.size(); i++)
461  {
462  cloud_infos_[i]->selection_handler_.reset();
463  cloud_infos_[i]->cloud_->setPickColor(Ogre::ColourValue(0.0f, 0.0f, 0.0f, 0.0f));
464  }
465  }
466 }
467 
469 {
471  if (mode == PointCloud::RM_POINTS)
472  {
475  }
476  else
477  {
480  }
481  for (unsigned int i = 0; i < cloud_infos_.size(); i++)
482  {
483  cloud_infos_[i]->cloud_->setRenderMode(mode);
484  }
486 }
487 
489 {
491  float size;
492  if (mode == PointCloud::RM_POINTS)
493  {
495  }
496  else
497  {
499  }
500  for (unsigned i = 0; i < cloud_infos_.size(); i++)
501  {
502  cloud_infos_[i]->cloud_->setDimensions(size, size, size);
503  cloud_infos_[i]->selection_handler_->setBoxSize(getSelectionBoxSize());
504  }
506 }
507 
509 {
510  boost::mutex::scoped_lock lock(new_clouds_mutex_);
511  cloud_infos_.clear();
512  new_cloud_infos_.clear();
513 }
514 
516 {
517  needs_retransform_ = true;
518 }
519 
520 void PointCloudCommon::update(float /*wall_dt*/, float /*ros_dt*/)
521 {
523 
524  float point_decay_time = decay_time_property_->getFloat();
525  if (needs_retransform_)
526  {
527  retransform();
528  needs_retransform_ = false;
529  }
530 
531  // instead of deleting cloud infos, we just clear them
532  // and put them into obsolete_cloud_infos, so active selections
533  // are preserved
534 
535  auto now_sec = ros::Time::now().toSec();
536 
537  // if decay time == 0, clear the old cloud when we get a new one
538  // otherwise, clear all the outdated ones
539  {
540  boost::mutex::scoped_lock lock(new_clouds_mutex_);
541  if (point_decay_time > 0.0 || !new_cloud_infos_.empty())
542  {
543  while (!cloud_infos_.empty() &&
544  now_sec - cloud_infos_.front()->receive_time_.toSec() >= point_decay_time)
545  {
546  cloud_infos_.front()->clear();
547  obsolete_cloud_infos_.push_back(cloud_infos_.front());
548  cloud_infos_.pop_front();
550  }
551  }
552  }
553 
554  // garbage-collect old point clouds that don't have an active selection
555  L_CloudInfo::iterator it = obsolete_cloud_infos_.begin();
556  L_CloudInfo::iterator end = obsolete_cloud_infos_.end();
557  for (; it != end; ++it)
558  {
559  if (!(*it)->selection_handler_.get() || !(*it)->selection_handler_->hasSelections())
560  {
561  it = obsolete_cloud_infos_.erase(it);
562  }
563  }
564 
565  {
566  boost::mutex::scoped_lock lock(new_clouds_mutex_);
567  if (!new_cloud_infos_.empty())
568  {
569  float size;
570  if (mode == PointCloud::RM_POINTS)
571  {
573  }
574  else
575  {
577  }
578 
579  V_CloudInfo::iterator it = new_cloud_infos_.begin();
580  V_CloudInfo::iterator end = new_cloud_infos_.end();
581  for (; it != end; ++it)
582  {
583  CloudInfoPtr cloud_info = *it;
584 
585  V_CloudInfo::iterator next = it;
586  next++;
587  // ignore point clouds that are too old, but keep at least one
588  if (next != end && now_sec - cloud_info->receive_time_.toSec() >= point_decay_time)
589  {
590  continue;
591  }
592 
593  bool per_point_alpha = findChannelIndex(cloud_info->message_, "rgba") != -1;
594 
595  cloud_info->cloud_.reset(new PointCloud());
596  cloud_info->cloud_->setRenderMode(mode);
597  cloud_info->cloud_->addPoints(&(cloud_info->transformed_points_.front()),
598  cloud_info->transformed_points_.size());
599  cloud_info->cloud_->setAlpha(alpha_property_->getFloat(), per_point_alpha);
600  cloud_info->cloud_->setDimensions(size, size, size);
601  cloud_info->cloud_->setAutoSize(auto_size_);
602 
603  cloud_info->manager_ = context_->getSceneManager();
604 
605  cloud_info->scene_node_ =
606  scene_node_->createChildSceneNode(cloud_info->position_, cloud_info->orientation_);
607 
608  cloud_info->scene_node_->attachObject(cloud_info->cloud_.get());
609 
610  cloud_info->selection_handler_.reset(
611  new PointCloudSelectionHandler(getSelectionBoxSize(), cloud_info.get(), context_));
612 
613  cloud_infos_.push_back(*it);
614  }
615 
616  new_cloud_infos_.clear();
617  }
618  }
619 
620  {
621  boost::recursive_mutex::scoped_try_lock lock(transformers_mutex_);
622 
623  if (lock.owns_lock())
624  {
626  {
627  M_TransformerInfo::iterator it = transformers_.begin();
628  M_TransformerInfo::iterator end = transformers_.end();
629  for (; it != end; ++it)
630  {
631  const std::string& name = it->first;
632  TransformerInfo& info = it->second;
633 
636  }
637  }
638  }
639 
640  new_xyz_transformer_ = false;
641  new_color_transformer_ = false;
642  }
643 
644  updateStatus();
645 }
646 
647 void PointCloudCommon::setPropertiesHidden(const QList<Property*>& props, bool hide)
648 {
649  for (int i = 0; i < props.size(); i++)
650  {
651  props[i]->setHidden(hide);
652  }
653 }
654 
655 void PointCloudCommon::updateTransformers(const sensor_msgs::PointCloud2ConstPtr& cloud)
656 {
657  std::string xyz_name = xyz_transformer_property_->getStdString();
658  std::string color_name = color_transformer_property_->getStdString();
659 
662 
663  // Get the channels that we could potentially render
664  typedef std::set<std::pair<uint8_t, std::string> > S_string;
665  S_string valid_xyz, valid_color;
666  bool cur_xyz_valid = false;
667  bool cur_color_valid = false;
668  bool has_rgb_transformer = false;
669  M_TransformerInfo::iterator trans_it = transformers_.begin();
670  M_TransformerInfo::iterator trans_end = transformers_.end();
671  for (; trans_it != trans_end; ++trans_it)
672  {
673  const std::string& name = trans_it->first;
674  const PointCloudTransformerPtr& trans = trans_it->second.transformer;
675  uint32_t mask = trans->supports(cloud);
677  {
678  valid_xyz.insert(std::make_pair(trans->score(cloud), name));
679  if (name == xyz_name)
680  {
681  cur_xyz_valid = true;
682  }
684  }
685 
687  {
688  valid_color.insert(std::make_pair(trans->score(cloud), name));
689  if (name == color_name)
690  {
691  cur_color_valid = true;
692  }
693  if (name == "RGB8")
694  {
695  has_rgb_transformer = true;
696  }
698  }
699  }
700 
701  if (!cur_xyz_valid)
702  {
703  if (!valid_xyz.empty())
704  {
705  xyz_transformer_property_->setStringStd(valid_xyz.rbegin()->second);
706  }
707  }
708 
709  if (!cur_color_valid)
710  {
711  if (!valid_color.empty())
712  {
713  if (has_rgb_transformer)
714  {
716  }
717  else
718  {
719  color_transformer_property_->setStringStd(valid_color.rbegin()->second);
720  }
721  }
722  }
723 }
724 
726 {
727  std::stringstream ss;
728  // ss << "Showing [" << total_point_count_ << "] points from [" << clouds_.size() << "] messages";
729  display_->setStatusStd(StatusProperty::Ok, "Points", ss.str());
730 }
731 
732 void PointCloudCommon::processMessage(const sensor_msgs::PointCloud2ConstPtr& cloud)
733 {
734  CloudInfoPtr info(new CloudInfo);
735  info->message_ = cloud;
736  info->receive_time_ = ros::Time::now();
737 
738  if (transformCloud(info, true))
739  {
740  boost::mutex::scoped_lock lock(new_clouds_mutex_);
741  new_cloud_infos_.push_back(info);
742  display_->emitTimeSignal(cloud->header.stamp);
743  }
744 }
745 
747 {
748  boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
750  {
751  return;
752  }
753  new_xyz_transformer_ = true;
755 }
756 
758 {
759  boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
761  {
762  return;
763  }
764  new_color_transformer_ = true;
766 }
767 
769 PointCloudCommon::getXYZTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud)
770 {
771  boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
772  M_TransformerInfo::iterator it = transformers_.find(xyz_transformer_property_->getStdString());
773  if (it != transformers_.end())
774  {
775  const PointCloudTransformerPtr& trans = it->second.transformer;
776  if (trans->supports(cloud) & PointCloudTransformer::Support_XYZ)
777  {
778  return trans;
779  }
780  }
781 
782  return PointCloudTransformerPtr();
783 }
784 
786 PointCloudCommon::getColorTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud)
787 {
788  boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
789  M_TransformerInfo::iterator it = transformers_.find(color_transformer_property_->getStdString());
790  if (it != transformers_.end())
791  {
792  const PointCloudTransformerPtr& trans = it->second.transformer;
793  if (trans->supports(cloud) & PointCloudTransformer::Support_Color)
794  {
795  return trans;
796  }
797  }
798 
799  return PointCloudTransformerPtr();
800 }
801 
802 
804 {
805  boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
806 
807  D_CloudInfo::iterator it = cloud_infos_.begin();
808  D_CloudInfo::iterator end = cloud_infos_.end();
809  for (; it != end; ++it)
810  {
811  const CloudInfoPtr& cloud_info = *it;
812  transformCloud(cloud_info, false);
813  cloud_info->cloud_->clear();
814  cloud_info->cloud_->addPoints(&cloud_info->transformed_points_.front(),
815  cloud_info->transformed_points_.size());
816  }
817 }
818 
819 bool PointCloudCommon::transformCloud(const CloudInfoPtr& cloud_info, bool update_transformers)
820 {
821  if (!cloud_info->scene_node_)
822  {
823  if (!context_->getFrameManager()->getTransform(cloud_info->message_->header, cloud_info->position_,
824  cloud_info->orientation_))
825  {
826  std::stringstream ss;
827  ss << "Failed to transform from frame [" << cloud_info->message_->header.frame_id << "] to frame ["
828  << context_->getFrameManager()->getFixedFrame() << "]";
829  display_->setStatusStd(StatusProperty::Error, "Message", ss.str());
830  return false;
831  }
832  }
833 
834  Ogre::Matrix4 transform;
835  transform.makeTransform(cloud_info->position_, Ogre::Vector3(1, 1, 1), cloud_info->orientation_);
836 
837  V_PointCloudPoint& cloud_points = cloud_info->transformed_points_;
838  cloud_points.clear();
839 
840  size_t size = cloud_info->message_->width * cloud_info->message_->height;
841  PointCloud::Point default_pt;
842  default_pt.color = Ogre::ColourValue(1, 1, 1);
843  default_pt.position = Ogre::Vector3::ZERO;
844  cloud_points.resize(size, default_pt);
845 
846  {
847  boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
848  if (update_transformers)
849  {
850  updateTransformers(cloud_info->message_);
851  }
852  PointCloudTransformerPtr xyz_trans = getXYZTransformer(cloud_info->message_);
853  PointCloudTransformerPtr color_trans = getColorTransformer(cloud_info->message_);
854 
855  if (!xyz_trans)
856  {
857  std::stringstream ss;
858  ss << "No position transformer available for cloud";
859  display_->setStatusStd(StatusProperty::Error, "Message", ss.str());
860  return false;
861  }
862 
863  if (!color_trans)
864  {
865  std::stringstream ss;
866  ss << "No color transformer available for cloud";
867  display_->setStatusStd(StatusProperty::Error, "Message", ss.str());
868  return false;
869  }
870 
871  xyz_trans->transform(cloud_info->message_, PointCloudTransformer::Support_XYZ, transform,
872  cloud_points);
873  color_trans->transform(cloud_info->message_, PointCloudTransformer::Support_Color, transform,
874  cloud_points);
875  }
876 
877  for (V_PointCloudPoint::iterator cloud_point = cloud_points.begin(); cloud_point != cloud_points.end();
878  ++cloud_point)
879  {
880  if (!validateFloats(cloud_point->position))
881  {
882  cloud_point->position.x = 999999.0f;
883  cloud_point->position.y = 999999.0f;
884  cloud_point->position.z = 999999.0f;
885  }
886  }
887 
888  return true;
889 }
890 
891 bool convertPointCloudToPointCloud2(const sensor_msgs::PointCloud& input,
892  sensor_msgs::PointCloud2& output)
893 {
894  output.header = input.header;
895  output.width = input.points.size();
896  output.height = 1;
897  output.fields.resize(3 + input.channels.size());
898  // Convert x/y/z to fields
899  output.fields[0].name = "x";
900  output.fields[1].name = "y";
901  output.fields[2].name = "z";
902  int offset = 0;
903  // All offsets are *4, as all field data types are float32
904  for (size_t d = 0; d < output.fields.size(); ++d, offset += 4)
905  {
906  output.fields[d].offset = offset;
907  output.fields[d].datatype = sensor_msgs::PointField::FLOAT32;
908  }
909  output.point_step = offset;
910  output.row_step = output.point_step * output.width;
911  // Convert the remaining of the channels to fields
912  for (size_t d = 0; d < input.channels.size(); ++d)
913  output.fields[3 + d].name = input.channels[d].name;
914  output.data.resize(input.points.size() * output.point_step);
915  output.is_bigendian = false; // @todo ?
916  output.is_dense = false;
917 
918  // Copy the data points
919  for (size_t cp = 0; cp < input.points.size(); ++cp)
920  {
921  memcpy(&output.data[cp * output.point_step + output.fields[0].offset], &input.points[cp].x,
922  sizeof(float));
923  memcpy(&output.data[cp * output.point_step + output.fields[1].offset], &input.points[cp].y,
924  sizeof(float));
925  memcpy(&output.data[cp * output.point_step + output.fields[2].offset], &input.points[cp].z,
926  sizeof(float));
927  for (size_t d = 0; d < input.channels.size(); ++d)
928  {
929  if (input.channels[d].values.size() == input.points.size())
930  {
931  memcpy(&output.data[cp * output.point_step + output.fields[3 + d].offset],
932  &input.channels[d].values[cp], sizeof(float));
933  }
934  }
935  }
936  return (true);
937 }
938 
939 void PointCloudCommon::addMessage(const sensor_msgs::PointCloudConstPtr& cloud)
940 {
941  sensor_msgs::PointCloud2Ptr out(new sensor_msgs::PointCloud2);
942  convertPointCloudToPointCloud2(*cloud, *out);
943  addMessage(out);
944 }
945 
946 void PointCloudCommon::addMessage(const sensor_msgs::PointCloud2ConstPtr& cloud)
947 {
948  processMessage(cloud);
949 }
950 
952 {
953  reset();
954 }
955 
957 {
959 }
960 
962 {
964 }
965 
967 {
968  prop->clearOptions();
969 
970  if (cloud_infos_.empty())
971  {
972  return;
973  }
974 
975  boost::recursive_mutex::scoped_lock tlock(transformers_mutex_);
976 
977  const sensor_msgs::PointCloud2ConstPtr& msg = cloud_infos_.front()->message_;
978 
979  M_TransformerInfo::iterator it = transformers_.begin();
980  M_TransformerInfo::iterator end = transformers_.end();
981  for (; it != end; ++it)
982  {
983  const PointCloudTransformerPtr& trans = it->second.transformer;
984  if ((trans->supports(msg) & mask) == mask)
985  {
986  prop->addOption(QString::fromStdString(it->first));
987  }
988  }
989 }
990 
992 {
994  {
996  }
997  else
998  {
999  return 0.004;
1000  }
1001 }
1002 
1003 } // namespace rviz
rviz::PointCloud
A visual representation of a set of points.
Definition: point_cloud.h:108
rviz::BoolProperty::getBool
virtual bool getBool() const
Definition: bool_property.cpp:48
rviz::EnumProperty::getOptionInt
virtual int getOptionInt()
Return the int value of the currently-chosen option, or 0 if the current option string does not have ...
Definition: enum_property.cpp:56
rviz::EnumProperty::clearOptions
virtual void clearOptions()
Clear the list of options.
Definition: enum_property.cpp:44
rviz::PointCloudSelectionHandler::getAABBs
void getAABBs(const Picked &obj, V_AABB &aabbs) override
Definition: point_cloud_common.cpp:241
rviz::PointCloudCommon::updateBillboardSize
void updateBillboardSize()
Definition: point_cloud_common.cpp:488
rviz::findChannelIndex
int32_t findChannelIndex(const sensor_msgs::PointCloud2ConstPtr &cloud, const std::string &channel)
Definition: point_cloud_transformers.h:47
rviz::PointCloudCommon::setAutoSize
void setAutoSize(bool auto_size)
Definition: point_cloud_common.cpp:425
validate_floats.h
rviz::PointCloudSelectionHandler::property_hash_
QHash< IndexAndMessage, Property * > property_hash_
Definition: point_cloud_common.h:252
frame_manager.h
rviz::DisplayContext::queueRender
virtual void queueRender()=0
Queues a render. Multiple calls before a render happens will only cause a single render.
rviz::PointCloudSelectionHandler::destroyProperties
void destroyProperties(const Picked &obj, Property *parent_property) override
Destroy all properties for the given picked object(s).
Definition: point_cloud_common.cpp:211
rviz::Display::emitTimeSignal
void emitTimeSignal(ros::Time time)
Emit a time signal that other Displays can synchronize to.
Definition: display.cpp:285
rviz::PointCloudCommon::updateSelectable
void updateSelectable()
Definition: point_cloud_common.cpp:444
rviz::PointCloudCommon::xyz_transformer_property_
EnumProperty * xyz_transformer_property_
Definition: point_cloud_common.h:141
rviz::Display::queueRender
void queueRender()
Convenience function which calls context_->queueRender().
Definition: display.cpp:99
boost::shared_ptr< PointCloudTransformer >
rviz::PointCloudCommon::initialize
void initialize(DisplayContext *context, Ogre::SceneNode *scene_node)
Definition: point_cloud_common.cpp:366
rviz::StatusProperty::Error
@ Error
Definition: status_property.h:46
rviz::Picked::handle
CollObjectHandle handle
Definition: forwards.h:59
rviz::PointCloudCommon::update
void update(float wall_dt, float ros_dt)
Definition: point_cloud_common.cpp:520
rviz::Property::show
void show()
Show this Property in any PropertyTreeWidgets.
Definition: property.h:471
rviz::PointCloudCommon::getSelectionBoxSize
float getSelectionBoxSize()
Definition: point_cloud_common.cpp:991
rviz::PointCloudCommon::TransformerInfo
Definition: point_cloud_common.h:191
rviz::PointCloudCommon::CloudInfo::message_
sensor_msgs::PointCloud2ConstPtr message_
Definition: point_cloud_common.h:100
rviz::SelectionHandler::getHandle
CollObjectHandle getHandle() const
Definition: selection_handler.h:135
time.h
point_cloud_common.h
rviz::IndexAndMessage::message
uint64_t message
Definition: point_cloud_common.cpp:64
rviz::BoolProperty
Property specialized to provide getter for booleans.
Definition: bool_property.h:38
rviz::FloatProperty::setMax
void setMax(float max)
Definition: float_property.cpp:57
rviz::PointCloudCommon::updateTransformers
void updateTransformers(const sensor_msgs::PointCloud2ConstPtr &cloud)
Definition: point_cloud_common.cpp:655
rviz::V_PointCloudPoint
std::vector< PointCloud::Point > V_PointCloudPoint
Definition: point_cloud_transformer.h:56
rviz::validateFloats
bool validateFloats(const sensor_msgs::CameraInfo &msg)
Definition: camera_display.cpp:72
rviz::PointCloudCommon::setXyzTransformerOptions
void setXyzTransformerOptions(EnumProperty *prop)
Definition: point_cloud_common.cpp:956
rviz::PointCloudSelectionHandler::preRenderPass
void preRenderPass(uint32_t pass) override
Definition: point_cloud_common.cpp:94
point_cloud.h
rviz::SelectionHandler::preRenderPass
virtual void preRenderPass(uint32_t pass)
Definition: selection_handler.cpp:75
TimeBase< Time, Duration >::toSec
double toSec() const
rviz::EnumProperty::requestOptions
void requestOptions(EnumProperty *property_in_need_of_options)
requestOptions() is emitted each time createEditor() is called.
rviz::PointCloudCommon::addMessage
void addMessage(const sensor_msgs::PointCloudConstPtr &cloud)
Definition: point_cloud_common.cpp:939
rviz::Picked::extra_handles
S_uint64 extra_handles
Definition: forwards.h:61
float_property.h
rviz::PointCloudSelectionHandler::PointCloudSelectionHandler
PointCloudSelectionHandler(float box_size, PointCloudCommon::CloudInfo *cloud_info, DisplayContext *context)
Definition: point_cloud_common.cpp:77
rviz::PointCloudCommon::selectable_property_
BoolProperty * selectable_property_
Definition: point_cloud_common.h:137
rviz::PointCloudCommon::new_clouds_mutex_
boost::mutex new_clouds_mutex_
Definition: point_cloud_common.h:187
bool_property.h
rviz::PointCloudTransformer::needRetransform
void needRetransform()
Subclasses should emit this signal whenever they think the points should be re-transformed.
rviz::PointCloudCommon::TransformerInfo::readable_name
std::string readable_name
Definition: point_cloud_common.h:197
get
def get(url)
rviz::PointCloudCommon::updateStatus
void updateStatus()
Definition: point_cloud_common.cpp:725
rviz::PointCloudCommon::point_world_size_property_
FloatProperty * point_world_size_property_
Definition: point_cloud_common.h:138
rviz::SelectionHandler::destroyBox
void destroyBox(const std::pair< CollObjectHandle, uint64_t > &handles)
Destroy the box associated with the given handle-int pair, if there is one.
Definition: selection_handler.cpp:212
rviz::PointCloud::RM_POINTS
@ RM_POINTS
Definition: point_cloud.h:113
rviz::PointCloudCommon::style_property_
EnumProperty * style_property_
Definition: point_cloud_common.h:143
rviz::PointCloud::RM_SPHERES
@ RM_SPHERES
Definition: point_cloud.h:116
rviz::FloatProperty::setMin
void setMin(float min)
Definition: float_property.cpp:51
rviz::DisplayContext::getSceneManager
virtual Ogre::SceneManager * getSceneManager() const =0
Returns the Ogre::SceneManager used for the main RenderPanel.
f
f
rviz::SelectionHandler::createBox
void createBox(const std::pair< CollObjectHandle, uint64_t > &handles, const Ogre::AxisAlignedBox &aabb, const std::string &material_name)
Create or update a box for the given handle-int pair, with the box specified by aabb.
Definition: selection_handler.cpp:181
rviz::ColorProperty
Definition: color_property.h:40
rviz::PointCloudCommon::cloud_infos_
D_CloudInfo cloud_infos_
Definition: point_cloud_common.h:182
rviz::PointCloudCommon::CloudInfo
Definition: point_cloud_common.h:88
rviz::IndexAndMessage::index
int index
Definition: point_cloud_common.cpp:63
rviz::PointCloudCommon::TransformerInfo::xyz_props
QList< Property * > xyz_props
Definition: point_cloud_common.h:194
rviz::Display
Definition: display.h:63
rviz::PointCloudCommon::getXYZTransformer
PointCloudTransformerPtr getXYZTransformer(const sensor_msgs::PointCloud2ConstPtr &cloud)
Definition: point_cloud_common.cpp:769
rviz::qHash
uint qHash(IndexAndMessage iam)
Definition: point_cloud_common.cpp:67
rviz::EnumProperty
Enum property.
Definition: enum_property.h:46
rviz::PointCloudCommon::display_
Display * display_
Definition: point_cloud_common.h:210
rviz::PointCloud::Point
Representation of a point, with x/y/z position and r/g/b color.
Definition: point_cloud.h:133
rviz::FloatProperty
Property specialized to enforce floating point max/min.
Definition: float_property.h:37
rviz::PointCloudCommon::new_color_transformer_
bool new_color_transformer_
Definition: point_cloud_common.h:205
rviz::Picked
Definition: forwards.h:53
rviz::Property
A single element of a property tree, with a name, value, description, and possibly children.
Definition: property.h:100
rviz::PointCloudCommon::CloudInfo::~CloudInfo
~CloudInfo()
Definition: point_cloud_common.cpp:294
rviz::Property::hide
void hide()
Hide this Property in any PropertyTreeWidgets.
Definition: property.h:462
rviz::operator==
bool operator==(IndexAndMessage a, IndexAndMessage b)
Definition: point_cloud_common.cpp:72
rviz::PointCloudCommon::updateXyzTransformer
void updateXyzTransformer()
Definition: point_cloud_common.cpp:746
rviz::PointCloudCommon::new_xyz_transformer_
bool new_xyz_transformer_
Definition: point_cloud_common.h:204
rviz::PointCloud::RM_SQUARES
@ RM_SQUARES
Definition: point_cloud.h:114
rviz::PointCloudCommon::context_
DisplayContext * context_
Definition: point_cloud_common.h:211
rviz::FrameManager::getFixedFrame
const std::string & getFixedFrame()
Return the current fixed frame name.
Definition: frame_manager.h:204
rviz::PointCloudCommon::needs_retransform_
bool needs_retransform_
Definition: point_cloud_common.h:206
rviz::PointCloudCommon::retransform
void retransform()
Definition: point_cloud_common.cpp:803
rviz::PointCloudSelectionHandler::createProperties
void createProperties(const Picked &obj, Property *parent_property) override
Override to create properties of the given picked object(s).
Definition: point_cloud_common.cpp:138
rviz::FloatProperty::getFloat
virtual float getFloat() const
Definition: float_property.h:79
rviz::EnumProperty::addOption
virtual void addOption(const QString &option, int value=0)
Definition: enum_property.cpp:50
rviz::PointCloud::Point::color
Ogre::ColourValue color
Definition: point_cloud.h:141
rviz::PointCloudCommon::point_pixel_size_property_
FloatProperty * point_pixel_size_property_
Definition: point_cloud_common.h:139
rviz
Definition: add_display_dialog.cpp:54
rviz::PointCloudCommon::setPropertiesHidden
void setPropertiesHidden(const QList< Property * > &props, bool hide)
Definition: point_cloud_common.cpp:647
rviz::StringProperty::getStdString
std::string getStdString()
Definition: string_property.h:71
rviz::StatusProperty::Ok
@ Ok
Definition: status_property.h:44
rviz::PointCloudCommon::processMessage
void processMessage(const sensor_msgs::PointCloud2ConstPtr &cloud)
Definition: point_cloud_common.cpp:732
rviz::PointCloudSelectionHandler::onDeselect
void onDeselect(const Picked &obj) override
Definition: point_cloud_common.cpp:278
rviz::PointCloudSelectionHandler::postRenderPass
void postRenderPass(uint32_t pass) override
Definition: point_cloud_common.cpp:111
rviz::pointFromCloud
Ogre::Vector3 pointFromCloud(const sensor_msgs::PointCloud2ConstPtr &cloud, uint32_t index)
Definition: point_cloud_common.cpp:121
rviz::PointCloudTransformer::Support_XYZ
@ Support_XYZ
Definition: point_cloud_transformer.h:76
rviz::PointCloudCommon::TransformerInfo::transformer
PointCloudTransformerPtr transformer
Definition: point_cloud_common.h:193
d
d
rviz::PointCloudCommon::transformers_mutex_
boost::recursive_mutex transformers_mutex_
Definition: point_cloud_common.h:202
rviz::PointCloudTransformerPtr
boost::shared_ptr< PointCloudTransformer > PointCloudTransformerPtr
Definition: point_cloud_common.h:69
rviz::PointCloudCommon::~PointCloudCommon
~PointCloudCommon() override
Definition: point_cloud_common.cpp:381
rviz::PointCloudCommon::PointCloudSelectionHandler
friend class PointCloudSelectionHandler
Definition: point_cloud_common.h:213
point_cloud_transformers.h
rviz::PointCloudCommon::loadTransformers
void loadTransformers()
Definition: point_cloud_common.cpp:389
pluginlib::ClassLoader
class_loader.hpp
rviz::SelectionHandler::boxes_
M_HandleToBox boxes_
Definition: selection_handler.h:154
rviz::DisplayContext
Pure-virtual base class for objects which give Display subclasses context in which to work.
Definition: display_context.h:81
rviz::PointCloudCommon::updateAlpha
void updateAlpha()
Definition: point_cloud_common.cpp:435
rviz::DisplayContext::getFrameManager
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
rviz::PointCloudCommon::PointCloudCommon
PointCloudCommon(Display *display)
Definition: point_cloud_common.cpp:308
rviz::PointCloudCommon::CloudInfo::transformed_points_
std::vector< PointCloud::Point > transformed_points_
Definition: point_cloud_common.h:106
rviz::EnumProperty::addOptionStd
void addOptionStd(const std::string &option, int value=0)
Definition: enum_property.h:84
rviz::PointCloudCommon::auto_size_
bool auto_size_
Definition: point_cloud_common.h:135
rviz::V_AABB
std::vector< Ogre::AxisAlignedBox > V_AABB
Definition: selection_handler.h:60
rviz::FrameManager::getTransform
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
Return the pose for a header, relative to the fixed frame, in Ogre classes.
Definition: frame_manager.h:125
display.h
rviz::PointCloudTransformer::Support_Color
@ Support_Color
Definition: point_cloud_transformer.h:77
rviz::PointCloud::Point::position
Ogre::Vector3 position
Definition: point_cloud.h:140
rviz::Display::setStatusStd
void setStatusStd(StatusProperty::Level level, const std::string &name, const std::string &text)
Show status level and text, using a std::string. Convenience function which converts std::string to Q...
Definition: display.h:163
rviz::PointCloudCommon::color_transformer_property_
EnumProperty * color_transformer_property_
Definition: point_cloud_common.h:142
rviz::PointCloudCommon::TransformerInfo::lookup_name
std::string lookup_name
Definition: point_cloud_common.h:198
rviz::PointCloudCommon::transformCloud
bool transformCloud(const CloudInfoPtr &cloud, bool fully_update_transformers)
Transforms the cloud into the correct frame, and sets up our renderable cloud.
Definition: point_cloud_common.cpp:819
rviz::Property::setReadOnly
virtual void setReadOnly(bool read_only)
Prevent or allow users to edit this property from a PropertyTreeWidget.
Definition: property.h:497
rviz::IndexAndMessage::IndexAndMessage
IndexAndMessage(int _index, const void *_message)
Definition: point_cloud_common.cpp:59
vector_property.h
rviz::PointCloudCommon::fillTransformerOptions
void fillTransformerOptions(EnumProperty *prop, uint32_t mask)
Definition: point_cloud_common.cpp:966
rviz::PointCloudCommon::updateColorTransformer
void updateColorTransformer()
Definition: point_cloud_common.cpp:757
uniform_string_stream.h
S_string
std::set< std::string > S_string
rviz::SelectionHandler
Definition: selection_handler.h:64
rviz::SelectionManager::handleToColor
static Ogre::ColourValue handleToColor(CollObjectHandle handle)
Definition: selection_manager.cpp:1106
rviz::VectorProperty::setReadOnly
void setReadOnly(bool read_only) override
Overridden from Property to propagate read-only-ness to children.
Definition: vector_property.cpp:143
ROS_ERROR
#define ROS_ERROR(...)
rviz::PointCloudCommon::setColorTransformerOptions
void setColorTransformerOptions(EnumProperty *prop)
Definition: point_cloud_common.cpp:961
rviz::PointCloud::RM_BOXES
@ RM_BOXES
Definition: point_cloud.h:118
rviz::PointCloudCommon::updateStyle
void updateStyle()
Definition: point_cloud_common.cpp:468
rviz::PointCloudCommon::transformers_
M_TransformerInfo transformers_
Definition: point_cloud_common.h:203
rviz::EnumProperty::setStringStd
void setStringStd(const std::string &str)
Set the value of this property to the given std::string. Does not force the value to be one of the li...
Definition: enum_property.h:102
rviz::PointCloudCommon::causeRetransform
void causeRetransform()
Definition: point_cloud_common.cpp:515
rviz::PointCloudCommon::obsolete_cloud_infos_
L_CloudInfo obsolete_cloud_infos_
Definition: point_cloud_common.h:189
rviz::PointCloudCommon::reset
void reset()
Definition: point_cloud_common.cpp:508
point_cloud_transformer.h
rviz::PointCloudCommon::CloudInfo::cloud_
boost::shared_ptr< PointCloud > cloud_
Definition: point_cloud_common.h:103
display_context.h
rviz::VectorProperty
Definition: vector_property.h:39
rviz::PointCloudCommon::alpha_property_
FloatProperty * alpha_property_
Definition: point_cloud_common.h:140
rviz::SelectionHandler::postRenderPass
virtual void postRenderPass(uint32_t pass)
Definition: selection_handler.cpp:86
rviz::PointCloudSelectionHandler::cloud_info_
PointCloudCommon::CloudInfo * cloud_info_
Definition: point_cloud_common.h:251
rviz::PointCloudCommon::getColorTransformer
PointCloudTransformerPtr getColorTransformer(const sensor_msgs::PointCloud2ConstPtr &cloud)
Definition: point_cloud_common.cpp:786
rviz::PointCloudCommon::transformer_class_loader_
pluginlib::ClassLoader< PointCloudTransformer > * transformer_class_loader_
Definition: point_cloud_common.h:208
rviz::PointCloudCommon::CloudInfo::scene_node_
Ogre::SceneNode * scene_node_
Definition: point_cloud_common.h:102
rviz::PointCloudCommon::CloudInfo::CloudInfo
CloudInfo()
Definition: point_cloud_common.cpp:290
rviz::PointCloudSelectionHandler::box_size_
float box_size_
Definition: point_cloud_common.h:253
rviz::PointCloudCommon::fixedFrameChanged
void fixedFrameChanged()
Definition: point_cloud_common.cpp:951
rviz::PointCloudSelectionHandler::onSelect
void onSelect(const Picked &obj) override
Definition: point_cloud_common.cpp:257
rviz::PointCloudCommon::CloudInfo::clear
void clear()
Definition: point_cloud_common.cpp:299
rviz::PointCloudCommon::decay_time_property_
FloatProperty * decay_time_property_
Definition: point_cloud_common.h:144
rviz::PointCloudSelectionHandler::~PointCloudSelectionHandler
~PointCloudSelectionHandler() override
Definition: point_cloud_common.cpp:84
rviz::PointCloudCommon::scene_node_
Ogre::SceneNode * scene_node_
Definition: point_cloud_common.h:184
rviz::PointCloudCommon::TransformerInfo::color_props
QList< Property * > color_props
Definition: point_cloud_common.h:195
rviz::PointCloudCommon::new_cloud_infos_
V_CloudInfo new_cloud_infos_
Definition: point_cloud_common.h:186
rviz::IndexAndMessage
Definition: point_cloud_common.cpp:57
enum_property.h
rviz::convertPointCloudToPointCloud2
bool convertPointCloudToPointCloud2(const sensor_msgs::PointCloud &input, sensor_msgs::PointCloud2 &output)
Definition: point_cloud_common.cpp:891
rviz::PointCloud::RenderMode
RenderMode
Definition: point_cloud.h:111
ros::Time::now
static Time now()
rviz::PointCloud::RM_FLAT_SQUARES
@ RM_FLAT_SQUARES
Definition: point_cloud.h:115


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust, William Woodall
autogenerated on Fri Aug 2 2024 08:43:10