Add voxel grid rendering to OSG renderer

dartsim · Apr 19, 2019 · 1c1996e · 1c1996e
1 parent b427e40
commit 1c1996e
Show file tree

Hide file tree

Showing 13 changed files with 741 additions and 34 deletions.
diff --git a/cmake/DARTFindDependencies.cmake b/cmake/DARTFindDependencies.cmake
@@ -90,30 +90,40 @@ dart_find_package(Boost)
 
 # octomap
 dart_find_package(octomap)
-if (octomap_FOUND AND NOT MSVC)
-  if (MSVC)
-    # Supporting Octomap on Windows is disabled for the following issue:
-    # https://github.com/OctoMap/octomap/pull/213
-    message(WARNING "Octomap ${octomap_VERSION} is found, but Octomap "
-        "is not supported on Windows until "
-        "'https://github.com/OctoMap/octomap/pull/213' "
-        "is resolved.")
-    set(HAVE_OCTOMAP FALSE CACHE BOOL "Check if octomap found." FORCE)
-  elseif (NOT octomap_VERSION VERSION_LESS 1.9.0)
-    message(WARNING "Octomap ${octomap_VERSION} is found, but Octomap 1.9.0 or "
-        "greater is not supported yet. Please see "
-        "'https://github.com/dartsim/dart/issues/1078' for the details")
-    set(HAVE_OCTOMAP FALSE CACHE BOOL "Check if octomap found." FORCE)
-  else()
-    set(HAVE_OCTOMAP TRUE CACHE BOOL "Check if octomap found." FORCE)
-    if(DART_VERBOSE)
-      message(STATUS "Looking for octomap - version ${octomap_VERSION} found")
+if(MSVC)
+  # Supporting Octomap on Windows is disabled for the following issue:
+  # https://github.com/OctoMap/octomap/pull/213
+  message(WARNING "Octomap ${octomap_VERSION} is found, but Octomap "
+      "is not supported on Windows until "
+      "'https://github.com/OctoMap/octomap/pull/213' "
+      "is resolved.")
+  set(HAVE_OCTOMAP FALSE CACHE BOOL "Check if octomap found." FORCE)
+else()
+  if(OCTOMAP_FOUND OR octomap_FOUND)
+    if(NOT DEFINED octomap_VERSION)
+      set(HAVE_OCTOMAP FALSE CACHE BOOL "Check if octomap found." FORCE)
+      message(STATUS "Looking for octomap - octomap_VERSION is not defined, "
+          "please install octomap with version information"
+      )
+    else()
+      if(NOT octomap_VERSION VERSION_LESS 1.9.0)
+        message(WARNING "Octomap ${octomap_VERSION} is found, but Octomap 1.9.0 or "
+            "greater is not supported yet. Please see "
+            "'https://github.com/dartsim/dart/issues/1078' for the details")
+        set(HAVE_OCTOMAP FALSE CACHE BOOL "Check if octomap found." FORCE)
+      else()
+        set(HAVE_OCTOMAP TRUE CACHE BOOL "Check if octomap found." FORCE)
+        if(DART_VERBOSE)
+          message(STATUS "Looking for octomap - version ${octomap_VERSION} found")
+        endif()
+      endif()
     endif()
+  else()
+    set(HAVE_OCTOMAP FALSE CACHE BOOL "Check if octomap found." FORCE)
+    message(STATUS "Looking for octomap - NOT found, to use VoxelGridShape, "
+        "please install octomap"
+    )
   endif()
-else()
-  set(HAVE_OCTOMAP FALSE CACHE BOOL "Check if octomap found." FORCE)
-  message(STATUS "Looking for octomap - NOT found, to use VoxelGridShape, "
-      "please install octomap")
 endif()
 
 #--------------------

diff --git a/dart/dynamics/CMakeLists.txt b/dart/dynamics/CMakeLists.txt
@@ -28,17 +28,14 @@ install(
   COMPONENT headers
 )
 
-dart_format_add(
-  TranslationalJoint2D.hpp
-  TranslationalJoint2D.cpp
-  detail/TranslationalJoint2DAspect.hpp
-  detail/TranslationalJoint2DAspect.cpp
-)
-
 dart_format_add(
   HeightmapShape.hpp
   VoxelGridShape.hpp
   VoxelGridShape.cpp
   SmartPointer.hpp
+  TranslationalJoint2D.hpp
+  TranslationalJoint2D.cpp
   detail/HeightmapShape-impl.hpp
+  detail/TranslationalJoint2DAspect.hpp
+  detail/TranslationalJoint2DAspect.cpp
 )
