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 "point_cloud_base.h"
00031 #include "point_cloud_transformer.h"
00032 #include "point_cloud_transformers.h"
00033 #include "rviz/visualization_manager.h"
00034 #include "rviz/selection/selection_manager.h"
00035 #include "rviz/properties/property.h"
00036 #include "rviz/properties/property_manager.h"
00037 #include "rviz/validate_floats.h"
00038 #include "rviz/frame_manager.h"
00039 #include "rviz/plugin/plugin_manager.h"
00040 #include "rviz/plugin/plugin.h"
00041 #include "rviz/plugin/type_registry.h"
00042
00043 #include <ros/time.h>
00044 #include "ogre_tools/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 std::stringstream 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 std::stringstream ss;
00209 ss << "Position";
00210 Ogre::Vector3 pos(cloud->transformed_points_.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 std::stringstream 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 std::stringstream 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( const std::string& name, VisualizationManager* manager )
00350 : Display( name, manager )
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 {
00364 cloud_ = new ogre_tools::PointCloud();
00365 scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00366 scene_node_->attachObject(cloud_);
00367 coll_handler_ = PointCloudSelectionHandlerPtr(new PointCloudSelectionHandler(this));
00368
00369 setStyle( style_ );
00370 setBillboardSize( billboard_size_ );
00371 setAlpha(1.0f);
00372
00373 setSelectable(true);
00374
00375 PluginManager* pman = vis_manager_->getPluginManager();
00376 const L_Plugin& plugins = pman->getPlugins();
00377 L_Plugin::const_iterator it = plugins.begin();
00378 L_Plugin::const_iterator end = plugins.end();
00379 for (; it != end; ++it)
00380 {
00381 const PluginPtr& plugin = *it;
00382 PluginConns pc;
00383 pc.loaded = plugin->getLoadedSignal().connect(boost::bind(&PointCloudBase::onPluginLoaded, this, _1));
00384 pc.unloading = plugin->getUnloadingSignal().connect(boost::bind(&PointCloudBase::onPluginUnloading, this, _1));
00385 loadTransformers(plugin.get());
00386 plugin_conns_[plugin.get()] = pc;
00387 }
00388
00389 threaded_nh_.setCallbackQueue(&cbqueue_);
00390 spinner_.start();
00391 }
00392
00393 void deleteProperties(PropertyManager* man, V_PropertyBaseWPtr& props)
00394 {
00395 V_PropertyBaseWPtr::iterator prop_it = props.begin();
00396 V_PropertyBaseWPtr::iterator prop_end = props.end();
00397 for (; prop_it != prop_end; ++prop_it)
00398 {
00399 man->deleteProperty(prop_it->lock());
00400 }
00401
00402 props.clear();
00403 }
00404
00405 PointCloudBase::~PointCloudBase()
00406 {
00407 spinner_.stop();
00408
00409 if (coll_handle_)
00410 {
00411 SelectionManager* sel_manager = vis_manager_->getSelectionManager();
00412 sel_manager->removeObject(coll_handle_);
00413 }
00414
00415 scene_manager_->destroySceneNode(scene_node_->getName());
00416 delete cloud_;
00417
00418 if (property_manager_)
00419 {
00420 M_TransformerInfo::iterator it = transformers_.begin();
00421 M_TransformerInfo::iterator end = transformers_.end();
00422 for (; it != end; ++it)
00423 {
00424 deleteProperties(property_manager_, it->second.xyz_props);
00425 deleteProperties(property_manager_, it->second.color_props);
00426 }
00427 }
00428
00429 {
00430 M_PluginConns::iterator it = plugin_conns_.begin();
00431 M_PluginConns::iterator end = plugin_conns_.end();
00432 for (; it != end; ++it)
00433 {
00434
00435 }
00436 }
00437 }
00438
00439 void PointCloudBase::onPluginLoaded(const PluginStatus& status)
00440 {
00441 loadTransformers(status.plugin);
00442 }
00443
00444 void PointCloudBase::onPluginUnloading(const PluginStatus& status)
00445 {
00446 boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
00447
00448 typedef std::set<std::string> S_string;
00449 S_string to_erase;
00450
00451 bool xyz_unloaded = false;
00452 bool color_unloaded = false;
00453
00454 M_TransformerInfo::iterator it = transformers_.begin();
00455 M_TransformerInfo::iterator end = transformers_.end();
00456 for (; it != end; ++it)
00457 {
00458 const std::string& name = it->first;
00459 TransformerInfo& info = it->second;
00460 if (info.plugin != status.plugin)
00461 {
00462 continue;
00463 }
00464
00465 if (name == xyz_transformer_)
00466 {
00467 xyz_unloaded = true;
00468 }
00469
00470 if (name == color_transformer_)
00471 {
00472 color_unloaded = true;
00473 }
00474
00475 to_erase.insert(it->first);
00476
00477 if (property_manager_)
00478 {
00479 deleteProperties(property_manager_, info.xyz_props);
00480 deleteProperties(property_manager_, info.color_props);
00481 }
00482
00483 info.transformer.reset();
00484 }
00485
00486 {
00487 S_string::iterator it = to_erase.begin();
00488 S_string::iterator end = to_erase.end();
00489 for (; it != end; ++it)
00490 {
00491 transformers_.erase(*it);
00492 }
00493 }
00494
00495 if (xyz_unloaded || color_unloaded)
00496 {
00497 boost::mutex::scoped_lock lock(clouds_mutex_);
00498 if (!clouds_.empty())
00499 {
00500 updateTransformers((*clouds_.rbegin())->message_, true);
00501 }
00502 }
00503 }
00504
00505 void PointCloudBase::loadTransformers(Plugin* plugin)
00506 {
00507 const L_ClassTypeInfo* trans_list = plugin->getClassTypeInfoList("rviz::PointCloudTransformer");
00508 if (trans_list)
00509 {
00510 L_ClassTypeInfo::const_iterator it = trans_list->begin();
00511 L_ClassTypeInfo::const_iterator end = trans_list->end();
00512 for (; it != end; ++it)
00513 {
00514 const ClassTypeInfoPtr& cti = *it;
00515 const std::string& name = cti->readable_name;
00516
00517 if (transformers_.count(name) > 0)
00518 {
00519 ROS_ERROR("Transformer type [%s] is already loaded from plugin [%s]", name.c_str(), transformers_[name].plugin->getPackageName().c_str());
00520 continue;
00521 }
00522
00523 PointCloudTransformerPtr trans(static_cast<PointCloudTransformer*>(cti->creator->create()));
00524 trans->init(boost::bind(&PointCloudBase::causeRetransform, this));
00525 TransformerInfo info;
00526 info.transformer = trans;
00527 info.plugin = plugin;
00528 info.readable_name = cti->readable_name;
00529 transformers_[name] = info;
00530
00531 if (property_manager_)
00532 {
00533 info.transformer->createProperties(property_manager_, parent_category_, property_prefix_ + "." + name, PointCloudTransformer::Support_XYZ, info.xyz_props);
00534 info.transformer->createProperties(property_manager_, parent_category_, property_prefix_ + "." + name, PointCloudTransformer::Support_Color, info.color_props);
00535 }
00536 }
00537 }
00538 }
00539
00540 void PointCloudBase::setAlpha( float alpha )
00541 {
00542 alpha_ = alpha;
00543
00544 cloud_->setAlpha(alpha_);
00545
00546 propertyChanged(alpha_property_);
00547 }
00548
00549 void PointCloudBase::setSelectable( bool selectable )
00550 {
00551 if (selectable_ != selectable)
00552 {
00553 SelectionManager* sel_manager = vis_manager_->getSelectionManager();
00554
00555 if (selectable)
00556 {
00557 coll_handle_ = sel_manager->createHandle();
00558
00559 sel_manager->addObject(coll_handle_, coll_handler_);
00560
00561
00562 float r = ((coll_handle_ >> 16) & 0xff) / 255.0f;
00563 float g = ((coll_handle_ >> 8) & 0xff) / 255.0f;
00564 float b = (coll_handle_ & 0xff) / 255.0f;
00565 Ogre::ColourValue col(r, g, b, 1.0f);
00566 cloud_->setPickColor(col);
00567 }
00568 else
00569 {
00570 sel_manager->removeObject(coll_handle_);
00571 coll_handle_ = 0;
00572 cloud_->setPickColor(Ogre::ColourValue(0.0f, 0.0f, 0.0f, 0.0f));
00573 }
00574 }
00575
00576 selectable_ = selectable;
00577
00578 propertyChanged(selectable_property_);
00579 }
00580
00581 void PointCloudBase::setDecayTime( float time )
00582 {
00583 point_decay_time_ = time;
00584
00585 propertyChanged(decay_time_property_);
00586
00587 causeRender();
00588 }
00589
00590 void PointCloudBase::setStyle( int style )
00591 {
00592 ROS_ASSERT( style < StyleCount );
00593
00594 style_ = style;
00595
00596 ogre_tools::PointCloud::RenderMode mode = ogre_tools::PointCloud::RM_POINTS;
00597 if (style == Billboards)
00598 {
00599 mode = ogre_tools::PointCloud::RM_BILLBOARDS;
00600 }
00601 else if (style == BillboardSpheres)
00602 {
00603 mode = ogre_tools::PointCloud::RM_BILLBOARD_SPHERES;
00604 }
00605 else if (style == Boxes)
00606 {
00607 mode = ogre_tools::PointCloud::RM_BOXES;
00608 }
00609
00610 if (style == Points)
00611 {
00612 hideProperty(billboard_size_property_);
00613 }
00614 else
00615 {
00616 showProperty(billboard_size_property_);
00617 }
00618
00619 cloud_->setRenderMode(mode);
00620
00621 propertyChanged(style_property_);
00622
00623 causeRender();
00624 }
00625
00626 void PointCloudBase::setBillboardSize( float size )
00627 {
00628 billboard_size_ = size;
00629
00630 cloud_->setDimensions( size, size, size );
00631
00632 propertyChanged(billboard_size_property_);
00633
00634 causeRender();
00635 }
00636
00637 void PointCloudBase::onEnable()
00638 {
00639 }
00640
00641 void PointCloudBase::onDisable()
00642 {
00643 clouds_.clear();
00644 cloud_->clear();
00645 messages_received_ = 0;
00646 total_point_count_ = 0;
00647 }
00648
00649 void PointCloudBase::causeRetransform()
00650 {
00651 boost::mutex::scoped_lock lock(clouds_mutex_);
00652 needs_retransform_ = true;
00653 }
00654
00655 void PointCloudBase::update(float wall_dt, float ros_dt)
00656 {
00657 {
00658 boost::mutex::scoped_lock lock(clouds_mutex_);
00659
00660 if (needs_retransform_)
00661 {
00662 retransform();
00663 needs_retransform_ = false;
00664 }
00665
00666 D_CloudInfo::iterator cloud_it = clouds_.begin();
00667 D_CloudInfo::iterator cloud_end = clouds_.end();
00668 for (;cloud_it != cloud_end; ++cloud_it)
00669 {
00670 const CloudInfoPtr& info = *cloud_it;
00671
00672 info->time_ += ros_dt;
00673 }
00674
00675 if (point_decay_time_ > 0.0f)
00676 {
00677 bool removed = false;
00678 uint32_t points_to_pop = 0;
00679 while (!clouds_.empty() && clouds_.front()->time_ > point_decay_time_)
00680 {
00681 total_point_count_ -= clouds_.front()->num_points_;
00682 points_to_pop += clouds_.front()->num_points_;
00683 clouds_.pop_front();
00684 removed = true;
00685 }
00686
00687 if (removed)
00688 {
00689 cloud_->popPoints(points_to_pop);
00690 causeRender();
00691 }
00692 }
00693 }
00694
00695 if (new_cloud_)
00696 {
00697 boost::mutex::scoped_lock lock(new_clouds_mutex_);
00698
00699 if (point_decay_time_ == 0.0f)
00700 {
00701 clouds_.clear();
00702 cloud_->clear();
00703
00704 ROS_ASSERT(!new_points_.empty());
00705 ROS_ASSERT(!new_clouds_.empty());
00706 V_Point& points = new_points_.back();
00707 cloud_->addPoints(&points.front(), points.size());
00708 clouds_.push_back(new_clouds_.back());
00709
00710 total_point_count_ = points.size();
00711 }
00712 else
00713 {
00714 {
00715 VV_Point::iterator it = new_points_.begin();
00716 VV_Point::iterator end = new_points_.end();
00717 for (; it != end; ++it)
00718 {
00719 V_Point& points = *it;
00720 total_point_count_ += points.size();
00721 cloud_->addPoints( &points.front(), points.size() );
00722 }
00723 }
00724
00725 {
00726 V_CloudInfo::iterator it = new_clouds_.begin();
00727 V_CloudInfo::iterator end = new_clouds_.end();
00728 for (; it != end; ++it)
00729 {
00730 clouds_.push_back(*it);
00731 }
00732 }
00733 }
00734
00735 new_clouds_.clear();
00736 new_points_.clear();
00737 new_cloud_ = false;
00738 }
00739
00740 {
00741 boost::recursive_mutex::scoped_try_lock lock(transformers_mutex_);
00742
00743 if (lock.owns_lock())
00744 {
00745 if (new_xyz_transformer_ || new_color_transformer_)
00746 {
00747 M_TransformerInfo::iterator it = transformers_.begin();
00748 M_TransformerInfo::iterator end = transformers_.end();
00749 for (; it != end; ++it)
00750 {
00751 const std::string& name = it->first;
00752 TransformerInfo& info = it->second;
00753
00754 if (name == getXYZTransformer())
00755 {
00756 std::for_each(info.xyz_props.begin(), info.xyz_props.end(), showProperty<PropertyBase>);
00757 }
00758 else
00759 {
00760 std::for_each(info.xyz_props.begin(), info.xyz_props.end(), hideProperty<PropertyBase>);
00761 }
00762
00763 if (name == getColorTransformer())
00764 {
00765 std::for_each(info.color_props.begin(), info.color_props.end(), showProperty<PropertyBase>);
00766 }
00767 else
00768 {
00769 std::for_each(info.color_props.begin(), info.color_props.end(), hideProperty<PropertyBase>);
00770 }
00771 }
00772 }
00773 }
00774
00775 new_xyz_transformer_ = false;
00776 new_color_transformer_ = false;
00777 }
00778
00779 updateStatus();
00780 }
00781
00782 void PointCloudBase::updateTransformers(const sensor_msgs::PointCloud2ConstPtr& cloud, bool fully_update)
00783 {
00784 EditEnumPropertyPtr xyz_prop = xyz_transformer_property_.lock();
00785 if (xyz_prop)
00786 {
00787 xyz_prop->clear();
00788 }
00789
00790 EditEnumPropertyPtr color_prop = color_transformer_property_.lock();
00791 if (color_prop)
00792 {
00793 color_prop->clear();
00794 }
00795
00796
00797 std::string xyz_name = getXYZTransformer();
00798 std::string color_name = getColorTransformer();
00799
00800 typedef std::set<std::pair<uint8_t, std::string> > S_string;
00801 S_string valid_xyz, valid_color;
00802 bool cur_xyz_valid = false;
00803 bool cur_color_valid = false;
00804 M_TransformerInfo::iterator trans_it = transformers_.begin();
00805 M_TransformerInfo::iterator trans_end = transformers_.end();
00806 for(;trans_it != trans_end; ++trans_it)
00807 {
00808 const std::string& name = trans_it->first;
00809 const PointCloudTransformerPtr& trans = trans_it->second.transformer;
00810 uint32_t mask = trans->supports(cloud);
00811 if (mask & PointCloudTransformer::Support_XYZ)
00812 {
00813 valid_xyz.insert(std::make_pair(trans->score(cloud), name));
00814 if (name == xyz_name)
00815 {
00816 cur_xyz_valid = true;
00817 }
00818
00819 if (xyz_prop)
00820 {
00821 xyz_prop->addOption(name);
00822 }
00823 }
00824
00825 if (mask & PointCloudTransformer::Support_Color)
00826 {
00827 valid_color.insert(std::make_pair(trans->score(cloud), name));
00828
00829 if (name == color_name)
00830 {
00831 cur_color_valid = true;
00832 }
00833
00834 if (color_prop)
00835 {
00836 color_prop->addOption(name);
00837 }
00838 }
00839 }
00840
00841 if (!cur_xyz_valid)
00842 {
00843 if (!valid_xyz.empty())
00844 {
00845 if (fully_update)
00846 {
00847 setXYZTransformer(valid_xyz.rbegin()->second);
00848 }
00849 else
00850 {
00851 xyz_transformer_ = valid_xyz.rbegin()->second;
00852 }
00853 }
00854 }
00855
00856 if (!cur_color_valid)
00857 {
00858 if (!valid_color.empty())
00859 {
00860 if (fully_update)
00861 {
00862 setColorTransformer(valid_color.rbegin()->second);
00863 }
00864 else
00865 {
00866 color_transformer_ = valid_color.rbegin()->second;
00867 }
00868 }
00869 }
00870
00871 if (xyz_prop)
00872 {
00873 xyz_prop->changed();
00874 }
00875
00876 if (color_prop)
00877 {
00878 color_prop->changed();
00879 }
00880 }
00881
00882 void PointCloudBase::updateStatus()
00883 {
00884 if (messages_received_ == 0)
00885 {
00886 setStatus(status_levels::Warn, "Topic", "No messages received");
00887 }
00888 else
00889 {
00890 std::stringstream ss;
00891 ss << messages_received_ << " messages received";
00892 setStatus(status_levels::Ok, "Topic", ss.str());
00893 }
00894
00895 {
00896 std::stringstream ss;
00897 ss << "Showing [" << total_point_count_ << "] points from [" << clouds_.size() << "] messages";
00898 setStatus(status_levels::Ok, "Points", ss.str());
00899 }
00900 }
00901
00902 void PointCloudBase::processMessage(const sensor_msgs::PointCloud2ConstPtr& cloud)
00903 {
00904 CloudInfoPtr info(new CloudInfo);
00905 info->message_ = cloud;
00906 info->time_ = 0;
00907
00908 V_Point points;
00909 if (transformCloud(info, points, true))
00910 {
00911 boost::mutex::scoped_lock lock(new_clouds_mutex_);
00912
00913 new_clouds_.push_back(info);
00914 new_points_.push_back(V_Point());
00915 new_points_.back().swap(points);
00916
00917 new_cloud_ = true;
00918 }
00919 }
00920
00921 void PointCloudBase::setXYZTransformer(const std::string& name)
00922 {
00923 boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
00924 if (xyz_transformer_ == name)
00925 {
00926 return;
00927 }
00928
00929 if (transformers_.count(name) == 0)
00930 {
00931 return;
00932 }
00933
00934 xyz_transformer_ = name;
00935 new_xyz_transformer_ = true;
00936 propertyChanged(xyz_transformer_property_);
00937
00938 causeRetransform();
00939 }
00940
00941 void PointCloudBase::setColorTransformer(const std::string& name)
00942 {
00943 boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
00944 if (color_transformer_ == name)
00945 {
00946 return;
00947 }
00948
00949 if (transformers_.count(name) == 0)
00950 {
00951 return;
00952 }
00953
00954 color_transformer_ = name;
00955 new_color_transformer_ = true;
00956 propertyChanged(color_transformer_property_);
00957
00958 causeRetransform();
00959 }
00960
00961 PointCloudTransformerPtr PointCloudBase::getXYZTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud)
00962 {
00963 boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
00964 M_TransformerInfo::iterator it = transformers_.find(xyz_transformer_);
00965 if (it != transformers_.end())
00966 {
00967 const PointCloudTransformerPtr& trans = it->second.transformer;
00968 if (trans->supports(cloud) & PointCloudTransformer::Support_XYZ)
00969 {
00970 return trans;
00971 }
00972 }
00973
00974 return PointCloudTransformerPtr();
00975 }
00976
00977 PointCloudTransformerPtr PointCloudBase::getColorTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud)
00978 {
00979 boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
00980 M_TransformerInfo::iterator it = transformers_.find(color_transformer_);
00981 if (it != transformers_.end())
00982 {
00983 const PointCloudTransformerPtr& trans = it->second.transformer;
00984 if (trans->supports(cloud) & PointCloudTransformer::Support_Color)
00985 {
00986 return trans;
00987 }
00988 }
00989
00990 return PointCloudTransformerPtr();
00991 }
00992
00993 void PointCloudBase::retransform()
00994 {
00995 boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
00996
00997 cloud_->clear();
00998
00999
01000 std::string xyz_trans = xyz_transformer_;
01001 std::string color_trans = color_transformer_;
01002
01003 D_CloudInfo::iterator it = clouds_.begin();
01004 D_CloudInfo::iterator end = clouds_.end();
01005 for (; it != end; ++it)
01006 {
01007 const CloudInfoPtr& cloud = *it;
01008 V_Point points;
01009 transformCloud(cloud, points, false);
01010 if (!points.empty())
01011 {
01012 cloud_->addPoints(&points.front(), points.size());
01013 }
01014 }
01015
01016 xyz_transformer_ = xyz_trans;
01017 color_transformer_ = color_trans;
01018 }
01019
01020 bool PointCloudBase::transformCloud(const CloudInfoPtr& info, V_Point& points, bool fully_update_transformers)
01021 {
01022 Ogre::Matrix4 transform = info->transform_;
01023
01024 if (transform == Ogre::Matrix4::ZERO)
01025 {
01026 Ogre::Vector3 pos;
01027 Ogre::Quaternion orient;
01028 if (!vis_manager_->getFrameManager()->getTransform(info->message_->header, pos, orient))
01029 {
01030 std::stringstream ss;
01031 ss << "Failed to transform from frame [" << info->message_->header.frame_id << "] to frame [" << vis_manager_->getFrameManager()->getFixedFrame() << "]";
01032 setStatus(status_levels::Error, "Message", ss.str());
01033 return false;
01034 }
01035
01036 transform = Ogre::Matrix4(orient);
01037 transform.setTrans(pos);
01038 info->transform_ = transform;
01039 }
01040
01041 PointCloud& cloud = info->transformed_points_;
01042 cloud.points.clear();
01043
01044 size_t size = info->message_->width * info->message_->height;
01045 info->num_points_ = size;
01046 PointCloudPoint default_pt;
01047 default_pt.color = Ogre::ColourValue(1, 1, 1);
01048 default_pt.position = Ogre::Vector3::ZERO;
01049 cloud.points.resize(size, default_pt);
01050
01051 {
01052 boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
01053 updateTransformers(info->message_, fully_update_transformers);
01054 PointCloudTransformerPtr xyz_trans = getXYZTransformer(info->message_);
01055 PointCloudTransformerPtr color_trans = getColorTransformer(info->message_);
01056
01057 if (!xyz_trans)
01058 {
01059 std::stringstream ss;
01060 ss << "No position transformer available for cloud";
01061 setStatus(status_levels::Error, "Message", ss.str());
01062 return false;
01063 }
01064
01065 if (!color_trans)
01066 {
01067 std::stringstream ss;
01068 ss << "No color transformer available for cloud";
01069 setStatus(status_levels::Error, "Message", ss.str());
01070 return false;
01071 }
01072
01073 xyz_trans->transform(info->message_, PointCloudTransformer::Support_XYZ, transform, cloud);
01074 color_trans->transform(info->message_, PointCloudTransformer::Support_Color, transform, cloud);
01075 }
01076
01077 points.resize(size);
01078 for (size_t i = 0; i < size; ++i)
01079 {
01080 Ogre::Vector3 pos = cloud.points[i].position;
01081 Ogre::ColourValue color = cloud.points[i].color;
01082 if (validateFloats(pos))
01083 {
01084 points[i].x = pos.x;
01085 points[i].y = pos.y;
01086 points[i].z = pos.z;
01087 }
01088 else
01089 {
01090 points[i].x = 999999.0f;
01091 points[i].y = 999999.0f;
01092 points[i].z = 999999.0f;
01093 }
01094 points[i].setColor(color.r, color.g, color.b);
01095 }
01096
01097 return true;
01098 }
01099
01100 bool convertPointCloudToPointCloud2(const sensor_msgs::PointCloud& input, sensor_msgs::PointCloud2& output)
01101 {
01102 output.header = input.header;
01103 output.width = input.points.size ();
01104 output.height = 1;
01105 output.fields.resize (3 + input.channels.size ());
01106
01107 output.fields[0].name = "x"; output.fields[1].name = "y"; output.fields[2].name = "z";
01108 int offset = 0;
01109
01110 for (size_t d = 0; d < output.fields.size (); ++d, offset += 4)
01111 {
01112 output.fields[d].offset = offset;
01113 output.fields[d].datatype = sensor_msgs::PointField::FLOAT32;
01114 }
01115 output.point_step = offset;
01116 output.row_step = output.point_step * output.width;
01117
01118 for (size_t d = 0; d < input.channels.size (); ++d)
01119 output.fields[3 + d].name = input.channels[d].name;
01120 output.data.resize (input.points.size () * output.point_step);
01121 output.is_bigendian = false;
01122 output.is_dense = false;
01123
01124
01125 for (size_t cp = 0; cp < input.points.size (); ++cp)
01126 {
01127 memcpy (&output.data[cp * output.point_step + output.fields[0].offset], &input.points[cp].x, sizeof (float));
01128 memcpy (&output.data[cp * output.point_step + output.fields[1].offset], &input.points[cp].y, sizeof (float));
01129 memcpy (&output.data[cp * output.point_step + output.fields[2].offset], &input.points[cp].z, sizeof (float));
01130 for (size_t d = 0; d < input.channels.size (); ++d)
01131 {
01132 if (input.channels[d].values.size() == input.points.size())
01133 {
01134 memcpy (&output.data[cp * output.point_step + output.fields[3 + d].offset], &input.channels[d].values[cp], sizeof (float));
01135 }
01136 }
01137 }
01138 return (true);
01139 }
01140
01141 void PointCloudBase::addMessage(const sensor_msgs::PointCloudConstPtr& cloud)
01142 {
01143 sensor_msgs::PointCloud2Ptr out(new sensor_msgs::PointCloud2);
01144 convertPointCloudToPointCloud2(*cloud, *out);
01145 addMessage(out);
01146 }
01147
01148 void PointCloudBase::addMessage(const sensor_msgs::PointCloud2ConstPtr& cloud)
01149 {
01150 ++messages_received_;
01151
01152 if (cloud->width * cloud->height == 0)
01153 {
01154 return;
01155 }
01156
01157 processMessage(cloud);
01158 }
01159
01160 void PointCloudBase::fixedFrameChanged()
01161 {
01162 reset();
01163 }
01164
01165 void PointCloudBase::onTransformerOptions(V_string& ops, uint32_t mask)
01166 {
01167 boost::mutex::scoped_lock clock(clouds_mutex_);
01168
01169 if (clouds_.empty())
01170 {
01171 return;
01172 }
01173
01174 boost::recursive_mutex::scoped_lock tlock(transformers_mutex_);
01175
01176 const sensor_msgs::PointCloud2ConstPtr& msg = clouds_.front()->message_;
01177
01178 M_TransformerInfo::iterator it = transformers_.begin();
01179 M_TransformerInfo::iterator end = transformers_.end();
01180 for (; it != end; ++it)
01181 {
01182 const PointCloudTransformerPtr& trans = it->second.transformer;
01183 if ((trans->supports(msg) & mask) == mask)
01184 {
01185 ops.push_back(it->first);
01186 }
01187 }
01188 }
01189
01190 void PointCloudBase::createProperties()
01191 {
01192 selectable_property_ = property_manager_->createProperty<BoolProperty>( "Selectable", property_prefix_, boost::bind( &PointCloudBase::getSelectable, this ),
01193 boost::bind( &PointCloudBase::setSelectable, this, _1 ), parent_category_, this );
01194 setPropertyHelpText(selectable_property_, "Whether or not the points in this point cloud are selectable.");
01195
01196 style_property_ = property_manager_->createProperty<EnumProperty>( "Style", property_prefix_, boost::bind( &PointCloudBase::getStyle, this ),
01197 boost::bind( &PointCloudBase::setStyle, this, _1 ), parent_category_, this );
01198 setPropertyHelpText(style_property_, "Rendering mode to use, in order of computational complexity.");
01199 EnumPropertyPtr enum_prop = style_property_.lock();
01200 enum_prop->addOption( "Points", Points );
01201 enum_prop->addOption( "Billboards", Billboards );
01202 enum_prop->addOption( "Billboard Spheres", BillboardSpheres );
01203 enum_prop->addOption( "Boxes", Boxes );
01204
01205 billboard_size_property_ = property_manager_->createProperty<FloatProperty>( "Billboard Size", property_prefix_, boost::bind( &PointCloudBase::getBillboardSize, this ),
01206 boost::bind( &PointCloudBase::setBillboardSize, this, _1 ), parent_category_, this );
01207 setPropertyHelpText(billboard_size_property_, "Length, in meters, of the side of each billboard (or face if using the Boxes style).");
01208 FloatPropertyPtr float_prop = billboard_size_property_.lock();
01209 float_prop->setMin( 0.0001 );
01210
01211 alpha_property_ = property_manager_->createProperty<FloatProperty>( "Alpha", property_prefix_, boost::bind( &PointCloudBase::getAlpha, this ),
01212 boost::bind( &PointCloudBase::setAlpha, this, _1 ), parent_category_, this );
01213 setPropertyHelpText(alpha_property_, "Amount of transparency to apply to the points. Note that this is experimental and does not always look correct.");
01214 decay_time_property_ = property_manager_->createProperty<FloatProperty>( "Decay Time", property_prefix_, boost::bind( &PointCloudBase::getDecayTime, this ),
01215 boost::bind( &PointCloudBase::setDecayTime, this, _1 ), parent_category_, this );
01216 setPropertyHelpText(decay_time_property_, "Duration, in seconds, to keep the incoming points. 0 means only show the latest points.");
01217
01218 xyz_transformer_property_ = property_manager_->createProperty<EditEnumProperty>( "Position Transformer", property_prefix_, boost::bind( &PointCloudBase::getXYZTransformer, this ),
01219 boost::bind( &PointCloudBase::setXYZTransformer, this, _1 ), parent_category_, this );
01220 setPropertyHelpText(xyz_transformer_property_, "Set the transformer to use to set the position of the points.");
01221 EditEnumPropertyPtr edit_enum_prop = xyz_transformer_property_.lock();
01222 edit_enum_prop->setOptionCallback(boost::bind(&PointCloudBase::onTransformerOptions, this, _1, PointCloudTransformer::Support_XYZ));
01223
01224 color_transformer_property_ = property_manager_->createProperty<EditEnumProperty>( "Color Transformer", property_prefix_, boost::bind( &PointCloudBase::getColorTransformer, this ),
01225 boost::bind( &PointCloudBase::setColorTransformer, this, _1 ), parent_category_, this );
01226 setPropertyHelpText(color_transformer_property_, "Set the transformer to use to set the color of the points.");
01227 edit_enum_prop = color_transformer_property_.lock();
01228 edit_enum_prop->setOptionCallback(boost::bind(&PointCloudBase::onTransformerOptions, this, _1, PointCloudTransformer::Support_Color));
01229
01230
01231 {
01232 boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
01233 M_TransformerInfo::iterator it = transformers_.begin();
01234 M_TransformerInfo::iterator end = transformers_.end();
01235 for (; it != end; ++it)
01236 {
01237 const std::string& name = it->first;
01238 TransformerInfo& info = it->second;
01239 info.transformer->createProperties(property_manager_, parent_category_, property_prefix_ + "." + name, PointCloudTransformer::Support_XYZ, info.xyz_props);
01240 info.transformer->createProperties(property_manager_, parent_category_, property_prefix_ + "." + name, PointCloudTransformer::Support_Color, info.color_props);
01241
01242 if (name != getXYZTransformer())
01243 {
01244 std::for_each(info.xyz_props.begin(), info.xyz_props.end(), hideProperty<PropertyBase>);
01245 }
01246
01247 if (name != getColorTransformer())
01248 {
01249 std::for_each(info.color_props.begin(), info.color_props.end(), hideProperty<PropertyBase>);
01250 }
01251 }
01252 }
01253 }
01254
01255 void PointCloudBase::reset()
01256 {
01257 Display::reset();
01258
01259 clouds_.clear();
01260 cloud_->clear();
01261 messages_received_ = 0;
01262 total_point_count_ = 0;
01263 }
01264
01265 }
