Merge pull request #5 from iceflame89/master

Add support for OcTree, update version to 0.0.9

Author: mmatl

MANIFEST.in

@@ -1,3 +1,5 @@
 # Include the license
 include LICENSE
 include fcl/fcl_defs.pxd
+include fcl/octomap_defs.pxd
+include fcl/std_defs.pxd

README.md

@@ -23,10 +23,12 @@ This package also supports most of FCL's object shapes, including:
 * Half-Space
 * Plane
 * Mesh
+* OcTree
 
 ## Installation
 
-First, install FCL using the instructions provided [here](https://github.com/flexible-collision-library/fcl).
+First, install [octomap](https://github.com/OctoMap/octomap), which is necessary using OcTree. For Ubuntu, using `sudo apt-get install liboctomap-dev`   
+Second, install FCL using the instructions provided [here](https://github.com/flexible-collision-library/fcl).
 If you're on Ubuntu 17.04 or newer, you can install FCL using `sudo apt-get install libfcl-dev`.
 Otherwise, just compile FCL from source -- it's quick and easy, and its dependencies are all easily installed via `apt` or `brew`.
 

fcl/__init__.py

@@ -1,4 +1,4 @@
-from .fcl import CollisionObject, CollisionGeometry, Transform, TriangleP, Box, Sphere, Ellipsoid, Capsule, Cone, Cylinder, Halfspace, Plane, BVHModel, DynamicAABBTreeCollisionManager, collide, continuousCollide, distance, defaultCollisionCallback, defaultDistanceCallback
+from .fcl import CollisionObject, CollisionGeometry, Transform, TriangleP, Box, Sphere, Ellipsoid, Capsule, Cone, Cylinder, Halfspace, Plane, BVHModel, OcTree, DynamicAABBTreeCollisionManager, collide, continuousCollide, distance, defaultCollisionCallback, defaultDistanceCallback
 
 from .collision_data import OBJECT_TYPE, NODE_TYPE, CCDMotionType, CCDSolverType, GJKSolverType, Contact, CostSource, CollisionRequest, CollisionResult, ContinuousCollisionRequest, ContinuousCollisionResult, DistanceRequest, DistanceResult, CollisionData, DistanceData
 

fcl/fcl.pyx

@@ -11,6 +11,8 @@ import numpy
 ctypedef np.float64_t DOUBLE_t
 
 cimport fcl_defs as defs
+cimport octomap_defs as octomap 
+cimport std_defs as std 
 from collision_data import Contact, CostSource, CollisionRequest, ContinuousCollisionRequest, CollisionResult, ContinuousCollisionResult, DistanceRequest, DistanceResult
 
 ###############################################################################
@@ -393,6 +395,18 @@ cdef class BVHModel(CollisionGeometry):
         else:
             return False
 
+cdef class OcTree(CollisionGeometry):
+    cdef octomap.OcTree* tree
+
+    def __cinit__(self, r, data):
+        cdef std.stringstream ss
+        cdef vector[char] vd = data
+        ss.write(vd.data(), len(data))
+
+        self.tree = new octomap.OcTree(r) 
+        self.tree.readBinaryData(ss)
+        self.thisptr = new defs.OcTree(defs.shared_ptr[octomap.OcTree](self.tree))
+
 
 ###############################################################################
 # Collision managers

fcl/fcl_defs.pxd

@@ -3,6 +3,7 @@ from libcpp.string cimport string
 from libcpp.vector cimport vector
 from libcpp.set cimport set
 from libcpp.memory cimport shared_ptr
+cimport octomap_defs as octomap
 
 cdef extern from "Python.h":
        ctypedef struct PyObject
@@ -400,9 +401,9 @@ cdef extern from "fcl/BVH/BVH_model.h" namespace "fcl":
         # void computeLocalAABB()
 
 
-
-
-
-
-
+cdef extern from "fcl/octree.h" namespace "fcl":
+    cdef cppclass OcTree(CollisionGeometry):
+        # Constructing
+        OcTree(FCL_REAL resolution) except +
+        OcTree(shared_ptr[octomap.OcTree]& tree_) except +
 

fcl/octomap_defs.pxd

@@ -0,0 +1,19 @@
+cimport std_defs as std 
+
+cdef extern from "octomap/OccupancyOcTreeBase.h" namespace "octomap":
+    # Cython only accepts type template parameters.
+    # see https://groups.google.com/forum/#!topic/cython-users/xAZxdCFw6Xs
+    cdef cppclass OccupancyOcTreeBase "octomap::OccupancyOcTreeBase<octomap::OcTreeNode>" :
+        # Constructing
+        OccupancyOcTreeBase(double resolution) except +
+
+        #  Reads only the data (=complete tree structure) from the input stream.
+        #  The tree needs to be constructed with the proper header information
+        #  beforehand, see readBinary().
+        std.istream& readBinaryData(std.istream &s) except +
+
+cdef extern from "octomap/OcTree.h" namespace "octomap":
+    cdef cppclass OcTree(OccupancyOcTreeBase):
+        # Constructing 
+        OcTree(double resolution) except +
+

fcl/std_defs.pxd

@@ -0,0 +1,12 @@
+
+cdef extern from "<iostream>" namespace "std":
+    cdef cppclass istream:
+        istream& write(const char*, int) except +
+
+    cdef cppclass iostream(istream):
+        iostream() except +
+
+    cdef cppclass stringstream(iostream):
+        stringstream() except +
+
+

fcl/version.py

@@ -1 +1 @@
-__version__ = '0.0.8'
+__version__ = '0.0.9'

requirements/build.bash

@@ -4,6 +4,12 @@ make -j4
 make install
 cd ..
 
+cd octomap 
+cmake .
+make -j4
+make install
+cd ..
+
 cd fcl
 cmake .
 make -j4

requirements/clone.bash

@@ -5,6 +5,13 @@ git pull
 git checkout 64f02f741ac94fccd0fb660a5bffcbe6d01d9939
 cd ..
 
+rm -rf octomap 
+git clone https://github.com/OctoMap/octomap.git
+cd octomap
+git pull
+git checkout b8c1d62a7a64ce0a5df278503f31d73acafa97e4 
+cd ..
+
 rm -rf fcl
 git clone https://github.com/flexible-collision-library/fcl.git
 cd fcl