diff --git a/dart/dynamics/VoxelGridShape.cpp b/dart/dynamics/VoxelGridShape.cpp
@@ -43,23 +43,35 @@ namespace dynamics {
 namespace {
 
 //==============================================================================
-octomap::point3d toPoint3d(const Eigen::Vector3d& point)
+octomap::point3d toPoint3f(const Eigen::Vector3f& point)
 {
   return octomap::point3d(point.x(), point.y(), point.z());
 }
 
 //==============================================================================
-octomath::Quaternion toQuaternion(const Eigen::Matrix3d& rotation)
+octomap::point3d toPoint3d(const Eigen::Vector3d& point)
+{
+  return toPoint3f(point.cast<float>());
+}
+
+//==============================================================================
+octomath::Quaternion toQuaternionf(const Eigen::Matrix3f& rotation)
 {
-  Eigen::Quaterniond quat(rotation);
+  Eigen::Quaternionf quat(rotation);
   return octomath::Quaternion(quat.w(), quat.x(), quat.y(), quat.z());
 }
 
+//==============================================================================
+octomath::Quaternion toQuaterniond(const Eigen::Matrix3d& rotation)
+{
+  return toQuaternionf(rotation.cast<float>());
+}
+
 //==============================================================================
 octomap::pose6d toPose6d(const Eigen::Isometry3d& frame)
 {
   return octomap::pose6d(
-      toPoint3d(frame.translation()), toQuaternion(frame.linear()));
+      toPoint3d(frame.translation()), toQuaterniond(frame.linear()));
 }
 
 } // namespace
@@ -68,6 +80,8 @@ octomap::pose6d toPose6d(const Eigen::Isometry3d& frame)
 VoxelGridShape::VoxelGridShape(double resolution) : Shape()
 {
   setOctree(fcl_make_shared<octomap::OcTree>(resolution));
+
+  mVariance = DYNAMIC_ELEMENTS;
 }
 
 //==============================================================================
@@ -82,6 +96,8 @@ VoxelGridShape::VoxelGridShape(fcl_shared_ptr<octomap::OcTree> octree) : Shape()
   }
 
   setOctree(std::move(octree));
+
+  mVariance = DYNAMIC_ELEMENTS;
 }
 
 //==============================================================================

diff --git a/dart/gui/osg/ShapeFrameNode.cpp b/dart/gui/osg/ShapeFrameNode.cpp
@@ -48,6 +48,7 @@
 #include "dart/gui/osg/render/MeshShapeNode.hpp"
 #include "dart/gui/osg/render/SoftMeshShapeNode.hpp"
 #include "dart/gui/osg/render/LineSegmentShapeNode.hpp"
+#include "dart/gui/osg/render/VoxelGridShapeNode.hpp"
 #include "dart/gui/osg/render/WarningShapeNode.hpp"
 
 #include "dart/dynamics/Frame.hpp"
@@ -64,6 +65,7 @@
 #include "dart/dynamics/MeshShape.hpp"
 #include "dart/dynamics/SoftMeshShape.hpp"
 #include "dart/dynamics/LineSegmentShape.hpp"
+#include "dart/dynamics/VoxelGridShape.hpp"
 #include "dart/dynamics/SimpleFrame.hpp"
 
 namespace dart {
@@ -285,6 +287,15 @@ void ShapeFrameNode::createShapeNode(
     else
       warnAboutUnsuccessfulCast(shapeType, mShapeFrame->getName());
   }
+  else if(VoxelGridShape::getStaticType() == shapeType)
+  {
+    std::shared_ptr<VoxelGridShape> lss =
+        std::dynamic_pointer_cast<VoxelGridShape>(shape);
+    if(lss)
+      mRenderShapeNode = new render::VoxelGridShapeNode(lss, this);
+    else
+      warnAboutUnsuccessfulCast(shapeType, mShapeFrame->getName());
+  }
   else
   {
     mRenderShapeNode = new render::WarningShapeNode(shape, this);

diff --git a/dart/gui/osg/render/CMakeLists.txt b/dart/gui/osg/render/CMakeLists.txt
@@ -18,3 +18,8 @@ install(
   DESTINATION include/dart/gui/osg/render
   COMPONENT headers
 )
+
+dart_format_add(
+  VoxelGridShapeNode.hpp
+  VoxelGridShapeNode.cpp
+)
diff --git a/dart/gui/osg/render/ShapeNode.cpp b/dart/gui/osg/render/ShapeNode.cpp
@@ -42,7 +42,7 @@ namespace render {
 ShapeNode::ShapeNode(std::shared_ptr<dart::dynamics::Shape> shape,
                      ShapeFrameNode* parentNode,
                      ::osg::Node* node)
-  : mShape(shape),
+  : mShape(std::move(shape)),
     mParentShapeFrameNode(parentNode),
     mNode(node),
     mUtilized(true)