Rebase from 'release/noetic/geometric_shapes'

Author: rhaschke

.github/workflows/ci.yaml

@@ -29,22 +29,23 @@ jobs:
       BASEDIR: ${{ github.workspace }}/.work
       CACHE_PREFIX: ${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }}
       CLANG_TIDY_BASE_REF: ${{ github.base_ref || github.ref }}
+      GHA_CACHE_SAVE: always
 
     name: ${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }}${{ matrix.env.CATKIN_LINT && ' + catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' + clang-tidy' || ''}}
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
       # The target directory cache doesn't include the source directory because
       # that comes from the checkout.  See "prepare target_ws for cache" task below
       - name: cache target_ws
-        uses: pat-s/always-upload-cache@v2.1.5
+        uses: rhaschke/cache@main
         with:
           path: ${{ env.BASEDIR }}/target_ws
           key: target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}-${{ github.run_id }}
           restore-keys: |
             target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}
       - name: cache ccache
-        uses: pat-s/always-upload-cache@v2.1.5
+        uses: rhaschke/cache@main
         with:
           path: ${{ env.CCACHE_DIR }}
           key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }}
@@ -55,7 +56,7 @@ jobs:
         uses: ros-industrial/industrial_ci@master
         env: ${{ matrix.env }}
       - name: upload test artifacts (on failure)
-        uses: actions/upload-artifact@v2
+        uses: actions/upload-artifact@v3
         if: failure()
         with:
           name: test-results

.github/workflows/format.yaml

@@ -11,11 +11,10 @@ on:
 jobs:
   pre-commit:
     name: Format
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-20.04
     steps:
-      - uses: actions/checkout@v2
-      - uses: actions/setup-python@v2
+      - uses: actions/checkout@v3
       - name: Install clang-format-10
         run: sudo apt-get install clang-format-10
       - uses: rhaschke/[email protected]
-      - uses: pre-commit/action@v2.0.0
+      - uses: pre-commit/action@v3.0.0

.github/workflows/prerelease.yaml

@@ -21,6 +21,6 @@ jobs:
     name: "${{ matrix.distro }}"
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
       - name: industrial_ci
         uses: ros-industrial/industrial_ci@master

CHANGELOG.rst

@@ -2,6 +2,12 @@
 Changelog for package geometric_shapes
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.7.5 (2023-04-12)
+------------------
+* Limit indefinite growth of OBBs during merging (`#232 <https://github.com/ros-planning/geometric_shapes/issues/232>`_)
+* Correctly initialize OBB with default constructor
+* Contributors: Martin Pecka
+
 0.7.4 (2023-03-31)
 ------------------
 * Body::getScaledDimensions(): avoid vtable lookup (`#225 <https://github.com/ros-planning/geometric_shapes/issues/225>`_)

debian/changelog.em

@@ -42,6 +42,7 @@
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 
+#include <eigen_stl_containers/eigen_stl_containers.h>
 #include <geometric_shapes/aabb.h>
 
 namespace bodies
@@ -52,6 +53,8 @@ class OBBPrivate;
 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);
@@ -114,14 +117,27 @@ class OBB
    * \param point The point to check.
    * \return Whether the point is inside or not.
    */
-  bool contains(const Eigen::Vector3d& point);
+  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);
+  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 */

debian/compat.em

@@ -1,6 +1,6 @@
 <package format="3">
   <name>geometric_shapes</name>
-  <version>0.7.4</version>
+  <version>0.7.5</version>
   <description>Generic definitions of geometric shapes and bodies.</description>
 
   <author email="[email protected]">Ioan Sucan</author>

debian/control.em

@@ -42,16 +42,29 @@ class OBBPrivate : public FCL_OBB
 
 OBB::OBB()
 {
-  this->obb_.reset(new OBBPrivate);
+  obb_.reset(new OBBPrivate);
+  // Initialize the OBB to position 0, with 0 extents and identity rotation
+#if FCL_MAJOR_VERSION > 0 || FCL_MINOR_VERSION > 5
+  // FCL 0.6+ does not zero-initialize the OBB.
+  obb_->extent.setZero();
+  obb_->To.setZero();
+  obb_->axis.setIdentity();
+#else
+  // FCL 0.5 zero-initializes the OBB, so we just put the identity into orientation.
+  obb_->axis[0][0] = 1.0;
+  obb_->axis[1][1] = 1.0;
+  obb_->axis[2][2] = 1.0;
+#endif
 }
 
