[ROS2] Porting bodies::Body::computeBoundingBox changes from noetic t…

…o ROS2 (#239) * merging obb into ros2 Co-authored-by: Tyler <[email protected]> * test bounding box updated * Added obb.h * updated tests and cpp to 232 * Fixed build * Fixed license * added obb to bodies header * Fixed FCL dependency * . * merged changes from 232 * Builds tests pass (copyright fails though) * Reverted copyright notice changes * Fixed copyright notices * ^ * Ran pre-commit * added pcl to package.xml * format * Copyright year Co-authored-by: Martin Pecka <[email protected]> * reversed white space change * removed commented depends * toFCL fromFCL removed, FCL 0.5 checks removed * addressing header and xml comments * re-added accidental removal of translation line * correct ifdef * fix redundant depends * added copyright notice * copied the copyright from the obb.h file * remove fcl export * Added libfcl to back to exec-depend * Fix whitespace * Update package.xml --------- Co-authored-by: Martin Pecka <[email protected]> Co-authored-by: Tyler <[email protected]> Co-authored-by: Martin Pecka <[email protected]> Co-authored-by: Robert Haschke <[email protected]>
moveit · Nov 21, 2024 · 100a053 · 100a053
1 parent 4f65a38
commit 100a053
Show file tree

Hide file tree

Showing 10 changed files with 662 additions and 5 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -52,6 +52,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
@@ -80,13 +81,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}
 )

diff --git a/include/geometric_shapes/bodies.h b/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;
 

diff --git a/include/geometric_shapes/body_operations.h b/include/geometric_shapes/body_operations.h
@@ -67,6 +67,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

diff --git a/include/geometric_shapes/obb.h b/include/geometric_shapes/obb.h
@@ -0,0 +1,142 @@
+// Copyright 2024 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
diff --git a/package.xml b/package.xml
@@ -36,6 +36,8 @@
   <depend>visualization_msgs</depend>
 
   <build_depend>assimp-dev</build_depend>
+  <build_depend>eigen</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>
@@ -45,6 +47,7 @@
   <exec_depend>assimp</exec_depend>
   <exec_depend>libboost-filesystem</exec_depend>
   <exec_depend>rosidl_default_runtime</exec_depend>
+  <exec_depend>libfcl</exec_depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>

diff --git a/src/aabb.cpp b/src/aabb.cpp
@@ -30,22 +30,24 @@
 
 #include <geometric_shapes/aabb.h>
 
+#include <fcl/geometry/shape/utility.h>
+
 void bodies::AABB::extendWithTransformedBox(const Eigen::Isometry3d& transform, const Eigen::Vector3d& box)
 {
   // 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();
 
   double x_range = 0.5 * (fabs(r(0, 0) * box[0]) + fabs(r(0, 1) * box[1]) + fabs(r(0, 2) * box[2]));
   double y_range = 0.5 * (fabs(r(1, 0) * box[0]) + fabs(r(1, 1) * box[1]) + fabs(r(1, 2) * box[2]));
   double z_range = 0.5 * (fabs(r(2, 0) * box[0]) + fabs(r(2, 1) * box[1]) + fabs(r(2, 2) * box[2]));
 
-  const Eigen::Vector3d v_delta(x_range, y_range, z_range);
-  extend(t + v_delta);
-  extend(t - v_delta);
+  fcl::AABBd aabb;
+  fcl::computeBV(fcl::Boxd(box), transform, aabb);
+  extend(aabb.min_);
+  extend(aabb.max_);
 }
diff --git a/src/bodies.cpp b/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();

diff --git a/src/body_operations.cpp b/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);
+}