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_,
319  SLOT(updateSelectable()), this);
320 
321  style_property_ = new EnumProperty("Style", "Flat Squares",
322  "Rendering mode to use, in order of computational complexity.",
323  display_, SLOT(updateStyle()), this);
329 
330  point_world_size_property_ = new FloatProperty("Size (m)", 0.01, "Point size in meters.", display_,
331  SLOT(updateBillboardSize()), this);
333 
334  point_pixel_size_property_ = new FloatProperty("Size (Pixels)", 3, "Point size in pixels.", display_,
335  SLOT(updateBillboardSize()), this);
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.",
341  display_, SLOT(updateAlpha()), this);
344 
346  "Decay Time", 0,
347  "Duration, in seconds, to keep the incoming points. 0 means only show the latest points.",
348  display_, SLOT(queueRender()));
350 
352  new EnumProperty("Position Transformer", "",
353  "Set the transformer to use to set the position of the points.", display_,
354  SLOT(updateXyzTransformer()), this);
355  connect(xyz_transformer_property_, SIGNAL(requestOptions(EnumProperty*)), this,
357 
359  new EnumProperty("Color Transformer", "",
360  "Set the transformer to use to set the color of the points.", display_,
361  SLOT(updateColorTransformer()), this);
362  connect(color_transformer_property_, SIGNAL(requestOptions(EnumProperty*)), this,
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(), SIGNAL(needRetransform()), this, SLOT(causeRetransform()));
408 
409  TransformerInfo info;
410  info.transformer = trans;
411  info.readable_name = name;
412  info.lookup_name = lookup_name;
413 
414  info.transformer->createProperties(display_, PointCloudTransformer::Support_XYZ, info.xyz_props);
415  setPropertiesHidden(info.xyz_props, true);
416 
417  info.transformer->createProperties(display_, PointCloudTransformer::Support_Color, info.color_props);
418  setPropertiesHidden(info.color_props, true);
419 
420  transformers_[name] = info;
421  }
422 }
423 
424 void PointCloudCommon::setAutoSize(bool auto_size)
425 {
426  auto_size_ = auto_size;
427  for (unsigned i = 0; i < cloud_infos_.size(); i++)
428  {
429  cloud_infos_[i]->cloud_->setAutoSize(auto_size);
430  }
431 }
432 
433 
435 {
436  for (unsigned i = 0; i < cloud_infos_.size(); i++)
437  {
438  bool per_point_alpha = findChannelIndex(cloud_infos_[i]->message_, "rgba") != -1;
439  cloud_infos_[i]->cloud_->setAlpha(alpha_property_->getFloat(), per_point_alpha);
440  }
441 }
442 
444 {
445  bool selectable = selectable_property_->getBool();
446 
447  if (selectable)
448  {
449  for (unsigned i = 0; i < cloud_infos_.size(); i++)
450  {
451  cloud_infos_[i]->selection_handler_.reset(
453  cloud_infos_[i]->cloud_->setPickColor(
454  SelectionManager::handleToColor(cloud_infos_[i]->selection_handler_->getHandle()));
455  }
456  }
457  else
458  {
459  for (unsigned i = 0; i < cloud_infos_.size(); i++)
460  {
461  cloud_infos_[i]->selection_handler_.reset();
462  cloud_infos_[i]->cloud_->setPickColor(Ogre::ColourValue(0.0f, 0.0f, 0.0f, 0.0f));
463  }
464  }
465 }
466 
468 {
470  if (mode == PointCloud::RM_POINTS)
471  {
474  }
475  else
476  {
479  }
480  for (unsigned int i = 0; i < cloud_infos_.size(); i++)
481  {
482  cloud_infos_[i]->cloud_->setRenderMode(mode);
483  }
485 }
486 
488 {
490  float size;
491  if (mode == PointCloud::RM_POINTS)
492  {
494  }
495  else
496  {
498  }
499  for (unsigned i = 0; i < cloud_infos_.size(); i++)
500  {
501  cloud_infos_[i]->cloud_->setDimensions(size, size, size);
502  cloud_infos_[i]->selection_handler_->setBoxSize(getSelectionBoxSize());
503  }
505 }
506 
508 {
509  boost::mutex::scoped_lock lock(new_clouds_mutex_);
510  cloud_infos_.clear();
511  new_cloud_infos_.clear();
512 }
513 
515 {
516  needs_retransform_ = true;
517 }
518 
519 void PointCloudCommon::update(float /*wall_dt*/, float /*ros_dt*/)
520 {
522 
523  float point_decay_time = decay_time_property_->getFloat();
524  if (needs_retransform_)
525  {
526  retransform();
527  needs_retransform_ = false;
528  }
529 
530  // instead of deleting cloud infos, we just clear them
531  // and put them into obsolete_cloud_infos, so active selections
532  // are preserved
533 
534  auto now_sec = ros::Time::now().toSec();
535 
536  // if decay time == 0, clear the old cloud when we get a new one
537  // otherwise, clear all the outdated ones
538  {
539  boost::mutex::scoped_lock lock(new_clouds_mutex_);
540  if (point_decay_time > 0.0 || !new_cloud_infos_.empty())
541  {
542  while (!cloud_infos_.empty() &&
543  now_sec - cloud_infos_.front()->receive_time_.toSec() >= point_decay_time)
544  {
545  cloud_infos_.front()->clear();
546  obsolete_cloud_infos_.push_back(cloud_infos_.front());
547  cloud_infos_.pop_front();
549  }
550  }
551  }
552 
553  // garbage-collect old point clouds that don't have an active selection
554  L_CloudInfo::iterator it = obsolete_cloud_infos_.begin();
555  L_CloudInfo::iterator end = obsolete_cloud_infos_.end();
556  for (; it != end; ++it)
557  {
558  if (!(*it)->selection_handler_.get() || !(*it)->selection_handler_->hasSelections())
559  {
560  it = obsolete_cloud_infos_.erase(it);
561  }
562  }
563 
564  {
565  boost::mutex::scoped_lock lock(new_clouds_mutex_);
566  if (!new_cloud_infos_.empty())
567  {
568  float size;
569  if (mode == PointCloud::RM_POINTS)
570  {
572  }
573  else
574  {
576  }
577 
578  V_CloudInfo::iterator it = new_cloud_infos_.begin();
579  V_CloudInfo::iterator end = new_cloud_infos_.end();
580  for (; it != end; ++it)
581  {
582  CloudInfoPtr cloud_info = *it;
583 
584  V_CloudInfo::iterator next = it;
585  next++;
586  // ignore point clouds that are too old, but keep at least one
587  if (next != end && now_sec - cloud_info->receive_time_.toSec() >= point_decay_time)
588  {
589  continue;
590  }
591 
592  bool per_point_alpha = findChannelIndex(cloud_info->message_, "rgba") != -1;
593 
594  cloud_info->cloud_.reset(new PointCloud());
595  cloud_info->cloud_->setRenderMode(mode);
596  cloud_info->cloud_->addPoints(&(cloud_info->transformed_points_.front()),
597  cloud_info->transformed_points_.size());
598  cloud_info->cloud_->setAlpha(alpha_property_->getFloat(), per_point_alpha);
599  cloud_info->cloud_->setDimensions(size, size, size);
600  cloud_info->cloud_->setAutoSize(auto_size_);
601 
602  cloud_info->manager_ = context_->getSceneManager();
603 
604  cloud_info->scene_node_ =
605  scene_node_->createChildSceneNode(cloud_info->position_, cloud_info->orientation_);
606 
607  cloud_info->scene_node_->attachObject(cloud_info->cloud_.get());
608 
609  cloud_info->selection_handler_.reset(
610  new PointCloudSelectionHandler(getSelectionBoxSize(), cloud_info.get(), context_));
611 
612  cloud_infos_.push_back(*it);
613  }
614 
615  new_cloud_infos_.clear();
616  }
617  }
618 
619  {
620  boost::recursive_mutex::scoped_try_lock lock(transformers_mutex_);
621 
622  if (lock.owns_lock())
623  {
625  {
626  M_TransformerInfo::iterator it = transformers_.begin();
627  M_TransformerInfo::iterator end = transformers_.end();
628  for (; it != end; ++it)
629  {
630  const std::string& name = it->first;
631  TransformerInfo& info = it->second;
632 
635  }
636  }
637  }
638 
639  new_xyz_transformer_ = false;
640  new_color_transformer_ = false;
641  }
642 
643  updateStatus();
644 }
645 
646 void PointCloudCommon::setPropertiesHidden(const QList<Property*>& props, bool hide)
647 {
648  for (int i = 0; i < props.size(); i++)
649  {
650  props[i]->setHidden(hide);
651  }
652 }
653 
654 void PointCloudCommon::updateTransformers(const sensor_msgs::PointCloud2ConstPtr& cloud)
655 {
656  std::string xyz_name = xyz_transformer_property_->getStdString();
657  std::string color_name = color_transformer_property_->getStdString();
658 
661 
662  // Get the channels that we could potentially render
663  typedef std::set<std::pair<uint8_t, std::string> > S_string;
664  S_string valid_xyz, valid_color;
665  bool cur_xyz_valid = false;
666  bool cur_color_valid = false;
667  bool has_rgb_transformer = false;
668  M_TransformerInfo::iterator trans_it = transformers_.begin();
669  M_TransformerInfo::iterator trans_end = transformers_.end();
670  for (; trans_it != trans_end; ++trans_it)
671  {
672  const std::string& name = trans_it->first;
673  const PointCloudTransformerPtr& trans = trans_it->second.transformer;
674  uint32_t mask = trans->supports(cloud);
676  {
677  valid_xyz.insert(std::make_pair(trans->score(cloud), name));
678  if (name == xyz_name)
679  {
680  cur_xyz_valid = true;
681  }
683  }
684 
686  {
687  valid_color.insert(std::make_pair(trans->score(cloud), name));
688  if (name == color_name)
689  {
690  cur_color_valid = true;
691  }
692  if (name == "RGB8")
693  {
694  has_rgb_transformer = true;
695  }
697  }
698  }
699 
700  if (!cur_xyz_valid)
701  {
702  if (!valid_xyz.empty())
703  {
704  xyz_transformer_property_->setStringStd(valid_xyz.rbegin()->second);
705  }
706  }
707 
708  if (!cur_color_valid)
709  {
710  if (!valid_color.empty())
711  {
712  if (has_rgb_transformer)
713  {
715  }
716  else
717  {
718  color_transformer_property_->setStringStd(valid_color.rbegin()->second);
719  }
720  }
721  }
722 }
723 
725 {
726  std::stringstream ss;
727  // ss << "Showing [" << total_point_count_ << "] points from [" << clouds_.size() << "] messages";
728  display_->setStatusStd(StatusProperty::Ok, "Points", ss.str());
729 }
730 
731 void PointCloudCommon::processMessage(const sensor_msgs::PointCloud2ConstPtr& cloud)
732 {
733  CloudInfoPtr info(new CloudInfo);
734  info->message_ = cloud;
735  info->receive_time_ = ros::Time::now();
736 
737  if (transformCloud(info, true))
738  {
739  boost::mutex::scoped_lock lock(new_clouds_mutex_);
740  new_cloud_infos_.push_back(info);
741  display_->emitTimeSignal(cloud->header.stamp);
742  }
743 }
744 
746 {
747  boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
749  {
750  return;
751  }
752  new_xyz_transformer_ = true;
754 }
755 
757 {
758  boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
760  {
761  return;
762  }
763  new_color_transformer_ = true;
765 }
766 
768 PointCloudCommon::getXYZTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud)
769 {
770  boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
771  M_TransformerInfo::iterator it = transformers_.find(xyz_transformer_property_->getStdString());
772  if (it != transformers_.end())
773  {
774  const PointCloudTransformerPtr& trans = it->second.transformer;
775  if (trans->supports(cloud) & PointCloudTransformer::Support_XYZ)
776  {
777  return trans;
778  }
779  }
780 
781  return PointCloudTransformerPtr();
782 }
783 
785 PointCloudCommon::getColorTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud)
786 {
787  boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
788  M_TransformerInfo::iterator it = transformers_.find(color_transformer_property_->getStdString());
789  if (it != transformers_.end())
790  {
791  const PointCloudTransformerPtr& trans = it->second.transformer;
792  if (trans->supports(cloud) & PointCloudTransformer::Support_Color)
793  {
794  return trans;
795  }
796  }
797 
798  return PointCloudTransformerPtr();
799 }
800 
801 
803 {
804  boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
805 
806  D_CloudInfo::iterator it = cloud_infos_.begin();
807  D_CloudInfo::iterator end = cloud_infos_.end();
808  for (; it != end; ++it)
809  {
810  const CloudInfoPtr& cloud_info = *it;
811  transformCloud(cloud_info, false);
812  cloud_info->cloud_->clear();
813  cloud_info->cloud_->addPoints(&cloud_info->transformed_points_.front(),
814  cloud_info->transformed_points_.size());
815  }
816 }
817 
818 bool PointCloudCommon::transformCloud(const CloudInfoPtr& cloud_info, bool update_transformers)
819 {
820  if (!cloud_info->scene_node_)
821  {
822  if (!context_->getFrameManager()->getTransform(cloud_info->message_->header, cloud_info->position_,
823  cloud_info->orientation_))
824  {
825  std::stringstream ss;
826  ss << "Failed to transform from frame [" << cloud_info->message_->header.frame_id << "] to frame ["
827  << context_->getFrameManager()->getFixedFrame() << "]";
828  display_->setStatusStd(StatusProperty::Error, "Message", ss.str());
829  return false;
830  }
831  }
832 
833  Ogre::Matrix4 transform;
834  transform.makeTransform(cloud_info->position_, Ogre::Vector3(1, 1, 1), cloud_info->orientation_);
835 
836  V_PointCloudPoint& cloud_points = cloud_info->transformed_points_;
837  cloud_points.clear();
838 
839  size_t size = cloud_info->message_->width * cloud_info->message_->height;
840  PointCloud::Point default_pt;
841  default_pt.color = Ogre::ColourValue(1, 1, 1);
842  default_pt.position = Ogre::Vector3::ZERO;
843  cloud_points.resize(size, default_pt);
844 
845  {
846  boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
847  if (update_transformers)
848  {
849  updateTransformers(cloud_info->message_);
850  }
851  PointCloudTransformerPtr xyz_trans = getXYZTransformer(cloud_info->message_);
852  PointCloudTransformerPtr color_trans = getColorTransformer(cloud_info->message_);
853 
854  if (!xyz_trans)
855  {
856  std::stringstream ss;
857  ss << "No position transformer available for cloud";
858  display_->setStatusStd(StatusProperty::Error, "Message", ss.str());
859  return false;
860  }
861 
862  if (!color_trans)
863  {
864  std::stringstream ss;
865  ss << "No color transformer available for cloud";
866  display_->setStatusStd(StatusProperty::Error, "Message", ss.str());
867  return false;
868  }
869 
870  xyz_trans->transform(cloud_info->message_, PointCloudTransformer::Support_XYZ, transform,
871  cloud_points);
872  color_trans->transform(cloud_info->message_, PointCloudTransformer::Support_Color, transform,
873  cloud_points);
874  }
875 
876  for (V_PointCloudPoint::iterator cloud_point = cloud_points.begin(); cloud_point != cloud_points.end();
877  ++cloud_point)
878  {
879  if (!validateFloats(cloud_point->position))
880  {
881  cloud_point->position.x = 999999.0f;
882  cloud_point->position.y = 999999.0f;
883  cloud_point->position.z = 999999.0f;
884  }
885  }
886 
887  return true;
888 }
889 
890 bool convertPointCloudToPointCloud2(const sensor_msgs::PointCloud& input,
891  sensor_msgs::PointCloud2& output)
892 {
893  output.header = input.header;
894  output.width = input.points.size();
895  output.height = 1;
896  output.fields.resize(3 + input.channels.size());
897  // Convert x/y/z to fields
898  output.fields[0].name = "x";
899  output.fields[1].name = "y";
900  output.fields[2].name = "z";
901  int offset = 0;
902  // All offsets are *4, as all field data types are float32
903  for (size_t d = 0; d < output.fields.size(); ++d, offset += 4)
904  {
905  output.fields[d].offset = offset;
906  output.fields[d].datatype = sensor_msgs::PointField::FLOAT32;
907  }
908  output.point_step = offset;
909  output.row_step = output.point_step * output.width;
910  // Convert the remaining of the channels to fields
911  for (size_t d = 0; d < input.channels.size(); ++d)
912  output.fields[3 + d].name = input.channels[d].name;
913  output.data.resize(input.points.size() * output.point_step);
914  output.is_bigendian = false; // @todo ?
915  output.is_dense = false;
916 
917  // Copy the data points
918  for (size_t cp = 0; cp < input.points.size(); ++cp)
919  {
920  memcpy(&output.data[cp * output.point_step + output.fields[0].offset], &input.points[cp].x,
921  sizeof(float));
922  memcpy(&output.data[cp * output.point_step + output.fields[1].offset], &input.points[cp].y,
923  sizeof(float));
924  memcpy(&output.data[cp * output.point_step + output.fields[2].offset], &input.points[cp].z,
925  sizeof(float));
926  for (size_t d = 0; d < input.channels.size(); ++d)
927  {
928  if (input.channels[d].values.size() == input.points.size())
929  {
930  memcpy(&output.data[cp * output.point_step + output.fields[3 + d].offset],
931  &input.channels[d].values[cp], sizeof(float));
932  }
933  }
934  }
935  return (true);
936 }
937 
938 void PointCloudCommon::addMessage(const sensor_msgs::PointCloudConstPtr& cloud)
939 {
940  sensor_msgs::PointCloud2Ptr out(new sensor_msgs::PointCloud2);
941  convertPointCloudToPointCloud2(*cloud, *out);
942  addMessage(out);
943 }
944 
945 void PointCloudCommon::addMessage(const sensor_msgs::PointCloud2ConstPtr& cloud)
946 {
947  processMessage(cloud);
948 }
949 
951 {
952  reset();
953 }
954 
956 {
958 }
959 
961 {
963 }
964 
966 {
967  prop->clearOptions();
968 
969  if (cloud_infos_.empty())
970  {
971  return;
972  }
973 
974  boost::recursive_mutex::scoped_lock tlock(transformers_mutex_);
975 
976  const sensor_msgs::PointCloud2ConstPtr& msg = cloud_infos_.front()->message_;
977 
978  M_TransformerInfo::iterator it = transformers_.begin();
979  M_TransformerInfo::iterator end = transformers_.end();
980  for (; it != end; ++it)
981  {
982  const PointCloudTransformerPtr& trans = it->second.transformer;
983  if ((trans->supports(msg) & mask) == mask)
984  {
985  prop->addOption(QString::fromStdString(it->first));
986  }
987  }
988 }
989 
991 {
993  {
995  }
996  else
997  {
998  return 0.004;
999  }
1000 }
1001 
1002 } // namespace rviz
d
void setXyzTransformerOptions(EnumProperty *prop)
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
FloatProperty * alpha_property_
void setMin(float min)
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:80
pluginlib::ClassLoader< PointCloudTransformer > * transformer_class_loader_
std::set< std::string > S_string
void setMax(float max)
void setPropertiesHidden(const QList< Property *> &props, bool hide)
void destroyBox(const std::pair< CollObjectHandle, uint64_t > &handles)
Destroy the box associated with the given handle-int pair, if there is one.
void updateTransformers(const sensor_msgs::PointCloud2ConstPtr &cloud)
CollObjectHandle getHandle() const
uint qHash(IndexAndMessage iam)
f
PointCloudTransformerPtr getColorTransformer(const sensor_msgs::PointCloud2ConstPtr &cloud)
PointCloudTransformerPtr getXYZTransformer(const sensor_msgs::PointCloud2ConstPtr &cloud)
std::vector< PointCloud::Point > transformed_points_
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.
void emitTimeSignal(ros::Time time)
Emit a time signal that other Displays can synchronize to.
Definition: display.cpp:282
FloatProperty * point_world_size_property_
FloatProperty * decay_time_property_
A single element of a property tree, with a name, value, description, and possibly children...
Definition: property.h:100
IndexAndMessage(int _index, const void *_message)
virtual void clearOptions()
Clear the list of options.
void postRenderPass(uint32_t pass) override
S_uint64 extra_handles
Definition: forwards.h:61
EnumProperty * xyz_transformer_property_
void initialize(DisplayContext *context, Ogre::SceneNode *scene_node)
virtual void preRenderPass(uint32_t pass)
CollObjectHandle handle
Definition: forwards.h:59
static Ogre::ColourValue handleToColor(CollObjectHandle handle)
void onSelect(const Picked &obj) override
friend class PointCloudSelectionHandler
Property specialized to enforce floating point max/min.
bool convertPointCloudToPointCloud2(const sensor_msgs::PointCloud &input, sensor_msgs::PointCloud2 &output)
QHash< IndexAndMessage, Property * > property_hash_
boost::shared_ptr< PointCloudTransformer > PointCloudTransformerPtr
void setReadOnly(bool read_only) override
Overridden from Property to propagate read-only-ness to children.
boost::recursive_mutex transformers_mutex_
std::string getStdString()
PointCloudCommon(Display *display)
Pure-virtual base class for objects which give Display subclasses context in which to work...
std::vector< Ogre::AxisAlignedBox > V_AABB
bool validateFloats(const sensor_msgs::CameraInfo &msg)
void addMessage(const sensor_msgs::PointCloudConstPtr &cloud)
virtual void addOption(const QString &option, int value=0)
EnumProperty * style_property_
void fillTransformerOptions(EnumProperty *prop, uint32_t mask)
EnumProperty * color_transformer_property_
void show()
Show this Property in any PropertyTreeWidgets.
Definition: property.h:407
Representation of a point, with x/y/z position and r/g/b color.
Definition: point_cloud.h:132
BoolProperty * selectable_property_
PointCloudCommon::CloudInfo * cloud_info_
std::vector< PointCloud::Point > V_PointCloudPoint
Ogre::Vector3 position
Definition: point_cloud.h:139
M_TransformerInfo transformers_
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
void destroyProperties(const Picked &obj, Property *parent_property) override
Destroy all properties for the given picked object(s).
void addOptionStd(const std::string &option, int value=0)
Definition: enum_property.h:62
void setColorTransformerOptions(EnumProperty *prop)
FloatProperty * point_pixel_size_property_
bool operator==(IndexAndMessage a, IndexAndMessage b)
const std::string & getFixedFrame()
Return the current fixed frame name.
void getAABBs(const Picked &obj, V_AABB &aabbs) override
void onDeselect(const Picked &obj) override
virtual Ogre::SceneManager * getSceneManager() const =0
Returns the Ogre::SceneManager used for the main RenderPanel.
void createProperties(const Picked &obj, Property *parent_property) override
Override to create properties of the given picked object(s).
virtual void queueRender()=0
Queues a render. Multiple calls before a render happens will only cause a single render.
int32_t findChannelIndex(const sensor_msgs::PointCloud2ConstPtr &cloud, const std::string &channel)
Ogre::ColourValue color
Definition: point_cloud.h:140
boost::shared_ptr< PointCloud > cloud_
Property specialized to provide getter for booleans.
Definition: bool_property.h:38
A visual representation of a set of points.
Definition: point_cloud.h:107
virtual float getFloat() const
void processMessage(const sensor_msgs::PointCloud2ConstPtr &cloud)
Ogre::Vector3 pointFromCloud(const sensor_msgs::PointCloud2ConstPtr &cloud, uint32_t index)
virtual void postRenderPass(uint32_t pass)
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.
void preRenderPass(uint32_t pass) override
void setAutoSize(bool auto_size)
static Time now()
Ogre::SceneNode * scene_node_
bool transformCloud(const CloudInfoPtr &cloud, bool fully_update_transformers)
Transforms the cloud into the correct frame, and sets up our renderable cloud.
void hide()
Hide this Property in any PropertyTreeWidgets.
Definition: property.h:398
sensor_msgs::PointCloud2ConstPtr message_
virtual bool getBool() const
virtual void setReadOnly(bool read_only)
Prevent or allow users to edit this property from a PropertyTreeWidget.
Definition: property.h:433
virtual int getOptionInt()
Return the int value of the currently-chosen option, or 0 if the current option string does not have ...
Enum property.
Definition: enum_property.h:46
#define ROS_ERROR(...)
PointCloudSelectionHandler(float box_size, PointCloudCommon::CloudInfo *cloud_info, DisplayContext *context)
void update(float wall_dt, float ros_dt)


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Mon Jan 9 2023 03:07:20