Forward port ABI compatibility workaround with OBBs to ros2 branch

moveit · Nov 26, 2024 · 1fe4313 · 1fe4313
1 parent 8f1002e
commit 1fe4313
Show file tree

Hide file tree

Showing 2 changed files with 32 additions and 6 deletions.
diff --git a/include/geometric_shapes/bodies.h b/include/geometric_shapes/bodies.h
@@ -242,8 +242,10 @@ class Body
   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;
+   * pose. Scaling and padding are accounted for.
+   * \note This function should be pure virtual, but it can't in order to maintain ABI compatibility.
+   **/
+  void computeBoundingBox(OBB& bbox) const;
 
   /** \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
@@ -312,7 +314,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;
+  void computeBoundingBox(OBB& bbox) const;
   bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
                      EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override;
 
@@ -365,7 +367,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;
+  void computeBoundingBox(OBB& bbox) const;
   bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
                      EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override;
 
@@ -428,7 +430,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;
+  void computeBoundingBox(OBB& bbox) const;
   bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
                      EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override;
 
@@ -491,7 +493,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;
+  void computeBoundingBox(OBB& bbox) const;
   bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
                      EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override;
 

diff --git a/src/bodies.cpp b/src/bodies.cpp
@@ -136,6 +136,30 @@ bool bodies::Body::samplePointInside(random_numbers::RandomNumberGenerator& rng,
   return false;
 }
 
+void bodies::Body::computeBoundingBox(bodies::OBB& bbox) const
+{
+  // NOTE: this switch is needed only because computeBoundingBox(OBB) could not be defined virtual to keep ABI
+  // compatibility; if porting to new ROS (2) releases, this function should have no base implementation for a generic
+  // body and should be pure virtual
+  switch (this->getType())
+  {
+    case shapes::SPHERE:
+      static_cast<const bodies::Sphere*>(this)->computeBoundingBox(bbox);
+      break;
+    case shapes::CYLINDER:
+      static_cast<const bodies::Cylinder*>(this)->computeBoundingBox(bbox);
+      break;
+    case shapes::BOX:
+      static_cast<const bodies::Box*>(this)->computeBoundingBox(bbox);
+      break;
+    case shapes::MESH:
+      static_cast<const bodies::ConvexMesh*>(this)->computeBoundingBox(bbox);
+      break;
+    default:
+      throw std::runtime_error("Unknown body type: " + std::to_string(this->getType()));
+  }
+}
+
 bool bodies::Sphere::containsPoint(const Eigen::Vector3d& p, bool /* verbose */) const
 {
   return (center_ - p).squaredNorm() <= radius2_;