sdf 1.8: add capsule geometry type

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)")

include/sdf/CMakeLists.txt

@@ -8,6 +8,7 @@ set (headers
   Atmosphere.hh
   Box.hh
   Camera.hh
+  Capsule.hh
   Collision.hh
   Console.hh
   Cylinder.hh

include/sdf/Capsule.hh

@@ -0,0 +1,105 @@
+/*
+ * 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_CAPSULE_HH_
+#define SDF_CAPSULE_HH_
+
+#include <ignition/math/Capsule.hh>
+#include <sdf/Error.hh>
+#include <sdf/Element.hh>
+#include <sdf/sdf_config.h>
+
+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

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 = 7,
   };
 
   /// \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

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

sdf/1.8/capsule_shape.sdf

@@ -0,0 +1,9 @@
+<element name="capsule" required="0">
+  <description>Capsule shape</description>
+  <element name="radius" type="double" default="0.5" required="1">
+    <description>Radius of the capsule</description>
+  </element>
+  <element name="length" type="double" default="1" required="1">
+    <description>Length of the cylindrical portion of the capsule along the z axis</description>
+  </element>
+</element>

sdf/1.8/geometry.sdf

@@ -7,6 +7,7 @@
   </element> <!-- End empty -->
 
   <include filename="box_shape.sdf" required="0"/>
+  <include filename="capsule_shape.sdf" required="0"/>
   <include filename="cylinder_shape.sdf" required="0"/>
   <include filename="heightmap_shape.sdf" required="0"/>
   <include filename="image_shape.sdf" required="0"/>

src/CMakeLists.txt

@@ -15,6 +15,7 @@ set (sources
   Atmosphere.cc
   Box.cc
   Camera.cc
+  Capsule.cc
   Collision.cc
   Console.cc
   Converter.cc
@@ -86,6 +87,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

src/Capsule.cc

@@ -0,0 +1,178 @@
+/*
+ * 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 "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.dataPtr;
+}
+
+//////////////////////////////////////////////////
+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 <capsule>."});
+    return errors;
+  }
+
+  if (_sdf->HasElement("radius"))
+  {
+    std::pair<double, bool> pair = _sdf->Get<double>("radius",
+        this->dataPtr->capsule.Radius());
+
+    if (!pair.second)
+    {
+      errors.push_back({ErrorCode::ELEMENT_INVALID,
+          "Invalid <radius> data for a <capsule> geometry. "
+          "Using a radius of 0.5."});
+    }
+    this->dataPtr->capsule.SetRadius(pair.first);
+  }
+  else
+  {
+    errors.push_back({ErrorCode::ELEMENT_MISSING,
+        "Capsule geometry is missing a <radius> child element. "
+        "Using a radius of 0.5."});
+  }
+
+  if (_sdf->HasElement("length"))
+  {
+    std::pair<double, bool> pair = _sdf->Get<double>("length",
+        this->dataPtr->capsule.Length());
+
+    if (!pair.second)
+    {
+      errors.push_back({ErrorCode::ELEMENT_INVALID,
+          "Invalid <length> data for a <capsule> geometry. "
+          "Using a length of 1."});
+    }
+    this->dataPtr->capsule.SetLength(pair.first);
+  }
+  else
+  {
+    errors.push_back({ErrorCode::ELEMENT_MISSING,
+        "Capsule geometry is missing a <length> 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;
+}