diff --git a/.github/workflows/linux-ubuntu-bionic.yml b/.github/workflows/linux-ubuntu-bionic.yml index de34e7ad5..0c9e64ca8 100644 --- a/.github/workflows/linux-ubuntu-bionic.yml +++ b/.github/workflows/linux-ubuntu-bionic.yml @@ -14,6 +14,7 @@ jobs: sudo apt update; sudo apt -y install wget lsb-release gnupg; sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" > /etc/apt/sources.list.d/gazebo-stable.list'; + sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-prerelease $(lsb_release -cs) main" > /etc/apt/sources.list.d/gazebo-prerelease.list'; sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-keys D2486D2DD83DB69272AFE98867170598AF249743; sudo apt-get update; sudo apt -y install cmake build-essential curl g++-8 git cppcheck; diff --git a/Migration.md b/Migration.md index e043bcd38..bffce89e8 100644 --- a/Migration.md +++ b/Migration.md @@ -269,10 +269,13 @@ but with improved human-readability.. ### Additions -1. **capsule.sdf** new shape type included in `//geometry` - + description: A shape consisting of a cylinder capped by hemispheres +1. **capsule.sdf and ellipsoid.sdf** new shape types included in `//geometry` + + `capsule.sdf`: A shape consisting of a cylinder capped by hemispheres with parameters for the `radius` and `length` of cylindrical section. + + `ellipsoid.sdf`: A convex shape with up to three radii defining its + shape in of the form (x^2/a^2 + y^2/b^2 + z^2/c^2 = 1). * [Pull request 389](https://github.com/osrf/sdformat/pull/389) + * [Pull request 434](https://github.com/osrf/sdformat/pull/434) ### Modifications diff --git a/cmake/SearchForStuff.cmake b/cmake/SearchForStuff.cmake index c35cf587a..b25c54600 100644 --- a/cmake/SearchForStuff.cmake +++ b/cmake/SearchForStuff.cmake @@ -97,7 +97,7 @@ endmacro() ######################################## # Find ignition math # Set a variable for generating ProjectConfig.cmake -find_package(ignition-math6 6.7 QUIET) +find_package(ignition-math6 6.8 QUIET) if (NOT ignition-math6_FOUND) message(STATUS "Looking for ignition-math6-config.cmake - not found") BUILD_ERROR ("Missing: Ignition math (libignition-math6-dev)") diff --git a/include/sdf/CMakeLists.txt b/include/sdf/CMakeLists.txt index d94f9226f..bfa65f3cf 100644 --- a/include/sdf/CMakeLists.txt +++ b/include/sdf/CMakeLists.txt @@ -13,6 +13,7 @@ set (headers Console.hh Cylinder.hh Element.hh + Ellipsoid.hh Error.hh Exception.hh Filesystem.hh diff --git a/include/sdf/Ellipsoid.hh b/include/sdf/Ellipsoid.hh new file mode 100644 index 000000000..e888591c5 --- /dev/null +++ b/include/sdf/Ellipsoid.hh @@ -0,0 +1,97 @@ +/* + * Copyright 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef SDF_ELLIPSOID_HH_ +#define SDF_ELLIPSOID_HH_ + +#include +#include +#include +#include + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + + // Forward declare private data class. + class EllipsoidPrivate; + + /// \brief Ellipsoid represents a ellipsoid shape, and is usually accessed + /// through a Geometry. + class SDFORMAT_VISIBLE Ellipsoid + { + /// \brief Constructor + public: Ellipsoid(); + + /// \brief Copy constructor + /// \param[in] _ellipsoid Ellipsoid to copy. + public: Ellipsoid(const Ellipsoid &_ellipsoid); + + /// \brief Move constructor + /// \param[in] _ellipsoid Ellipsoid to move. + public: Ellipsoid(Ellipsoid &&_ellipsoid) noexcept; + + /// \brief Destructor + public: virtual ~Ellipsoid(); + + /// \brief Move assignment operator. + /// \param[in] _ellipsoid Ellipsoid to move. + /// \return Reference to this. + public: Ellipsoid &operator=(Ellipsoid &&_ellipsoid); + + /// \brief Assignment operator. + /// \param[in] _ellipsoid The ellipsoid to set values from. + /// \return *this + public: Ellipsoid &operator=(const Ellipsoid &_ellipsoid); + + /// \brief Load the ellipsoid geometry based on a element pointer. + /// This is *not* the usual entry point. Typical usage of the SDF DOM is + /// through the Root object. + /// \param[in] _sdf The SDF Element pointer + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(ElementPtr _sdf); + + /// \brief Get the ellipsoid's radii in meters. + /// \return The radius of the ellipsoid in meters. + public: ignition::math::Vector3d Radii() const; + + /// \brief Set the ellipsoid's x, y, and z radii in meters. + /// \param[in] _radius Vector of radii (x, y, z) of the ellipsoid in meters. + public: void SetRadii(const ignition::math::Vector3d &_radii); + + /// \brief Get a pointer to the SDF element that was used during + /// load. + /// \return SDF element pointer. The value will be nullptr if Load has + /// not been called. + public: sdf::ElementPtr Element() const; + + /// \brief Get the Ignition Math representation of this Ellipsoid. + /// \return A const reference to an ignition::math::Ellipsoidd object. + public: const ignition::math::Ellipsoidd &Shape() const; + + /// \brief Get a mutable Ignition Math representation of this Ellipsoid. + /// \return A reference to an ignition::math::Ellipsoidd object. + public: ignition::math::Ellipsoidd &Shape(); + + /// \brief Private data pointer. + private: EllipsoidPrivate *dataPtr; + }; + } +} +#endif diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index 348d38d98..1ce3c730e 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -32,6 +32,7 @@ namespace sdf class Box; class Capsule; class Cylinder; + class Ellipsoid; class Mesh; class Plane; class Sphere; @@ -60,6 +61,9 @@ namespace sdf /// \brief A capsule geometry. CAPSULE = 7, + + /// \brief An ellipsoid geometry + ELLIPSOID = 8, }; /// \brief Geometry provides access to a shape, such as a Box. Use the @@ -141,6 +145,17 @@ namespace sdf /// \param[in] _cylinder The cylinder shape. public: void SetCylinderShape(const Cylinder &_cylinder); + /// \brief Get the ellipsoid geometry, or nullptr if the contained + /// geometry is not an ellipsoid. + /// \return Pointer to the ellipsoid geometry, or nullptr if the geometry is + /// not an ellipsoid. + /// \sa GeometryType Type() const + public: const Ellipsoid *EllipsoidShape() const; + + /// \brief Set the ellipsoid shape. + /// \param[in] _ellipsoid The ellipsoid shape. + public: void SetEllipsoidShape(const Ellipsoid &_ellipsoid); + /// \brief Get the sphere geometry, or nullptr if the contained geometry is /// not a sphere. /// \return Pointer to the visual's sphere geometry, or nullptr if the diff --git a/sdf/1.8/CMakeLists.txt b/sdf/1.8/CMakeLists.txt index e0b3a612c..a1a70717b 100644 --- a/sdf/1.8/CMakeLists.txt +++ b/sdf/1.8/CMakeLists.txt @@ -11,6 +11,7 @@ set (sdfs collision.sdf contact.sdf cylinder_shape.sdf + ellipsoid_shape.sdf frame.sdf forcetorque.sdf geometry.sdf diff --git a/sdf/1.8/ellipsoid_shape.sdf b/sdf/1.8/ellipsoid_shape.sdf new file mode 100644 index 000000000..821aadf12 --- /dev/null +++ b/sdf/1.8/ellipsoid_shape.sdf @@ -0,0 +1,6 @@ + + Ellipsoid shape + + The three radii of the ellipsoid. The origin of the ellipsoid is in its geometric center (inside the center of the ellipsoid). + + diff --git a/sdf/1.8/geometry.sdf b/sdf/1.8/geometry.sdf index 9317ec9a6..884902afb 100644 --- a/sdf/1.8/geometry.sdf +++ b/sdf/1.8/geometry.sdf @@ -9,6 +9,7 @@ + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 0016b166c..c9769c651 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -21,6 +21,7 @@ set (sources Converter.cc Cylinder.cc Element.cc + Ellipsoid.cc EmbeddedSdf.cc Error.cc Exception.cc @@ -92,6 +93,7 @@ if (BUILD_SDF_TEST) Console_TEST.cc Cylinder_TEST.cc Element_TEST.cc + Ellipsoid_TEST.cc Error_TEST.cc Exception_TEST.cc Frame_TEST.cc diff --git a/src/Ellipsoid.cc b/src/Ellipsoid.cc new file mode 100644 index 000000000..5c67eccea --- /dev/null +++ b/src/Ellipsoid.cc @@ -0,0 +1,148 @@ +/* + * Copyright 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include +#include "sdf/Ellipsoid.hh" + +using namespace sdf; + +// Private data class +class sdf::EllipsoidPrivate +{ + /// \brief An ellipsoid with all three radii of 1 meter + public: ignition::math::Ellipsoidd ellipsoid{ignition::math::Vector3d::One}; + + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf; +}; + +///////////////////////////////////////////////// +Ellipsoid::Ellipsoid() + : dataPtr(new EllipsoidPrivate) +{ +} + +///////////////////////////////////////////////// +Ellipsoid::~Ellipsoid() +{ + delete this->dataPtr; + this->dataPtr = nullptr; +} + +////////////////////////////////////////////////// +Ellipsoid::Ellipsoid(const Ellipsoid &_ellipsoid) + : dataPtr(new EllipsoidPrivate) +{ + *this->dataPtr = *_ellipsoid.dataPtr; +} + +////////////////////////////////////////////////// +Ellipsoid::Ellipsoid(Ellipsoid &&_ellipsoid) noexcept + : dataPtr(std::exchange(_ellipsoid.dataPtr, nullptr)) +{ +} + +///////////////////////////////////////////////// +Ellipsoid &Ellipsoid::operator=(const Ellipsoid &_ellipsoid) +{ + return *this = Ellipsoid(_ellipsoid); +} + +///////////////////////////////////////////////// +Ellipsoid &Ellipsoid::operator=(Ellipsoid &&_ellipsoid) +{ + std::swap(this->dataPtr, _ellipsoid.dataPtr); + return *this; +} + +///////////////////////////////////////////////// +Errors Ellipsoid::Load(ElementPtr _sdf) +{ + Errors errors; + + this->dataPtr->sdf = _sdf; + + // Check that sdf is a valid pointer + if (!_sdf) + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Attempting to load a ellipsoid, but the provided SDF " + "element is null."}); + return errors; + } + + // We need a ellipsoid child element + if (_sdf->GetName() != "ellipsoid") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load a ellipsoid geometry, but the provided SDF " + "element is not a ."}); + return errors; + } + + if (_sdf->HasElement("radii")) + { + std::pair pair = + _sdf->Get( + "radii", this->dataPtr->ellipsoid.Radii()); + + if (!pair.second) + { + errors.push_back({ErrorCode::ELEMENT_INVALID, + "Invalid data for a geometry. " + "Using a radii of 1, 1, 1 "}); + } + this->dataPtr->ellipsoid.SetRadii(pair.first); + } + else + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Ellipsoid geometry is missing a child element. " + "Using a radii of 1, 1, 1."}); + } + + return errors; +} + +////////////////////////////////////////////////// +ignition::math::Vector3d Ellipsoid::Radii() const +{ + return this->dataPtr->ellipsoid.Radii(); +} + +////////////////////////////////////////////////// +void Ellipsoid::SetRadii(const ignition::math::Vector3d &_radii) +{ + this->dataPtr->ellipsoid.SetRadii(_radii); +} + +///////////////////////////////////////////////// +sdf::ElementPtr Ellipsoid::Element() const +{ + return this->dataPtr->sdf; +} + +///////////////////////////////////////////////// +const ignition::math::Ellipsoidd &Ellipsoid::Shape() const +{ + return this->dataPtr->ellipsoid; +} + +///////////////////////////////////////////////// +ignition::math::Ellipsoidd &Ellipsoid::Shape() +{ + return this->dataPtr->ellipsoid; +} diff --git a/src/Ellipsoid_TEST.cc b/src/Ellipsoid_TEST.cc new file mode 100644 index 000000000..8a4bf4138 --- /dev/null +++ b/src/Ellipsoid_TEST.cc @@ -0,0 +1,141 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include "sdf/Ellipsoid.hh" + +///////////////////////////////////////////////// +TEST(DOMEllipsoid, Construction) +{ + sdf::Ellipsoid ellipsoid; + EXPECT_EQ(nullptr, ellipsoid.Element()); + // A default ellipsoid has all three radii set to 1 + EXPECT_DOUBLE_EQ(IGN_PI * 4. / 3., ellipsoid.Shape().Volume()); + EXPECT_EQ(ignition::math::Vector3d::One, ellipsoid.Shape().Radii()); + + const ignition::math::Vector3d expectedRadii(1.0, 2.0, 3.0); + ellipsoid.SetRadii(expectedRadii); + EXPECT_EQ(expectedRadii, ellipsoid.Shape().Radii()); +} + +///////////////////////////////////////////////// +TEST(DOMEllipsoid, MoveConstructor) +{ + sdf::Ellipsoid ellipsoid; + const ignition::math::Vector3d expectedRadii(1.0, 2.0, 3.0); + ellipsoid.SetRadii(expectedRadii); + + sdf::Ellipsoid ellipsoid2(std::move(ellipsoid)); + EXPECT_EQ(expectedRadii, ellipsoid2.Shape().Radii()); +} + +///////////////////////////////////////////////// +TEST(DOMEllipsoid, CopyConstructor) +{ + sdf::Ellipsoid ellipsoid; + const ignition::math::Vector3d expectedRadii(1.0, 2.0, 3.0); + ellipsoid.SetRadii(expectedRadii); + + sdf::Ellipsoid ellipsoid2(ellipsoid); + EXPECT_EQ(expectedRadii, ellipsoid2.Shape().Radii()); +} + +///////////////////////////////////////////////// +TEST(DOMEllipsoid, CopyAssignmentOperator) +{ + sdf::Ellipsoid ellipsoid; + const ignition::math::Vector3d expectedRadii(1.0, 2.0, 3.0); + ellipsoid.SetRadii(expectedRadii); + + sdf::Ellipsoid ellipsoid2; + ellipsoid2 = ellipsoid; + EXPECT_EQ(expectedRadii, ellipsoid2.Shape().Radii()); +} + +///////////////////////////////////////////////// +TEST(DOMEllipsoid, MoveAssignmentConstructor) +{ + sdf::Ellipsoid ellipsoid; + const ignition::math::Vector3d expectedRadii(1.0, 2.0, 3.0); + ellipsoid.SetRadii(expectedRadii); + + sdf::Ellipsoid ellipsoid2; + ellipsoid2 = std::move(ellipsoid); + EXPECT_EQ(expectedRadii, ellipsoid2.Shape().Radii()); +} + +///////////////////////////////////////////////// +TEST(DOMEllipsoid, CopyAssignmentAfterMove) +{ + sdf::Ellipsoid ellipsoid1; + const ignition::math::Vector3d expectedRadii1(1.0, 2.0, 3.0); + ellipsoid1.SetRadii(expectedRadii1); + + sdf::Ellipsoid ellipsoid2; + const ignition::math::Vector3d expectedRadii2(10.0, 20.0, 30.0); + ellipsoid2.SetRadii(expectedRadii2); + + // This is similar to what std::swap does except it uses std::move for each + // assignment + sdf::Ellipsoid tmp = std::move(ellipsoid1); + ellipsoid1 = ellipsoid2; + ellipsoid2 = tmp; + + EXPECT_EQ(expectedRadii1, ellipsoid2.Shape().Radii()); + EXPECT_EQ(expectedRadii2, ellipsoid1.Shape().Radii()); +} + +///////////////////////////////////////////////// +TEST(DOMEllipsoid, Load) +{ + sdf::Ellipsoid ellipsoid; + sdf::Errors errors; + + // Null element name + errors = ellipsoid.Load(nullptr); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + EXPECT_EQ(nullptr, ellipsoid.Element()); + + // Bad element name + sdf::ElementPtr sdf(new sdf::Element()); + sdf->SetName("bad"); + errors = ellipsoid.Load(sdf); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INCORRECT_TYPE, errors[0].Code()); + EXPECT_NE(nullptr, ellipsoid.Element()); + + // Missing element + sdf->SetName("ellipsoid"); + errors = ellipsoid.Load(sdf); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + EXPECT_NE(std::string::npos, errors[0].Message().find("missing a ")) + << errors[0].Message(); + EXPECT_NE(nullptr, ellipsoid.Element()); +} + +///////////////////////////////////////////////// +TEST(DOMEllipsoid, Shape) +{ + sdf::Ellipsoid ellipsoid; + EXPECT_EQ(ignition::math::Vector3d::One, ellipsoid.Radii()); + + const ignition::math::Vector3d expectedRadii(1.0, 2.0, 3.0); + ellipsoid.Shape().SetRadii(expectedRadii); + EXPECT_EQ(expectedRadii, ellipsoid.Radii()); +} diff --git a/src/Geometry.cc b/src/Geometry.cc index a91596af6..624f93920 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -18,6 +18,7 @@ #include "sdf/Box.hh" #include "sdf/Capsule.hh" #include "sdf/Cylinder.hh" +#include "sdf/Ellipsoid.hh" #include "sdf/Mesh.hh" #include "sdf/Plane.hh" #include "sdf/Sphere.hh" @@ -39,6 +40,9 @@ class sdf::GeometryPrivate /// \brief Pointer to a cylinder. public: std::unique_ptr cylinder; + /// \brief Pointer to an ellipsoid + public: std::unique_ptr ellipsoid; + /// \brief Pointer to a plane. public: std::unique_ptr plane; @@ -88,6 +92,12 @@ Geometry::Geometry(const Geometry &_geometry) *_geometry.dataPtr->cylinder); } + if (_geometry.dataPtr->ellipsoid) + { + this->dataPtr->ellipsoid = std::make_unique( + *_geometry.dataPtr->ellipsoid); + } + if (_geometry.dataPtr->plane) { this->dataPtr->plane = std::make_unique( @@ -174,6 +184,13 @@ Errors Geometry::Load(ElementPtr _sdf) Errors err = this->dataPtr->cylinder->Load(_sdf->GetElement("cylinder")); errors.insert(errors.end(), err.begin(), err.end()); } + else if (_sdf->HasElement("ellipsoid")) + { + this->dataPtr->type = GeometryType::ELLIPSOID; + this->dataPtr->ellipsoid.reset(new Ellipsoid()); + Errors err = this->dataPtr->ellipsoid->Load(_sdf->GetElement("ellipsoid")); + errors.insert(errors.end(), err.begin(), err.end()); + } else if (_sdf->HasElement("plane")) { this->dataPtr->type = GeometryType::PLANE; @@ -259,6 +276,18 @@ void Geometry::SetCylinderShape(const Cylinder &_cylinder) this->dataPtr->cylinder = std::make_unique(_cylinder); } +///////////////////////////////////////////////// +const Ellipsoid *Geometry::EllipsoidShape() const +{ + return this->dataPtr->ellipsoid.get(); +} + +///////////////////////////////////////////////// +void Geometry::SetEllipsoidShape(const Ellipsoid &_ellipsoid) +{ + this->dataPtr->ellipsoid = std::make_unique(_ellipsoid); +} + ///////////////////////////////////////////////// const Plane *Geometry::PlaneShape() const { diff --git a/src/Geometry_TEST.cc b/src/Geometry_TEST.cc index 2150f68ac..6aaab1a2a 100644 --- a/src/Geometry_TEST.cc +++ b/src/Geometry_TEST.cc @@ -19,6 +19,7 @@ #include "sdf/Box.hh" #include "sdf/Capsule.hh" #include "sdf/Cylinder.hh" +#include "sdf/Ellipsoid.hh" #include "sdf/Geometry.hh" #include "sdf/Mesh.hh" #include "sdf/Plane.hh" @@ -40,6 +41,9 @@ TEST(DOMGeometry, Construction) geom.SetType(sdf::GeometryType::CYLINDER); EXPECT_EQ(sdf::GeometryType::CYLINDER, geom.Type()); + geom.SetType(sdf::GeometryType::ELLIPSOID); + EXPECT_EQ(sdf::GeometryType::ELLIPSOID, geom.Type()); + geom.SetType(sdf::GeometryType::PLANE); EXPECT_EQ(sdf::GeometryType::PLANE, geom.Type()); @@ -202,6 +206,22 @@ TEST(DOMGeometry, Cylinder) EXPECT_DOUBLE_EQ(4.56, geom.CylinderShape()->Length()); } +///////////////////////////////////////////////// +TEST(DOMGeometry, Ellipsoid) +{ + sdf::Geometry geom; + geom.SetType(sdf::GeometryType::ELLIPSOID); + + sdf::Ellipsoid ellipsoidShape; + const ignition::math::Vector3d expectedRadii(1, 2, 3); + ellipsoidShape.SetRadii(expectedRadii); + geom.SetEllipsoidShape(ellipsoidShape); + + EXPECT_EQ(sdf::GeometryType::ELLIPSOID, geom.Type()); + EXPECT_NE(nullptr, geom.EllipsoidShape()); + EXPECT_EQ(expectedRadii, geom.EllipsoidShape()->Radii()); +} + ///////////////////////////////////////////////// TEST(DOMGeometry, Mesh) { diff --git a/test/integration/geometry_dom.cc b/test/integration/geometry_dom.cc index 3d9c4637e..a19cfa822 100644 --- a/test/integration/geometry_dom.cc +++ b/test/integration/geometry_dom.cc @@ -23,6 +23,7 @@ #include "sdf/Cylinder.hh" #include "sdf/Collision.hh" #include "sdf/Element.hh" +#include "sdf/Ellipsoid.hh" #include "sdf/Filesystem.hh" #include "sdf/Geometry.hh" #include "sdf/Link.hh" @@ -112,6 +113,26 @@ TEST(DOMGeometry, Shapes) EXPECT_DOUBLE_EQ(2.1, cylinderVisGeom->Radius()); EXPECT_DOUBLE_EQ(10.2, cylinderVisGeom->Length()); + // Test ellipsoid collision + const sdf::Collision *ellipsoidCol = link->CollisionByName("ellipsoid_col"); + ASSERT_NE(nullptr, ellipsoidCol); + ASSERT_NE(nullptr, ellipsoidCol->Geom()); + EXPECT_EQ(sdf::GeometryType::ELLIPSOID, ellipsoidCol->Geom()->Type()); + const sdf::Ellipsoid *ellipsoidColGeom = + ellipsoidCol->Geom()->EllipsoidShape(); + ASSERT_NE(nullptr, ellipsoidColGeom); + EXPECT_EQ(ignition::math::Vector3d(1.0, 2.0, 3.0), ellipsoidColGeom->Radii()); + + // Test ellipsoid visual + const sdf::Visual *ellipsoidVis = link->VisualByName("ellipsoid_vis"); + ASSERT_NE(nullptr, ellipsoidVis); + ASSERT_NE(nullptr, ellipsoidVis->Geom()); + EXPECT_EQ(sdf::GeometryType::ELLIPSOID, ellipsoidVis->Geom()->Type()); + const sdf::Ellipsoid *ellipsoidVisGeom = + ellipsoidVis->Geom()->EllipsoidShape(); + ASSERT_NE(nullptr, ellipsoidVisGeom); + EXPECT_EQ(ignition::math::Vector3d(0.1, 0.2, 0.3), ellipsoidVisGeom->Radii()); + // Test plane collision const sdf::Collision *planeCol = link->CollisionByName("plane_col"); ASSERT_NE(nullptr, planeCol); diff --git a/test/sdf/shapes.sdf b/test/sdf/shapes.sdf index 05a2be133..76b15f4cd 100644 --- a/test/sdf/shapes.sdf +++ b/test/sdf/shapes.sdf @@ -59,6 +59,22 @@ + + + + 1.0 2.0 3.0 + + + + + + + + 0.1 0.2 0.3 + + + +