Skip to content

Commit

Permalink
Merge pull request #129 from rapyuta/octomap_1.8
Browse files Browse the repository at this point in the history 
API changes to support Octomap 1.8
  • Loading branch information
@jslee02
jslee02 committed Jun 19, 2016
2 parents a0802df + 3558c1e commit 1bcb2c3
Show file tree
Hide file tree
Showing 7 changed files with 131 additions and 75 deletions.
4 changes: 4 additions & 0 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,10 @@ if(NOT OCTOMAP_FOUND)
endif()
endif()
if (OCTOMAP_FOUND OR (OCTOMAP_INCLUDE_DIRS AND OCTOMAP_LIBRARY_DIRS))
string(REPLACE "." ";" VERSION_LIST ${OCTOMAP_VERSION})
list(GET VERSION_LIST 0 OCTOMAP_MAJOR_VERSION)
list(GET VERSION_LIST 1 OCTOMAP_MINOR_VERSION)
list(GET VERSION_LIST 2 OCTOMAP_PATCH_VERSION)
include_directories(${OCTOMAP_INCLUDE_DIRS})
link_directories(${OCTOMAP_LIBRARY_DIRS})
set(FCL_HAVE_OCTOMAP 1)
Expand Down
2 changes: 1 addition & 1 deletion ci/install_linux.sh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@ sudo apt-get -qq --yes --force-yes install libccd-dev
# Octomap
git clone https://github.com/OctoMap/octomap
cd octomap
git checkout tags/v1.6.8
git checkout tags/v1.8.0
mkdir build
cd build
cmake ..
Expand Down
16 changes: 16 additions & 0 deletions include/fcl/config.h.in
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,4 +48,20 @@
#cmakedefine01 FCL_BUILD_TYPE_DEBUG
#cmakedefine01 FCL_BUILD_TYPE_RELEASE

#if FCL_HAVE_OCTOMAP
#define OCTOMAP_MAJOR_VERSION @OCTOMAP_MAJOR_VERSION@
#define OCTOMAP_MINOR_VERSION @OCTOMAP_MINOR_VERSION@
#define OCTOMAP_PATCH_VERSION @OCTOMAP_PATCH_VERSION@

#define OCTOMAP_VERSION_AT_LEAST(x,y,z) \
(OCTOMAP_MAJOR_VERSION > x || (OCTOMAP_MAJOR_VERSION >= x && \
(OCTOMAP_MINOR_VERSION > y || (OCTOMAP_MINOR_VERSION >= y && \
OCTOMAP_PATCH_VERSION >= z))))

#define OCTOMAP_VERSION_AT_MOST(x,y,z) \
(OCTOMAP_MAJOR_VERSION < x || (OCTOMAP_MAJOR_VERSION <= x && \
(OCTOMAP_MINOR_VERSION < y || (OCTOMAP_MINOR_VERSION <= y && \
OCTOMAP_PATCH_VERSION <= z))))
#endif // FCL_HAVE_OCTOMAP

#endif
56 changes: 46 additions & 10 deletions include/fcl/octree.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,10 +63,6 @@ class OcTree : public CollisionGeometry

public:

/// @brief OcTreeNode must implement the following interfaces:
/// 1) childExists(i)
/// 2) getChild(i)
/// 3) hasChildren()
typedef octomap::OcTreeNode OcTreeNode;

/// @brief construct octree with a given resolution
Expand Down Expand Up @@ -98,7 +94,7 @@ class OcTree : public CollisionGeometry
}

/// @brief get the bounding volume for the root
inline AABB getRootBV() const
AABB getRootBV() const
{
FCL_REAL delta = (1 << tree->getTreeDepth()) * tree->getResolution() / 2;

Expand All @@ -107,34 +103,34 @@ class OcTree : public CollisionGeometry
}

/// @brief get the root node of the octree
inline OcTreeNode* getRoot() const
OcTreeNode* getRoot() const
{
return tree->getRoot();
}

/// @brief whether one node is completely occupied
inline bool isNodeOccupied(const OcTreeNode* node) const
bool isNodeOccupied(const OcTreeNode* node) const
{
// return tree->isNodeOccupied(node);
return node->getOccupancy() >= occupancy_threshold;
}

/// @brief whether one node is completely free
inline bool isNodeFree(const OcTreeNode* node) const
bool isNodeFree(const OcTreeNode* node) const
{
// return false; // default no definitely free node
return node->getOccupancy() <= free_threshold;
}

/// @brief whether one node is uncertain
inline bool isNodeUncertain(const OcTreeNode* node) const
bool isNodeUncertain(const OcTreeNode* node) const
{
return (!isNodeOccupied(node)) && (!isNodeFree(node));
}

