Added OcTree BBX pruning

perception/occupancy_map_monitor/CMakeLists.txt

@@ -1,9 +1,10 @@
 set(MOVEIT_LIB_NAME moveit_occupancy_map_monitor)
 
 add_library(${MOVEIT_LIB_NAME}
+  src/occupancy_map.cpp
   src/occupancy_map_monitor.cpp
   src/occupancy_map_updater.cpp
-  )
+)
 target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
 install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION lib)

perception/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h

@@ -32,7 +32,7 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
 
-/* Author: Ioan Sucan, Jon Binney */
+/* Author: Ioan Sucan, Jon Binney, Connor Brew */
 
 #ifndef MOVEIT_OCCUPANCY_MAP_MONITOR_OCCUPANCY_MAP_
 #define MOVEIT_OCCUPANCY_MAP_MONITOR_OCCUPANCY_MAP_
@@ -44,8 +44,65 @@ namespace occupancy_map_monitor
 {
 
 typedef octomap::OcTreeNode OccMapNode;
-typedef boost::shared_ptr<octomap::OcTree> OccMapTreePtr;
-typedef boost::shared_ptr<const octomap::OcTree> OccMapTreeConstPtr;
+
+class OccMapTree : public octomap::OcTree
+{
+public:
+  OccMapTree(double resolution) : octomap::OcTree(resolution), bbx_size(0.0), bbx_height(0.0) {}
+  OccMapTree(double resolution, double size) : octomap::OcTree(resolution), bbx_size(size), bbx_height(size) {}
+  OccMapTree(double resolution, double size, double height) : octomap::OcTree(resolution), bbx_size(size), bbx_height(height) {}
+  OccMapTree(const std::string &filename) : octomap::OcTree(filename), bbx_size(0.0), bbx_height(0.0) {}
+  OccMapTree(const OccMapTree& rhs) : octomap::OcTree(rhs), bbx_size(rhs.bbx_size), bbx_height(rhs.bbx_height) {}
+
+  void pruneBBX()
+  {
+    if (!root || bbx_size <= 0.0)
+      return;
+    octomap::OcTreeKey root_key;
+    root_key[0] = root_key[1] = root_key[2] = tree_max_val;
+    if (pruneBBXRecurs(root, root_key, 0))
+      tree_size = this->calcNumNodes();
+  }
+
+  void setBBXCenter(double x, double y, double z)
+  {
+    octomap::point3d min(x-bbx_size, y-bbx_size, z-bbx_height);
+    octomap::point3d max(x+bbx_size, y+bbx_size, z+bbx_height);
+    setBBXMin(min);
+    setBBXMax(max);
+  }
+
+  void setBBXSize(double size)
+  {
+    bbx_size = size;
+  }
+
+  double getBBXSize()
+  {
+    return bbx_size;
+  }
+
+  void setBBXHeight(double height)
+  {
+    bbx_height = height;
+  }
+
+  double getBBXHeight()
+  {
+    return bbx_height;
+  }
+
+protected:
+  bool inBBX(const octomap::OcTreeKey& key, unsigned int depth) const;
+  bool inBBXStrict(const octomap::OcTreeKey& key, unsigned int depth) const;
+  bool pruneBBXRecurs(octomap::OcTreeNode* node, const octomap::OcTreeKey& parent_key, unsigned int depth);
+
+  double bbx_size;
+  double bbx_height;
+};
+
+typedef boost::shared_ptr<OccMapTree> OccMapTreePtr;
+typedef boost::shared_ptr<const OccMapTree> OccMapTreeConstPtr;
 
 }
 

perception/occupancy_map_monitor/src/occupancy_map.cpp

