Add function to load a point cloud as obstacle.

Author: jmirabel

idl/hpp/corbaserver/obstacle.idl

@@ -50,6 +50,20 @@ module hpp
     void loadObstacleModelFromString (in string urdfString, in string prefix)
       raises (Error);
 
+    /// Load (or update if it exists) a point cloud from a filename.
+    ///
+    /// \param filename a filename whose format must be compatible with Octomap
+    void loadPointCloudFromFilename (in string objectName, in string filename)
+      raises (Error);
+
+    /// Load (or update if it exists) a point cloud from a set of points
+    ///
+    /// \param resolution OcTree resolution. See Octomap::OcTree for more info.
+    /// \param points a Nx3 matrix representing the point cloud.
+    void loadPointCloudFromPoints (in string objectName, in double resolution,
+                                   in floatSeqSeq points)
+      raises (Error);
+
     /// \brief Remove an obstacle
     ///
     /// \param objectName name of the object to remove

src/obstacle.impl.cc

@@ -36,6 +36,7 @@
 #include <coal/BVH/BVH_model.h>
 #include <coal/mesh_loader/loader.h>
 #include <coal/shape/geometric_shapes.h>
+#include <coal/octree.h>
 
 #include <hpp/corbaserver/server-plugin.hh>
 #include <hpp/pinocchio/collision-object.hh>
@@ -82,6 +83,89 @@ void Obstacle::loadObstacleModelFromString(const char* urdfString,
   }
 }
 
+void Obstacle::loadPointCloudFromFilename(const char* objectName,
+                                          const char* filename) {
+  try {
+    core::ProblemSolverPtr_t ps = problemSolver();
+    pinocchio::GeomModelPtr_t gm = ps->obstacleGeomModel();
+  
+    if (gm->existGeometryName(objectName)) {
+      // check that the existing geometry is of type coal::OcTree
+      ::pinocchio::GeomIndex id = gm->getGeometryId(objectName);
+      coal::shared_ptr<coal::OcTree> octree =
+      coal::dynamic_pointer_cast<coal::OcTree>
+      (gm->geometryObjects[id].geometry);
+      if (!octree) {
+        HPP_THROW(Error, "Obstacle " << objectName << " exists but is not of the correct type.");
+      }
+      std::shared_ptr<octomap::OcTree> tree = std::const_pointer_cast<octomap::OcTree>(octree->getTree());
+      // Update octree
+      tree->clear();
+      tree->readBinary(filename);
+      octree->computeLocalAABB();
+    } else {
+      std::shared_ptr<octomap::OcTree> octree(new octomap::OcTree(filename));
+      coal::CollisionObject colobj (
+        coal::CollisionGeometryPtr_t(new coal::OcTree(octree))
+      ); 
+      ps->addObstacle(objectName, colobj, true, true);
+    }
+
+  } catch (const std::exception& exc) {
+    throw hpp::Error(exc.what());
+  }
+}
+
+void Obstacle::loadPointCloudFromPoints(const char* objectName,
+			                                  const CORBA::Double resolution,
+                                        const floatSeqSeq& points) {
+  auto N = points.length();
+  if (N > 0 && points[0].length() != 3) {
+    HPP_THROW(Error, "points must be a Nx3 matrix (or an empty matrix).");
+  }
+  try {
+    core::ProblemSolverPtr_t ps = problemSolver();
+    pinocchio::GeomModelPtr_t gm = ps->obstacleGeomModel();
+  
+    coal::shared_ptr<coal::OcTree> octree;
+    std::shared_ptr<octomap::OcTree> tree;
+    if (gm->existGeometryName(objectName)) {
+      // check that the existing geometry is of type coal::OcTree
+      ::pinocchio::GeomIndex id = gm->getGeometryId(objectName);
+      octree = coal::dynamic_pointer_cast<coal::OcTree> (gm->geometryObjects[id].geometry);
+      if (!octree) {
+        HPP_THROW(Error, "Obstacle " << objectName << " exists but is not of the correct type.");
+      }
+      tree = std::const_pointer_cast<octomap::OcTree>(octree->getTree());
+      if (tree->getResolution() != resolution) {
+        HPP_THROW(Error, "A point cloud with name " << objectName <<
+          " exists with resolution " << tree->getResolution() <<
+          ", which is different from the asked resolution (" << resolution
+          << ").\nYou can either remove the obstacle or stick the existing "
+          "resolution."
+        );
+      }
+      tree->clear();
+    } else {
+      tree = std::make_shared<octomap::OcTree>(resolution);
+      octree = std::make_shared<coal::OcTree>(tree);
+      coal::CollisionObject colobj (
+        octree,
+        false // Dont compute the local AABB as the octree is not ready yet.
+      );
+      ps->addObstacle(objectName, colobj, true, true);
+    }
+
+    for (decltype(N) i = 0; i < N; ++i) {
+      tree->updateNode(points[i][0], points[i][1], points[i][2], true, true);
+    }
+    tree->updateInnerOccupancy();
+    octree->computeLocalAABB();
+  } catch (const std::exception& exc) {
+    throw hpp::Error(exc.what());
+  }
+}
+
 void Obstacle::loadPolyhedron(const char* name, const char* resourcename) {
   try {
     coal::MeshLoader loader(coal::BV_OBBRSS);

src/obstacle.impl.hh

@@ -57,6 +57,13 @@ class Obstacle : public virtual POA_hpp::corbaserver::Obstacle {
   virtual void loadObstacleModelFromString(const char* urdfString,
                                            const char* prefix);
 
+  virtual void loadPointCloudFromFilename(const char* objectName,
+                                          const char* filename);
+
+  virtual void loadPointCloudFromPoints(const char* objectName,
+                                        const CORBA::Double resolution,
+                                        const floatSeqSeq& points);
+
   virtual void loadPolyhedron(const char* name, const char* filename);
 
   virtual void removeObstacleFromJoint(const char* objectName,