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" )
00192           {
00193             uint32_t val = valueFromCloud<uint32_t>( message, f.offset, f.datatype, message->point_step, index );
00194             ColorProperty* prop = new ColorProperty( QString( "%1: %2" ).arg( field ).arg( QString::fromStdString( name )),
00195                                                      QColor( val >> 16, (val >> 8) & 0xff, val & 0xff ), "", cat );
00196             prop->setReadOnly( true );
00197           }
00198           else
00199           {
00200             float val = valueFromCloud<float>( message, f.offset, f.datatype, message->point_step, index );
00201             
00202             FloatProperty* prop = new FloatProperty( QString( "%1: %2" ).arg( field ).arg( QString::fromStdString( name )),
00203                                                      val, "", cat );
00204             prop->setReadOnly( true );
00205           }
00206         }
00207       }
00208     }
00209   }
00210 }
00211 
00212 void PointCloudSelectionHandler::destroyProperties( const Picked& obj, Property* parent_property )
00213 {
00214   typedef std::set<int> S_int;
00215   S_int indices;
00216   {
00217     S_uint64::const_iterator it = obj.extra_handles.begin();
00218     S_uint64::const_iterator end = obj.extra_handles.end();
00219     for (; it != end; ++it)
00220     {
00221       uint64_t handle = *it;
00222       indices.insert((handle & 0xffffffff) - 1);
00223     }
00224   }
00225 
00226   {
00227     S_int::iterator it = indices.begin();
00228     S_int::iterator end = indices.end();
00229     for (; it != end; ++it)
00230     {
00231       int index = *it;
00232       const sensor_msgs::PointCloud2ConstPtr& message = cloud_info_->message_;
00233 
00234       IndexAndMessage hash_key( index, message.get() );
00235       
00236       Property* prop = property_hash_.take( hash_key );
00237       delete prop;
00238     }
00239   }
00240 }
00241 
00242 void PointCloudSelectionHandler::getAABBs(const Picked& obj, V_AABB& aabbs)
00243 {
00244   S_uint64::iterator it = obj.extra_handles.begin();
00245   S_uint64::iterator end = obj.extra_handles.end();
00246   for (; it != end; ++it)
00247   {
00248     M_HandleToBox::iterator find_it = boxes_.find(std::make_pair(obj.handle, *it - 1));
00249     if (find_it != boxes_.end())
00250     {
00251       Ogre::WireBoundingBox* box = find_it->second.second;
00252 
00253       aabbs.push_back(box->getWorldBoundingBox());
00254     }
00255   }
00256 }
00257 
00258 void PointCloudSelectionHandler::onSelect(const Picked& obj)
00259 {
00260   S_uint64::iterator it = obj.extra_handles.begin();
00261   S_uint64::iterator end = obj.extra_handles.end();
00262   for (; it != end; ++it)
00263   {
00264     int index = (*it & 0xffffffff) - 1;
00265 
00266     sensor_msgs::PointCloud2ConstPtr message = cloud_info_->message_;
00267 
00268     Ogre::Vector3 pos = cloud_info_->transformed_points_[index].position;
00269     pos = cloud_info_->scene_node_->convertLocalToWorldPosition( pos );
00270 
00271     float size = box_size_ * 0.5f;
00272 
00273     Ogre::AxisAlignedBox aabb(pos - size, pos + size);
00274 
00275     createBox(std::make_pair(obj.handle, index), aabb, "RVIZ/Cyan" );
00276   }
00277 }
00278 
00279 void PointCloudSelectionHandler::onDeselect(const Picked& obj)
00280 {
00281   S_uint64::iterator it = obj.extra_handles.begin();
00282   S_uint64::iterator end = obj.extra_handles.end();
00283   for (; it != end; ++it)
00284   {
00285     int global_index = (*it & 0xffffffff) - 1;
00286 
00287     destroyBox(std::make_pair(obj.handle, global_index));
00288   }
00289 }
00290 
00291 PointCloudCommon::CloudInfo::CloudInfo()
00292 : manager_(0)
00293 , scene_node_(0)
00294 {}
00295 
00296 PointCloudCommon::CloudInfo::~CloudInfo()
00297 {
00298   clear();
00299 }
00300 
00301 void PointCloudCommon::CloudInfo::clear()
00302 {
00303   if ( scene_node_ )
00304   {
00305     manager_->destroySceneNode( scene_node_ );
00306     scene_node_=0;
00307   }
00308 }
00309 
00310 PointCloudCommon::PointCloudCommon( Display* display )
00311 : spinner_(1, &cbqueue_)
00312 , new_xyz_transformer_(false)
00313 , new_color_transformer_(false)
00314 , needs_retransform_(false)
00315 , transformer_class_loader_(NULL)
00316 , display_( display )
00317 , auto_size_(false)
00318 {
00319   selectable_property_ = new BoolProperty( "Selectable", true,
00320                                            "Whether or not the points in this point cloud are selectable.",
00321                                            display_, SLOT( updateSelectable() ), this );
00322 
00323   style_property_ = new EnumProperty( "Style", "Flat Squares",
00324                                       "Rendering mode to use, in order of computational complexity.",
00325                                       display_, SLOT( updateStyle() ), this );
00326   style_property_->addOption( "Points", PointCloud::RM_POINTS );
00327   style_property_->addOption( "Squares", PointCloud::RM_SQUARES );
00328   style_property_->addOption( "Flat Squares", PointCloud::RM_FLAT_SQUARES );
00329   style_property_->addOption( "Spheres", PointCloud::RM_SPHERES );
00330   style_property_->addOption( "Boxes", PointCloud::RM_BOXES );
00331 
00332   point_world_size_property_ = new FloatProperty( "Size (m)", 0.01,
00333                                                 "Point size in meters.",
00334                                                 display_, SLOT( updateBillboardSize() ), this );
00335   point_world_size_property_->setMin( 0.0001 );
00336 
00337   point_pixel_size_property_ = new FloatProperty( "Size (Pixels)", 3,
00338                                                 "Point size in pixels.",
00339                                                 display_, SLOT( updateBillboardSize() ), this );
00340   point_pixel_size_property_->setMin( 1 );
00341 
00342   alpha_property_ = new FloatProperty( "Alpha", 1.0,
00343                                        "Amount of transparency to apply to the points.  Note that this is experimental and does not always look correct.",
00344                                        display_, SLOT( updateAlpha() ), this );
00345   alpha_property_->setMin( 0 );
00346   alpha_property_->setMax( 1 );
00347 
00348   decay_time_property_ = new FloatProperty( "Decay Time", 0,
00349                                             "Duration, in seconds, to keep the incoming points.  0 means only show the latest points.",
00350                                             display_, SLOT( queueRender() ));
00351   decay_time_property_->setMin( 0 );
00352 
00353   xyz_transformer_property_ = new EnumProperty( "Position Transformer", "",
00354                                                 "Set the transformer to use to set the position of the points.",
00355                                                 display_, SLOT( updateXyzTransformer() ), this );
00356   connect( xyz_transformer_property_, SIGNAL( requestOptions( EnumProperty* )),
00357            this, SLOT( setXyzTransformerOptions( EnumProperty* )));
00358 
00359   color_transformer_property_ = new EnumProperty( "Color Transformer", "",
00360                                                   "Set the transformer to use to set the color of the points.",
00361                                                   display_, SLOT( updateColorTransformer() ), this );
00362   connect( color_transformer_property_, SIGNAL( requestOptions( EnumProperty* )),
00363            this, SLOT( setColorTransformerOptions( EnumProperty* )));
00364 }
00365 
00366 void PointCloudCommon::initialize( DisplayContext* context, Ogre::SceneNode* scene_node )
00367 {
00368   transformer_class_loader_ = new pluginlib::ClassLoader<PointCloudTransformer>( "rviz", "rviz::PointCloudTransformer" );
00369   loadTransformers();
00370 
00371   context_ = context;
00372   scene_node_ = scene_node;
00373 
00374   updateStyle();
00375   updateBillboardSize();
00376   updateAlpha();
00377   updateSelectable();
00378 
00379   spinner_.start();
00380 }
00381 
00382 PointCloudCommon::~PointCloudCommon()
00383 {
00384   spinner_.stop();
00385 
00386   if ( transformer_class_loader_ )
00387   {
00388     delete transformer_class_loader_;
00389   }
00390 }
00391 
00392 void PointCloudCommon::loadTransformers()
00393 {
00394   std::vector<std::string> classes = transformer_class_loader_->getDeclaredClasses();
00395   std::vector<std::string>::iterator ci;
00396   
00397   for( ci = classes.begin(); ci != classes.end(); ci++ )
00398   {
00399     const std::string& lookup_name = *ci;
00400     std::string name = transformer_class_loader_->getName( lookup_name );
00401 
00402     if( transformers_.count( name ) > 0 )
00403     {
00404       ROS_ERROR( "Transformer type [%s] is already loaded.", name.c_str() );
00405       continue;
00406     }
00407 
00408     PointCloudTransformerPtr trans( transformer_class_loader_->createUnmanagedInstance( lookup_name ));
00409     trans->init();
00410     connect( trans.get(), SIGNAL( needRetransform() ), this, SLOT( causeRetransform() ));
00411 
00412     TransformerInfo info;
00413     info.transformer = trans;
00414     info.readable_name = name;
00415     info.lookup_name = lookup_name;
00416 
00417     info.transformer->createProperties( display_, PointCloudTransformer::Support_XYZ, info.xyz_props );
00418     setPropertiesHidden( info.xyz_props, true );
00419 
00420     info.transformer->createProperties( display_, PointCloudTransformer::Support_Color, info.color_props );
00421     setPropertiesHidden( info.color_props, true );
00422 
00423     transformers_[ name ] = info;
00424   }
00425 }
00426 
00427 void PointCloudCommon::setAutoSize( bool auto_size )
00428 {
00429   auto_size_ = auto_size;
00430   for ( unsigned i=0; i<cloud_infos_.size(); i++ )
00431   {
00432     cloud_infos_[i]->cloud_->setAutoSize( auto_size );
00433   }
00434 }
00435 
00436 
00437 
00438 void PointCloudCommon::updateAlpha()
00439 {
00440   for ( unsigned i=0; i<cloud_infos_.size(); i++ )
00441   {
00442     cloud_infos_[i]->cloud_->setAlpha( alpha_property_->getFloat() );
00443   }
00444 }
00445 
00446 void PointCloudCommon::updateSelectable()
00447 {
00448   bool selectable = selectable_property_->getBool();
00449 
00450   if( selectable )
00451   {
00452     for ( unsigned i=0; i<cloud_infos_.size(); i++ )
00453     {
00454       cloud_infos_[i]->selection_handler_.reset( new PointCloudSelectionHandler( getSelectionBoxSize(), cloud_infos_[i].get(), context_ ));
00455       cloud_infos_[i]->cloud_->setPickColor( SelectionManager::handleToColor( cloud_infos_[i]->selection_handler_->getHandle() ));
00456     }
00457   }
00458   else
00459   {
00460     for ( unsigned i=0; i<cloud_infos_.size(); i++ )
00461     {
00462       cloud_infos_[i]->selection_handler_.reset( );
00463       cloud_infos_[i]->cloud_->setPickColor( Ogre::ColourValue( 0.0f, 0.0f, 0.0f, 0.0f ) );
00464     }
00465   }
00466 }
00467 
00468 void PointCloudCommon::updateStyle()
00469 {
00470   PointCloud::RenderMode mode = (PointCloud::RenderMode) style_property_->getOptionInt();
00471   if( mode == PointCloud::RM_POINTS )
00472   {
00473     point_world_size_property_->hide();
00474     point_pixel_size_property_->show();
00475   }
00476   else
00477   {
00478     point_world_size_property_->show();
00479     point_pixel_size_property_->hide();
00480   }
00481   for( unsigned int i = 0; i < cloud_infos_.size(); i++ )
00482   {
00483     cloud_infos_[i]->cloud_->setRenderMode( mode );
00484   }
00485   updateBillboardSize();
00486 }
00487 
00488 void PointCloudCommon::updateBillboardSize()
00489 {
00490   PointCloud::RenderMode mode = (PointCloud::RenderMode) style_property_->getOptionInt();
00491   float size;
00492   if( mode == PointCloud::RM_POINTS ) {
00493     size = point_pixel_size_property_->getFloat();
00494   } else {
00495     size = point_world_size_property_->getFloat();
00496   }
00497   for ( unsigned i=0; i<cloud_infos_.size(); i++ )
00498   {
00499     cloud_infos_[i]->cloud_->setDimensions( size, size, size );
00500     cloud_infos_[i]->selection_handler_->setBoxSize( getSelectionBoxSize() );
00501   }
00502   context_->queueRender();
00503 }
00504 
00505 void PointCloudCommon::reset()
00506 {
00507   boost::mutex::scoped_lock lock(new_clouds_mutex_);
00508   cloud_infos_.clear();
00509   new_cloud_infos_.clear();
00510 }
00511 
00512 void PointCloudCommon::causeRetransform()
00513 {
00514   needs_retransform_ = true;
00515 }
00516 
00517 void PointCloudCommon::update(float wall_dt, float ros_dt)
00518 {
00519   PointCloud::RenderMode mode = (PointCloud::RenderMode) style_property_->getOptionInt();
00520 
00521   float point_decay_time = decay_time_property_->getFloat();
00522   if (needs_retransform_)
00523   {
00524     retransform();
00525     needs_retransform_ = false;
00526   }
00527 
00528   // instead of deleting cloud infos, we just clear them
00529   // and put them into obsolete_cloud_infos, so active selections
00530   // are preserved
00531 
00532   ros::Time now = ros::Time::now();
00533 
00534   // if decay time == 0, clear the old cloud when we get a new one
00535   // otherwise, clear all the outdated ones
00536   {
00537     boost::mutex::scoped_lock lock(new_clouds_mutex_);
00538     if ( point_decay_time > 0.0 || !new_cloud_infos_.empty() )
00539     {
00540       while( !cloud_infos_.empty() && now.toSec() - cloud_infos_.front()->receive_time_.toSec() > point_decay_time )
00541       {
00542         cloud_infos_.front()->clear();
00543         obsolete_cloud_infos_.push_back( cloud_infos_.front() );
00544         cloud_infos_.pop_front();
00545         context_->queueRender();
00546       }
00547     }
00548   }
00549 
00550   // garbage-collect old point clouds that don't have an active selection
00551   L_CloudInfo::iterator it = obsolete_cloud_infos_.begin();
00552   L_CloudInfo::iterator end = obsolete_cloud_infos_.end();
00553   for (; it != end; ++it)
00554   {
00555     if ( !(*it)->selection_handler_.get() ||
00556          !(*it)->selection_handler_->hasSelections() )
00557     {
00558       it = obsolete_cloud_infos_.erase(it);
00559     }
00560   }
00561 
00562   {
00563     boost::mutex::scoped_lock lock(new_clouds_mutex_);
00564     if( !new_cloud_infos_.empty() )
00565     {
00566       float size;
00567       if( mode == PointCloud::RM_POINTS ) {
00568         size = point_pixel_size_property_->getFloat();
00569       } else {
00570         size = point_world_size_property_->getFloat();
00571       }
00572 
00573       V_CloudInfo::iterator it = new_cloud_infos_.begin();
00574       V_CloudInfo::iterator end = new_cloud_infos_.end();
00575       for (; it != end; ++it)
00576       {
00577         CloudInfoPtr cloud_info = *it;
00578 
00579         V_CloudInfo::iterator next = it; next++;
00580         // ignore point clouds that are too old, but keep at least one
00581         if ( next != end && now.toSec() - cloud_info->receive_time_.toSec() > point_decay_time ) {
00582           continue;
00583         }
00584 
00585         cloud_info->cloud_.reset( new PointCloud() );
00586         cloud_info->cloud_->addPoints( &(cloud_info->transformed_points_.front()), cloud_info->transformed_points_.size() );
00587         cloud_info->cloud_->setRenderMode( mode );
00588         cloud_info->cloud_->setAlpha( alpha_property_->getFloat() );
00589         cloud_info->cloud_->setDimensions( size, size, size );
00590         cloud_info->cloud_->setAutoSize(auto_size_);
00591 
00592         cloud_info->manager_ = context_->getSceneManager();
00593 
00594         cloud_info->scene_node_ = scene_node_->createChildSceneNode( cloud_info->position_, cloud_info->orientation_ );
00595 
00596         cloud_info->scene_node_->attachObject( cloud_info->cloud_.get() );
00597 
00598         cloud_info->selection_handler_.reset( new PointCloudSelectionHandler( getSelectionBoxSize(), cloud_info.get(), context_ ));
00599 
00600         cloud_infos_.push_back(*it);
00601       }
00602 
00603       new_cloud_infos_.clear();
00604     }
00605   }
00606 
00607   {
00608     boost::recursive_mutex::scoped_try_lock lock( transformers_mutex_ );
00609 
00610     if( lock.owns_lock() )
00611     {
00612       if( new_xyz_transformer_ || new_color_transformer_ )
00613       {
00614         M_TransformerInfo::iterator it = transformers_.begin();
00615         M_TransformerInfo::iterator end = transformers_.end();
00616         for (; it != end; ++it)
00617         {
00618           const std::string& name = it->first;
00619           TransformerInfo& info = it->second;
00620 
00621           setPropertiesHidden( info.xyz_props, name != xyz_transformer_property_->getStdString() );
00622           setPropertiesHidden( info.color_props, name != color_transformer_property_->getStdString() );
00623         }
00624       }
00625     }
00626 
00627     new_xyz_transformer_ = false;
00628     new_color_transformer_ = false;
00629   }
00630 
00631   updateStatus();
00632 }
00633 
00634 void PointCloudCommon::setPropertiesHidden( const QList<Property*>& props, bool hide )
00635 {
00636   for( int i = 0; i < props.size(); i++ )
00637   {
00638     props[ i ]->setHidden( hide );
00639   }
00640 }
00641 
00642 void PointCloudCommon::updateTransformers( const sensor_msgs::PointCloud2ConstPtr& cloud )
00643 {
00644   std::string xyz_name = xyz_transformer_property_->getStdString();
00645   std::string color_name = color_transformer_property_->getStdString();
00646 
00647   xyz_transformer_property_->clearOptions();
00648   color_transformer_property_->clearOptions();
00649 
00650   // Get the channels that we could potentially render
00651   typedef std::set<std::pair<uint8_t, std::string> > S_string;
00652   S_string valid_xyz, valid_color;
00653   bool cur_xyz_valid = false;
00654   bool cur_color_valid = false;
00655   bool has_rgb_transformer = false;
00656   M_TransformerInfo::iterator trans_it = transformers_.begin();
00657   M_TransformerInfo::iterator trans_end = transformers_.end();
00658   for(;trans_it != trans_end; ++trans_it)
00659   {
00660     const std::string& name = trans_it->first;
00661     const PointCloudTransformerPtr& trans = trans_it->second.transformer;
00662     uint32_t mask = trans->supports(cloud);
00663     if (mask & PointCloudTransformer::Support_XYZ)
00664     {
00665       valid_xyz.insert(std::make_pair(trans->score(cloud), name));
00666       if (name == xyz_name)
00667       {
00668         cur_xyz_valid = true;
00669       }
00670       xyz_transformer_property_->addOptionStd( name );
00671     }
00672 
00673     if (mask & PointCloudTransformer::Support_Color)
00674     {
00675       valid_color.insert(std::make_pair(trans->score(cloud), name));
00676       if (name == color_name)
00677       {
00678         cur_color_valid = true;
00679       }
00680       if (name == "RGB8")
00681       {
00682         has_rgb_transformer = true;
00683       }
00684       color_transformer_property_->addOptionStd( name );
00685     }
00686   }
00687 
00688   if( !cur_xyz_valid )
00689   {
00690     if( !valid_xyz.empty() )
00691     {
00692       xyz_transformer_property_->setStringStd( valid_xyz.rbegin()->second );
00693     }
00694   }
00695 
00696   if( !cur_color_valid )
00697   {
00698     if( !valid_color.empty() )
00699     {
00700       if (has_rgb_transformer)
00701       {
00702         color_transformer_property_->setStringStd( "RGB8" );
00703       } else
00704       {
00705         color_transformer_property_->setStringStd( valid_color.rbegin()->second );
00706       }
00707     }
00708   }
00709 }
00710 
00711 void PointCloudCommon::updateStatus()
00712 {
00713   std::stringstream ss;
00714   //ss << "Showing [" << total_point_count_ << "] points from [" << clouds_.size() << "] messages";
00715   display_->setStatusStd(StatusProperty::Ok, "Points", ss.str());
00716 }
00717 
00718 void PointCloudCommon::processMessage(const sensor_msgs::PointCloud2ConstPtr& cloud)
00719 {
00720   CloudInfoPtr info(new CloudInfo);
00721   info->message_ = cloud;
00722   info->receive_time_ = ros::Time::now();
00723 
00724   if (transformCloud(info, true))
00725   {
00726     boost::mutex::scoped_lock lock(new_clouds_mutex_);
00727     new_cloud_infos_.push_back(info);
00728     display_->emitTimeSignal( cloud->header.stamp );
00729   }
00730 }
00731 
00732 void PointCloudCommon::updateXyzTransformer()
00733 {
00734   boost::recursive_mutex::scoped_lock lock( transformers_mutex_ );
00735   if( transformers_.count( xyz_transformer_property_->getStdString() ) == 0 )
00736   {
00737     return;
00738   }
00739   new_xyz_transformer_ = true;
00740   causeRetransform();
00741 }
00742 
00743 void PointCloudCommon::updateColorTransformer()
00744 {
00745   boost::recursive_mutex::scoped_lock lock( transformers_mutex_ );
00746   if( transformers_.count( color_transformer_property_->getStdString() ) == 0 )
00747   {
00748     return;
00749   }
00750   new_color_transformer_ = true;
00751   causeRetransform();
00752 }
00753 
00754 PointCloudTransformerPtr PointCloudCommon::getXYZTransformer( const sensor_msgs::PointCloud2ConstPtr& cloud )
00755 {
00756   boost::recursive_mutex::scoped_lock lock( transformers_mutex_);
00757   M_TransformerInfo::iterator it = transformers_.find( xyz_transformer_property_->getStdString() );
00758   if( it != transformers_.end() )
00759   {
00760     const PointCloudTransformerPtr& trans = it->second.transformer;
00761     if( trans->supports( cloud ) & PointCloudTransformer::Support_XYZ )
00762     {
00763       return trans;
00764     }
00765   }
00766 
00767   return PointCloudTransformerPtr();
00768 }
00769 
00770 PointCloudTransformerPtr PointCloudCommon::getColorTransformer( const sensor_msgs::PointCloud2ConstPtr& cloud )
00771 {
00772   boost::recursive_mutex::scoped_lock lock( transformers_mutex_ );
00773   M_TransformerInfo::iterator it = transformers_.find( color_transformer_property_->getStdString() );
00774   if( it != transformers_.end() )
00775   {
00776     const PointCloudTransformerPtr& trans = it->second.transformer;
00777     if( trans->supports( cloud ) & PointCloudTransformer::Support_Color )
00778     {
00779       return trans;
00780     }
00781   }
00782 
00783   return PointCloudTransformerPtr();
00784 }
00785 
00786 
00787 void PointCloudCommon::retransform()
00788 {
00789   boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
00790 
00791   D_CloudInfo::iterator it = cloud_infos_.begin();
00792   D_CloudInfo::iterator end = cloud_infos_.end();
00793   for (; it != end; ++it)
00794   {
00795     const CloudInfoPtr& cloud_info = *it;
00796     transformCloud(cloud_info, false);
00797     cloud_info->cloud_->clear();
00798     cloud_info->cloud_->addPoints(&cloud_info->transformed_points_.front(), cloud_info->transformed_points_.size());
00799   }
00800 }
00801 
00802 bool PointCloudCommon::transformCloud(const CloudInfoPtr& cloud_info, bool update_transformers)
00803 {
00804 
00805   if ( !cloud_info->scene_node_ )
00806   {
00807     if (!context_->getFrameManager()->getTransform(cloud_info->message_->header, cloud_info->position_, cloud_info->orientation_))
00808     {
00809       std::stringstream ss;
00810       ss << "Failed to transform from frame [" << cloud_info->message_->header.frame_id << "] to frame [" << context_->getFrameManager()->getFixedFrame() << "]";
00811       display_->setStatusStd(StatusProperty::Error, "Message", ss.str());
00812       return false;
00813     }
00814   }
00815 
00816   Ogre::Matrix4 transform;
00817   transform.makeTransform( cloud_info->position_, Ogre::Vector3(1,1,1), cloud_info->orientation_ );
00818 
00819   V_PointCloudPoint& cloud_points = cloud_info->transformed_points_;
00820   cloud_points.clear();
00821 
00822   size_t size = cloud_info->message_->width * cloud_info->message_->height;
00823   PointCloud::Point default_pt;
00824   default_pt.color = Ogre::ColourValue(1, 1, 1);
00825   default_pt.position = Ogre::Vector3::ZERO;
00826   cloud_points.resize(size, default_pt);
00827 
00828   {
00829     boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
00830     if( update_transformers )
00831     {
00832       updateTransformers( cloud_info->message_ );
00833     }
00834     PointCloudTransformerPtr xyz_trans = getXYZTransformer(cloud_info->message_);
00835     PointCloudTransformerPtr color_trans = getColorTransformer(cloud_info->message_);
00836 
00837     if (!xyz_trans)
00838     {
00839       std::stringstream ss;
00840       ss << "No position transformer available for cloud";
00841       display_->setStatusStd(StatusProperty::Error, "Message", ss.str());
00842       return false;
00843     }
00844 
00845     if (!color_trans)
00846     {
00847       std::stringstream ss;
00848       ss << "No color transformer available for cloud";
00849       display_->setStatusStd(StatusProperty::Error, "Message", ss.str());
00850       return false;
00851     }
00852 
00853     xyz_trans->transform(cloud_info->message_, PointCloudTransformer::Support_XYZ, transform, cloud_points);
00854     color_trans->transform(cloud_info->message_, PointCloudTransformer::Support_Color, transform, cloud_points);
00855   }
00856 
00857   for (V_PointCloudPoint::iterator cloud_point = cloud_points.begin(); cloud_point != cloud_points.end(); ++cloud_point)
00858   {
00859     if (!validateFloats(cloud_point->position))
00860     {
00861       cloud_point->position.x = 999999.0f;
00862       cloud_point->position.y = 999999.0f;
00863       cloud_point->position.z = 999999.0f;
00864     }
00865   }
00866 
00867   return true;
00868 }
00869 
00870 bool convertPointCloudToPointCloud2(const sensor_msgs::PointCloud& input, sensor_msgs::PointCloud2& output)
00871 {
00872   output.header = input.header;
00873   output.width  = input.points.size ();
00874   output.height = 1;
00875   output.fields.resize (3 + input.channels.size ());
00876   // Convert x/y/z to fields
00877   output.fields[0].name = "x"; output.fields[1].name = "y"; output.fields[2].name = "z";
00878   int offset = 0;
00879   // All offsets are *4, as all field data types are float32
00880   for (size_t d = 0; d < output.fields.size (); ++d, offset += 4)
00881   {
00882     output.fields[d].offset = offset;
00883     output.fields[d].datatype = sensor_msgs::PointField::FLOAT32;
00884   }
00885   output.point_step = offset;
00886   output.row_step   = output.point_step * output.width;
00887   // Convert the remaining of the channels to fields
00888   for (size_t d = 0; d < input.channels.size (); ++d)
00889     output.fields[3 + d].name = input.channels[d].name;
00890   output.data.resize (input.points.size () * output.point_step);
00891   output.is_bigendian = false;  // @todo ?
00892   output.is_dense     = false;
00893 
00894   // Copy the data points
00895   for (size_t cp = 0; cp < input.points.size (); ++cp)
00896   {
00897     memcpy (&output.data[cp * output.point_step + output.fields[0].offset], &input.points[cp].x, sizeof (float));
00898     memcpy (&output.data[cp * output.point_step + output.fields[1].offset], &input.points[cp].y, sizeof (float));
00899     memcpy (&output.data[cp * output.point_step + output.fields[2].offset], &input.points[cp].z, sizeof (float));
00900     for (size_t d = 0; d < input.channels.size (); ++d)
00901     {
00902       if (input.channels[d].values.size() == input.points.size())
00903       {
00904         memcpy (&output.data[cp * output.point_step + output.fields[3 + d].offset], &input.channels[d].values[cp], sizeof (float));
00905       }
00906     }
00907   }
00908   return (true);
00909 }
00910 
00911 void PointCloudCommon::addMessage(const sensor_msgs::PointCloudConstPtr& cloud)
00912 {
00913   sensor_msgs::PointCloud2Ptr out(new sensor_msgs::PointCloud2);
00914   convertPointCloudToPointCloud2(*cloud, *out);
00915   addMessage(out);
00916 }
00917 
00918 void PointCloudCommon::addMessage(const sensor_msgs::PointCloud2ConstPtr& cloud)
00919 {
00920   if (cloud->width * cloud->height == 0)
00921   {
00922     return;
00923   }
00924 
00925   processMessage(cloud);
00926 }
00927 
00928 void PointCloudCommon::fixedFrameChanged()
00929 {
00930   reset();
00931 }
00932 
00933 void PointCloudCommon::setXyzTransformerOptions( EnumProperty* prop )
00934 {
00935   fillTransformerOptions( prop, PointCloudTransformer::Support_XYZ );
00936 }
00937 
00938 void PointCloudCommon::setColorTransformerOptions( EnumProperty* prop )
00939 {
00940   fillTransformerOptions( prop, PointCloudTransformer::Support_Color );
00941 }
00942 
00943 void PointCloudCommon::fillTransformerOptions( EnumProperty* prop, uint32_t mask )
00944 {
00945   prop->clearOptions();
00946 
00947   if (cloud_infos_.empty())
00948   {
00949     return;
00950   }
00951 
00952   boost::recursive_mutex::scoped_lock tlock(transformers_mutex_);
00953 
00954   const sensor_msgs::PointCloud2ConstPtr& msg = cloud_infos_.front()->message_;
00955 
00956   M_TransformerInfo::iterator it = transformers_.begin();
00957   M_TransformerInfo::iterator end = transformers_.end();
00958   for (; it != end; ++it)
00959   {
00960     const PointCloudTransformerPtr& trans = it->second.transformer;
00961     if ((trans->supports(msg) & mask) == mask)
00962     {
00963       prop->addOption( QString::fromStdString( it->first ));
00964     }
00965   }
00966 }
00967 
00968 float PointCloudCommon::getSelectionBoxSize()
00969 {
00970   if( style_property_->getOptionInt() != PointCloud::RM_POINTS )
00971   {
00972     return point_world_size_property_->getFloat();
00973   }
00974   else
00975   {
00976     return 0.004;
00977   }
00978 }
00979 
00980 } // namespace rviz


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Aug 27 2015 15:02:27