Adding cone primitives.

Squashing commits to make requested target of main with backports to
harmonic.

Signed-off-by: Benjamin Perseghetti <[email protected]>

include/sdf/Cone.hh

@@ -0,0 +1,108 @@
+/*
+ * Copyright 2024 CogniPilot Foundation
+ * Copyright 2024 Open Source Robotics Foundation
+ * Copyright 2024 Rudis Laboratories
+ *
+ * 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_CONE_HH_
+#define SDF_CONE_HH_
+
+#include <optional>
+
+#include <gz/math/Cone.hh>
+#include <gz/math/Inertial.hh>
+#include <gz/utils/ImplPtr.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 {
+  /// \brief Cone represents a cone shape, and is usually accessed
+  /// through a Geometry.
+  class SDFORMAT_VISIBLE Cone
+  {
+    /// \brief Constructor
+    public: Cone();
+
+    /// \brief Load the cone 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 cone's radius in meters.
+    /// \return The radius of the cone in meters.
+    public: double Radius() const;
+
+    /// \brief Set the cone's radius in meters.
+    /// \param[in] _radius The radius of the cone in meters.
+    public: void SetRadius(const double _radius);
+
+    /// \brief Get the cone's length in meters.
+    /// \return The length of the cone in meters.
+    public: double Length() const;
+
+    /// \brief Set the cone's length in meters.
+    /// \param[in] _length The length of the cone 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 Gazebo Math representation of this cone.
+    /// \return A const reference to a gz::math::Sphered object.
+    public: const gz::math::Coned &Shape() const;
+
+    /// \brief Get a mutable Gazebo Math representation of this cone.
+    /// \return A reference to a gz::math::Coned object.
+    public: gz::math::Coned &Shape();
+
+    /// \brief Calculate and return the Inertial values for the cone. In
+    /// order to calculate the inertial properties, the function mutates the
+    /// object by updating its material properties.
+    /// \param[in] _density Density of the cone in kg/m^3
+    /// \return A std::optional with gz::math::Inertiald object or std::nullopt
+    public: std::optional<gz::math::Inertiald>
+            CalculateInertial(double _density);
+
+    /// \brief Create and return an SDF element filled with data from this
+    /// cone.
+    /// Note that parameter passing functionality is not captured with this
+    /// function.
+    /// \return SDF element pointer with updated cone values.
+    public: sdf::ElementPtr ToElement() const;
+
+    /// \brief Create and return an SDF element filled with data from this
+    /// cone.
+    /// Note that parameter passing functionality is not captured with this
+    /// function.
+    /// \param[out] _errors Vector of errors.
+    /// \return SDF element pointer with updated cone values.
+    public: sdf::ElementPtr ToElement(sdf::Errors &_errors) const;
+
+    /// \brief Private data pointer.
+    GZ_UTILS_IMPL_PTR(dataPtr)
+  };
+  }
+}
+#endif

include/sdf/Geometry.hh

@@ -37,6 +37,7 @@ namespace sdf
   // Forward declare private data class.
   class Box;
   class Capsule;
+  class Cone;
   class Cylinder;
   class Ellipsoid;
   class Heightmap;
@@ -79,6 +80,9 @@ namespace sdf
 
     /// \brief A polyline geometry.
     POLYLINE = 9,
+
+    /// \brief A polyline geometry.
+    CONE = 10,
   };
 
   /// \brief Geometry provides access to a shape, such as a Box. Use the
@@ -137,6 +141,17 @@ namespace sdf
     /// \param[in] _capsule The capsule shape.
     public: void SetCapsuleShape(const Capsule &_capsule);
 
+    /// \brief Get the cone geometry, or nullptr if the contained
+    /// geometry is not a cone.
+    /// \return Pointer to the visual's cone geometry, or nullptr if the
+    /// geometry is not a cone.
+    /// \sa GeometryType Type() const
+    public: const Cone *ConeShape() const;
+
+    /// \brief Set the cone shape.
+    /// \param[in] _cone The cone shape.
+    public: void SetConeShape(const Cone &_cone);
+
     /// \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

include/sdf/ParticleEmitter.hh

@@ -52,6 +52,9 @@ namespace sdf
 
     /// \brief An ellipsoid emitter.
     ELLIPSOID = 3,
+
+    /// \brief An cone emitter.
+    CONE = 4,
   };
 
   /// \brief A description of a particle emitter, which can be attached

sdf/1.11/CMakeLists.txt

@@ -11,6 +11,7 @@ set (sdfs
   camera.sdf
   capsule_shape.sdf
   collision.sdf
+  cone_shape.sdf
   contact.sdf
   cylinder_shape.sdf
   ellipsoid_shape.sdf

sdf/1.11/cone_shape.sdf

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

sdf/1.11/geometry.sdf

@@ -8,6 +8,7 @@
 
   <include filename="box_shape.sdf" required="0"/>
   <include filename="capsule_shape.sdf" required="0"/>
+  <include filename="cone_shape.sdf" required="0"/>
   <include filename="cylinder_shape.sdf" required="0"/>
   <include filename="ellipsoid_shape.sdf" required="0"/>
   <include filename="heightmap_shape.sdf" required="0"/>

sdf/1.11/particle_emitter.sdf

@@ -7,7 +7,7 @@
   </attribute>
 
   <attribute name="type" type="string" default="point" required="1">
-    <description>The type of a particle emitter. One of "box", "cylinder", "ellipsoid", or "point".</description>
+    <description>The type of a particle emitter. One of "box", "cylinder", "cone", "ellipsoid", or "point".</description>
   </attribute>
 
   <element name="emitting" type="bool" default="true" required="0">
@@ -26,6 +26,8 @@
     depending on the emmiter type:
       - point: The area is ignored.
       - box: The area is interpreted as width X height X depth.
+      - cone: The area is interpreted as the bounding box of the
+                  cone. The cone is oriented along the Z-axis.
       - cylinder: The area is interpreted as the bounding box of the
                   cylinder. The cylinder is oriented along the Z-axis.
       - ellipsoid: The area is interpreted as the bounding box of an

src/Cone.cc

@@ -0,0 +1,186 @@
+/*
+ * Copyright 2024 CogniPilot Foundation
+ * Copyright 2024 Open Source Robotics Foundation
+ * Copyright 2024 Rudis Laboratories
+ *
+ * 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 <sstream>
+#include <optional>
+
+#include <gz/math/Inertial.hh>
+#include "sdf/Cone.hh"
+#include "sdf/parser.hh"
+#include "Utils.hh"
+
+using namespace sdf;
+
+// Private data class
+class sdf::Cone::Implementation
+{
+  // A cone with a length of 1 meter and radius if 0.5 meters.
+  public: gz::math::Coned cone{1.0, 0.5};
+
+  /// \brief The SDF element pointer used during load.
+  public: sdf::ElementPtr sdf;
+};
+
+/////////////////////////////////////////////////
+Cone::Cone()
+  : dataPtr(gz::utils::MakeImpl<Implementation>())
+{
+}
+
+/////////////////////////////////////////////////
+Errors Cone::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 cone, but the provided SDF "
+        "element is null."});
+    return errors;
+  }
+
+  // We need a cone child element
+  if (_sdf->GetName() != "cone")
+  {
+    errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE,
+        "Attempting to load a cone geometry, but the provided SDF "
+        "element is not a <cone>."});
+    return errors;
+  }
+
+  {
+    std::pair<double, bool> pair = _sdf->Get<double>(errors, "radius",
+        this->dataPtr->cone.Radius());
+
+    if (!pair.second)
+    {
+      std::stringstream ss;
+      ss << "Invalid <radius> data for a <cone> geometry. "
+         << "Using a radius of "
+         << this->dataPtr->cone.Radius() << ".";
+      errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()});
+    }
+    this->dataPtr->cone.SetRadius(pair.first);
+  }
+
+  {
+    std::pair<double, bool> pair = _sdf->Get<double>(errors, "length",
+        this->dataPtr->cone.Length());
+
+    if (!pair.second)
+    {
+      std::stringstream ss;
+      ss << "Invalid <length> data for a <cone> geometry. "
+         << "Using a length of "
+         << this->dataPtr->cone.Length() << ".";
+      errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()});
+    }
+    this->dataPtr->cone.SetLength(pair.first);
+  }
+
+  return errors;
+}
+
+//////////////////////////////////////////////////
+double Cone::Radius() const
+{
+  return this->dataPtr->cone.Radius();
+}
+
+//////////////////////////////////////////////////
+void Cone::SetRadius(const double _radius)
+{
+  this->dataPtr->cone.SetRadius(_radius);
+}
+
+//////////////////////////////////////////////////
+double Cone::Length() const
+{
+  return this->dataPtr->cone.Length();
+}
+
+//////////////////////////////////////////////////
+void Cone::SetLength(const double _length)
+{
+  this->dataPtr->cone.SetLength(_length);
+}
+
+/////////////////////////////////////////////////
+sdf::ElementPtr Cone::Element() const
+{
+  return this->dataPtr->sdf;
+}
+
+/////////////////////////////////////////////////
+const gz::math::Coned &Cone::Shape() const
+{
+  return this->dataPtr->cone;
+}
+
+/////////////////////////////////////////////////
+gz::math::Coned &Cone::Shape()
+{
+  return this->dataPtr->cone;
+}
+
+std::optional<gz::math::Inertiald> Cone::CalculateInertial(double _density)
+{
+  gz::math::Material material = gz::math::Material(_density);
+  this->dataPtr->cone.SetMat(material);
+
+  auto coneMassMatrix = this->dataPtr->cone.MassMatrix();
+
+  if (!coneMassMatrix)
+  {
+    return std::nullopt;
+  }
+  else
+  {
+    gz::math::Inertiald coneInertial;
+    coneInertial.SetMassMatrix(coneMassMatrix.value());
+    return std::make_optional(coneInertial);
+  }
+}
+
+/////////////////////////////////////////////////
+sdf::ElementPtr Cone::ToElement() const
+{
+  sdf::Errors errors;
+  auto result = this->ToElement(errors);
+  sdf::throwOrPrintErrors(errors);
+  return result;
+}
+
+/////////////////////////////////////////////////
+sdf::ElementPtr Cone::ToElement(sdf::Errors &_errors) const
+{
+  sdf::ElementPtr elem(new sdf::Element);
+  sdf::initFile("cone_shape.sdf", elem);
+
+  sdf::ElementPtr radiusElem = elem->GetElement("radius", _errors);
+  radiusElem->Set<double>(_errors, this->Radius());
+
+  sdf::ElementPtr lengthElem = elem->GetElement("length", _errors);
+  lengthElem->Set<double>(_errors, this->Length());
+
+  return elem;
+}