@@ -0,0 +1,100 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015, Fetch Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Connor Brew */
+
+#include <moveit/occupancy_map_monitor/occupancy_map.h>
+#include <ros/console.h>
+
+namespace occupancy_map_monitor
+{
+
+bool OccMapTree::inBBX(const octomap::OcTreeKey& key, unsigned int depth) const {
+  octomap::OcTreeKey bbx_min_at_depth = adjustKeyAtDepth(bbx_min_key, depth);
+  octomap::OcTreeKey bbx_max_at_depth = adjustKeyAtDepth(bbx_max_key, depth);
+  return ((key[0] >= bbx_min_at_depth[0]) && (key[1] >= bbx_min_at_depth[1]) && (key[2] >= bbx_min_at_depth[2]) &&
+          (key[0] <= bbx_max_at_depth[0]) && (key[1] <= bbx_max_at_depth[1]) && (key[2] <= bbx_max_at_depth[2]) );
+}
+
+bool OccMapTree::inBBXStrict(const octomap::OcTreeKey& key, unsigned int depth) const {
+  octomap::OcTreeKey bbx_min_at_depth = adjustKeyAtDepth(bbx_min_key, depth);
+  octomap::OcTreeKey bbx_max_at_depth = adjustKeyAtDepth(bbx_max_key, depth);
+  return ((key[0] > bbx_min_at_depth[0]) && (key[1] > bbx_min_at_depth[1]) && (key[2] > bbx_min_at_depth[2]) &&
+          (key[0] < bbx_max_at_depth[0]) && (key[1] < bbx_max_at_depth[1]) && (key[2] < bbx_max_at_depth[2]) );
+}
+
+bool OccMapTree::pruneBBXRecurs(octomap::OcTreeNode* node, const octomap::OcTreeKey& parent_key, unsigned int depth)
+{
+	if (!node->hasChildren())
+		return false;
+
+  bool pruned = false;
+  unsigned int child_depth = depth + 1;
+  unsigned short int center_offset_key = tree_max_val >> child_depth;
+  octomap::OcTreeKey child_key;
+
+  for (int i=0; i<8; ++i)
+  {
+    if (node->childExists(i))
+    {
+      octomap::computeChildKey(i, center_offset_key, parent_key, child_key);
+      if (inBBX(child_key, child_depth))
+      {
+        if (!inBBXStrict(child_key, child_depth))
+        {
+          if (pruneBBXRecurs(node->getChild(i), child_key, child_depth))
+          {
+          	pruned = true;
+          	if (!node->getChild(i)->hasChildren()) // All of the childs children were pruned
+          	{
+          		node->deleteChild(i);
+          	}
+          }
+        }
+      }
+      else
+      {
+        node->deleteChild(i);
+        size_changed = true;
+        pruned = true;
+      }
+    }
+  }
+  if (pruned && node->hasChildren()) {
+    node->updateOccupancyChildren();
+  }
+  return pruned;
+}
+
+}

perception/occupancy_map_monitor/src/occupancy_map_monitor.cpp

@@ -84,7 +84,18 @@ void OccupancyMapMonitor::initialize()
   if (!tf_ && !map_frame_.empty())
     ROS_WARN("Target frame specified but no TF instance specified. No transforms will be applied to received data.");
 
-  tree_.reset(new octomap::OcTree(map_resolution_));
+  double map_size_limit = 0.0;
+  double map_height_limit = 0.0;
+  if (nh_.getParam("octomap_size_limit", map_size_limit))
+  {
+    ROS_INFO("Octomap size limit set to %g, Octomap pruning enabled", map_size_limit);
+    if (nh_.getParam("octomap_height_limit", map_height_limit))
+      ROS_INFO("Octomap height limit set to %g", map_size_limit);
+    else
+      map_height_limit = map_size_limit;
+  }
+
+  tree_.reset(new OccMapTree(map_resolution_, map_size_limit, map_height_limit));
   tree_const_ = tree_;
 
   XmlRpc::XmlRpcValue sensor_list;

planning/planning_scene_monitor/src/planning_scene_monitor.cpp

@@ -1064,6 +1064,13 @@ void planning_scene_monitor::PlanningSceneMonitor::octomapUpdateCallback()
     return;
 
   updateFrameTransforms();
+  const Eigen::Affine3d rt = scene_->getCurrentState().getGlobalLinkTransform(robot_model_->getRootLinkName());
+  {
+    occupancy_map_monitor::WriteLock lock = octomap_monitor_->writingMap();
+    octomap_monitor_->getOcTreePtr()->setBBXCenter(rt.translation().x(), rt.translation().y(), rt.translation().z());
+    octomap_monitor_->getOcTreePtr()->pruneBBX();
+  }
+
   {
     boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
     last_update_time_ = ros::Time::now();