point_cloud_common.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <QColor>
00031 
00032 #include <OgreSceneManager.h>
00033 #include <OgreSceneNode.h>
00034 #include <OgreWireBoundingBox.h>
00035 
00036 #include <ros/time.h>
00037 
00038 #include <tf/transform_listener.h>
00039 
00040 #include <pluginlib/class_loader.h>
00041 
00042 #include "rviz/default_plugin/point_cloud_transformer.h"
00043 #include "rviz/default_plugin/point_cloud_transformers.h"
00044 #include "rviz/display.h"
00045 #include "rviz/display_context.h"
00046 #include "rviz/frame_manager.h"
00047 #include "rviz/ogre_helpers/point_cloud.h"
00048 #include "rviz/properties/bool_property.h"
00049 #include "rviz/properties/enum_property.h"
00050 #include "rviz/properties/float_property.h"
00051 #include "rviz/properties/vector_property.h"
00052 #include "rviz/uniform_string_stream.h"
00053 #include "rviz/validate_floats.h"
00054 
00055 #include "rviz/default_plugin/point_cloud_common.h"
00056 
00057 namespace rviz
00058 {
00059 
00060 struct IndexAndMessage
00061 {
00062   IndexAndMessage( int _index, const void* _message )
00063     : index( _index )
00064     , message( (uint64_t) _message )
00065     {}
00066 
00067   int index;
00068   uint64_t message;
00069 };
00070 
00071 uint qHash( IndexAndMessage iam )
00072 {
00073   return
00074     ((uint) iam.index) +
00075     ((uint) (iam.message >> 32)) +
00076     ((uint) (iam.message & 0xffffffff));
00077 }
00078 
00079 bool operator==( IndexAndMessage a, IndexAndMessage b )
00080 {
00081   return a.index == b.index && a.message == b.message;
00082 }
00083 
00084 PointCloudSelectionHandler::PointCloudSelectionHandler(
00085     float box_size,
00086     PointCloudCommon::CloudInfo *cloud_info,
00087     DisplayContext* context )
00088   : SelectionHandler( context )
00089   , cloud_info_( cloud_info )
00090   , box_size_(box_size)
00091 {
00092 }
00093 
00094 PointCloudSelectionHandler::~PointCloudSelectionHandler()
00095 {
00096   // delete all the Property objects on our way out.
00097   QHash<IndexAndMessage, Property*>::const_iterator iter;
00098   for( iter = property_hash_.begin(); iter != property_hash_.end(); iter++ )
00099   {
00100     delete iter.value();
00101   }
00102 }
00103 
00104 void PointCloudSelectionHandler::preRenderPass(uint32_t pass)
00105 {
00106   SelectionHandler::preRenderPass(pass);
00107 
00108   switch( pass )
00109   {
00110   case 0:
00111     cloud_info_->cloud_->setPickColor( SelectionManager::handleToColor( getHandle() ));
00112     break;
00113   case 1:
00114     cloud_info_->cloud_->setColorByIndex(true);
00115     break;
00116   default:
00117     break;
00118   }
00119 }
00120 
00121 void PointCloudSelectionHandler::postRenderPass(uint32_t pass)
00122 {
00123   SelectionHandler::postRenderPass(pass);
00124 
00125   if (pass == 1)
00126   {
00127     cloud_info_->cloud_->setColorByIndex(false);
00128   }
00129 }
00130 
00131 Ogre::Vector3 pointFromCloud(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t index)
00132 {
00133   int32_t xi = findChannelIndex(cloud, "x");
00134   int32_t yi = findChannelIndex(cloud, "y");
00135   int32_t zi = findChannelIndex(cloud, "z");
00136 
00137   const uint32_t xoff = cloud->fields[xi].offset;
00138   const uint32_t yoff = cloud->fields[yi].offset;
00139   const uint32_t zoff = cloud->fields[zi].offset;
00140   const uint8_t type = cloud->fields[xi].datatype;
00141   const uint32_t point_step = cloud->point_step;
00142   float x = valueFromCloud<float>(cloud, xoff, type, point_step, index);
00143   float y = valueFromCloud<float>(cloud, yoff, type, point_step, index);
00144   float z = valueFromCloud<float>(cloud, zoff, type, point_step, index);
00145   return Ogre::Vector3(x, y, z);
00146 }
00147 
00148 void PointCloudSelectionHandler::createProperties( const Picked& obj, Property* parent_property )
00149 {
00150   typedef std::set<int> S_int;
00151   S_int indices;
00152   {
00153     S_uint64::const_iterator it = obj.extra_handles.begin();
00154     S_uint64::const_iterator end = obj.extra_handles.end();
00155     for (; it != end; ++it)
00156     {
00157       uint64_t handle = *it;
00158       indices.insert((handle & 0xffffffff) - 1);
00159     }
00160   }
00161 
00162   {
00163     S_int::iterator it = indices.begin();
00164     S_int::iterator end = indices.end();
00165     for (; it != end; ++it)
00166     {
00167       int index = *it;
00168       const sensor_msgs::PointCloud2ConstPtr& message = cloud_info_->message_;
00169 
00170       IndexAndMessage hash_key( index, message.get() );
00171       if( !property_hash_.contains( hash_key ))
00172       {
00173         Property* cat = new Property( QString( "Point %1 [cloud 0x%2]" ).arg( index ).arg( (uint64_t) message.get() ),
00174                                       QVariant(), "", parent_property );
00175         property_hash_.insert( hash_key, cat );
00176 
00177         // First add the position.
00178         VectorProperty* pos_prop = new VectorProperty( "Position", cloud_info_->transformed_points_[index].position, "", cat );
00179         pos_prop->setReadOnly( true );
00180 
00181         // Then add all other fields as well.
00182         for( size_t field = 0; field < message->fields.size(); ++field )
00183         {
00184           const sensor_msgs::PointField& f = message->fields[ field ];
00185           const std::string& name = f.name;
00186 
00187           if( name == "x" || name == "y" || name == "z" || name == "X" || name == "Y" || name == "Z" )
00188           {
00189             continue;
00190           }
00191           if( name == "rgb" || name == "rgba")
00192           {
00193             float float_val = valueFromCloud<float>( message, f.offset, f.datatype, message->point_step, index );
00194             // Convertion hack because rgb are stored int float (datatype=7) and valueFromCloud can't cast float to uint32_t
00195             uint32_t val = *((uint32_t*) &float_val);
00196             ColorProperty* prop = new ColorProperty( QString( "%1: %2" ).arg( field ).arg( QString::fromStdString( name )),
00197                                                      QColor( (val >> 16) & 0xff, (val >> 8) & 0xff, val & 0xff), "", cat );
00198             prop->setReadOnly( true );
00199 
00200             FloatProperty* aprop = new FloatProperty( QString( "alpha" ), ((val >> 24) / 255.0), "", cat );
00201             aprop->setReadOnly( true );
00202           }
00203           else
00204           {
00205             float val = valueFromCloud<float>( message, f.offset, f.datatype, message->point_step, index );
00206             FloatProperty* prop = new FloatProperty( QString( "%1: %2" ).arg( field ).arg( QString::fromStdString( name )),
00207                                                      val, "", cat );
00208             prop->setReadOnly( true );
00209           }
00210         }
00211       }
00212     }
00213   }
00214 }
00215 
00216 void PointCloudSelectionHandler::destroyProperties( const Picked& obj, Property* parent_property )
00217 {
00218   typedef std::set<int> S_int;
00219   S_int indices;
00220   {
00221     S_uint64::const_iterator it = obj.extra_handles.begin();
00222     S_uint64::const_iterator end = obj.extra_handles.end();
00223     for (; it != end; ++it)
00224     {
00225       uint64_t handle = *it;
00226       indices.insert((handle & 0xffffffff) - 1);
00227     }
00228   }
00229 
00230   {
00231     S_int::iterator it = indices.begin();
00232     S_int::iterator end = indices.end();
00233     for (; it != end; ++it)
00234     {
00235       int index = *it;
00236       const sensor_msgs::PointCloud2ConstPtr& message = cloud_info_->message_;
00237 
00238       IndexAndMessage hash_key( index, message.get() );
00239       
00240       Property* prop = property_hash_.take( hash_key );
00241       delete prop;
00242     }
00243   }
00244 }
00245 
00246 void PointCloudSelectionHandler::getAABBs(const Picked& obj, V_AABB& aabbs)
00247 {
00248   S_uint64::iterator it = obj.extra_handles.begin();
00249   S_uint64::iterator end = obj.extra_handles.end();
00250   for (; it != end; ++it)
00251   {
00252     M_HandleToBox::iterator find_it = boxes_.find(std::make_pair(obj.handle, *it - 1));
00253     if (find_it != boxes_.end())
00254     {
00255       Ogre::WireBoundingBox* box = find_it->second.second;
00256 
00257       aabbs.push_back(box->getWorldBoundingBox());
00258     }
00259   }
00260 }
00261 
00262 void PointCloudSelectionHandler::onSelect(const Picked& obj)
00263 {
00264   S_uint64::iterator it = obj.extra_handles.begin();
00265   S_uint64::iterator end = obj.extra_handles.end();
00266   for (; it != end; ++it)
00267   {
00268     int index = (*it & 0xffffffff) - 1;
00269 
00270     sensor_msgs::PointCloud2ConstPtr message = cloud_info_->message_;
00271 
00272     Ogre::Vector3 pos = cloud_info_->transformed_points_[index].position;
00273     pos = cloud_info_->scene_node_->convertLocalToWorldPosition( pos );
00274 
00275     float size = box_size_ * 0.5f;
00276 
00277     Ogre::AxisAlignedBox aabb(pos - size, pos + size);
00278 
00279     createBox(std::make_pair(obj.handle, index), aabb, "RVIZ/Cyan" );
00280   }
00281 }
00282 
00283 void PointCloudSelectionHandler::onDeselect(const Picked& obj)
00284 {
00285   S_uint64::iterator it = obj.extra_handles.begin();
00286   S_uint64::iterator end = obj.extra_handles.end();
00287   for (; it != end; ++it)
00288   {
00289     int global_index = (*it & 0xffffffff) - 1;
00290 
00291     destroyBox(std::make_pair(obj.handle, global_index));
00292   }
00293 }
00294 
00295 PointCloudCommon::CloudInfo::CloudInfo()
00296 : manager_(0)
00297 , scene_node_(0)
00298 {}
00299 
00300 PointCloudCommon::CloudInfo::~CloudInfo()
00301 {
00302   clear();
00303 }
00304 
00305 void PointCloudCommon::CloudInfo::clear()
00306 {
00307   if ( scene_node_ )
00308   {
00309     manager_->destroySceneNode( scene_node_ );
00310     scene_node_=0;
00311   }
00312 }
00313 
00314 PointCloudCommon::PointCloudCommon( Display* display )
00315 : spinner_(1, &cbqueue_)
00316 , new_xyz_transformer_(false)
00317 , new_color_transformer_(false)
00318 , needs_retransform_(false)
00319 , transformer_class_loader_(NULL)
00320 , display_( display )
00321 , auto_size_(false)
00322 {
00323   selectable_property_ = new BoolProperty( "Selectable", true,
00324                                            "Whether or not the points in this point cloud are selectable.",
00325                                            display_, SLOT( updateSelectable() ), this );
00326 
00327   style_property_ = new EnumProperty( "Style", "Flat Squares",
00328                                       "Rendering mode to use, in order of computational complexity.",
00329                                       display_, SLOT( updateStyle() ), this );
00330   style_property_->addOption( "Points", PointCloud::RM_POINTS );
00331   style_property_->addOption( "Squares", PointCloud::RM_SQUARES );
00332   style_property_->addOption( "Flat Squares", PointCloud::RM_FLAT_SQUARES );
00333   style_property_->addOption( "Spheres", PointCloud::RM_SPHERES );
00334   style_property_->addOption( "Boxes", PointCloud::RM_BOXES );
00335 
00336   point_world_size_property_ = new FloatProperty( "Size (m)", 0.01,
00337                                                 "Point size in meters.",
00338                                                 display_, SLOT( updateBillboardSize() ), this );
00339   point_world_size_property_->setMin( 0.0001 );
00340 
00341   point_pixel_size_property_ = new FloatProperty( "Size (Pixels)", 3,
00342                                                 "Point size in pixels.",
00343                                                 display_, SLOT( updateBillboardSize() ), this );
00344   point_pixel_size_property_->setMin( 1 );
00345 
00346   alpha_property_ = new FloatProperty( "Alpha", 1.0,
00347                                        "Amount of transparency to apply to the points.  Note that this is experimental and does not always look correct.",
00348                                        display_, SLOT( updateAlpha() ), this );
00349   alpha_property_->setMin( 0 );
00350   alpha_property_->setMax( 1 );
00351 
00352   decay_time_property_ = new FloatProperty( "Decay Time", 0,
00353                                             "Duration, in seconds, to keep the incoming points.  0 means only show the latest points.",
00354                                             display_, SLOT( queueRender() ));
00355   decay_time_property_->setMin( 0 );
00356 
00357   xyz_transformer_property_ = new EnumProperty( "Position Transformer", "",
00358                                                 "Set the transformer to use to set the position of the points.",
00359                                                 display_, SLOT( updateXyzTransformer() ), this );
00360   connect( xyz_transformer_property_, SIGNAL( requestOptions( EnumProperty* )),
00361            this, SLOT( setXyzTransformerOptions( EnumProperty* )));
00362 
00363   color_transformer_property_ = new EnumProperty( "Color Transformer", "",
00364                                                   "Set the transformer to use to set the color of the points.",
00365                                                   display_, SLOT( updateColorTransformer() ), this );
00366   connect( color_transformer_property_, SIGNAL( requestOptions( EnumProperty* )),
00367            this, SLOT( setColorTransformerOptions( EnumProperty* )));
00368 }
00369 
00370 void PointCloudCommon::initialize( DisplayContext* context, Ogre::SceneNode* scene_node )
00371 {
00372   transformer_class_loader_ = new pluginlib::ClassLoader<PointCloudTransformer>( "rviz", "rviz::PointCloudTransformer" );
00373   loadTransformers();
00374 
00375   context_ = context;
00376   scene_node_ = scene_node;
00377 
00378   updateStyle();
00379   updateBillboardSize();
00380   updateAlpha();
00381   updateSelectable();
00382 
00383   spinner_.start();
00384 }
00385 
00386 PointCloudCommon::~PointCloudCommon()
00387 {
00388   spinner_.stop();
00389 
00390   if ( transformer_class_loader_ )
00391   {
00392     delete transformer_class_loader_;
00393   }
00394 }
00395 
00396 void PointCloudCommon::loadTransformers()
00397 {
00398   std::vector<std::string> classes = transformer_class_loader_->getDeclaredClasses();
00399   std::vector<std::string>::iterator ci;
00400   
00401   for( ci = classes.begin(); ci != classes.end(); ci++ )
00402   {
00403     const std::string& lookup_name = *ci;
00404     std::string name = transformer_class_loader_->getName( lookup_name );
00405 
00406     if( transformers_.count( name ) > 0 )
00407     {
00408       ROS_ERROR( "Transformer type [%s] is already loaded.", name.c_str() );
00409       continue;
00410     }
00411 
00412     PointCloudTransformerPtr trans( transformer_class_loader_->createUnmanagedInstance( lookup_name ));
00413     trans->init();
00414     connect( trans.get(), SIGNAL( needRetransform() ), this, SLOT( causeRetransform() ));
00415 
00416     TransformerInfo info;
00417     info.transformer = trans;
00418     info.readable_name = name;
00419     info.lookup_name = lookup_name;
00420 
00421     info.transformer->createProperties( display_, PointCloudTransformer::Support_XYZ, info.xyz_props );
00422     setPropertiesHidden( info.xyz_props, true );
00423 
00424     info.transformer->createProperties( display_, PointCloudTransformer::Support_Color, info.color_props );
00425     setPropertiesHidden( info.color_props, true );
00426 
00427     transformers_[ name ] = info;
00428   }
00429 }
00430 
00431 void PointCloudCommon::setAutoSize( bool auto_size )
00432 {
00433   auto_size_ = auto_size;
00434   for ( unsigned i=0; i<cloud_infos_.size(); i++ )
00435   {
00436     cloud_infos_[i]->cloud_->setAutoSize( auto_size );
00437   }
00438 }
00439 
00440 
00441 
00442 void PointCloudCommon::updateAlpha()
00443 {
00444   for ( unsigned i=0; i<cloud_infos_.size(); i++ )
00445   {
00446     bool per_point_alpha = findChannelIndex(cloud_infos_[i]->message_, "rgba") != -1;
00447     cloud_infos_[i]->cloud_->setAlpha( alpha_property_->getFloat(), per_point_alpha );
00448   }
00449 }
00450 
00451 void PointCloudCommon::updateSelectable()
00452 {
00453   bool selectable = selectable_property_->getBool();
00454 
00455   if( selectable )
00456   {
00457     for ( unsigned i=0; i<cloud_infos_.size(); i++ )
00458     {
00459       cloud_infos_[i]->selection_handler_.reset( new PointCloudSelectionHandler( getSelectionBoxSize(), cloud_infos_[i].get(), context_ ));
00460       cloud_infos_[i]->cloud_->setPickColor( SelectionManager::handleToColor( cloud_infos_[i]->selection_handler_->getHandle() ));
00461     }
00462   }
00463   else
00464   {
00465     for ( unsigned i=0; i<cloud_infos_.size(); i++ )
00466     {
00467       cloud_infos_[i]->selection_handler_.reset( );
00468       cloud_infos_[i]->cloud_->setPickColor( Ogre::ColourValue( 0.0f, 0.0f, 0.0f, 0.0f ) );
00469     }
00470   }
00471 }
00472 
00473 void PointCloudCommon::updateStyle()
00474 {
00475   PointCloud::RenderMode mode = (PointCloud::RenderMode) style_property_->getOptionInt();
00476   if( mode == PointCloud::RM_POINTS )
00477   {
00478     point_world_size_property_->hide();
00479     point_pixel_size_property_->show();
00480   }
00481   else
00482   {
00483     point_world_size_property_->show();
00484     point_pixel_size_property_->hide();
00485   }
00486   for( unsigned int i = 0; i < cloud_infos_.size(); i++ )
00487   {
00488     cloud_infos_[i]->cloud_->setRenderMode( mode );
00489   }
00490   updateBillboardSize();
00491 }
00492 
00493 void PointCloudCommon::updateBillboardSize()
00494 {
00495   PointCloud::RenderMode mode = (PointCloud::RenderMode) style_property_->getOptionInt();
00496   float size;
00497   if( mode == PointCloud::RM_POINTS ) {
00498     size = point_pixel_size_property_->getFloat();
00499   } else {
00500     size = point_world_size_property_->getFloat();
00501   }
00502   for ( unsigned i=0; i<cloud_infos_.size(); i++ )
00503   {
00504     cloud_infos_[i]->cloud_->setDimensions( size, size, size );
00505     cloud_infos_[i]->selection_handler_->setBoxSize( getSelectionBoxSize() );
00506   }
00507   context_->queueRender();
00508 }
00509 
00510 void PointCloudCommon::reset()
00511 {
00512   boost::mutex::scoped_lock lock(new_clouds_mutex_);
00513   cloud_infos_.clear();
00514   new_cloud_infos_.clear();
00515 }
00516 
00517 void PointCloudCommon::causeRetransform()
00518 {
00519   needs_retransform_ = true;
00520 }
00521 
00522 void PointCloudCommon::update(float wall_dt, float ros_dt)
00523 {
00524   PointCloud::RenderMode mode = (PointCloud::RenderMode) style_property_->getOptionInt();
00525 
00526   float point_decay_time = decay_time_property_->getFloat();
00527   if (needs_retransform_)
00528   {
00529     retransform();
00530     needs_retransform_ = false;
00531   }
00532 
00533   // instead of deleting cloud infos, we just clear them
00534   // and put them into obsolete_cloud_infos, so active selections
00535   // are preserved
00536 
00537   ros::Time now = ros::Time::now();
00538 
00539   // if decay time == 0, clear the old cloud when we get a new one
00540   // otherwise, clear all the outdated ones
00541   {
00542     boost::mutex::scoped_lock lock(new_clouds_mutex_);
00543     if ( point_decay_time > 0.0 || !new_cloud_infos_.empty() )
00544     {
00545       while( !cloud_infos_.empty() && now.toSec() - cloud_infos_.front()->receive_time_.toSec() > point_decay_time )
00546       {
00547         cloud_infos_.front()->clear();
00548         obsolete_cloud_infos_.push_back( cloud_infos_.front() );
00549         cloud_infos_.pop_front();
00550         context_->queueRender();
00551       }
00552     }
00553   }
00554 
00555   // garbage-collect old point clouds that don't have an active selection
00556   L_CloudInfo::iterator it = obsolete_cloud_infos_.begin();
00557   L_CloudInfo::iterator end = obsolete_cloud_infos_.end();
00558   for (; it != end; ++it)
00559   {
00560     if ( !(*it)->selection_handler_.get() ||
00561          !(*it)->selection_handler_->hasSelections() )
00562     {
00563       it = obsolete_cloud_infos_.erase(it);
00564     }
00565   }
00566 
00567   {
00568     boost::mutex::scoped_lock lock(new_clouds_mutex_);
00569     if( !new_cloud_infos_.empty() )
00570     {
00571       float size;
00572       if( mode == PointCloud::RM_POINTS ) {
00573         size = point_pixel_size_property_->getFloat();
00574       } else {
00575         size = point_world_size_property_->getFloat();
00576       }
00577 
00578       V_CloudInfo::iterator it = new_cloud_infos_.begin();
00579       V_CloudInfo::iterator end = new_cloud_infos_.end();
00580       for (; it != end; ++it)
00581       {
00582         CloudInfoPtr cloud_info = *it;
00583 
00584         V_CloudInfo::iterator next = it; next++;
00585         // ignore point clouds that are too old, but keep at least one
00586         if ( next != end && now.toSec() - cloud_info->receive_time_.toSec() > point_decay_time ) {
00587           continue;
00588         }
00589 
00590         bool per_point_alpha = findChannelIndex(cloud_info->message_, "rgba") != -1;
00591 
00592         cloud_info->cloud_.reset( new PointCloud() );
00593         cloud_info->cloud_->addPoints( &(cloud_info->transformed_points_.front()), cloud_info->transformed_points_.size() );
00594         cloud_info->cloud_->setRenderMode( mode );
00595         cloud_info->cloud_->setAlpha( alpha_property_->getFloat(), per_point_alpha);
00596         cloud_info->cloud_->setDimensions( size, size, size );
00597         cloud_info->cloud_->setAutoSize(auto_size_);
00598 
00599         cloud_info->manager_ = context_->getSceneManager();
00600 
00601         cloud_info->scene_node_ = scene_node_->createChildSceneNode( cloud_info->position_, cloud_info->orientation_ );
00602 
00603         cloud_info->scene_node_->attachObject( cloud_info->cloud_.get() );
00604 
00605         cloud_info->selection_handler_.reset( new PointCloudSelectionHandler( getSelectionBoxSize(), cloud_info.get(), context_ ));
00606 
00607         cloud_infos_.push_back(*it);
00608       }
00609 
00610       new_cloud_infos_.clear();
00611     }
00612   }
00613 
00614   {
00615     boost::recursive_mutex::scoped_try_lock lock( transformers_mutex_ );
00616 
00617     if( lock.owns_lock() )
00618     {
00619       if( new_xyz_transformer_ || new_color_transformer_ )
00620       {
00621         M_TransformerInfo::iterator it = transformers_.begin();
00622         M_TransformerInfo::iterator end = transformers_.end();
00623         for (; it != end; ++it)
00624         {
00625           const std::string& name = it->first;
00626           TransformerInfo& info = it->second;
00627 
00628           setPropertiesHidden( info.xyz_props, name != xyz_transformer_property_->getStdString() );
00629           setPropertiesHidden( info.color_props, name != color_transformer_property_->getStdString() );
00630         }
00631       }
00632     }
00633 
00634     new_xyz_transformer_ = false;
00635     new_color_transformer_ = false;
00636   }
00637 
00638   updateStatus();
00639 }
00640 
00641 void PointCloudCommon::setPropertiesHidden( const QList<Property*>& props, bool hide )
00642 {
00643   for( int i = 0; i < props.size(); i++ )
00644   {
00645     props[ i ]->setHidden( hide );
00646   }
00647 }
00648 
00649 void PointCloudCommon::updateTransformers( const sensor_msgs::PointCloud2ConstPtr& cloud )
00650 {
00651   std::string xyz_name = xyz_transformer_property_->getStdString();
00652   std::string color_name = color_transformer_property_->getStdString();
00653 
00654   xyz_transformer_property_->clearOptions();
00655   color_transformer_property_->clearOptions();
00656 
00657   // Get the channels that we could potentially render
00658   typedef std::set<std::pair<uint8_t, std::string> > S_string;
00659   S_string valid_xyz, valid_color;
00660   bool cur_xyz_valid = false;
00661   bool cur_color_valid = false;
00662   bool has_rgb_transformer = false;
00663   M_TransformerInfo::iterator trans_it = transformers_.begin();
00664   M_TransformerInfo::iterator trans_end = transformers_.end();
00665   for(;trans_it != trans_end; ++trans_it)
00666   {
00667     const std::string& name = trans_it->first;
00668     const PointCloudTransformerPtr& trans = trans_it->second.transformer;
00669     uint32_t mask = trans->supports(cloud);
00670     if (mask & PointCloudTransformer::Support_XYZ)
00671     {
00672       valid_xyz.insert(std::make_pair(trans->score(cloud), name));
00673       if (name == xyz_name)
00674       {
00675         cur_xyz_valid = true;
00676       }
00677       xyz_transformer_property_->addOptionStd( name );
00678     }
00679 
00680     if (mask & PointCloudTransformer::Support_Color)
00681     {
00682       valid_color.insert(std::make_pair(trans->score(cloud), name));
00683       if (name == color_name)
00684       {
00685         cur_color_valid = true;
00686       }
00687       if (name == "RGB8")
00688       {
00689         has_rgb_transformer = true;
00690       }
00691       color_transformer_property_->addOptionStd( name );
00692     }
00693   }
00694 
00695   if( !cur_xyz_valid )
00696   {
00697     if( !valid_xyz.empty() )
00698     {
00699       xyz_transformer_property_->setStringStd( valid_xyz.rbegin()->second );
00700     }
00701   }
00702 
00703   if( !cur_color_valid )
00704   {
00705     if( !valid_color.empty() )
00706     {
00707       if (has_rgb_transformer)
00708       {
00709         color_transformer_property_->setStringStd( "RGB8" );
00710       } else
00711       {
00712         color_transformer_property_->setStringStd( valid_color.rbegin()->second );
00713       }
00714     }
00715   }
00716 }
00717 
00718 void PointCloudCommon::updateStatus()
00719 {
00720   std::stringstream ss;
00721   //ss << "Showing [" << total_point_count_ << "] points from [" << clouds_.size() << "] messages";
00722   display_->setStatusStd(StatusProperty::Ok, "Points", ss.str());
00723 }
00724 
00725 void PointCloudCommon::processMessage(const sensor_msgs::PointCloud2ConstPtr& cloud)
00726 {
00727   CloudInfoPtr info(new CloudInfo);
00728   info->message_ = cloud;
00729   info->receive_time_ = ros::Time::now();
00730 
00731   if (transformCloud(info, true))
00732   {
00733     boost::mutex::scoped_lock lock(new_clouds_mutex_);
00734     new_cloud_infos_.push_back(info);
00735     display_->emitTimeSignal( cloud->header.stamp );
00736   }
00737 }
00738 
00739 void PointCloudCommon::updateXyzTransformer()
00740 {
00741   boost::recursive_mutex::scoped_lock lock( transformers_mutex_ );
00742   if( transformers_.count( xyz_transformer_property_->getStdString() ) == 0 )
00743   {
00744     return;
00745   }
00746   new_xyz_transformer_ = true;
00747   causeRetransform();
00748 }
00749 
00750 void PointCloudCommon::updateColorTransformer()
00751 {
00752   boost::recursive_mutex::scoped_lock lock( transformers_mutex_ );
00753   if( transformers_.count( color_transformer_property_->getStdString() ) == 0 )
00754   {
00755     return;
00756   }
00757   new_color_transformer_ = true;
00758   causeRetransform();
00759 }
00760 
00761 PointCloudTransformerPtr PointCloudCommon::getXYZTransformer( const sensor_msgs::PointCloud2ConstPtr& cloud )
00762 {
00763   boost::recursive_mutex::scoped_lock lock( transformers_mutex_);
00764   M_TransformerInfo::iterator it = transformers_.find( xyz_transformer_property_->getStdString() );
00765   if( it != transformers_.end() )
00766   {
00767     const PointCloudTransformerPtr& trans = it->second.transformer;
00768     if( trans->supports( cloud ) & PointCloudTransformer::Support_XYZ )
00769     {
00770       return trans;
00771     }
00772   }
00773 
00774   return PointCloudTransformerPtr();
00775 }
00776 
00777 PointCloudTransformerPtr PointCloudCommon::getColorTransformer( const sensor_msgs::PointCloud2ConstPtr& cloud )
00778 {
00779   boost::recursive_mutex::scoped_lock lock( transformers_mutex_ );
00780   M_TransformerInfo::iterator it = transformers_.find( color_transformer_property_->getStdString() );
00781   if( it != transformers_.end() )
00782   {
00783     const PointCloudTransformerPtr& trans = it->second.transformer;
00784     if( trans->supports( cloud ) & PointCloudTransformer::Support_Color )
00785     {
00786       return trans;
00787     }
00788   }
00789 
00790   return PointCloudTransformerPtr();
00791 }
00792 
00793 
00794 void PointCloudCommon::retransform()
00795 {
00796   boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
00797 
00798   D_CloudInfo::iterator it = cloud_infos_.begin();
00799   D_CloudInfo::iterator end = cloud_infos_.end();
00800   for (; it != end; ++it)
00801   {
00802     const CloudInfoPtr& cloud_info = *it;
00803     transformCloud(cloud_info, false);
00804     cloud_info->cloud_->clear();
00805     cloud_info->cloud_->addPoints(&cloud_info->transformed_points_.front(), cloud_info->transformed_points_.size());
00806   }
00807 }
00808 
00809 bool PointCloudCommon::transformCloud(const CloudInfoPtr& cloud_info, bool update_transformers)
00810 {
00811 
00812   if ( !cloud_info->scene_node_ )
00813   {
00814     if (!context_->getFrameManager()->getTransform(cloud_info->message_->header, cloud_info->position_, cloud_info->orientation_))
00815     {
00816       std::stringstream ss;
00817       ss << "Failed to transform from frame [" << cloud_info->message_->header.frame_id << "] to frame [" << context_->getFrameManager()->getFixedFrame() << "]";
00818       display_->setStatusStd(StatusProperty::Error, "Message", ss.str());
00819       return false;
00820     }
00821   }
00822 
00823   Ogre::Matrix4 transform;
00824   transform.makeTransform( cloud_info->position_, Ogre::Vector3(1,1,1), cloud_info->orientation_ );
00825 
00826   V_PointCloudPoint& cloud_points = cloud_info->transformed_points_;
00827   cloud_points.clear();
00828 
00829   size_t size = cloud_info->message_->width * cloud_info->message_->height;
00830   PointCloud::Point default_pt;
00831   default_pt.color = Ogre::ColourValue(1, 1, 1);
00832   default_pt.position = Ogre::Vector3::ZERO;
00833   cloud_points.resize(size, default_pt);
00834 
00835   {
00836     boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
00837     if( update_transformers )
00838     {
00839       updateTransformers( cloud_info->message_ );
00840     }
00841     PointCloudTransformerPtr xyz_trans = getXYZTransformer(cloud_info->message_);
00842     PointCloudTransformerPtr color_trans = getColorTransformer(cloud_info->message_);
00843 
00844     if (!xyz_trans)
00845     {
00846       std::stringstream ss;
00847       ss << "No position transformer available for cloud";
00848       display_->setStatusStd(StatusProperty::Error, "Message", ss.str());
00849       return false;
00850     }
00851 
00852     if (!color_trans)
00853     {
00854       std::stringstream ss;
00855       ss << "No color transformer available for cloud";
00856       display_->setStatusStd(StatusProperty::Error, "Message", ss.str());
00857       return false;
00858     }
00859 
00860     xyz_trans->transform(cloud_info->message_, PointCloudTransformer::Support_XYZ, transform, cloud_points);
00861     color_trans->transform(cloud_info->message_, PointCloudTransformer::Support_Color, transform, cloud_points);
00862   }
00863 
00864   for (V_PointCloudPoint::iterator cloud_point = cloud_points.begin(); cloud_point != cloud_points.end(); ++cloud_point)
00865   {
00866     if (!validateFloats(cloud_point->position))
00867     {
00868       cloud_point->position.x = 999999.0f;
00869       cloud_point->position.y = 999999.0f;
00870       cloud_point->position.z = 999999.0f;
00871     }
00872   }
00873 
00874   return true;
00875 }
00876 
00877 bool convertPointCloudToPointCloud2(const sensor_msgs::PointCloud& input, sensor_msgs::PointCloud2& output)
00878 {
00879   output.header = input.header;
00880   output.width  = input.points.size ();
00881   output.height = 1;
00882   output.fields.resize (3 + input.channels.size ());
00883   // Convert x/y/z to fields
00884   output.fields[0].name = "x"; output.fields[1].name = "y"; output.fields[2].name = "z";
00885   int offset = 0;
00886   // All offsets are *4, as all field data types are float32
00887   for (size_t d = 0; d < output.fields.size (); ++d, offset += 4)
00888   {
00889     output.fields[d].offset = offset;
00890     output.fields[d].datatype = sensor_msgs::PointField::FLOAT32;
00891   }
00892   output.point_step = offset;
00893   output.row_step   = output.point_step * output.width;
00894   // Convert the remaining of the channels to fields
00895   for (size_t d = 0; d < input.channels.size (); ++d)
00896     output.fields[3 + d].name = input.channels[d].name;
00897   output.data.resize (input.points.size () * output.point_step);
00898   output.is_bigendian = false;  // @todo ?
00899   output.is_dense     = false;
00900 
00901   // Copy the data points
00902   for (size_t cp = 0; cp < input.points.size (); ++cp)
00903   {
00904     memcpy (&output.data[cp * output.point_step + output.fields[0].offset], &input.points[cp].x, sizeof (float));
00905     memcpy (&output.data[cp * output.point_step + output.fields[1].offset], &input.points[cp].y, sizeof (float));
00906     memcpy (&output.data[cp * output.point_step + output.fields[2].offset], &input.points[cp].z, sizeof (float));
00907     for (size_t d = 0; d < input.channels.size (); ++d)
00908     {
00909       if (input.channels[d].values.size() == input.points.size())
00910       {
00911         memcpy (&output.data[cp * output.point_step + output.fields[3 + d].offset], &input.channels[d].values[cp], sizeof (float));
00912       }
00913     }
00914   }
00915   return (true);
00916 }
00917 
00918 void PointCloudCommon::addMessage(const sensor_msgs::PointCloudConstPtr& cloud)
00919 {
00920   sensor_msgs::PointCloud2Ptr out(new sensor_msgs::PointCloud2);
00921   convertPointCloudToPointCloud2(*cloud, *out);
00922   addMessage(out);
00923 }
00924 
00925 void PointCloudCommon::addMessage(const sensor_msgs::PointCloud2ConstPtr& cloud)
00926 {
00927   processMessage(cloud);
00928 }
00929 
00930 void PointCloudCommon::fixedFrameChanged()
00931 {
00932   reset();
00933 }
00934 
00935 void PointCloudCommon::setXyzTransformerOptions( EnumProperty* prop )
00936 {
00937   fillTransformerOptions( prop, PointCloudTransformer::Support_XYZ );
00938 }
00939 
00940 void PointCloudCommon::setColorTransformerOptions( EnumProperty* prop )
00941 {
00942   fillTransformerOptions( prop, PointCloudTransformer::Support_Color );
00943 }
00944 
00945 void PointCloudCommon::fillTransformerOptions( EnumProperty* prop, uint32_t mask )
00946 {
00947   prop->clearOptions();
00948 
00949   if (cloud_infos_.empty())
00950   {
00951     return;
00952   }
00953 
00954   boost::recursive_mutex::scoped_lock tlock(transformers_mutex_);
00955 
00956   const sensor_msgs::PointCloud2ConstPtr& msg = cloud_infos_.front()->message_;
00957 
00958   M_TransformerInfo::iterator it = transformers_.begin();
00959   M_TransformerInfo::iterator end = transformers_.end();
00960   for (; it != end; ++it)
00961   {
00962     const PointCloudTransformerPtr& trans = it->second.transformer;
00963     if ((trans->supports(msg) & mask) == mask)
00964     {
00965       prop->addOption( QString::fromStdString( it->first ));
00966     }
00967   }
00968 }
00969 
00970 float PointCloudCommon::getSelectionBoxSize()
00971 {
00972   if( style_property_->getOptionInt() != PointCloud::RM_POINTS )
00973   {
00974     return point_world_size_property_->getFloat();
00975   }
00976   else
00977   {
00978     return 0.004;
00979   }
00980 }
00981 
00982 } // namespace rviz


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Tue Oct 3 2017 03:19:31