From 792cedb7f453d734241a922d78ed4c8c728d4266 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Mon, 22 Nov 2021 02:46:23 +0100 Subject: [PATCH] Melodic ABI compatibility for Body::getScaledDimensions(). --- include/geometric_shapes/bodies.h | 15 +++--- src/bodies.cpp | 20 ++++++++ src/body_operations.cpp | 6 +-- test/test_body_operations.cpp | 79 ++++++++++++++++++------------- 4 files changed, 79 insertions(+), 41 deletions(-) diff --git a/include/geometric_shapes/bodies.h b/include/geometric_shapes/bodies.h index 5d4e0dd8..f9251960 100644 --- a/include/geometric_shapes/bodies.h +++ b/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 getDimensions() const = 0; - /** \brief Get the dimensions associated to this body (scaled and padded) */ - virtual std::vector 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 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 getDimensions() const override; - std::vector getScaledDimensions() const override; + std::vector 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 getDimensions() const override; - std::vector getScaledDimensions() const override; + std::vector 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 getDimensions() const override; - std::vector getScaledDimensions() const override; + std::vector 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 getDimensions() const override; /** \brief Returns an empty vector */ - std::vector getScaledDimensions() const override; + std::vector getScaledDimensions() const; bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const override; double computeVolume() const override; diff --git a/src/bodies.cpp b/src/bodies.cpp index 1fdf32d2..0cb2d811 100644 --- a/src/bodies.cpp +++ b/src/bodies.cpp @@ -165,6 +165,26 @@ void bodies::Body::setDimensions(const shapes::Shape* shape) updateInternalData(); } +std::vector 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(this)->getScaledDimensions(); + case shapes::CYLINDER: + return static_cast(this)->getScaledDimensions(); + case shapes::BOX: + return static_cast(this)->getScaledDimensions(); + case shapes::MESH: + return static_cast(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 { diff --git a/src/body_operations.cpp b/src/body_operations.cpp index b01459dc..e0bb09e8 100644 --- a/src/body_operations.cpp +++ b/src/body_operations.cpp @@ -86,19 +86,19 @@ shapes::ShapeConstPtr bodies::constructShapeFromBody(const bodies::Body* body) { case shapes::SPHERE: { - const auto& dims = dynamic_cast(body)->getScaledDimensions(); + const auto& dims = body->getScaledDimensions(); result.reset(new shapes::Sphere(dims[0])); break; } case shapes::BOX: { - const auto& dims = dynamic_cast(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(body)->getScaledDimensions(); + const auto& dims = body->getScaledDimensions(); result.reset(new shapes::Cylinder(dims[0], dims[1])); break; } diff --git a/test/test_body_operations.cpp b/test/test_body_operations.cpp index f524543a..d03f166f 100644 --- a/test/test_body_operations.cpp +++ b/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 #include @@ -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(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);