/// @brief transform the octree into a bunch of boxes; uncertainty information is kept in the boxes. However, we
/// only keep the occupied boxes (i.e., the boxes whose occupied probability is higher enough).
inline std::vector<std::array<FCL_REAL, 6> > toBoxes() const
std::vector<std::array<FCL_REAL, 6> > toBoxes() const
{
std::vector<std::array<FCL_REAL, 6> > boxes;
boxes.reserve(tree->size() / 2);
Expand Down Expand Up @@ -191,6 +187,46 @@ class OcTree : public CollisionGeometry
free_threshold = d;
}

/// @return ptr to child number childIdx of node
OcTreeNode* getNodeChild(OcTreeNode* node, unsigned int childIdx)
{
#if OCTOMAP_VERSION_AT_LEAST(1,8,0)
return tree->getNodeChild(node, childIdx);
#else
return node->getChild(childIdx);
#endif
}

/// @return const ptr to child number childIdx of node
const OcTreeNode* getNodeChild(const OcTreeNode* node, unsigned int childIdx) const
{
#if OCTOMAP_VERSION_AT_LEAST(1,8,0)
return tree->getNodeChild(node, childIdx);
#else
return node->getChild(childIdx);
#endif
}

/// @brief return true if the child at childIdx exists
bool nodeChildExists(const OcTreeNode* node, unsigned int childIdx) const
{
#if OCTOMAP_VERSION_AT_LEAST(1,8,0)
return tree->nodeChildExists(node, childIdx);
#else
return node->childExists(childIdx);
#endif
}

/// @brief return true if node has at least one child
bool nodeHasChildren(const OcTreeNode* node) const
{
#if OCTOMAP_VERSION_AT_LEAST(1,8,0)
return tree->nodeHasChildren(node);
#else
return node->hasChildren();
#endif
}

/// @brief return object type, it is an octree
OBJECT_TYPE getObjectType() const { return OT_OCTREE; }

