From 5b52ec8fac6b7434d520eb442c8bb62ddf542fe7 Mon Sep 17 00:00:00 2001
From: ahr <ahrasheed@gmail.com>
Date: Fri, 17 May 2024 14:17:03 +0200
Subject: [PATCH] Creates an octree from point cloud using fcl.OcTree

In order to do the collision detection using fcl.OcTree: use the insertPointCloud method of octomap.OcTree
Originally done by @cedriclmenard
---
 src/fcl/fcl.pyx          | 52 +++++++++++++++++++++++++++++++++++++---
 src/fcl/octomap_defs.pxd | 24 +++++++++++++++++++
 2 files changed, 73 insertions(+), 3 deletions(-)

diff --git a/src/fcl/fcl.pyx b/src/fcl/fcl.pyx
index 7738bad..01d5de1 100644
--- a/src/fcl/fcl.pyx
+++ b/src/fcl/fcl.pyx
@@ -464,15 +464,61 @@ cdef class BVHModel(CollisionGeometry):
 cdef class OcTree(CollisionGeometry):
     cdef octomap.OcTree* tree
 
-    def __cinit__(self, r, data):
+    def __cinit__(self, r, data=None, points=None, origin=None, maxrange=-1., lazy_eval=False, discretize=False):
+        self.thisptr = NULL
+
+        self.tree = new octomap.OcTree(r)
+
+        if data is not None:
+            self.readBinaryData(data)
+        elif points is not None:
+            self.insertPointCloud(points, origin, maxrange, lazy_eval, discretize)
+
+    def readBinaryData(self, data):
         cdef std.stringstream ss
         cdef vector[char] vd = data
-        ss.write(vd.data(), len(data))
 
-        self.tree = new octomap.OcTree(r)
+        if self.thisptr != NULL:
+            del self.thisptr
+
+        ss.write(vd.data(), len(data))
         self.tree.readBinaryData(ss)
         self.thisptr = new defs.OcTreed(defs.shared_ptr[octomap.OcTree](self.tree))
 
+    def insertPointCloud(self, points, origin=None, maxrange=-1., lazy_eval=False, discretize=False):
+        cdef octomap.Pointcloud pc = octomap.Pointcloud()
+
+        if self.thisptr != NULL:
+            del self.thisptr
+
+        for p in points:
+            pc.push_back(<float>p[0],
+                         <float>p[1],
+                         <float>p[2])
+        if origin is not None:
+            self.tree.insertPointCloud(
+                pc,
+                octomap.Vector3(<float>origin[0],
+                                <float>origin[1],
+                                <float>origin[2]),
+                <double?>maxrange,
+                <bool>lazy_eval,
+                <bool>discretize
+            )
+        else:
+            self.tree.insertPointCloud(
+                pc,
+                octomap.Vector3(<float>0.,
+                                <float>0.,
+                                <float>0.),
+                <double?>maxrange,
+                <bool>lazy_eval,
+                <bool>discretize
+            )
+        self.thisptr = new defs.OcTreed(defs.shared_ptr[octomap.OcTree](self.tree))
+
+
+
 
 ###############################################################################
 # Collision managers
diff --git a/src/fcl/octomap_defs.pxd b/src/fcl/octomap_defs.pxd
index 196b37c..8937fec 100644
--- a/src/fcl/octomap_defs.pxd
+++ b/src/fcl/octomap_defs.pxd
@@ -1,4 +1,23 @@
 cimport std_defs as std
+from libcpp cimport bool
+
+cdef extern from "octomap/math/Vector3.h" namespace "octomath":
+    cdef cppclass Vector3:
+        Vector3() except +
+        Vector3(float, float, float) except +
+        Vector3(Vector3& other) except +
+        float& x()
+        float& y()
+        float& z()
+
+cdef extern from "octomap/octomap_types.h" namespace "octomap":
+    ctypedef Vector3 point3d
+
+cdef extern from "octomap/Pointcloud.h" namespace "octomap":
+    cdef cppclass Pointcloud:
+        Pointcloud() except +
+        void push_back(float, float, float)
+        void push_back(point3d* p)
 
 
 cdef extern from "octomap/OccupancyOcTreeBase.h" namespace "octomap":
@@ -17,3 +36,8 @@ cdef extern from "octomap/OcTree.h" namespace "octomap":
     cdef cppclass OcTree(OccupancyOcTreeBase):
         # Constructing
         OcTree(double resolution) except +
+
+        void insertPointCloud(Pointcloud& scan, point3d& sensor_origin,
+                              double maxrange, bool lazy_eval, bool discretize) except +
+
+