[ROS2] Porting bodies::Body::computeBoundingBox changes from noetic to ROS2

CMakeLists.txt

@@ -50,6 +50,7 @@ find_package(rclcpp REQUIRED)
 find_package(resource_retriever REQUIRED)
 find_package(shape_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
+find_package(fcl REQUIRED)
 include(ConfigExtras)
 
 set(THIS_PACKAGE_EXPORT_DEPENDS
@@ -66,6 +67,7 @@ set(THIS_PACKAGE_EXPORT_DEPENDS
   resource_retriever
   shape_msgs
   visualization_msgs
+  fcl
 )
 
 # Set VERSION from package.xml
@@ -78,13 +80,15 @@ add_library(${PROJECT_NAME} SHARED
   src/bodies.cpp
   src/body_operations.cpp
   src/mesh_operations.cpp
+  src/obb.cpp
   src/shape_extents.cpp
   src/shape_operations.cpp
   src/shape_to_marker.cpp
   src/shapes.cpp
 )
 set_target_properties(${PROJECT_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION} WINDOWS_EXPORT_ALL_SYMBOLS TRUE)
 target_compile_options(${PROJECT_NAME} PRIVATE ${PROJECT_COMPILE_OPTIONS})
+target_link_libraries(${PROJECT_NAME} fcl)
 ament_target_dependencies(${PROJECT_NAME}
   ${THIS_PACKAGE_EXPORT_DEPENDS}
 )

include/geometric_shapes/bodies.h

@@ -33,6 +33,7 @@
 
 #define _USE_MATH_DEFINES
 #include "geometric_shapes/aabb.h"
+#include "geometric_shapes/obb.h"
 #include "geometric_shapes/shapes.h"
 #include <eigen_stl_containers/eigen_stl_containers.h>
 #include <random_numbers/random_numbers.h>
@@ -240,6 +241,10 @@ class Body
       pose. Scaling and padding are accounted for. */
   virtual void computeBoundingBox(AABB& bbox) const = 0;
 
+  /** \brief Compute the oriented bounding box for the body, in its current
+     pose. Scaling and padding are accounted for. */
+  virtual void computeBoundingBox(OBB& bbox) const = 0;
+
   /** \brief Get a clone of this body, but one that is located at the pose \e pose */
   inline BodyPtr cloneAt(const Eigen::Isometry3d& pose) const
   {
@@ -307,6 +312,7 @@ class Sphere : public Body
   void computeBoundingSphere(BoundingSphere& sphere) const override;
   void computeBoundingCylinder(BoundingCylinder& cylinder) const override;
   void computeBoundingBox(AABB& bbox) const override;
+  void computeBoundingBox(OBB& bbox) const override;
   bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
                      EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override;
 
@@ -359,6 +365,7 @@ class Cylinder : public Body
   void computeBoundingSphere(BoundingSphere& sphere) const override;
   void computeBoundingCylinder(BoundingCylinder& cylinder) const override;
   void computeBoundingBox(AABB& bbox) const override;
+  void computeBoundingBox(OBB& bbox) const override;
   bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
                      EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override;
 
@@ -421,6 +428,7 @@ class Box : public Body
   void computeBoundingSphere(BoundingSphere& sphere) const override;
   void computeBoundingCylinder(BoundingCylinder& cylinder) const override;
   void computeBoundingBox(AABB& bbox) const override;
+  void computeBoundingBox(OBB& bbox) const override;
   bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
                      EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override;
 
@@ -483,6 +491,7 @@ class ConvexMesh : public Body
   void computeBoundingSphere(BoundingSphere& sphere) const override;
   void computeBoundingCylinder(BoundingCylinder& cylinder) const override;
   void computeBoundingBox(AABB& bbox) const override;
+  void computeBoundingBox(OBB& bbox) const override;
   bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
                      EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override;
 

include/geometric_shapes/body_operations.h

@@ -25,7 +25,6 @@
 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
-
 /* Author: Ioan Sucan, E. Gil Jones */
 
 #ifndef GEOMETRIC_SHAPES_BODY_OPERATIONS_
@@ -67,6 +66,9 @@ void mergeBoundingSpheres(const std::vector<BoundingSphere>& spheres, BoundingSp
 /** \brief Compute an axis-aligned bounding box to enclose a set of bounding boxes. */
 void mergeBoundingBoxes(const std::vector<AABB>& boxes, AABB& mergedBox);
 
+/** \brief Compute an approximate oriented bounding box to enclose a set of bounding boxes. */
+void mergeBoundingBoxesApprox(const std::vector<OBB>& boxes, OBB& mergedBox);
+
 /** \brief Compute the bounding sphere for a set of \e bodies and store the resulting sphere in \e mergedSphere */
 void computeBoundingSphere(const std::vector<const Body*>& bodies, BoundingSphere& mergedSphere);
 }  // namespace bodies

include/geometric_shapes/obb.h

@@ -0,0 +1,142 @@
+// Copyright 2019 Open Robotics
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the Open Robotics nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+/* Author: Martin Pecka */
+
+#ifndef GEOMETRIC_SHAPES_OBB_H
+#define GEOMETRIC_SHAPES_OBB_H
+
+#include <memory>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <eigen_stl_containers/eigen_stl_containers.h>
+#include <geometric_shapes/aabb.h>
+
+namespace bodies
+{
+class OBBPrivate;
+
+/** \brief Represents an oriented bounding box. */
+class OBB
+{
+public:
+  /** \brief Initialize an oriented bounding box at position 0, with 0 extents and
+   *         identity orientation. */
+  OBB();
+  OBB(const OBB& other);
+  OBB(const Eigen::Isometry3d& pose, const Eigen::Vector3d& extents);
+  virtual ~OBB();
+
+  OBB& operator=(const OBB& other);
+
+  /**
+   * \brief Set both the pose and extents of the OBB.
+   * \param [in] pose New pose of the OBB.
+   * \param [in] extents New extents of the OBB.
+   */
+  void setPoseAndExtents(const Eigen::Isometry3d& pose, const Eigen::Vector3d& extents);
+
+  /**
+   * \brief Get the extents of the OBB.
+   * \return The extents.
+   */
+  Eigen::Vector3d getExtents() const;
+
+  /**
+   * \brief Get the extents of the OBB.
+   * \param extents [out] The extents.
+   */
+  void getExtents(Eigen::Vector3d& extents) const;
+
+  /**
+   * \brief Get the pose of the OBB.
+   * \return The pose.
+   */
+  Eigen::Isometry3d getPose() const;
+
+  /**
+   * \brief Get The pose of the OBB.
+   * \param pose The pose.
+   */
+  void getPose(Eigen::Isometry3d& pose) const;
+
+  /**
+   * \brief Convert this OBB to an axis-aligned BB.
+   * \return The AABB.
+   */
+  AABB toAABB() const;
+
+  /**
+   * \brief Convert this OBB to an axis-aligned BB.
+   * \param aabb The AABB.
+   */
+  void toAABB(AABB& aabb) const;
+
+  /**
+   * \brief Add the other OBB to this one and compute an approximate enclosing OBB.
+   * \param box The other box to add.
+   * \return Pointer to this OBB after the update.
+   */
+  OBB* extendApprox(const OBB& box);
+
+  /**
+   * \brief Check if this OBB contains the given point.
+   * \param point The point to check.
+   * \return Whether the point is inside or not.
+   */
+  bool contains(const Eigen::Vector3d& point) const;
+
+  /**
+   * \brief Check whether this and the given OBBs have nonempty intersection.
+   * \param other The other OBB to check.
+   * \return Whether the OBBs overlap.
+   */
+  bool overlaps(const OBB& other) const;
+
+  /**
+   * \brief Check if this OBB contains whole other OBB.
+   * \param point The point to check.
+   * \return Whether the point is inside or not.
+   */
+  bool contains(const OBB& obb) const;
+
+  /**
+   * \brief Compute coordinates of the 8 vertices of this OBB.
+   * \return The vertices.
+   */
+  EigenSTL::vector_Vector3d computeVertices() const;
+
+protected:
+  /** \brief PIMPL pointer */
+  std::unique_ptr<OBBPrivate> obb_;
+};
+}  // namespace bodies
+
+#endif  // GEOMETRIC_SHAPES_OBB_H

package.xml

@@ -36,6 +36,15 @@
   <depend>visualization_msgs</depend>
 
   <build_depend>assimp-dev</build_depend>
+  <!-- <build_depend>boost</build_depend> -->
+  <!-- <build_depend>eigen</build_depend>
+  <build_depend>eigen_stl_containers</build_depend> -->
+  <!-- <build_depend>libconsole-bridge-dev</build_depend> -->
+  <!--
+  <build_depend condition="$ROS_DISTRO == noetic">fcl</build_depend> -->
+  <!-- <build_depend>libqhull</build_depend>
+  <build_depend>octomap</build_depend> -->
+  <build_depend>libfcl-dev</build_depend>
   <build_depend>pkg-config</build_depend>
   <build_depend>libboost-dev</build_depend>
   <build_depend>libboost-filesystem-dev</build_depend>

src/aabb.cpp

@@ -30,14 +30,19 @@
 
 #include <geometric_shapes/aabb.h>
 
+#include <fcl/config.h>
+#if FCL_MAJOR_VERSION > 0 || FCL_MINOR_VERSION > 5
+#include <fcl/geometry/shape/utility.h>
+#endif
+
 void bodies::AABB::extendWithTransformedBox(const Eigen::Isometry3d& transform, const Eigen::Vector3d& box)
 {
+#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
   // Method adapted from FCL src/shape/geometric_shapes_utility.cpp#computeBV<AABB, Box>(...) (BSD-licensed code):
   // https://github.com/flexible-collision-library/fcl/blob/fcl-0.4/src/shape/geometric_shapes_utility.cpp#L292
-  // We don't call their code because it would need creating temporary objects, and their method is in floats.
+  // We don't call their code because it would need creating temporary objects, and their method is in floats in FCL 0.5
   //
   // Here's a nice explanation why it works: https://zeuxcg.org/2010/10/17/aabb-from-obb-with-component-wise-abs/
-
   const Eigen::Matrix3d& r = transform.rotation();
   const Eigen::Vector3d& t = transform.translation();
 
@@ -48,4 +53,10 @@ void bodies::AABB::extendWithTransformedBox(const Eigen::Isometry3d& transform,
   const Eigen::Vector3d v_delta(x_range, y_range, z_range);
   extend(t + v_delta);
   extend(t - v_delta);
+#else
+  fcl::AABBd aabb;
+  fcl::computeBV(fcl::Boxd(box), transform, aabb);
+  extend(aabb.min_);
+  extend(aabb.max_);
+#endif
 }

src/bodies.cpp

@@ -206,6 +206,15 @@ void bodies::Sphere::computeBoundingBox(bodies::AABB& bbox) const
   bbox.extendWithTransformedBox(transform, Eigen::Vector3d(2 * radiusU_, 2 * radiusU_, 2 * radiusU_));
 }
 
+void bodies::Sphere::computeBoundingBox(bodies::OBB& bbox) const
+{
+  // it's a sphere, so we do not rotate the bounding box
+  Eigen::Isometry3d transform = Eigen::Isometry3d::Identity();
+  transform.translation() = getPose().translation();
+
+  bbox.setPoseAndExtents(transform, 2 * Eigen::Vector3d(radiusU_, radiusU_, radiusU_));
+}
+
 bool bodies::Sphere::samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts,
                                        Eigen::Vector3d& result) const
 {
@@ -427,6 +436,11 @@ void bodies::Cylinder::computeBoundingBox(bodies::AABB& bbox) const
   bbox.extend(pb + e);
 }
 
+void bodies::Cylinder::computeBoundingBox(bodies::OBB& bbox) const
+{
+  bbox.setPoseAndExtents(getPose(), 2 * Eigen::Vector3d(radiusU_, radiusU_, length2_));
+}
+
 bool bodies::Cylinder::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
                                      EigenSTL::vector_Vector3d* intersections, unsigned int count) const
 {
@@ -663,6 +677,11 @@ void bodies::Box::computeBoundingBox(bodies::AABB& bbox) const
   bbox.extendWithTransformedBox(getPose(), 2 * Eigen::Vector3d(length2_, width2_, height2_));
 }
 
+void bodies::Box::computeBoundingBox(bodies::OBB& bbox) const
+{
+  bbox.setPoseAndExtents(getPose(), 2 * Eigen::Vector3d(length2_, width2_, height2_));
+}
+
 bool bodies::Box::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
                                 EigenSTL::vector_Vector3d* intersections, unsigned int count) const
 {
@@ -1155,6 +1174,11 @@ void bodies::ConvexMesh::computeBoundingBox(bodies::AABB& bbox) const
   bounding_box_.computeBoundingBox(bbox);
 }
 
+void bodies::ConvexMesh::computeBoundingBox(bodies::OBB& bbox) const
+{
+  bounding_box_.computeBoundingBox(bbox);
+}
+
 bool bodies::ConvexMesh::isPointInsidePlanes(const Eigen::Vector3d& point) const
 {
   unsigned int numplanes = mesh_data_->planes_.size();

src/body_operations.cpp

@@ -259,3 +259,9 @@ void bodies::mergeBoundingBoxes(const std::vector<bodies::AABB>& boxes, bodies::
   for (const auto& box : boxes)
     mergedBox.extend(box);
 }
+
+void bodies::mergeBoundingBoxesApprox(const std::vector<bodies::OBB>& boxes, bodies::OBB& mergedBox)
+{
+  for (const auto& box : boxes)
+    mergedBox.extendApprox(box);
+}