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