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 <OGRE/OgreSceneManager.h>
00031 #include <OGRE/OgreSceneNode.h>
00032 #include <OGRE/OgreWireBoundingBox.h>
00033
00034 #include <ros/time.h>
00035
00036 #include <tf/transform_listener.h>
00037
00038 #include <pluginlib/class_loader.h>
00039
00040 #include "rviz/default_plugin/point_cloud_transformer.h"
00041 #include "rviz/default_plugin/point_cloud_transformers.h"
00042 #include "rviz/display_context.h"
00043 #include "rviz/frame_manager.h"
00044 #include "rviz/ogre_helpers/point_cloud.h"
00045 #include "rviz/properties/property.h"
00046 #include "rviz/properties/property_manager.h"
00047 #include "rviz/selection/selection_manager.h"
00048 #include "rviz/uniform_string_stream.h"
00049 #include "rviz/validate_floats.h"
00050
00051 #include "rviz/default_plugin/point_cloud_base.h"
00052
00053 namespace rviz
00054 {
00055
00056 template<typename T>
00057 T getValue(const T& val)
00058 {
00059 return val;
00060 }
00061
00062 struct IndexAndMessage
00063 {
00064 int index;
00065 void* message;
00066 };
00067
00068 uint qHash( IndexAndMessage iam )
00069 {
00070 return
00071 ((uint) iam.index) +
00072 ((uint) (iam.message >> 32)) +
00073 ((uint) (iam.message & 0xffffffff));
00074 }
00075
00076 bool operator==( IndexAndMessage a, IndexAndMessage b )
00077 {
00078 return a.index == b.index && a.message == b.message;
00079 }
00080
00081 class PointCloudSelectionHandler: public SelectionHandler
00082 {
00083 public:
00084 PointCloudSelectionHandler(PointCloudBase* display);
00085 virtual ~PointCloudSelectionHandler();
00086
00087 virtual void createProperties( const Picked& obj, Property* parent_property );
00088 virtual void destroyProperties( const Picked& obj, Property* parent_property );
00089
00090 virtual bool needsAdditionalRenderPass(uint32_t pass)
00091 {
00092 if (pass < 2)
00093 {
00094 return true;
00095 }
00096
00097 return false;
00098 }
00099
00100 virtual void preRenderPass(uint32_t pass);
00101 virtual void postRenderPass(uint32_t pass);
00102
00103 virtual void onSelect(const Picked& obj);
00104 virtual void onDeselect(const Picked& obj);
00105
00106 virtual void getAABBs(const Picked& obj, V_AABB& aabbs);
00107
00108 private:
00109 void getCloudAndLocalIndexByGlobalIndex(int global_index, PointCloudBase::CloudInfoPtr& cloud_out, int& index_out);
00110
00111 PointCloudBase* display_;
00112 QHash<IndexAndMessage, Property*> property_hash_;
00113 };
00114
00115 PointCloudSelectionHandler::PointCloudSelectionHandler(PointCloudBase* display)
00116 : display_(display)
00117 {
00118 }
00119
00120 PointCloudSelectionHandler::~PointCloudSelectionHandler()
00121 {
00122 }
00123
00124 void PointCloudSelectionHandler::preRenderPass(uint32_t pass)
00125 {
00126 SelectionHandler::preRenderPass(pass);
00127
00128 if (pass == 1)
00129 {
00130 display_->cloud_->setColorByIndex(true);
00131 }
00132 }
00133
00134 void PointCloudSelectionHandler::postRenderPass(uint32_t pass)
00135 {
00136 SelectionHandler::postRenderPass(pass);
00137
00138 if (pass == 1)
00139 {
00140 display_->cloud_->setColorByIndex(false);
00141 }
00142 }
00143
00144 void PointCloudSelectionHandler::getCloudAndLocalIndexByGlobalIndex(int global_index, PointCloudBase::CloudInfoPtr& cloud_out, int& index_out)
00145 {
00146 boost::mutex::scoped_lock lock(display_->clouds_mutex_);
00147
00148 int count = 0;
00149
00150 PointCloudBase::D_CloudInfo::iterator cloud_it = display_->clouds_.begin();
00151 PointCloudBase::D_CloudInfo::iterator cloud_end = display_->clouds_.end();
00152 for (;cloud_it != cloud_end; ++cloud_it)
00153 {
00154 const PointCloudBase::CloudInfoPtr& info = *cloud_it;
00155
00156 if (global_index < count + (int)info->num_points_)
00157 {
00158 index_out = global_index - count;
00159 cloud_out = info;
00160
00161 return;
00162 }
00163
00164 count += info->message_->width * info->message_->height;
00165 }
00166 }
00167
00168 Ogre::Vector3 pointFromCloud(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t index)
00169 {
00170 int32_t xi = findChannelIndex(cloud, "x");
00171 int32_t yi = findChannelIndex(cloud, "y");
00172 int32_t zi = findChannelIndex(cloud, "z");
00173
00174 const uint32_t xoff = cloud->fields[xi].offset;
00175 const uint32_t yoff = cloud->fields[yi].offset;
00176 const uint32_t zoff = cloud->fields[zi].offset;
00177 const uint8_t type = cloud->fields[xi].datatype;
00178 const uint32_t point_step = cloud->point_step;
00179 float x = valueFromCloud<float>(cloud, xoff, type, point_step, index);
00180 float y = valueFromCloud<float>(cloud, yoff, type, point_step, index);
00181 float z = valueFromCloud<float>(cloud, zoff, type, point_step, index);
00182 return Ogre::Vector3(x, y, z);
00183 }
00184
00185 void PointCloudSelectionHandler::createProperties( const Picked& obj, Property* parent_property )
00186 {
00187 typedef std::set<int> S_int;
00188 S_int indices;
00189 {
00190 S_uint64::const_iterator it = obj.extra_handles.begin();
00191 S_uint64::const_iterator end = obj.extra_handles.end();
00192 for (; it != end; ++it)
00193 {
00194 uint64_t handle = *it;
00195 indices.insert((handle & 0xffffffff) - 1);
00196 }
00197 }
00198
00199 {
00200 S_int::iterator it = indices.begin();
00201 S_int::iterator end = indices.end();
00202 for (; it != end; ++it)
00203 {
00204 int global_index = *it;
00205 int index = 0;
00206 PointCloudBase::CloudInfoPtr cloud;
00207
00208 getCloudAndLocalIndexByGlobalIndex(global_index, cloud, index);
00209
00210 if (!cloud)
00211 {
00212 continue;
00213 }
00214
00215 const sensor_msgs::PointCloud2ConstPtr& message = cloud->message_;
00216
00217 IndexAndMessage hash_key;
00218 hash_key.index = index;
00219 hash_key.message = message.get();
00220
00221 if( !property_hash_.contains( hash_key ))
00222 {
00223 Property* cat = new Property( QString( "Point %1 [cloud 0x%2]" ).arg( index ).arg( (uint64_t) message.get() ),
00224 QVariant(), "", parent_property );
00225 property_hash_.insert( hash_key, cat );
00226
00227
00228 VectorProperty* pos_prop = new VectorProperty( "Position", cloud->transformed_points_[index].position, "", cat );
00229 pos_prop->setReadOnly( true );
00230
00231
00232 for( size_t field = 0; field < message->fields.size(); ++field )
00233 {
00234 const sensor_msgs::PointField& f = message->fields[ field ];
00235 const std::string& name = f.name;
00236
00237 if( name == "x" || name == "y" || name == "z" || name == "X" || name == "Y" || name == "Z" )
00238 {
00239 continue;
00240 }
00241
00242 float val = valueFromCloud<float>( message, f.offset, f.datatype, message->point_step, index );
00243
00244 FloatProperty* prop = new FloatProperty( QString( "%1: %2" ).arg( field ).arg( QString::fromStdString( name )),
00245 val, "", cat );
00246 prop->setReadOnly( true );
00247 }
00248 }
00249 }
00250 }
00251 }
00252
00253 void PointCloudSelectionHandler::destroyProperties( const Picked& obj, Property* parent_property )
00254 {
00255 typedef std::set<int> S_int;
00256 S_int indices;
00257 {
00258 S_uint64::const_iterator it = obj.extra_handles.begin();
00259 S_uint64::const_iterator end = obj.extra_handles.end();
00260 for (; it != end; ++it)
00261 {
00262 uint64_t handle = *it;
00263 indices.insert((handle & 0xffffffff) - 1);
00264 }
00265 }
00266
00267 {
00268 S_int::iterator it = indices.begin();
00269 S_int::iterator end = indices.end();
00270 for (; it != end; ++it)
00271 {
00272 int global_index = *it;
00273 int index = 0;
00274 PointCloudBase::CloudInfoPtr cloud;
00275
00276 getCloudAndLocalIndexByGlobalIndex(global_index, cloud, index);
00277
00278 if (!cloud)
00279 {
00280 continue;
00281 }
00282
00283 const sensor_msgs::PointCloud2ConstPtr& message = cloud->message_;
00284
00285 IndexAndMessage hash_key;
00286 hash_key.index = index;
00287 hash_key.message = message.get();
00288
00289 Property* prop = property_hash_.take( hash_key );
00290 delete prop;
00291 }
00292 }
00293 }
00294
00295 void PointCloudSelectionHandler::getAABBs(const Picked& obj, V_AABB& aabbs)
00296 {
00297 S_uint64::iterator it = obj.extra_handles.begin();
00298 S_uint64::iterator end = obj.extra_handles.end();
00299 for (; it != end; ++it)
00300 {
00301 M_HandleToBox::iterator find_it = boxes_.find(std::make_pair(obj.handle, *it - 1));
00302 if (find_it != boxes_.end())
00303 {
00304 Ogre::WireBoundingBox* box = find_it->second.second;
00305
00306 aabbs.push_back(box->getWorldBoundingBox());
00307 }
00308 }
00309 }
00310
00311 void PointCloudSelectionHandler::onSelect(const Picked& obj)
00312 {
00313 S_uint64::iterator it = obj.extra_handles.begin();
00314 S_uint64::iterator end = obj.extra_handles.end();
00315 for (; it != end; ++it)
00316 {
00317 int global_index = (*it & 0xffffffff) - 1;
00318
00319 int index = 0;
00320 PointCloudBase::CloudInfoPtr cloud;
00321
00322 getCloudAndLocalIndexByGlobalIndex(global_index, cloud, index);
00323
00324 if (!cloud)
00325 {
00326 continue;
00327 }
00328
00329 sensor_msgs::PointCloud2ConstPtr message = cloud->message_;
00330
00331 Ogre::Vector3 pos = cloud->transform_ * pointFromCloud(message, index);
00332
00333 float size = display_->getSelectionBoxSize() * 0.5f;
00334
00335 Ogre::AxisAlignedBox aabb(pos - size, pos + size);
00336
00337 createBox(std::make_pair(obj.handle, global_index), aabb, "RVIZ/Cyan");
00338 }
00339 }
00340
00341 void PointCloudSelectionHandler::onDeselect(const Picked& obj)
00342 {
00343 S_uint64::iterator it = obj.extra_handles.begin();
00344 S_uint64::iterator end = obj.extra_handles.end();
00345 for (; it != end; ++it)
00346 {
00347 int global_index = (*it & 0xffffffff) - 1;
00348
00349 destroyBox(std::make_pair(obj.handle, global_index));
00350 }
00351 }
00352
00353 PointCloudBase::CloudInfo::CloudInfo()
00354 : time_(0.0f)
00355 , transform_(Ogre::Matrix4::ZERO)
00356 , num_points_(0)
00357 {}
00358
00359 PointCloudBase::CloudInfo::~CloudInfo()
00360 {
00361 }
00362
00363 PointCloudBase::PointCloudBase()
00364 : Display()
00365 , spinner_(1, &cbqueue_)
00366 , new_cloud_(false)
00367 , new_xyz_transformer_(false)
00368 , new_color_transformer_(false)
00369 , needs_retransform_(false)
00370 , billboard_size_( 0.01 )
00371 , coll_handle_(0)
00372 , messages_received_(0)
00373 , total_point_count_(0)
00374 , transformer_class_loader_( new pluginlib::ClassLoader<PointCloudTransformer>( "rviz", "rviz::PointCloudTransformer" ))
00375 {
00376 cloud_ = new PointCloud();
00377
00378 selectable_property_ = new BoolProperty( "Selectable", true,
00379 "Whether or not the points in this point cloud are selectable.",
00380 this, SLOT( updateSelectable() ));
00381
00382 style_property_ = new EnumProperty( "Style", "Billboards",
00383 "Rendering mode to use, in order of computational complexity.",
00384 this, SLOT( updateStyle() ));
00385 style_property_->addOption( "Points", PointCloud::RM_POINTS );
00386 style_property_->addOption( "Billboards", PointCloud::RM_BILLBOARDS );
00387 style_property_->addOption( "Billboard Spheres", PointCloud::RM_BILLBOARD_SPHERES );
00388 style_property_->addOption( "Boxes", PointCloud::RM_BOXES );
00389
00390 billboard_size_property_ = new FloatProperty( "Billboard Size", 0.01,
00391 "Length, in meters, of the side of each billboard (or face if using the Boxes style).",
00392 this, SLOT( updateBillboardSize() ));
00393 billboard_size_property_->setMin( 0.0001 );
00394
00395 alpha_property_ = new FloatProperty( "Alpha", 1.0,
00396 "Amount of transparency to apply to the points. Note that this is experimental and does not always look correct.",
00397 this, SLOT( updateAlpha() ));
00398 alpha_property_->setMin( 0 );
00399 alpha_property_->setMax( 1 );
00400
00401 decay_time_property_ = new FloatProperty( "Decay Time", 0,
00402 "Duration, in seconds, to keep the incoming points. 0 means only show the latest points.",
00403 this );
00404 decay_time_property_->setMin( 0 );
00405
00406 xyz_transformer_property_ = new EnumProperty( "Position Transformer", "",
00407 "Set the transformer to use to set the position of the points.",
00408 this, SLOT( updateXyzTransformer() ));
00409 connect( xyz_transformer_property_, SIGNAL( requestOptions( EnumProperty* )),
00410 this, SLOT( setXyzTransformerOptions( EnumProperty* )));
00411
00412 color_transformer_property_ = new EnumProperty( "Color Transformer", "",
00413 "Set the transformer to use to set the color of the points.",
00414 this, SLOT( updateColorTransformer() ));
00415 connect( color_transformer_property_, SIGNAL( requestOptions( EnumProperty* )),
00416 this, SLOT( setColorTransformerOptions( EnumProperty* )));
00417
00418 loadTransformers();
00419 }
00420
00421 void PointCloudBase::onInitialize()
00422 {
00423 scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00424 scene_node_->attachObject(cloud_);
00425 coll_handler_ = PointCloudSelectionHandlerPtr(new PointCloudSelectionHandler(this));
00426
00427 updateStyle();
00428 updateBillboardSize();
00429 updateAlpha();
00430 updateSelectable();
00431
00432 connect( decay_time_property_, SIGNAL( changed() ), context_, SLOT( queueRender() ));
00433
00434 threaded_nh_.setCallbackQueue(&cbqueue_);
00435 spinner_.start();
00436 }
00437
00438 PointCloudBase::~PointCloudBase()
00439 {
00440 spinner_.stop();
00441
00442 if (coll_handle_)
00443 {
00444 SelectionManager* sel_manager = context_->getSelectionManager();
00445 sel_manager->removeObject(coll_handle_);
00446 }
00447
00448 scene_manager_->destroySceneNode(scene_node_->getName());
00449 delete cloud_;
00450 delete transformer_class_loader_;
00451 }
00452
00453 void PointCloudBase::loadTransformers()
00454 {
00455 std::vector<std::string> classes = transformer_class_loader_->getDeclaredClasses();
00456 std::vector<std::string>::iterator ci;
00457
00458 for( ci = classes.begin(); ci != classes.end(); ci++ )
00459 {
00460 const std::string& lookup_name = *ci;
00461 std::string name = transformer_class_loader_->getName( lookup_name );
00462
00463 if( transformers_.count( name ) > 0 )
00464 {
00465 ROS_ERROR( "Transformer type [%s] is already loaded.", name.c_str() );
00466 continue;
00467 }
00468
00469 PointCloudTransformerPtr trans( transformer_class_loader_->createClassInstance( lookup_name, true ));
00470 trans->init( boost::bind( &PointCloudBase::causeRetransform, this ));
00471 TransformerInfo info;
00472 info.transformer = trans;
00473 info.readable_name = name;
00474 info.lookup_name = lookup_name;
00475 transformers_[ name ] = info;
00476
00477 info.transformer->createProperties( this, PointCloudTransformer::Support_XYZ, info.xyz_props );
00478 setPropertiesHidden( info.xyz_props, true );
00479 info.transformer->createProperties( this, PointCloudTransformer::Support_Color, info.color_props );
00480 setPropertiesHidden( info.color_props, true );
00481 }
00482 }
00483
00484 void PointCloudBase::updateAlpha()
00485 {
00486 cloud_->setAlpha( alpha_property_->getFloat() );
00487 }
00488
00489 void PointCloudBase::updateSelectable()
00490 {
00491 bool selectable = selectable_property_->getBool();
00492
00493 SelectionManager* sel_manager = context_->getSelectionManager();
00494
00495 if( selectable )
00496 {
00497 coll_handle_ = sel_manager->createHandle();
00498
00499 sel_manager->addObject( coll_handle_, coll_handler_ );
00500
00501
00502 float r = ((coll_handle_ >> 16) & 0xff) / 255.0f;
00503 float g = ((coll_handle_ >> 8) & 0xff) / 255.0f;
00504 float b = (coll_handle_ & 0xff) / 255.0f;
00505 Ogre::ColourValue col( r, g, b, 1.0f );
00506 cloud_->setPickColor( col );
00507 }
00508 else
00509 {
00510 sel_manager->removeObject( coll_handle_ );
00511 coll_handle_ = 0;
00512 cloud_->setPickColor( Ogre::ColourValue( 0.0f, 0.0f, 0.0f, 0.0f ));
00513 }
00514 }
00515
00516 void PointCloudBase::updateStyle()
00517 {
00518 PointCloud::RenderMode mode = style_property_->getOptionInt();
00519 if( mode == PointCloud::RM_POINTS )
00520 {
00521 billboard_size_property_->hide();
00522 }
00523 else
00524 {
00525 billboard_size_property_->show();
00526 }
00527 cloud_->setRenderMode( mode );
00528 context_->queueRender();
00529 }
00530
00531 void PointCloudBase::updateBillboardSize()
00532 {
00533 float size = billboard_size_property_->getFloat();
00534 cloud_->setDimensions( size, size, size );
00535 context_->queueRender();
00536 }
00537
00538 void PointCloudBase::onEnable()
00539 {
00540 }
00541
00542 void PointCloudBase::onDisable()
00543 {
00544 clouds_.clear();
00545 cloud_->clear();
00546 messages_received_ = 0;
00547 total_point_count_ = 0;
00548 }
00549
00550 void PointCloudBase::causeRetransform()
00551 {
00552 boost::mutex::scoped_lock lock(clouds_mutex_);
00553 needs_retransform_ = true;
00554 }
00555
00556 void PointCloudBase::update(float wall_dt, float ros_dt)
00557 {
00558 float point_decay_time = decay_time_property_->getFloat();
00559 {
00560 boost::mutex::scoped_lock lock(clouds_mutex_);
00561
00562 if (needs_retransform_)
00563 {
00564 retransform();
00565 needs_retransform_ = false;
00566 }
00567
00568 D_CloudInfo::iterator cloud_it = clouds_.begin();
00569 D_CloudInfo::iterator cloud_end = clouds_.end();
00570 for (;cloud_it != cloud_end; ++cloud_it)
00571 {
00572 const CloudInfoPtr& info = *cloud_it;
00573
00574 info->time_ += ros_dt;
00575 }
00576
00577 if( point_decay_time > 0.0f )
00578 {
00579 bool removed = false;
00580 uint32_t points_to_pop = 0;
00581 while( !clouds_.empty() && clouds_.front()->time_ > point_decay_time )
00582 {
00583 total_point_count_ -= clouds_.front()->num_points_;
00584 points_to_pop += clouds_.front()->num_points_;
00585 clouds_.pop_front();
00586 removed = true;
00587 }
00588
00589 if (removed)
00590 {
00591 cloud_->popPoints(points_to_pop);
00592 context_->queueRender();
00593 }
00594 }
00595 }
00596
00597 if( new_cloud_ )
00598 {
00599 boost::mutex::scoped_lock lock(new_clouds_mutex_);
00600
00601 if( point_decay_time == 0.0f )
00602 {
00603 clouds_.clear();
00604 cloud_->clear();
00605
00606 ROS_ASSERT(!new_points_.empty());
00607 ROS_ASSERT(!new_clouds_.empty());
00608 V_Point& points = new_points_.back();
00609 cloud_->addPoints(&points.front(), points.size());
00610 clouds_.push_back(new_clouds_.back());
00611
00612 total_point_count_ = points.size();
00613 }
00614 else
00615 {
00616 {
00617 VV_Point::iterator it = new_points_.begin();
00618 VV_Point::iterator end = new_points_.end();
00619 for (; it != end; ++it)
00620 {
00621 V_Point& points = *it;
00622 total_point_count_ += points.size();
00623 cloud_->addPoints( &points.front(), points.size() );
00624 }
00625 }
00626
00627 {
00628 V_CloudInfo::iterator it = new_clouds_.begin();
00629 V_CloudInfo::iterator end = new_clouds_.end();
00630 for (; it != end; ++it)
00631 {
00632 clouds_.push_back(*it);
00633 }
00634 }
00635 }
00636
00637 new_clouds_.clear();
00638 new_points_.clear();
00639 new_cloud_ = false;
00640 }
00641
00642 {
00643 boost::recursive_mutex::scoped_try_lock lock( transformers_mutex_ );
00644
00645 if( lock.owns_lock() )
00646 {
00647 if( new_xyz_transformer_ || new_color_transformer_ )
00648 {
00649 M_TransformerInfo::iterator it = transformers_.begin();
00650 M_TransformerInfo::iterator end = transformers_.end();
00651 for (; it != end; ++it)
00652 {
00653 const std::string& name = it->first;
00654 TransformerInfo& info = it->second;
00655
00656 setPropertiesHidden( info.xyz_props, name != getXYZTransformer() );
00657 setPropertiesHidden( info.color_props, name != getColorTransformer() );
00658 }
00659 }
00660 }
00661
00662 new_xyz_transformer_ = false;
00663 new_color_transformer_ = false;
00664 }
00665
00666 updateStatus();
00667 }
00668
00669 void PointCloudBase::setPropertiesHidden( const QList<Property*>& props, bool hide )
00670 {
00671 for( int i = 0; i < props.size(); i++ )
00672 {
00673 props[ i ]->setHidden( hide );
00674 }
00675 }
00676
00677 void PointCloudBase::updateTransformers( const sensor_msgs::PointCloud2ConstPtr& cloud )
00678 {
00679 std::string xyz_name = xyz_transformer_property_->getStdString();
00680 std::string color_name = color_transformer_property_->getStdString();
00681
00682 xyz_transformer_property_->clearOptions();
00683 color_transformer_property_->clearOptions();
00684
00685
00686 typedef std::set<std::pair<uint8_t, std::string> > S_string;
00687 S_string valid_xyz, valid_color;
00688 bool cur_xyz_valid = false;
00689 bool cur_color_valid = false;
00690 M_TransformerInfo::iterator trans_it = transformers_.begin();
00691 M_TransformerInfo::iterator trans_end = transformers_.end();
00692 for(;trans_it != trans_end; ++trans_it)
00693 {
00694 const std::string& name = trans_it->first;
00695 const PointCloudTransformerPtr& trans = trans_it->second.transformer;
00696 uint32_t mask = trans->supports(cloud);
00697 if (mask & PointCloudTransformer::Support_XYZ)
00698 {
00699 valid_xyz.insert(std::make_pair(trans->score(cloud), name));
00700 if (name == xyz_name)
00701 {
00702 cur_xyz_valid = true;
00703 }
00704 xyz_transformer_property_->addOptionStd( name );
00705 }
00706
00707 if (mask & PointCloudTransformer::Support_Color)
00708 {
00709 valid_color.insert(std::make_pair(trans->score(cloud), name));
00710 if (name == color_name)
00711 {
00712 cur_color_valid = true;
00713 }
00714 color_transformer_property_->addOptionStd( name );
00715 }
00716 }
00717
00718 if( !cur_xyz_valid )
00719 {
00720 if( !valid_xyz.empty() )
00721 {
00722 xyz_transformer_property_->setStringStd( valid_xyz.rbegin()->second );
00723 }
00724 }
00725
00726 if( !cur_color_valid )
00727 {
00728 if( !valid_color.empty() )
00729 {
00730 color_transformer_property_->setStringStd( valid_color.rbegin()->second );
00731 }
00732 }
00733 }
00734
00735 void PointCloudBase::updateStatus()
00736 {
00737 if (messages_received_ == 0)
00738 {
00739 setStatus(StatusProperty::Warn, "Topic", "No messages received");
00740 }
00741 else
00742 {
00743 std::stringstream ss;
00744 ss << messages_received_ << " messages received";
00745 setStatusStd(StatusProperty::Ok, "Topic", ss.str());
00746 }
00747
00748 {
00749 std::stringstream ss;
00750 ss << "Showing [" << total_point_count_ << "] points from [" << clouds_.size() << "] messages";
00751 setStatusStd(StatusProperty::Ok, "Points", ss.str());
00752 }
00753 }
00754
00755 void PointCloudBase::processMessage(const sensor_msgs::PointCloud2ConstPtr& cloud)
00756 {
00757 CloudInfoPtr info(new CloudInfo);
00758 info->message_ = cloud;
00759 info->time_ = 0;
00760
00761 V_Point points;
00762 if (transformCloud(info, points, true))
00763 {
00764 boost::mutex::scoped_lock lock(new_clouds_mutex_);
00765
00766 new_clouds_.push_back(info);
00767 new_points_.push_back(V_Point());
00768 new_points_.back().swap(points);
00769
00770 new_cloud_ = true;
00771 }
00772 }
00773
00774 void PointCloudBase::updateXyzTransformer()
00775 {
00776 boost::recursive_mutex::scoped_lock lock( transformers_mutex_ );
00777 if( transformers_.count( xyz_transformer_property_->getStdString() ) == 0 )
00778 {
00779 return;
00780 }
00781 new_xyz_transformer_ = true;
00782 causeRetransform();
00783 }
00784
00785 void PointCloudBase::updateColorTransformer()
00786 {
00787 boost::recursive_mutex::scoped_lock lock( transformers_mutex_ );
00788 if( transformers_.count( color_transformer_property_->getStdString() ) == 0 )
00789 {
00790 return;
00791 }
00792 new_color_transformer_ = true;
00793 causeRetransform();
00794 }
00795
00796 PointCloudTransformerPtr PointCloudBase::getXYZTransformer( const sensor_msgs::PointCloud2ConstPtr& cloud )
00797 {
00798 boost::recursive_mutex::scoped_lock lock( transformers_mutex_);
00799 M_TransformerInfo::iterator it = transformers_.find( xyz_transformer_property_->getStdString() );
00800 if( it != transformers_.end() )
00801 {
00802 const PointCloudTransformerPtr& trans = it->second.transformer;
00803 if( trans->supports( cloud ) & PointCloudTransformer::Support_XYZ )
00804 {
00805 return trans;
00806 }
00807 }
00808
00809 return PointCloudTransformerPtr();
00810 }
00811
00812 PointCloudTransformerPtr PointCloudBase::getColorTransformer( const sensor_msgs::PointCloud2ConstPtr& cloud )
00813 {
00814 boost::recursive_mutex::scoped_lock lock( transformers_mutex_ );
00815 M_TransformerInfo::iterator it = transformers_.find( color_transformer_property_->getStdString() );
00816 if( it != transformers_.end() )
00817 {
00818 const PointCloudTransformerPtr& trans = it->second.transformer;
00819 if( trans->supports( cloud ) & PointCloudTransformer::Support_Color )
00820 {
00821 return trans;
00822 }
00823 }
00824
00825 return PointCloudTransformerPtr();
00826 }
00827
00828 void PointCloudBase::retransform()
00829 {
00830 boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
00831
00832 cloud_->clear();
00833
00834 D_CloudInfo::iterator it = clouds_.begin();
00835 D_CloudInfo::iterator end = clouds_.end();
00836 for (; it != end; ++it)
00837 {
00838 const CloudInfoPtr& cloud = *it;
00839 V_Point points;
00840 transformCloud(cloud, points, false);
00841 if (!points.empty())
00842 {
00843 cloud_->addPoints(&points.front(), points.size());
00844 }
00845 }
00846 }
00847
00848 bool PointCloudBase::transformCloud(const CloudInfoPtr& info, V_Point& points, bool update_transformers)
00849 {
00850 Ogre::Matrix4 transform = info->transform_;
00851
00852 if (transform == Ogre::Matrix4::ZERO)
00853 {
00854 Ogre::Vector3 pos;
00855 Ogre::Quaternion orient;
00856 if (!context_->getFrameManager()->getTransform(info->message_->header, pos, orient))
00857 {
00858 std::stringstream ss;
00859 ss << "Failed to transform from frame [" << info->message_->header.frame_id << "] to frame [" << context_->getFrameManager()->getFixedFrame() << "]";
00860 setStatusStd(StatusProperty::Error, "Message", ss.str());
00861 return false;
00862 }
00863
00864 transform = Ogre::Matrix4(orient);
00865 transform.setTrans(pos);
00866 info->transform_ = transform;
00867 }
00868
00869 V_PointCloudPoint& cloud_points = info->transformed_points_;
00870 cloud_points.clear();
00871
00872 size_t size = info->message_->width * info->message_->height;
00873 info->num_points_ = size;
00874 PointCloudPoint default_pt;
00875 default_pt.color = Ogre::ColourValue(1, 1, 1);
00876 default_pt.position = Ogre::Vector3::ZERO;
00877 cloud_points.resize(size, default_pt);
00878
00879 {
00880 boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
00881 if( update_transformers )
00882 {
00883 updateTransformers( info->message_ );
00884 }
00885 PointCloudTransformerPtr xyz_trans = getXYZTransformer(info->message_);
00886 PointCloudTransformerPtr color_trans = getColorTransformer(info->message_);
00887
00888 if (!xyz_trans)
00889 {
00890 std::stringstream ss;
00891 ss << "No position transformer available for cloud";
00892 setStatusStd(StatusProperty::Error, "Message", ss.str());
00893 return false;
00894 }
00895
00896 if (!color_trans)
00897 {
00898 std::stringstream ss;
00899 ss << "No color transformer available for cloud";
00900 setStatusStd(StatusProperty::Error, "Message", ss.str());
00901 return false;
00902 }
00903
00904 xyz_trans->transform(info->message_, PointCloudTransformer::Support_XYZ, transform, cloud_points);
00905 color_trans->transform(info->message_, PointCloudTransformer::Support_Color, transform, cloud_points);
00906 }
00907
00908 points.resize(size);
00909 for (size_t i = 0; i < size; ++i)
00910 {
00911 Ogre::Vector3 pos = cloud_points[i].position;
00912 Ogre::ColourValue color = cloud_points[i].color;
00913 if (validateFloats(pos))
00914 {
00915 points[i].x = pos.x;
00916 points[i].y = pos.y;
00917 points[i].z = pos.z;
00918 }
00919 else
00920 {
00921 points[i].x = 999999.0f;
00922 points[i].y = 999999.0f;
00923 points[i].z = 999999.0f;
00924 }
00925 points[i].setColor(color.r, color.g, color.b);
00926 }
00927
00928 return true;
00929 }
00930
00931 bool convertPointCloudToPointCloud2(const sensor_msgs::PointCloud& input, sensor_msgs::PointCloud2& output)
00932 {
00933 output.header = input.header;
00934 output.width = input.points.size ();
00935 output.height = 1;
00936 output.fields.resize (3 + input.channels.size ());
00937
00938 output.fields[0].name = "x"; output.fields[1].name = "y"; output.fields[2].name = "z";
00939 int offset = 0;
00940
00941 for (size_t d = 0; d < output.fields.size (); ++d, offset += 4)
00942 {
00943 output.fields[d].offset = offset;
00944 output.fields[d].datatype = sensor_msgs::PointField::FLOAT32;
00945 }
00946 output.point_step = offset;
00947 output.row_step = output.point_step * output.width;
00948
00949 for (size_t d = 0; d < input.channels.size (); ++d)
00950 output.fields[3 + d].name = input.channels[d].name;
00951 output.data.resize (input.points.size () * output.point_step);
00952 output.is_bigendian = false;
00953 output.is_dense = false;
00954
00955
00956 for (size_t cp = 0; cp < input.points.size (); ++cp)
00957 {
00958 memcpy (&output.data[cp * output.point_step + output.fields[0].offset], &input.points[cp].x, sizeof (float));
00959 memcpy (&output.data[cp * output.point_step + output.fields[1].offset], &input.points[cp].y, sizeof (float));
00960 memcpy (&output.data[cp * output.point_step + output.fields[2].offset], &input.points[cp].z, sizeof (float));
00961 for (size_t d = 0; d < input.channels.size (); ++d)
00962 {
00963 if (input.channels[d].values.size() == input.points.size())
00964 {
00965 memcpy (&output.data[cp * output.point_step + output.fields[3 + d].offset], &input.channels[d].values[cp], sizeof (float));
00966 }
00967 }
00968 }
00969 return (true);
00970 }
00971
00972 void PointCloudBase::addMessage(const sensor_msgs::PointCloudConstPtr& cloud)
00973 {
00974 sensor_msgs::PointCloud2Ptr out(new sensor_msgs::PointCloud2);
00975 convertPointCloudToPointCloud2(*cloud, *out);
00976 addMessage(out);
00977 }
00978
00979 void PointCloudBase::addMessage(const sensor_msgs::PointCloud2ConstPtr& cloud)
00980 {
00981 ++messages_received_;
00982
00983 if (cloud->width * cloud->height == 0)
00984 {
00985 return;
00986 }
00987
00988 processMessage(cloud);
00989 }
00990
00991 void PointCloudBase::fixedFrameChanged()
00992 {
00993 reset();
00994 }
00995
00996 void PointCloudBase::setXyzTransformerOptions( EnumProperty* prop )
00997 {
00998 fillTransformerOptions( prop, SUPPORT_XYZ );
00999 }
01000
01001 void PointCloudBase::setColorTransformerOptions( EnumProperty* prop )
01002 {
01003 fillTransformerOptions( prop, SUPPORT_COLOR );
01004 }
01005
01006 void PointCloudBase::fillTransformerOptions( EnumProperty* prop, uint32_t mask )
01007 {
01008 prop->clearOptions();
01009
01010 boost::mutex::scoped_lock clock(clouds_mutex_);
01011
01012 if (clouds_.empty())
01013 {
01014 return;
01015 }
01016
01017 boost::recursive_mutex::scoped_lock tlock(transformers_mutex_);
01018
01019 const sensor_msgs::PointCloud2ConstPtr& msg = clouds_.front()->message_;
01020
01021 M_TransformerInfo::iterator it = transformers_.begin();
01022 M_TransformerInfo::iterator end = transformers_.end();
01023 for (; it != end; ++it)
01024 {
01025 const PointCloudTransformerPtr& trans = it->second.transformer;
01026 if ((trans->supports(msg) & mask) == mask)
01027 {
01028 prop->addOption( QString::fromStdString( it->first ));
01029 }
01030 }
01031 }
01032
01033 void PointCloudBase::reset()
01034 {
01035 Display::reset();
01036
01037 clouds_.clear();
01038 cloud_->clear();
01039 messages_received_ = 0;
01040 total_point_count_ = 0;
01041 }
01042
01043 float PointCloudBase::getSelectionBoxSize()
01044 {
01045 if( style_property_->getOptionInt() != PointCloud::RM_POINTS )
01046 {
01047 return billboard_size_property_->getFloat();
01048 }
01049 else
01050 {
01051 return 0.004;
01052 }
01053 }
01054
01055 }