Expand Down
64 changes: 32 additions & 32 deletions include/fcl/traversal/traversal_node_octree.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -239,7 +239,7 @@ class OcTreeSolver
const S& s, const AABB& aabb2,
const Transform3f& tf1, const Transform3f& tf2) const
{
if(!root1->hasChildren())
if(!tree1->nodeHasChildren(root1))
{
if(tree1->isNodeOccupied(root1))
{
Expand All @@ -263,9 +263,9 @@ class OcTreeSolver

for(unsigned int i = 0; i < 8; ++i)
{
if(root1->childExists(i))
if(tree1->nodeChildExists(root1, i))
{
const OcTree::OcTreeNode* child = root1->getChild(i);
const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
AABB child_bv;
computeChildBV(bv1, i, child_bv);

Expand Down Expand Up @@ -311,7 +311,7 @@ class OcTreeSolver

return false;
}
else if(!root1->hasChildren())
else if(!tree1->nodeHasChildren(root1))
{
if(tree1->isNodeOccupied(root1) && s.isOccupied()) // occupied area
{
Expand Down Expand Up @@ -414,9 +414,9 @@ class OcTreeSolver

for(unsigned int i = 0; i < 8; ++i)
{
if(root1->childExists(i))
if(tree1->nodeChildExists(root1, i))
{
const OcTree::OcTreeNode* child = root1->getChild(i);
const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
AABB child_bv;
computeChildBV(bv1, i, child_bv);

Expand All @@ -441,7 +441,7 @@ class OcTreeSolver
const BVHModel<BV>* tree2, int root2,
const Transform3f& tf1, const Transform3f& tf2) const
{
if(!root1->hasChildren() && tree2->getBV(root2).isLeaf())
if(!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf())
{
if(tree1->isNodeOccupied(root1))
{
Expand Down Expand Up @@ -469,13 +469,13 @@ class OcTreeSolver

if(!tree1->isNodeOccupied(root1)) return false;

if(tree2->getBV(root2).isLeaf() || (root1->hasChildren() && (bv1.size() > tree2->getBV(root2).bv.size())))
if(tree2->getBV(root2).isLeaf() || (tree1->nodeHasChildren(root1) && (bv1.size() > tree2->getBV(root2).bv.size())))
{
for(unsigned int i = 0; i < 8; ++i)
{
if(root1->childExists(i))
if(tree1->nodeChildExists(root1, i))
{
const OcTree::OcTreeNode* child = root1->getChild(i);
const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
AABB child_bv;
computeChildBV(bv1, i, child_bv);

Expand Down Expand Up @@ -571,7 +571,7 @@ class OcTreeSolver
return false;
}
}
else if(!root1->hasChildren() && tree2->getBV(root2).isLeaf())
else if(!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf())
{
if(tree1->isNodeOccupied(root1) && tree2->isOccupied())
{
Expand Down Expand Up @@ -676,13 +676,13 @@ class OcTreeSolver
if(!obb1.overlap(obb2)) return false;
}

if(tree2->getBV(root2).isLeaf() || (root1->hasChildren() && (bv1.size() > tree2->getBV(root2).bv.size())))
if(tree2->getBV(root2).isLeaf() || (tree1->nodeHasChildren(root1) && (bv1.size() > tree2->getBV(root2).bv.size())))
{
for(unsigned int i = 0; i < 8; ++i)
{
if(root1->childExists(i))
if(tree1->nodeChildExists(root1, i))
{
const OcTree::OcTreeNode* child = root1->getChild(i);
const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
AABB child_bv;
computeChildBV(bv1, i, child_bv);

Expand Down Expand Up @@ -716,7 +716,7 @@ class OcTreeSolver
const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& bv2,
const Transform3f& tf1, const Transform3f& tf2) const
{
if(!root1->hasChildren() && !root2->hasChildren())
if(!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2))
{
if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2))
{
Expand All @@ -739,13 +739,13 @@ class OcTreeSolver

if(!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2)) return false;

if(!root2->hasChildren() || (root1->hasChildren() && (bv1.size() > bv2.size())))
if(!tree2->nodeHasChildren(root2) || (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size())))
{
for(unsigned int i = 0; i < 8; ++i)
{
if(root1->childExists(i))
if(tree1->nodeChildExists(root1, i))
{
const OcTree::OcTreeNode* child = root1->getChild(i);
const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
AABB child_bv;
computeChildBV(bv1, i, child_bv);

Expand All @@ -768,9 +768,9 @@ class OcTreeSolver
{
for(unsigned int i = 0; i < 8; ++i)
{
if(root2->childExists(i))
if(tree2->nodeChildExists(root2, i))
{
const OcTree::OcTreeNode* child = root2->getChild(i);
const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
AABB child_bv;
computeChildBV(bv2, i, child_bv);

Expand Down Expand Up @@ -822,13 +822,13 @@ class OcTreeSolver
}
else if(!root1 && root2)
{
if(root2->hasChildren())
if(tree2->nodeHasChildren(root2))
{
for(unsigned int i = 0; i < 8; ++i)
{
if(root2->childExists(i))
if(tree2->nodeChildExists(root2, i))
{
const OcTree::OcTreeNode* child = root2->getChild(i);
const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
AABB child_bv;
computeChildBV(bv2, i, child_bv);
if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, child, child_bv, tf1, tf2))
Expand All @@ -853,13 +853,13 @@ class OcTreeSolver
}
else if(root1 && !root2)
{
if(root1->hasChildren())
if(tree1->nodeHasChildren(root1))
{
for(unsigned int i = 0; i < 8; ++i)
{
if(root1->childExists(i))
if(tree1->nodeChildExists(root1, i))
{
const OcTree::OcTreeNode* child = root1->getChild(i);
const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
AABB child_bv;
computeChildBV(bv1, i, child_bv);
if(OcTreeIntersectRecurse(tree1, child, child_bv, tree2, NULL, bv2, tf1, tf2))
Expand All @@ -882,7 +882,7 @@ class OcTreeSolver

return false;
}
else if(!root1->hasChildren() && !root2->hasChildren())
else if(!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2))
{
if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) // occupied area
{
Expand Down Expand Up @@ -990,13 +990,13 @@ class OcTreeSolver
if(!obb1.overlap(obb2)) return false;
}

if(!root2->hasChildren() || (root1->hasChildren() && (bv1.size() > bv2.size())))
if(!tree2->nodeHasChildren(root2) || (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size())))
{
for(unsigned int i = 0; i < 8; ++i)
{
if(root1->childExists(i))
if(tree1->nodeChildExists(root1, i))
{
const OcTree::OcTreeNode* child = root1->getChild(i);
const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
AABB child_bv;
computeChildBV(bv1, i, child_bv);

Expand All @@ -1021,9 +1021,9 @@ class OcTreeSolver
{
for(unsigned int i = 0; i < 8; ++i)
{
if(root2->childExists(i))
if(tree2->nodeChildExists(root2, i))
{
const OcTree::OcTreeNode* child = root2->getChild(i);
const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
AABB child_bv;
computeChildBV(bv2, i, child_bv);

Expand Down
Loading

0 comments on commit 1bcb2c3

Please sign in to comment.