00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
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
00178 VectorProperty* pos_prop = new VectorProperty( "Position", cloud_info_->transformed_points_[index].position, "", cat );
00179 pos_prop->setReadOnly( true );
00180
00181
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
00529
00530
00531
00532 ros::Time now = ros::Time::now();
00533
00534
00535
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
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
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
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
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
00877 output.fields[0].name = "x"; output.fields[1].name = "y"; output.fields[2].name = "z";
00878 int offset = 0;
00879
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
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;
00892 output.is_dense = false;
00893
00894
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 }