Merge pull request #1 from gauthamm/clip_along_z

clipping functionality added to remove the floor and/or ceiling

include/octomap_rviz_plugins/occupancy_grid_display.h

@@ -77,6 +77,8 @@ private Q_SLOTS:
   void updateOctreeRenderMode();
   void updateOctreeColorMode();
   void updateAlpha();
+  void updateMaxHeight();
+  void updateMinHeight();
 
 protected:
   // overrides from Display
@@ -115,12 +117,12 @@ private Q_SLOTS:
   rviz::EnumProperty* octree_coloring_property_;
   rviz::IntProperty* tree_depth_property_;
   rviz::FloatProperty* alpha_property_;
+  rviz::FloatProperty* max_height_property_;
+  rviz::FloatProperty* min_height_property_;
 
   u_int32_t queue_size_;
-  std::size_t octree_depth_;
   uint32_t messages_received_;
   double color_factor_;
-  double alpha_;
 };
 
 } // namespace octomap_rviz_plugin

include/octomap_rviz_plugins/occupancy_map_display.h

@@ -72,7 +72,6 @@ private Q_SLOTS:
 
   unsigned int octree_depth_;
   rviz::IntProperty* tree_depth_property_;
-
 };
 
 } // namespace rviz

src/occupancy_grid_display.cpp

@@ -76,8 +76,7 @@ OccupancyGridDisplay::OccupancyGridDisplay() :
     new_points_received_(false),
     messages_received_(0),
     queue_size_(5),
-    color_factor_(0.8),
-    octree_depth_(0)
+    color_factor_(0.8)
 {
 
   octomap_topic_property_ = new RosTopicProperty( "Octomap Topic",
@@ -124,6 +123,18 @@ OccupancyGridDisplay::OccupancyGridDisplay() :
                                          this,
                                          SLOT (updateTreeDepth() ));
   tree_depth_property_->setMin(0);
+
+  max_height_property_ = new FloatProperty("Max. Height Display",
+                                           std::numeric_limits<double>::infinity(),
+                                           "Defines the maximum height to display",
+                                           this,
+                                           SLOT (updateMaxHeight() ));
+
+  min_height_property_ = new FloatProperty("Min. Height Display",
+                                           -std::numeric_limits<double>::infinity(),
+                                           "Defines the minimum height to display",
+                                           this,
+                                           SLOT (updateMinHeight() ));
 }
 
 void OccupancyGridDisplay::onInitialize()
