diff --git a/perception/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h b/perception/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h index 2db2ddc1b9..c581c48f56 100644 --- a/perception/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h +++ b/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_ @@ -93,9 +93,8 @@ class OccMapTree : public octomap::OcTree } protected: - bool inBBXa(const octomap::OcTreeKey& key, unsigned int depth) const; - bool inBBXb(const octomap::OcTreeKey& key, unsigned short int center_offset_key) const; - + 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; diff --git a/perception/occupancy_map_monitor/src/occupancy_map.cpp b/perception/occupancy_map_monitor/src/occupancy_map.cpp index 823f7af221..37aa00ae04 100644 --- a/perception/occupancy_map_monitor/src/occupancy_map.cpp +++ b/perception/occupancy_map_monitor/src/occupancy_map.cpp @@ -1,20 +1,57 @@ +/********************************************************************* + * 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 #include namespace occupancy_map_monitor { -bool OccMapTree::inBBXa(const octomap::OcTreeKey& key, unsigned int depth) const { +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::inBBXb(const octomap::OcTreeKey& key, unsigned short int center_offset_key) const { - return ((bbx_min_key[0] <= (key[0] + center_offset_key)) && (bbx_max_key[0] >= (key[0] - center_offset_key)) - && (bbx_min_key[1] <= (key[1] + center_offset_key)) && (bbx_max_key[1] >= (key[1] - center_offset_key)) - && (bbx_min_key[2] <= (key[2] + center_offset_key)) && (bbx_max_key[2] >= (key[2] - center_offset_key))); +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) @@ -32,26 +69,23 @@ bool OccMapTree::pruneBBXRecurs(octomap::OcTreeNode* node, const octomap::OcTree if (node->childExists(i)) { octomap::computeChildKey(i, center_offset_key, parent_key, child_key); - - //if (inBBXa(child_key, child_depth) != inBBXb(child_key, center_offset_key)) - //ROS_WARN("Pruning doesn't agree if key is in BBX"); - if (inBBXa(child_key, child_depth)) - //if (inBBXb(child_key, center_offset_key)) + if (inBBX(child_key, child_depth)) { - if (pruneBBXRecurs(node->getChild(i), child_key, child_depth)) + if (!inBBXStrict(child_key, child_depth)) { - pruned = true; - if (!node->getChild(i)->hasChildren()) // All of the childs children were pruned - { - node->deleteChild(i); - } + 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 { - //ROS_INFO("Pruning node"); node->deleteChild(i); - //node->createChild(i); size_changed = true; pruned = true; }