Melodic ABI compatibility for Body::getScaledDimensions().

include/geometric_shapes/bodies.h

@@ -199,8 +199,11 @@ class Body
   /** \brief Get the dimensions associated to this body (as read from corresponding shape) */
   virtual std::vector<double> getDimensions() const = 0;
 
-  /** \brief Get the dimensions associated to this body (scaled and padded) */
-  virtual std::vector<double> getScaledDimensions() const = 0;
+  /** \brief Get the dimensions associated to this body (scaled and padded).
+   *  \note This functions is pure virtual in noetic-devel, but is non-virtual
+   *  in melodic-devel to keep ABI compatibility.
+   * */
+  std::vector<double> getScaledDimensions() const;
 
   /** \brief Set the dimensions of the body (from corresponding shape) */
   void setDimensions(const shapes::Shape* shape);
@@ -316,7 +319,7 @@ class Sphere : public Body
 
   /** \brief Get the radius of the sphere */
   std::vector<double> getDimensions() const override;
-  std::vector<double> getScaledDimensions() const override;
+  std::vector<double> getScaledDimensions() const;
 
   bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const override;
   double computeVolume() const override;
@@ -376,7 +379,7 @@ class Cylinder : public Body
 
   /** \brief Get the radius & length of the cylinder */
   std::vector<double> getDimensions() const override;
-  std::vector<double> getScaledDimensions() const override;
+  std::vector<double> getScaledDimensions() const;
 
   bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const override;
   double computeVolume() const override;
@@ -449,7 +452,7 @@ class Box : public Body
 
   /** \brief Get the length & width & height (x, y, z) of the box */
   std::vector<double> getDimensions() const override;
-  std::vector<double> getScaledDimensions() const override;
+  std::vector<double> getScaledDimensions() const;
 
   bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const override;
   double computeVolume() const override;
@@ -514,7 +517,7 @@ class ConvexMesh : public Body
   /** \brief Returns an empty vector */
   std::vector<double> getDimensions() const override;
   /** \brief Returns an empty vector */
-  std::vector<double> getScaledDimensions() const override;
+  std::vector<double> getScaledDimensions() const;
 
   bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const override;
   double computeVolume() const override;

src/bodies.cpp

@@ -165,6 +165,26 @@ void bodies::Body::setDimensions(const shapes::Shape* shape)
   updateInternalData();
 }
 
+std::vector<double> bodies::Body::getScaledDimensions() const
+{
+  // NOTE: this switch is needed only in melodic-devel because getScaledDimensions() could not be defined virtual to
+  // keep ABI compatibility; in noetic-devel, this function has no base implementation for a generic body and is
+  // pure virtual
+  switch (this->getType())
+  {
+    case shapes::SPHERE:
+      return static_cast<const bodies::Sphere*>(this)->getScaledDimensions();
+    case shapes::CYLINDER:
+      return static_cast<const bodies::Cylinder*>(this)->getScaledDimensions();
+    case shapes::BOX:
+      return static_cast<const bodies::Box*>(this)->getScaledDimensions();
+    case shapes::MESH:
+      return static_cast<const bodies::ConvexMesh*>(this)->getScaledDimensions();
+    default:
+      throw std::runtime_error("Unknown body type: " + std::to_string(this->getType()));
+  }
+}
+
 bool bodies::Body::samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts,
                                      Eigen::Vector3d& result) const
 {

src/body_operations.cpp

@@ -86,19 +86,19 @@ shapes::ShapeConstPtr bodies::constructShapeFromBody(const bodies::Body* body)
   {
     case shapes::SPHERE:
     {
-      const auto& dims = dynamic_cast<const bodies::Sphere*>(body)->getScaledDimensions();
+      const auto& dims = body->getScaledDimensions();
       result.reset(new shapes::Sphere(dims[0]));
       break;
     }
     case shapes::BOX:
     {
-      const auto& dims = dynamic_cast<const bodies::Box*>(body)->getScaledDimensions();
+      const auto& dims = body->getScaledDimensions();
       result.reset(new shapes::Box(dims[0], dims[1], dims[2]));
       break;
     }
     case shapes::CYLINDER:
     {
-      const auto& dims = dynamic_cast<const bodies::Cylinder*>(body)->getScaledDimensions();
+      const auto& dims = body->getScaledDimensions();
       result.reset(new shapes::Cylinder(dims[0], dims[1]));
       break;
     }

test/test_body_operations.cpp

@@ -1,36 +1,36 @@
 /*********************************************************************
-* Software License Agreement (BSD License)
-*
-*  Copyright (c) 2019, Bielefeld University
-*  All rights reserved.
-*
-*  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 Bielefeld University 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 OWNER 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.
-*********************************************************************/
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2019, Bielefeld University
+ *  All rights reserved.
+ *
+ *  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 Bielefeld University 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 OWNER 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.
+ *********************************************************************/
 
 #include <geometric_shapes/body_operations.h>
 #include <geometric_shapes/shape_operations.h>
@@ -273,6 +273,21 @@ TEST(Bodies, ConstructMarkerFromBodyMesh)
   expectVector3dSetsEqual(shapeVertices, markerVertices);
 }
 
+TEST(Bodies, ConstructShapeFromGenericBodyPtr)
+{
+  // Test the polymorphic call
+
+  const shapes::Shape* shape = new shapes::Sphere(2.0);
+  const bodies::Body* body = new Sphere(shape);
+
+  const auto constructedShape = constructShapeFromBody(body);
+  const auto constructedSphere = std::dynamic_pointer_cast<const shapes::Sphere>(constructedShape);
+
+  EXPECT_EQ(shape->type, constructedShape->type);
+  ASSERT_NE(nullptr, constructedSphere);
+  EXPECT_EQ(2.0, constructedSphere->radius);
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);