diff --git a/include/octomap_rviz_plugins/occupancy_grid_display.h b/include/octomap_rviz_plugins/occupancy_grid_display.h index 473c95a..f91ba73 100644 --- a/include/octomap_rviz_plugins/occupancy_grid_display.h +++ b/include/octomap_rviz_plugins/occupancy_grid_display.h @@ -82,6 +82,7 @@ private Q_SLOTS: void updateAlpha(); void updateMaxHeight(); void updateMinHeight(); + void updateStyle(); protected: // overrides from Display @@ -118,6 +119,7 @@ private Q_SLOTS: rviz::RosTopicProperty* octomap_topic_property_; rviz::EnumProperty* octree_render_property_; rviz::EnumProperty* octree_coloring_property_; + rviz::EnumProperty* style_property_; rviz::IntProperty* tree_depth_property_; rviz::FloatProperty* alpha_property_; rviz::FloatProperty* max_height_property_; diff --git a/src/occupancy_grid_display.cpp b/src/occupancy_grid_display.cpp index 9166a4a..c43b1c5 100644 --- a/src/occupancy_grid_display.cpp +++ b/src/occupancy_grid_display.cpp @@ -140,6 +140,15 @@ OccupancyGridDisplay::OccupancyGridDisplay() : "Defines the minimum height to display", this, SLOT (updateMinHeight() )); + + style_property_ = new EnumProperty( "Style", "Boxes", + "Rendering mode to use, in order of computational complexity.", + this, SLOT( updateStyle() )); + style_property_->addOption( "Points", PointCloud::RM_POINTS ); + style_property_->addOption( "Squares", PointCloud::RM_SQUARES ); + style_property_->addOption( "Flat Squares", PointCloud::RM_FLAT_SQUARES ); + style_property_->addOption( "Spheres", PointCloud::RM_SPHERES ); + style_property_->addOption( "Boxes", PointCloud::RM_BOXES ); } void OccupancyGridDisplay::onInitialize() @@ -157,7 +166,7 @@ void OccupancyGridDisplay::onInitialize() sname << "PointCloud Nr." << i; cloud_[i] = new rviz::PointCloud(); cloud_[i]->setName(sname.str()); - cloud_[i]->setRenderMode(rviz::PointCloud::RM_BOXES); + cloud_[i]->setRenderMode(static_cast(style_property_->getOptionInt())); scene_node_->attachObject(cloud_[i]); } } @@ -315,6 +324,14 @@ void OccupancyGridDisplay::updateMinHeight() { } +void OccupancyGridDisplay::updateStyle() +{ + for (std::size_t i = 0; i < max_octree_depth_; ++i) + { + cloud_[i]->setRenderMode(static_cast(style_property_->getOptionInt())); + } +} + void OccupancyGridDisplay::clear() {