From e1846dba430037c510ccedb25174ba2f0ab0293c Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 9 Oct 2020 16:46:02 -0700 Subject: [PATCH 01/11] Add capsule type to sdf/1.8/geometry.sdf Signed-off-by: Steve Peters --- sdf/1.8/CMakeLists.txt | 1 + sdf/1.8/capsule_shape.sdf | 9 +++ sdf/1.8/geometry.sdf | 1 + test/sdf/shapes_world.sdf | 151 ++++++++++++++++++++++++++++++++++++++ 4 files changed, 162 insertions(+) create mode 100644 sdf/1.8/capsule_shape.sdf create mode 100644 test/sdf/shapes_world.sdf diff --git a/sdf/1.8/CMakeLists.txt b/sdf/1.8/CMakeLists.txt index 7ad9f795f..e0b3a612c 100644 --- a/sdf/1.8/CMakeLists.txt +++ b/sdf/1.8/CMakeLists.txt @@ -7,6 +7,7 @@ set (sdfs battery.sdf box_shape.sdf camera.sdf + capsule_shape.sdf collision.sdf contact.sdf cylinder_shape.sdf diff --git a/sdf/1.8/capsule_shape.sdf b/sdf/1.8/capsule_shape.sdf new file mode 100644 index 000000000..2831099ba --- /dev/null +++ b/sdf/1.8/capsule_shape.sdf @@ -0,0 +1,9 @@ + + Capsule shape + + Radius of the capsule + + + Length of the cylindrical portion of the capsule along the z axis + + diff --git a/sdf/1.8/geometry.sdf b/sdf/1.8/geometry.sdf index 5fe95ed88..9317ec9a6 100644 --- a/sdf/1.8/geometry.sdf +++ b/sdf/1.8/geometry.sdf @@ -7,6 +7,7 @@ + diff --git a/test/sdf/shapes_world.sdf b/test/sdf/shapes_world.sdf new file mode 100644 index 000000000..bfbcfcd18 --- /dev/null +++ b/test/sdf/shapes_world.sdf @@ -0,0 +1,151 @@ + + + + + true + + + + + 1 0 0 + 1.4 6.3 + + + + + + + + 0 1 0 + 2 4 + + + + + + + + 0 0 2.5 0 0 0 + + + + + 1 2 3 + + + + + 0xAB + + + + + + + + 1 2 3 + + + + + + + + 2 0 0 0 0 0 + + + + + 0.2 + 0.1 + + + + + + + + 0.2 + 0.1 + + + + + + + + 2 0 0 0 0 0 + + + + + 1.23 + + + + + + + + 1.23 + + + + + + + + 2 0 0 0 0 0 + + + + + 0.2 + 0.1 + + + + + + + + 0.2 + 0.1 + + + + + + + + 2 0 0 0 0 0 + + + + + https://ignitionfuel.org/an_org/models/a_model/mesh/mesh.dae + + my_submesh +
true
+ + 0.1 0.2 0.3 + + + + + + + + https://ignitionfuel.org/an_org/models/a_model/mesh/mesh.dae + + another_submesh +
false
+ + 1.2 2.3 3.4 + + + + + + + From febd6683946380507511fcca729c3d70b9cae98b Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 9 Oct 2020 17:03:59 -0700 Subject: [PATCH 02/11] Add Capsule DOM class based on Cylinder Copy Cylinder class and unit tests to create Capsule. Add Capsule to Geometry enum, logic, and integration test. Signed-off-by: Steve Peters --- include/sdf/CMakeLists.txt | 1 + include/sdf/Capsule.hh | 105 ++++++++++++++++++ include/sdf/Geometry.hh | 15 +++ src/CMakeLists.txt | 2 + src/Capsule.cc | 179 +++++++++++++++++++++++++++++++ src/Capsule_TEST.cc | 178 ++++++++++++++++++++++++++++++ src/Geometry.cc | 29 +++++ src/Geometry_TEST.cc | 21 ++++ test/integration/geometry_dom.cc | 21 ++++ test/sdf/shapes.sdf | 18 ++++ 10 files changed, 569 insertions(+) create mode 100644 include/sdf/Capsule.hh create mode 100644 src/Capsule.cc create mode 100644 src/Capsule_TEST.cc diff --git a/include/sdf/CMakeLists.txt b/include/sdf/CMakeLists.txt index ed0930a5a..b533e90f3 100644 --- a/include/sdf/CMakeLists.txt +++ b/include/sdf/CMakeLists.txt @@ -8,6 +8,7 @@ set (headers Atmosphere.hh Box.hh Camera.hh + Capsule.hh Collision.hh Console.hh Cylinder.hh diff --git a/include/sdf/Capsule.hh b/include/sdf/Capsule.hh new file mode 100644 index 000000000..fd51a9408 --- /dev/null +++ b/include/sdf/Capsule.hh @@ -0,0 +1,105 @@ +/* + * Copyright 2018 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_CAPSULE_HH_ +#define SDF_CAPSULE_HH_ + +#include +#include +#include +#include + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + + // Forward declare private data class. + class CapsulePrivate; + + /// \brief Capsule represents a capsule shape, and is usually accessed + /// through a Geometry. + class SDFORMAT_VISIBLE Capsule + { + /// \brief Constructor + public: Capsule(); + + /// \brief Copy constructor + /// \param[in] _capsule Capsule to copy. + public: Capsule(const Capsule &_capsule); + + /// \brief Move constructor + /// \param[in] _capsule Capsule to move. + public: Capsule(Capsule &&_capsule) noexcept; + + /// \brief Destructor + public: virtual ~Capsule(); + + /// \brief Move assignment operator. + /// \param[in] _capsule Capsule to move. + /// \return Reference to this. + public: Capsule &operator=(Capsule &&_capsule); + + /// \brief Assignment operator. + /// \param[in] _capsule The capsule to set values from. + /// \return *this + public: Capsule &operator=(const Capsule &_capsule); + + /// \brief Load the capsule 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 capsule's radius in meters. + /// \return The radius of the capsule in meters. + public: double Radius() const; + + /// \brief Set the capsule's radius in meters. + /// \param[in] _radius The radius of the capsule in meters. + public: void SetRadius(const double _radius); + + /// \brief Get the capsule's length in meters. + /// \return The length of the capsule in meters. + public: double Length() const; + + /// \brief Set the capsule's length in meters. + /// \param[in] _length The length of the capsule in meters. + public: void SetLength(const double _length); + + /// \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 Capsule. + /// \return A const reference to an ignition::math::Sphered object. + public: const ignition::math::Capsuled &Shape() const; + + /// \brief Get a mutable Ignition Math representation of this Capsule. + /// \return A reference to an ignition::math::Capsuled object. + public: ignition::math::Capsuled &Shape(); + + /// \brief Private data pointer. + private: CapsulePrivate *dataPtr; + }; + } +} +#endif diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index 148943ad3..765063344 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -30,6 +30,7 @@ namespace sdf // Forward declare private data class. class GeometryPrivate; class Box; + class Capsule; class Cylinder; class Mesh; class Plane; @@ -56,6 +57,9 @@ namespace sdf /// \brief A mesh geometry. MESH = 5, + + /// \brief A capsule geometry. + CAPSULE = 6, }; /// \brief Geometry provides access to a shape, such as a Box. Use the @@ -115,6 +119,17 @@ namespace sdf /// \param[in] _box The box shape. public: void SetBoxShape(const Box &_box); + /// \brief Get the capsule geometry, or nullptr if the contained + /// geometry is not a capsule. + /// \return Pointer to the capsule geometry, or nullptr if the + /// geometry is not a capsule. + /// \sa GeometryType Type() const + public: const Capsule *CapsuleShape() const; + + /// \brief Set the capsule shape. + /// \param[in] _capsule The capsule shape. + public: void SetCapsuleShape(const Capsule &_capsule); + /// \brief Get the cylinder geometry, or nullptr if the contained /// geometry is not a cylinder. /// \return Pointer to the visual's cylinder geometry, or nullptr if the diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 5df95ae35..e76562df8 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -15,6 +15,7 @@ set (sources Atmosphere.cc Box.cc Camera.cc + Capsule.cc Collision.cc Console.cc Converter.cc @@ -85,6 +86,7 @@ if (BUILD_SDF_TEST) Atmosphere_TEST.cc Box_TEST.cc Camera_TEST.cc + Capsule_TEST.cc Collision_TEST.cc Console_TEST.cc Cylinder_TEST.cc diff --git a/src/Capsule.cc b/src/Capsule.cc new file mode 100644 index 000000000..da6c3a3ea --- /dev/null +++ b/src/Capsule.cc @@ -0,0 +1,179 @@ +/* + * Copyright 2018 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 "sdf/Capsule.hh" + +using namespace sdf; + +// Private data class +class sdf::CapsulePrivate +{ + // A capsule with a length of 1 meter and radius if 0.5 meters. + public: ignition::math::Capsuled capsule{1.0, 0.5}; + + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf; +}; + +///////////////////////////////////////////////// +Capsule::Capsule() + : dataPtr(new CapsulePrivate) +{ +} + +///////////////////////////////////////////////// +Capsule::~Capsule() +{ + delete this->dataPtr; + this->dataPtr = nullptr; +} + +////////////////////////////////////////////////// +Capsule::Capsule(const Capsule &_capsule) + : dataPtr(new CapsulePrivate) +{ + this->dataPtr->capsule = _capsule.dataPtr->capsule; + this->dataPtr->sdf = _capsule.dataPtr->sdf; +} + +////////////////////////////////////////////////// +Capsule::Capsule(Capsule &&_capsule) noexcept + : dataPtr(std::exchange(_capsule.dataPtr, nullptr)) +{ +} + +///////////////////////////////////////////////// +Capsule &Capsule::operator=(const Capsule &_capsule) +{ + return *this = Capsule(_capsule); +} + +///////////////////////////////////////////////// +Capsule &Capsule::operator=(Capsule &&_capsule) +{ + std::swap(this->dataPtr, _capsule.dataPtr); + return *this; +} + +///////////////////////////////////////////////// +Errors Capsule::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 capsule, but the provided SDF " + "element is null."}); + return errors; + } + + // We need a capsule child element + if (_sdf->GetName() != "capsule") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load a capsule geometry, but the provided SDF " + "element is not a ."}); + return errors; + } + + if (_sdf->HasElement("radius")) + { + std::pair pair = _sdf->Get("radius", + this->dataPtr->capsule.Radius()); + + if (!pair.second) + { + errors.push_back({ErrorCode::ELEMENT_INVALID, + "Invalid data for a geometry. " + "Using a radius of 1."}); + } + this->dataPtr->capsule.SetRadius(pair.first); + } + else + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Capsule geometry is missing a child element. " + "Using a radius of 1."}); + } + + if (_sdf->HasElement("length")) + { + std::pair pair = _sdf->Get("length", + this->dataPtr->capsule.Length()); + + if (!pair.second) + { + errors.push_back({ErrorCode::ELEMENT_INVALID, + "Invalid data for a geometry. " + "Using a length of 1."}); + } + this->dataPtr->capsule.SetLength(pair.first); + } + else + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Capsule geometry is missing a child element. " + "Using a length of 1."}); + } + + return errors; +} + +////////////////////////////////////////////////// +double Capsule::Radius() const +{ + return this->dataPtr->capsule.Radius(); +} + +////////////////////////////////////////////////// +void Capsule::SetRadius(const double _radius) +{ + this->dataPtr->capsule.SetRadius(_radius); +} + +////////////////////////////////////////////////// +double Capsule::Length() const +{ + return this->dataPtr->capsule.Length(); +} + +////////////////////////////////////////////////// +void Capsule::SetLength(const double _length) +{ + this->dataPtr->capsule.SetLength(_length); +} + +///////////////////////////////////////////////// +sdf::ElementPtr Capsule::Element() const +{ + return this->dataPtr->sdf; +} + +///////////////////////////////////////////////// +const ignition::math::Capsuled &Capsule::Shape() const +{ + return this->dataPtr->capsule; +} + +///////////////////////////////////////////////// +ignition::math::Capsuled &Capsule::Shape() +{ + return this->dataPtr->capsule; +} diff --git a/src/Capsule_TEST.cc b/src/Capsule_TEST.cc new file mode 100644 index 000000000..67ac5d2b4 --- /dev/null +++ b/src/Capsule_TEST.cc @@ -0,0 +1,178 @@ +/* + * Copyright (C) 2018 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/Capsule.hh" + +///////////////////////////////////////////////// +TEST(DOMCapsule, Construction) +{ + sdf::Capsule capsule; + EXPECT_EQ(nullptr, capsule.Element()); + // A default capsule has a length of 1 meter and radius if 0.5 meters. + EXPECT_DOUBLE_EQ(IGN_PI * std::pow(0.5, 2) * (1.0 + 4./3. * 0.5), + capsule.Shape().Volume()); + + EXPECT_DOUBLE_EQ(0.5, capsule.Radius()); + EXPECT_DOUBLE_EQ(1.0, capsule.Length()); + + capsule.SetRadius(0.5); + capsule.SetLength(2.3); + + EXPECT_DOUBLE_EQ(0.5, capsule.Radius()); + EXPECT_DOUBLE_EQ(2.3, capsule.Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCapsule, MoveConstructor) +{ + sdf::Capsule capsule; + capsule.SetRadius(0.2); + capsule.SetLength(3.0); + EXPECT_DOUBLE_EQ(IGN_PI * std::pow(0.2, 2) * (3.0 + 4./3. * 0.2), + capsule.Shape().Volume()); + + sdf::Capsule capsule2(std::move(capsule)); + EXPECT_DOUBLE_EQ(0.2, capsule2.Radius()); + EXPECT_DOUBLE_EQ(3.0, capsule2.Length()); + + EXPECT_DOUBLE_EQ(IGN_PI * std::pow(0.2, 2) * (3.0 + 4./3. * 0.2), + capsule2.Shape().Volume()); + EXPECT_DOUBLE_EQ(0.2, capsule2.Shape().Radius()); + EXPECT_DOUBLE_EQ(3.0, capsule2.Shape().Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCapsule, CopyConstructor) +{ + sdf::Capsule capsule; + capsule.SetRadius(0.2); + capsule.SetLength(3.0); + + sdf::Capsule capsule2(capsule); + EXPECT_DOUBLE_EQ(0.2, capsule2.Radius()); + EXPECT_DOUBLE_EQ(3.0, capsule2.Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCapsule, CopyAssignmentOperator) +{ + sdf::Capsule capsule; + capsule.SetRadius(0.2); + capsule.SetLength(3.0); + + sdf::Capsule capsule2; + capsule2 = capsule; + EXPECT_DOUBLE_EQ(0.2, capsule2.Radius()); + EXPECT_DOUBLE_EQ(3.0, capsule2.Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCapsule, MoveAssignmentConstructor) +{ + sdf::Capsule capsule; + capsule.SetRadius(0.2); + capsule.SetLength(3.0); + + sdf::Capsule capsule2; + capsule2 = std::move(capsule); + EXPECT_DOUBLE_EQ(0.2, capsule2.Radius()); + EXPECT_DOUBLE_EQ(3.0, capsule2.Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCapsule, CopyAssignmentAfterMove) +{ + sdf::Capsule capsule1; + capsule1.SetRadius(0.2); + capsule1.SetLength(3.0); + + sdf::Capsule capsule2; + capsule2.SetRadius(2); + capsule2.SetLength(30.0); + + // This is similar to what std::swap does except it uses std::move for each + // assignment + sdf::Capsule tmp = std::move(capsule1); + capsule1 = capsule2; + capsule2 = tmp; + + EXPECT_DOUBLE_EQ(2, capsule1.Radius()); + EXPECT_DOUBLE_EQ(30, capsule1.Length()); + + EXPECT_DOUBLE_EQ(0.2, capsule2.Radius()); + EXPECT_DOUBLE_EQ(3.0, capsule2.Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCapsule, Load) +{ + sdf::Capsule capsule; + sdf::Errors errors; + + // Null element name + errors = capsule.Load(nullptr); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + EXPECT_EQ(nullptr, capsule.Element()); + + // Bad element name + sdf::ElementPtr sdf(new sdf::Element()); + sdf->SetName("bad"); + errors = capsule.Load(sdf); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INCORRECT_TYPE, errors[0].Code()); + EXPECT_NE(nullptr, capsule.Element()); + + // Missing and elements + sdf->SetName("capsule"); + errors = capsule.Load(sdf); + ASSERT_EQ(2u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + EXPECT_NE(std::string::npos, errors[0].Message().find("missing a ")); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[1].Code()); + EXPECT_NE(std::string::npos, errors[1].Message().find("missing a ")); + EXPECT_NE(nullptr, capsule.Element()); + + // Add a radius element + sdf::ElementPtr radiusDesc(new sdf::Element()); + radiusDesc->SetName("radius"); + radiusDesc->AddValue("double", "1.0", "1", "radius"); + sdf->AddElementDescription(radiusDesc); + sdf::ElementPtr radiusElem = sdf->AddElement("radius"); + radiusElem->Set(2.0); + + // Missing element + sdf->SetName("capsule"); + errors = capsule.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 ")); +} + +///////////////////////////////////////////////// +TEST(DOMCapsule, Shape) +{ + sdf::Capsule capsule; + EXPECT_DOUBLE_EQ(0.5, capsule.Radius()); + EXPECT_DOUBLE_EQ(1.0, capsule.Length()); + + capsule.Shape().SetRadius(0.123); + capsule.Shape().SetLength(0.456); + EXPECT_DOUBLE_EQ(0.123, capsule.Radius()); + EXPECT_DOUBLE_EQ(0.456, capsule.Length()); +} diff --git a/src/Geometry.cc b/src/Geometry.cc index 96eba0f0a..a91596af6 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -16,6 +16,7 @@ */ #include "sdf/Geometry.hh" #include "sdf/Box.hh" +#include "sdf/Capsule.hh" #include "sdf/Cylinder.hh" #include "sdf/Mesh.hh" #include "sdf/Plane.hh" @@ -32,6 +33,9 @@ class sdf::GeometryPrivate /// \brief Pointer to a box. public: std::unique_ptr box; + /// \brief Pointer to a capsule. + public: std::unique_ptr capsule; + /// \brief Pointer to a cylinder. public: std::unique_ptr cylinder; @@ -72,6 +76,12 @@ Geometry::Geometry(const Geometry &_geometry) this->dataPtr->box = std::make_unique(*_geometry.dataPtr->box); } + if (_geometry.dataPtr->capsule) + { + this->dataPtr->capsule = std::make_unique( + *_geometry.dataPtr->capsule); + } + if (_geometry.dataPtr->cylinder) { this->dataPtr->cylinder = std::make_unique( @@ -150,6 +160,13 @@ Errors Geometry::Load(ElementPtr _sdf) Errors err = this->dataPtr->box->Load(_sdf->GetElement("box")); errors.insert(errors.end(), err.begin(), err.end()); } + else if (_sdf->HasElement("capsule")) + { + this->dataPtr->type = GeometryType::CAPSULE; + this->dataPtr->capsule.reset(new Capsule()); + Errors err = this->dataPtr->capsule->Load(_sdf->GetElement("capsule")); + errors.insert(errors.end(), err.begin(), err.end()); + } else if (_sdf->HasElement("cylinder")) { this->dataPtr->type = GeometryType::CYLINDER; @@ -218,6 +235,18 @@ void Geometry::SetSphereShape(const Sphere &_sphere) this->dataPtr->sphere = std::make_unique(_sphere); } +///////////////////////////////////////////////// +const Capsule *Geometry::CapsuleShape() const +{ + return this->dataPtr->capsule.get(); +} + +///////////////////////////////////////////////// +void Geometry::SetCapsuleShape(const Capsule &_capsule) +{ + this->dataPtr->capsule = std::make_unique(_capsule); +} + ///////////////////////////////////////////////// const Cylinder *Geometry::CylinderShape() const { diff --git a/src/Geometry_TEST.cc b/src/Geometry_TEST.cc index 77f19fc55..2150f68ac 100644 --- a/src/Geometry_TEST.cc +++ b/src/Geometry_TEST.cc @@ -17,6 +17,7 @@ #include #include "sdf/Box.hh" +#include "sdf/Capsule.hh" #include "sdf/Cylinder.hh" #include "sdf/Geometry.hh" #include "sdf/Mesh.hh" @@ -33,6 +34,9 @@ TEST(DOMGeometry, Construction) geom.SetType(sdf::GeometryType::BOX); EXPECT_EQ(sdf::GeometryType::BOX, geom.Type()); + geom.SetType(sdf::GeometryType::CAPSULE); + EXPECT_EQ(sdf::GeometryType::CAPSULE, geom.Type()); + geom.SetType(sdf::GeometryType::CYLINDER); EXPECT_EQ(sdf::GeometryType::CYLINDER, geom.Type()); @@ -164,6 +168,23 @@ TEST(DOMGeometry, Sphere) EXPECT_DOUBLE_EQ(0.123, geom.SphereShape()->Radius()); } +///////////////////////////////////////////////// +TEST(DOMGeometry, Capsule) +{ + sdf::Geometry geom; + geom.SetType(sdf::GeometryType::CAPSULE); + + sdf::Capsule capsuleShape; + capsuleShape.SetRadius(0.123); + capsuleShape.SetLength(4.56); + geom.SetCapsuleShape(capsuleShape); + + EXPECT_EQ(sdf::GeometryType::CAPSULE, geom.Type()); + EXPECT_NE(nullptr, geom.CapsuleShape()); + EXPECT_DOUBLE_EQ(0.123, geom.CapsuleShape()->Radius()); + EXPECT_DOUBLE_EQ(4.56, geom.CapsuleShape()->Length()); +} + ///////////////////////////////////////////////// TEST(DOMGeometry, Cylinder) { diff --git a/test/integration/geometry_dom.cc b/test/integration/geometry_dom.cc index 3f1e44f21..3d9c4637e 100644 --- a/test/integration/geometry_dom.cc +++ b/test/integration/geometry_dom.cc @@ -19,6 +19,7 @@ #include #include "sdf/Box.hh" +#include "sdf/Capsule.hh" #include "sdf/Cylinder.hh" #include "sdf/Collision.hh" #include "sdf/Element.hh" @@ -71,6 +72,26 @@ TEST(DOMGeometry, Shapes) ASSERT_NE(nullptr, boxVisGeom); EXPECT_EQ(ignition::math::Vector3d(1, 2, 3), boxVisGeom->Size()); + // Test capsule collision + const sdf::Collision *capsuleCol = link->CollisionByName("capsule_col"); + ASSERT_NE(nullptr, capsuleCol); + ASSERT_NE(nullptr, capsuleCol->Geom()); + EXPECT_EQ(sdf::GeometryType::CAPSULE, capsuleCol->Geom()->Type()); + const sdf::Capsule *capsuleColGeom = capsuleCol->Geom()->CapsuleShape(); + ASSERT_NE(nullptr, capsuleColGeom); + EXPECT_DOUBLE_EQ(0.2, capsuleColGeom->Radius()); + EXPECT_DOUBLE_EQ(0.1, capsuleColGeom->Length()); + + // Test capsule visual + const sdf::Visual *capsuleVis = link->VisualByName("capsule_vis"); + ASSERT_NE(nullptr, capsuleVis); + ASSERT_NE(nullptr, capsuleVis->Geom()); + EXPECT_EQ(sdf::GeometryType::CAPSULE, capsuleVis->Geom()->Type()); + const sdf::Capsule *capsuleVisGeom = capsuleVis->Geom()->CapsuleShape(); + ASSERT_NE(nullptr, capsuleVisGeom); + EXPECT_DOUBLE_EQ(2.1, capsuleVisGeom->Radius()); + EXPECT_DOUBLE_EQ(10.2, capsuleVisGeom->Length()); + // Test cylinder collision const sdf::Collision *cylinderCol = link->CollisionByName("cylinder_col"); ASSERT_NE(nullptr, cylinderCol); diff --git a/test/sdf/shapes.sdf b/test/sdf/shapes.sdf index 3ba9278d2..05a2be133 100644 --- a/test/sdf/shapes.sdf +++ b/test/sdf/shapes.sdf @@ -23,6 +23,24 @@ + + + + 0.2 + 0.1 + + + + + + + + 2.1 + 10.2 + + + + From a5d381c8a234fd81f144b48fa15836de279edb99 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 12 Oct 2020 23:44:50 -0700 Subject: [PATCH 03/11] Require ignition-math6 6.7 Signed-off-by: Steve Peters --- cmake/SearchForStuff.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/SearchForStuff.cmake b/cmake/SearchForStuff.cmake index 4c0311789..c35cf587a 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 QUIET) +find_package(ignition-math6 6.7 QUIET) if (NOT ignition-math6_FOUND) message(STATUS "Looking for ignition-math6-config.cmake - not found") BUILD_ERROR ("Missing: Ignition math (libignition-math6-dev)") From b00ed0b4af47ffeb29a740ad855327cf918538f2 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 6 Nov 2020 19:58:03 -0800 Subject: [PATCH 04/11] Fix copyright dates Signed-off-by: Steve Peters --- include/sdf/Capsule.hh | 2 +- src/Capsule.cc | 2 +- src/Capsule_TEST.cc | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/include/sdf/Capsule.hh b/include/sdf/Capsule.hh index fd51a9408..bfb0adb65 100644 --- a/include/sdf/Capsule.hh +++ b/include/sdf/Capsule.hh @@ -1,5 +1,5 @@ /* - * Copyright 2018 Open Source Robotics Foundation + * 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. diff --git a/src/Capsule.cc b/src/Capsule.cc index da6c3a3ea..8aa75df30 100644 --- a/src/Capsule.cc +++ b/src/Capsule.cc @@ -1,5 +1,5 @@ /* - * Copyright 2018 Open Source Robotics Foundation + * 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. diff --git a/src/Capsule_TEST.cc b/src/Capsule_TEST.cc index 67ac5d2b4..cfebc67b7 100644 --- a/src/Capsule_TEST.cc +++ b/src/Capsule_TEST.cc @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. From d0a8dd582666bfd6d70654236d241d560f411426 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 6 Nov 2020 19:58:23 -0800 Subject: [PATCH 05/11] Bump enum value since HEIGHTMAP will be 6 Signed-off-by: Steve Peters --- include/sdf/Geometry.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index 765063344..348d38d98 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -59,7 +59,7 @@ namespace sdf MESH = 5, /// \brief A capsule geometry. - CAPSULE = 6, + CAPSULE = 7, }; /// \brief Geometry provides access to a shape, such as a Box. Use the From 861a346d2f177fea1c78052572ab5ed3ee981824 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 24 Nov 2020 18:32:33 -0800 Subject: [PATCH 06/11] Fix error messages about default radius Signed-off-by: Steve Peters --- src/Capsule.cc | 4 ++-- src/Cylinder.cc | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/Capsule.cc b/src/Capsule.cc index 8aa75df30..4c24a8c4a 100644 --- a/src/Capsule.cc +++ b/src/Capsule.cc @@ -102,7 +102,7 @@ Errors Capsule::Load(ElementPtr _sdf) { errors.push_back({ErrorCode::ELEMENT_INVALID, "Invalid data for a geometry. " - "Using a radius of 1."}); + "Using a radius of 0.5."}); } this->dataPtr->capsule.SetRadius(pair.first); } @@ -110,7 +110,7 @@ Errors Capsule::Load(ElementPtr _sdf) { errors.push_back({ErrorCode::ELEMENT_MISSING, "Capsule geometry is missing a child element. " - "Using a radius of 1."}); + "Using a radius of 0.5."}); } if (_sdf->HasElement("length")) diff --git a/src/Cylinder.cc b/src/Cylinder.cc index 0b878e070..07058d58c 100644 --- a/src/Cylinder.cc +++ b/src/Cylinder.cc @@ -102,7 +102,7 @@ Errors Cylinder::Load(ElementPtr _sdf) { errors.push_back({ErrorCode::ELEMENT_INVALID, "Invalid data for a geometry. " - "Using a radius of 1."}); + "Using a radius of 0.5."}); } this->dataPtr->cylinder.SetRadius(pair.first); } @@ -110,7 +110,7 @@ Errors Cylinder::Load(ElementPtr _sdf) { errors.push_back({ErrorCode::ELEMENT_MISSING, "Cylinder geometry is missing a child element. " - "Using a radius of 1."}); + "Using a radius of 0.5."}); } if (_sdf->HasElement("length")) From 09f0658e335f479b149a67e7dbb19feb785fd4b2 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 24 Nov 2020 19:23:47 -0800 Subject: [PATCH 07/11] Test shapes files with `ign sdf --check` Signed-off-by: Steve Peters --- src/ign_TEST.cc | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index ecf31ae9c..1dd77bf29 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -809,6 +809,29 @@ TEST(check, SDF) } } +///////////////////////////////////////////////// +TEST(check_shapes_sdf, SDF) +{ + std::string pathBase = PROJECT_SOURCE_PATH; + pathBase += "/test/sdf"; + + { + std::string path = pathBase +"/shapes.sdf"; + + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_EQ("Valid.\n", output); + } + + { + std::string path = pathBase +"/shapes_world.sdf"; + + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_EQ("Valid.\n", output); + } +} + ///////////////////////////////////////////////// TEST(check_model_sdf, SDF) { From feb90f6349db409fe440894f368ead987ee2ad4d Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 24 Nov 2020 19:28:18 -0800 Subject: [PATCH 08/11] Simplify copy constructor Signed-off-by: Steve Peters --- src/Capsule.cc | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/Capsule.cc b/src/Capsule.cc index 4c24a8c4a..1b27dfb97 100644 --- a/src/Capsule.cc +++ b/src/Capsule.cc @@ -45,8 +45,7 @@ Capsule::~Capsule() Capsule::Capsule(const Capsule &_capsule) : dataPtr(new CapsulePrivate) { - this->dataPtr->capsule = _capsule.dataPtr->capsule; - this->dataPtr->sdf = _capsule.dataPtr->sdf; + *this->dataPtr = *_capsule.dataPtr; } ////////////////////////////////////////////////// From 778e3fbe4f896ea16abbf7f4bfa4de64e38e54fc Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 25 Nov 2020 00:24:02 -0800 Subject: [PATCH 09/11] Update Migration guide for capsule Signed-off-by: Steve Peters --- Migration.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/Migration.md b/Migration.md index 313586594..10744c24d 100644 --- a/Migration.md +++ b/Migration.md @@ -264,6 +264,13 @@ but with improved human-readability.. ## SDF protocol 1.7 to 1.8 +### Additions + +1. **capsule.sdf** new shape type included in `//geometry` + + description: A shape consisting of a cylinder capped by hemispheres + with parameters for the `radius` and `length` of cylindrical section. + * [Pull request 389](https://github.com/osrf/sdformat/pull/389) + ### Modifications 1. **joint.sdf** `child` and `parent` elements accept frame names instead of only link names From 1c091565ac48be243c688d6da583a46e769b29e0 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 2 Dec 2020 00:03:13 -0800 Subject: [PATCH 10/11] Make ~Capsule non-virtual Signed-off-by: Steve Peters --- include/sdf/Capsule.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/sdf/Capsule.hh b/include/sdf/Capsule.hh index bfb0adb65..0739e554c 100644 --- a/include/sdf/Capsule.hh +++ b/include/sdf/Capsule.hh @@ -47,7 +47,7 @@ namespace sdf public: Capsule(Capsule &&_capsule) noexcept; /// \brief Destructor - public: virtual ~Capsule(); + public: ~Capsule(); /// \brief Move assignment operator. /// \param[in] _capsule Capsule to move. From 6169fecab13375404372cca36d3136f97909a095 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 2 Dec 2020 01:11:03 -0800 Subject: [PATCH 11/11] Don't check for missing required elements The parser will populate them with default values. Signed-off-by: Steve Peters --- src/Capsule.cc | 31 +++++++++++-------------------- src/Capsule_TEST.cc | 15 +++++++++------ src/Cylinder.cc | 31 +++++++++++-------------------- src/Cylinder_TEST.cc | 15 +++++++++------ 4 files changed, 40 insertions(+), 52 deletions(-) diff --git a/src/Capsule.cc b/src/Capsule.cc index 1b27dfb97..33c496036 100644 --- a/src/Capsule.cc +++ b/src/Capsule.cc @@ -14,6 +14,7 @@ * limitations under the License. * */ +#include #include "sdf/Capsule.hh" using namespace sdf; @@ -92,45 +93,35 @@ Errors Capsule::Load(ElementPtr _sdf) return errors; } - if (_sdf->HasElement("radius")) { std::pair pair = _sdf->Get("radius", this->dataPtr->capsule.Radius()); if (!pair.second) { - errors.push_back({ErrorCode::ELEMENT_INVALID, - "Invalid data for a geometry. " - "Using a radius of 0.5."}); + std::stringstream ss; + ss << "Invalid data for a geometry. " + << "Using a radius of " + << this->dataPtr->capsule.Radius() << "."; + errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()}); } this->dataPtr->capsule.SetRadius(pair.first); } - else - { - errors.push_back({ErrorCode::ELEMENT_MISSING, - "Capsule geometry is missing a child element. " - "Using a radius of 0.5."}); - } - if (_sdf->HasElement("length")) { std::pair pair = _sdf->Get("length", this->dataPtr->capsule.Length()); if (!pair.second) { - errors.push_back({ErrorCode::ELEMENT_INVALID, - "Invalid data for a geometry. " - "Using a length of 1."}); + std::stringstream ss; + ss << "Invalid data for a geometry. " + << "Using a length of " + << this->dataPtr->capsule.Length() << "."; + errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()}); } this->dataPtr->capsule.SetLength(pair.first); } - else - { - errors.push_back({ErrorCode::ELEMENT_MISSING, - "Capsule geometry is missing a child element. " - "Using a length of 1."}); - } return errors; } diff --git a/src/Capsule_TEST.cc b/src/Capsule_TEST.cc index cfebc67b7..20b55c644 100644 --- a/src/Capsule_TEST.cc +++ b/src/Capsule_TEST.cc @@ -142,10 +142,12 @@ TEST(DOMCapsule, Load) sdf->SetName("capsule"); errors = capsule.Load(sdf); ASSERT_EQ(2u, errors.size()); - EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); - EXPECT_NE(std::string::npos, errors[0].Message().find("missing a ")); - EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[1].Code()); - EXPECT_NE(std::string::npos, errors[1].Message().find("missing a ")); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INVALID, errors[0].Code()); + EXPECT_NE(std::string::npos, errors[0].Message().find("Invalid ")) + << errors[0].Message(); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INVALID, errors[1].Code()); + EXPECT_NE(std::string::npos, errors[1].Message().find("Invalid ")) + << errors[1].Message(); EXPECT_NE(nullptr, capsule.Element()); // Add a radius element @@ -160,8 +162,9 @@ TEST(DOMCapsule, Load) sdf->SetName("capsule"); errors = capsule.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 ")); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INVALID, errors[0].Code()); + EXPECT_NE(std::string::npos, errors[0].Message().find("Invalid ")) + << errors[0].Message(); } ///////////////////////////////////////////////// diff --git a/src/Cylinder.cc b/src/Cylinder.cc index 07058d58c..0b4893921 100644 --- a/src/Cylinder.cc +++ b/src/Cylinder.cc @@ -14,6 +14,7 @@ * limitations under the License. * */ +#include #include "sdf/Cylinder.hh" using namespace sdf; @@ -93,45 +94,35 @@ Errors Cylinder::Load(ElementPtr _sdf) return errors; } - if (_sdf->HasElement("radius")) { std::pair pair = _sdf->Get("radius", this->dataPtr->cylinder.Radius()); if (!pair.second) { - errors.push_back({ErrorCode::ELEMENT_INVALID, - "Invalid data for a geometry. " - "Using a radius of 0.5."}); + std::stringstream ss; + ss << "Invalid data for a geometry. " + << "Using a radius of " + << this->dataPtr->cylinder.Radius() << "."; + errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()}); } this->dataPtr->cylinder.SetRadius(pair.first); } - else - { - errors.push_back({ErrorCode::ELEMENT_MISSING, - "Cylinder geometry is missing a child element. " - "Using a radius of 0.5."}); - } - if (_sdf->HasElement("length")) { std::pair pair = _sdf->Get("length", this->dataPtr->cylinder.Length()); if (!pair.second) { - errors.push_back({ErrorCode::ELEMENT_INVALID, - "Invalid data for a geometry. " - "Using a length of 1."}); + std::stringstream ss; + ss << "Invalid data for a geometry. " + << "Using a length of " + << this->dataPtr->cylinder.Length() << "."; + errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()}); } this->dataPtr->cylinder.SetLength(pair.first); } - else - { - errors.push_back({ErrorCode::ELEMENT_MISSING, - "Cylinder geometry is missing a child element. " - "Using a length of 1."}); - } return errors; } diff --git a/src/Cylinder_TEST.cc b/src/Cylinder_TEST.cc index ddc74d064..741dc77d4 100644 --- a/src/Cylinder_TEST.cc +++ b/src/Cylinder_TEST.cc @@ -138,10 +138,12 @@ TEST(DOMCylinder, Load) sdf->SetName("cylinder"); errors = cylinder.Load(sdf); ASSERT_EQ(2u, errors.size()); - EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); - EXPECT_NE(std::string::npos, errors[0].Message().find("missing a ")); - EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[1].Code()); - EXPECT_NE(std::string::npos, errors[1].Message().find("missing a ")); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INVALID, errors[0].Code()); + EXPECT_NE(std::string::npos, errors[0].Message().find("Invalid ")) + << errors[0].Message(); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INVALID, errors[1].Code()); + EXPECT_NE(std::string::npos, errors[1].Message().find("Invalid ")) + << errors[1].Message(); EXPECT_NE(nullptr, cylinder.Element()); // Add a radius element @@ -156,8 +158,9 @@ TEST(DOMCylinder, Load) sdf->SetName("cylinder"); errors = cylinder.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 ")); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INVALID, errors[0].Code()); + EXPECT_NE(std::string::npos, errors[0].Message().find("Invalid ")) + << errors[0].Message(); } /////////////////////////////////////////////////