@@ -319,6 +330,12 @@ void OccupancyGridDisplay::incomingMessageCallback(const octomap_msgs::OctomapCo
   octomap->getMetricMin(minX, minY, minZ);
   octomap->getMetricMax(maxX, maxY, maxZ);
 
+  max_height_property_->setMin(minZ);
+  max_height_property_->setMax(maxZ);
+
+  min_height_property_->setMin(minZ);
+  min_height_property_->setMax(maxZ);
+
   // reset rviz pointcloud classes
   for (std::size_t i = 0; i < max_octree_depth_; ++i)
   {
@@ -330,80 +347,84 @@ void OccupancyGridDisplay::incomingMessageCallback(const octomap_msgs::OctomapCo
   {
     // traverse all leafs in the tree:
     unsigned int treeDepth = std::min<unsigned int>(tree_depth_property_->getInt(), octomap->getTreeDepth());
+    double maxHeight = std::min<double>(max_height_property_->getFloat(), maxZ);
+    double minHeight = std::max<double>(min_height_property_->getFloat(), minZ);
     for (octomap::OcTree::iterator it = octomap->begin(treeDepth), end = octomap->end(); it != end; ++it)
     {
 
       if (octomap->isNodeOccupied(*it))
       {
-
-        int render_mode_mask = octree_render_property_->getOptionInt();
-
-        bool display_voxel = false;
-
-        // the left part evaluates to 1 for free voxels and 2 for occupied voxels
-        if (((int)octomap->isNodeOccupied(*it) + 1) & render_mode_mask)
+        if(it.getZ() <= maxHeight && it.getZ() >= minHeight)
         {
-          // check if current voxel has neighbors on all sides -> no need to be displayed
-          bool allNeighborsFound = true;
+          int render_mode_mask = octree_render_property_->getOptionInt();
 
-          octomap::OcTreeKey key;
-          octomap::OcTreeKey nKey = it.getKey();
+          bool display_voxel = false;
 
-          for (key[2] = nKey[2] - 1; allNeighborsFound && key[2] <= nKey[2] + 1; ++key[2])
+          // the left part evaluates to 1 for free voxels and 2 for occupied voxels
+          if (((int)octomap->isNodeOccupied(*it) + 1) & render_mode_mask)
           {
-            for (key[1] = nKey[1] - 1; allNeighborsFound && key[1] <= nKey[1] + 1; ++key[1])
+            // check if current voxel has neighbors on all sides -> no need to be displayed
+            bool allNeighborsFound = true;
+
+            octomap::OcTreeKey key;
+            octomap::OcTreeKey nKey = it.getKey();
+
+            for (key[2] = nKey[2] - 1; allNeighborsFound && key[2] <= nKey[2] + 1; ++key[2])
             {
-              for (key[0] = nKey[0] - 1; allNeighborsFound && key[0] <= nKey[0] + 1; ++key[0])
+              for (key[1] = nKey[1] - 1; allNeighborsFound && key[1] <= nKey[1] + 1; ++key[1])
               {
-                if (key != nKey)
+                for (key[0] = nKey[0] - 1; allNeighborsFound && key[0] <= nKey[0] + 1; ++key[0])
                 {
-                  octomap::OcTreeNode* node = octomap->search(key);
-
-                  // the left part evaluates to 1 for free voxels and 2 for occupied voxels
-                  if (!(node && (((int)octomap->isNodeOccupied(node)) + 1) & render_mode_mask))
+                  if (key != nKey)
                   {
-                    // we do not have a neighbor => break!
-                    allNeighborsFound = false;
+                    octomap::OcTreeNode* node = octomap->search(key);
+
+                    // the left part evaluates to 1 for free voxels and 2 for occupied voxels
+                    if (!(node && ((((int)octomap->isNodeOccupied(node)) + 1) & render_mode_mask)))
+                    {
+                      // we do not have a neighbor => break!
+                      allNeighborsFound = false;
+                    }
                   }
                 }
               }
             }
-          }
 
-          display_voxel |= !allNeighborsFound;
-        }
+            display_voxel |= !allNeighborsFound;
+          }
 
 
-        if (display_voxel)
-        {
-          PointCloud::Point newPoint;
+          if (display_voxel)
+          {
+            PointCloud::Point newPoint;
 
-          newPoint.position.x = it.getX();
-          newPoint.position.y = it.getY();
-          newPoint.position.z = it.getZ();
+            newPoint.position.x = it.getX();
+            newPoint.position.y = it.getY();
+            newPoint.position.z = it.getZ();
 
-          float cell_probability;
+            float cell_probability;
 
-          OctreeVoxelColorMode octree_color_mode = static_cast<OctreeVoxelColorMode>(octree_coloring_property_->getOptionInt());
+            OctreeVoxelColorMode octree_color_mode = static_cast<OctreeVoxelColorMode>(octree_coloring_property_->getOptionInt());
 
-          switch (octree_color_mode)
-          {
-            case OCTOMAP_Z_AXIS_COLOR:
-              setColor(newPoint.position.z, minZ, maxZ, color_factor_, newPoint);
-              break;
-            case OCTOMAP_PROBABLILTY_COLOR:
-              cell_probability = it->getOccupancy();
-              newPoint.setColor((1.0f-cell_probability), cell_probability, 0.0);
-              break;
-            default:
-              break;
-          }
+            switch (octree_color_mode)
+            {
+              case OCTOMAP_Z_AXIS_COLOR:
+                setColor(newPoint.position.z, minZ, maxZ, color_factor_, newPoint);
+                break;
+              case OCTOMAP_PROBABLILTY_COLOR:
+                cell_probability = it->getOccupancy();
+                newPoint.setColor((1.0f-cell_probability), cell_probability, 0.0);
+                break;
+              default:
+                break;
+            }
 
-          // push to point vectors
-          unsigned int depth = it.getDepth();
-          point_buf_[depth - 1].push_back(newPoint);
+            // push to point vectors
+            unsigned int depth = it.getDepth();
+            point_buf_[depth - 1].push_back(newPoint);
 
-          ++pointCount;
+            ++pointCount;
+          }
         }
       }
     }
@@ -438,6 +459,14 @@ void OccupancyGridDisplay::updateAlpha()
 {
 }
 
+void OccupancyGridDisplay::updateMaxHeight()
+{
+}
+
+void OccupancyGridDisplay::updateMinHeight()
+{
+}
+
 void OccupancyGridDisplay::clear()
 {
 

src/occupancy_map_display.cpp

@@ -172,7 +172,7 @@ void OccupancyMapDisplay::handleOctomapBinaryMessage(const octomap_msgs::Octomap
   occupancy_map->info.width = width = (maxX-minX) / res + 1;
   occupancy_map->info.height = height = (maxY-minY) / res + 1;
   occupancy_map->info.origin.position.x = minX  - (res / (float)(1<<ds_shift) ) + res;
-  occupancy_map->info.origin.position.y = minY  - (res / (float)(1<<ds_shift) );;
+  occupancy_map->info.origin.position.y = minY  - (res / (float)(1<<ds_shift) );
 
   occupancy_map->data.clear();
   occupancy_map->data.resize(width*height, -1);