-OBB::OBB(const OBB& other) : OBB()
+OBB::OBB(const OBB& other)
 {
-  *obb_ = *other.obb_;
+  obb_.reset(new OBBPrivate(*other.obb_));
 }
 
-OBB::OBB(const Eigen::Isometry3d& pose, const Eigen::Vector3d& extents) : OBB()
+OBB::OBB(const Eigen::Isometry3d& pose, const Eigen::Vector3d& extents)
 {
+  obb_.reset(new OBBPrivate);
   setPoseAndExtents(pose, extents);
 }
 
@@ -94,20 +107,12 @@ void OBB::getPose(Eigen::Isometry3d& pose) const
 {
   pose = Eigen::Isometry3d::Identity();
   pose.translation() = fromFcl(obb_->To);
-  // If all axes are zero, we report the rotation as identity
-  // This happens if OBB is default-constructed
 #if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5
-  if (!obb_->axis[0].isZero() && !obb_->axis[3].isZero() && !obb_->axis[2].isZero())
-  {
-    pose.linear().col(0) = fromFcl(obb_->axis[0]);
-    pose.linear().col(1) = fromFcl(obb_->axis[1]);
-    pose.linear().col(2) = fromFcl(obb_->axis[2]);
-  }
+  pose.linear().col(0) = fromFcl(obb_->axis[0]);
+  pose.linear().col(1) = fromFcl(obb_->axis[1]);
+  pose.linear().col(2) = fromFcl(obb_->axis[2]);
 #else
-  if (!obb_->axis.isApprox(fcl::Matrix3d::Zero()))
-  {
-    pose.linear() = obb_->axis;
-  }
+  pose.linear() = obb_->axis;
 #endif
 }
 
@@ -132,20 +137,70 @@ void OBB::toAABB(AABB& aabb) const
 
 OBB* OBB::extendApprox(const OBB& box)
 {
+  if (this->getExtents() == Eigen::Vector3d::Zero())
+  {
+    *obb_ = *box.obb_;
+    return this;
+  }
+
+  if (this->contains(box))
+    return this;
+
+  if (box.contains(*this))
+  {
+    *obb_ = *box.obb_;
+    return this;
+  }
+
   *this->obb_ += *box.obb_;
   return this;
 }
 
-bool OBB::contains(const Eigen::Vector3d& point)
+bool OBB::contains(const Eigen::Vector3d& point) const
 {
   return obb_->contain(toFcl(point));
 }
 
-bool OBB::overlaps(const bodies::OBB& other)
+bool OBB::overlaps(const bodies::OBB& other) const
 {
   return obb_->overlap(*other.obb_);
 }
 
+EigenSTL::vector_Vector3d OBB::computeVertices() const
+{
+  const Eigen::Vector3d e = getExtents() / 2;  // do not use auto type, Eigen would be inefficient
+  // clang-format off
+  EigenSTL::vector_Vector3d result = {
+    { -e[0], -e[1], -e[2] },
+    { -e[0], -e[1],  e[2] },
+    { -e[0],  e[1], -e[2] },
+    { -e[0],  e[1],  e[2] },
+    {  e[0], -e[1], -e[2] },
+    {  e[0], -e[1],  e[2] },
+    {  e[0],  e[1], -e[2] },
+    {  e[0],  e[1],  e[2] },
+  };
+  // clang-format on
+
+  const auto pose = getPose();
+  for (auto& v : result)
+  {
+    v = pose * v;
+  }
+
+  return result;
+}
+
+bool OBB::contains(const OBB& obb) const
+{
+  for (const auto& v : obb.computeVertices())
+  {
+    if (!contains(v))
+      return false;
+  }
+  return true;
+}
+
 OBB::~OBB() = default;
 
 }  // namespace bodies