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