Adding cone primitives. (#2410)

Signed-off-by: Benjamin Perseghetti <[email protected]> Co-authored-by: Steve Peters <[email protected]>
gazebosim · Jun 17, 2024 · 0d78194 · 0d78194
1 parent 48d3ef2
commit 0d78194
Show file tree

Hide file tree

Showing 14 changed files with 136 additions and 2 deletions.
diff --git a/include/gz/sim/Primitives.hh b/include/gz/sim/Primitives.hh
@@ -35,6 +35,7 @@ namespace gz
  {
  kBox,
  kCapsule,
+ kCone,
  kCylinder,
  kEllipsoid,
  kSphere,
@@ -67,8 +68,8 @@ namespace gz
  /// \brief Return an SDF string of one of the available primitive shape or
  /// light types.
  /// \param[in] _typeName Type name of the of shape or light to retrieve.
- /// Must be one of: box, sphere, cylinder, capsule, ellipsoid, directional,
- /// point, or spot.
+ /// Must be one of: box, sphere, cylinder, cone, capsule, ellipsoid,
+ /// directional, point, or spot.
  /// \return String containing SDF description of primitive shape or light.
  /// Empty string if the _typeName is invalid.
  std::string GZ_SIM_VISIBLE

diff --git a/src/Conversions.cc b/src/Conversions.cc
@@ -20,6 +20,7 @@
 #include <gz/msgs/axis_aligned_box.pb.h>
 #include <gz/msgs/boxgeom.pb.h>
 #include <gz/msgs/capsulegeom.pb.h>
+#include <gz/msgs/conegeom.pb.h>
 #include <gz/msgs/cylindergeom.pb.h>
 #include <gz/msgs/ellipsoidgeom.pb.h>
 #include <gz/msgs/entity.pb.h>
@@ -53,6 +54,7 @@
 #include <sdf/Box.hh>
 #include <sdf/Camera.hh>
 #include <sdf/Capsule.hh>
+#include <sdf/Cone.hh>
 #include <sdf/Cylinder.hh>
 #include <sdf/Ellipsoid.hh>
 #include <sdf/Geometry.hh>
@@ -176,6 +178,12 @@ msgs::Geometry gz::sim::convert(const sdf::Geometry &_in)
  out.mutable_capsule()->set_radius(_in.CapsuleShape()->Radius());
  out.mutable_capsule()->set_length(_in.CapsuleShape()->Length());
  }
+ else if (_in.Type() == sdf::GeometryType::CONE && _in.ConeShape())
+ {
+ out.set_type(msgs::Geometry::CONE);
+ out.mutable_cone()->set_radius(_in.ConeShape()->Radius());
+ out.mutable_cone()->set_length(_in.ConeShape()->Length());
+ }
  else if (_in.Type() == sdf::GeometryType::CYLINDER && _in.CylinderShape())
  {
  out.set_type(msgs::Geometry::CYLINDER);
@@ -293,6 +301,16 @@ sdf::Geometry gz::sim::convert(const msgs::Geometry &_in)
 
  out.SetCapsuleShape(capsuleShape);
  }
+ else if (_in.type() == msgs::Geometry::CONE && _in.has_cone())
+ {
+ out.SetType(sdf::GeometryType::CONE);
+
+ sdf::Cone coneShape;
+ coneShape.SetRadius(_in.cone().radius());
+ coneShape.SetLength(_in.cone().length());
+
+ out.SetConeShape(coneShape);
+ }
  else if (_in.type() == msgs::Geometry::CYLINDER && _in.has_cylinder())
  {
  out.SetType(sdf::GeometryType::CYLINDER);
@@ -1710,6 +1728,9 @@ sdf::ParticleEmitter gz::sim::convert(const msgs::ParticleEmitter &_in)
  case msgs::ParticleEmitter::BOX:
  out.SetType(sdf::ParticleEmitterType::BOX);
  break;
+ case msgs::ParticleEmitter::CONE:
+ out.SetType(sdf::ParticleEmitterType::CONE);
+ break;
  case msgs::ParticleEmitter::CYLINDER:
  out.SetType(sdf::ParticleEmitterType::CYLINDER);
  break;

diff --git a/src/Primitives.cc b/src/Primitives.cc
@@ -104,6 +104,49 @@ constexpr const char * kSphereSdf = R"(<?xml version="1.0"?>
 </sdf>
 )";
 
+/////////////////////////////////////////////////
+constexpr const char * kConeSdf = R"(<?xml version="1.0"?>
+<sdf version="1.9">
+ <model name="cone">
+ <pose>0 0 0.5 0 0 0</pose>
+ <link name="cone_link">
+ <inertial>
+ <inertia>
+ <ixx>0.075</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.075</iyy>
+ <iyz>0</iyz>
+ <izz>0.075</izz>
+ </inertia>
+ <mass>1.0</mass>
+ </inertial>
+ <collision name="cone_collision">
+ <geometry>
+ <cone>
+ <radius>0.5</radius>
+ <length>1.0</length>
+ </cone>
+ </geometry>
+ </collision>
+ <visual name="cone_visual">
+ <geometry>
+ <cone>
+ <radius>0.5</radius>
+ <length>1.0</length>
+ </cone>
+ </geometry>
+ <material>
+ <ambient>0.3 0.3 0.3 1</ambient>
+ <diffuse>0.7 0.7 0.7 1</diffuse>
+ <specular>1 1 1 1</specular>
+ </material>
+ </visual>
+ </link>
+ </model>
+</sdf>
+)";
+
 /////////////////////////////////////////////////
 constexpr const char * kCylinderSdf = R"(<?xml version="1.0"?>
 <sdf version="1.9">
@@ -301,6 +344,8 @@ std::string gz::sim::getPrimitiveShape(const PrimitiveShape &_type)
  return kBoxSdf;
  case PrimitiveShape::kSphere:
  return kSphereSdf;
+ case PrimitiveShape::kCone:
+ return kConeSdf;
  case PrimitiveShape::kCylinder:
  return kCylinderSdf;
  case PrimitiveShape::kCapsule:
@@ -339,6 +384,8 @@ std::string gz::sim::getPrimitive(const std::string &_typeName)
  return getPrimitiveShape(PrimitiveShape::kSphere);
  else if (type == "cylinder")
  return getPrimitiveShape(PrimitiveShape::kCylinder);
+ else if (type == "cone")
+ return getPrimitiveShape(PrimitiveShape::kCone);
  else if (type == "capsule")
  return getPrimitiveShape(PrimitiveShape::kCapsule);
  else if (type == "ellipsoid")
@@ -354,6 +401,7 @@ std::string gz::sim::getPrimitive(const std::string &_typeName)
  gzwarn << "The valid options are:\n";
  gzwarn << " - box\n";
  gzwarn << " - sphere\n";
+ gzwarn << " - cone\n";
  gzwarn << " - cylinder\n";
  gzwarn << " - capsule\n";
  gzwarn << " - ellipsoid\n";

diff --git a/src/gui/plugins/component_inspector_editor/ComponentInspectorEditor.qml b/src/gui/plugins/component_inspector_editor/ComponentInspectorEditor.qml
@@ -373,6 +373,14 @@ Rectangle {
  }
  }
 
+ MenuItem {
+ id: coneLink
+ text: "Cone"
+ onClicked: {
+ ComponentInspectorEditor.OnAddEntity("cone", "link");
+ }
+ }
+
  MenuItem {
  id: cylinderLink
  text: "Cylinder"

diff --git a/src/gui/plugins/component_inspector_editor/ModelEditor.cc b/src/gui/plugins/component_inspector_editor/ModelEditor.cc
@@ -32,6 +32,7 @@
 #include <sdf/Box.hh>
 #include <sdf/Ellipsoid.hh>
 #include <sdf/Capsule.hh>
+#include <sdf/Cone.hh>
 #include <sdf/Cylinder.hh>
 #include <sdf/Mesh.hh>
 #include <sdf/Sphere.hh>
@@ -330,6 +331,14 @@ std::optional<sdf::Geometry> ModelEditorPrivate::CreateGeom(
  geom.SetSphereShape(shape);
  geom.SetType(sdf::GeometryType::SPHERE);
  }
+ else if (_eta.geomOrLightType == "cone")
+ {
+ sdf::Cone shape;
+ shape.SetRadius(size.X() * 0.5);
+ shape.SetLength(size.Z());
+ geom.SetConeShape(shape);
+ geom.SetType(sdf::GeometryType::CONE);
+ }
  else if (_eta.geomOrLightType == "cylinder")
  {
  sdf::Cylinder shape;

diff --git a/src/gui/plugins/entity_tree/EntityTree.qml b/src/gui/plugins/entity_tree/EntityTree.qml
@@ -202,6 +202,15 @@ Rectangle {
  }
  }
 
+ MenuItem
+ {
+ id: cone
+ text: "Cone"
+ onClicked: {
+ EntityTree.OnInsertEntity("cone")
+ }
+ }
+
  MenuItem
  {
  id: cylinder

diff --git a/src/gui/plugins/shapes/Shapes.qml b/src/gui/plugins/shapes/Shapes.qml
@@ -67,6 +67,23 @@ ToolBar {
  Shapes.OnMode("sphere")
  }
  }
+ ToolButton {
+ id: cone
+ ToolTip.text: "Cone"
+ ToolTip.visible: hovered
+ ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval
+ contentItem: Image {
+ fillMode: Image.Pad
+ horizontalAlignment: Image.AlignHCenter
+ verticalAlignment: Image.AlignVCenter
+ source: "cone.png"
+ sourceSize.width: 24;
+ sourceSize.height: 24;
+ }
+ onClicked: {
+ Shapes.OnMode("cone")
+ }
+ }
  ToolButton {
  id: cylinder
  ToolTip.text: "Cylinder"

diff --git a/src/gui/plugins/shapes/Shapes.qrc b/src/gui/plugins/shapes/Shapes.qrc
@@ -3,6 +3,7 @@
  <file>Shapes.qml</file>
  <file>box.png</file>
  <file>sphere.png</file>
+ <file>cone.png</file>
  <file>cylinder.png</file>
  <file>capsule.png</file>
  <file>ellipsoid.png</file>

diff --git a/src/gui/plugins/shapes/cone.png b/src/gui/plugins/shapes/cone.png
diff --git a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc
@@ -63,6 +63,7 @@
 #include <gz/transport/Node.hh>
 
 #include <sdf/Capsule.hh>
+#include <sdf/Cone.hh>
 #include <sdf/Ellipsoid.hh>
 #include <sdf/Joint.hh>
 #include <sdf/Heightmap.hh>
@@ -1191,6 +1192,13 @@ rendering::GeometryPtr VisualizationCapabilitiesPrivate::CreateGeometry(
  capsule->SetLength(_geom.CapsuleShape()->Length());
  geom = capsule;
  }
+ else if (_geom.Type() == sdf::GeometryType::CONE)
+ {
+ geom = this->scene->CreateCone();
+ scale.X() = _geom.ConeShape()->Radius() * 2;
+ scale.Y() = scale.X();
+ scale.Z() = _geom.ConeShape()->Length();
+ }
  else if (_geom.Type() == sdf::GeometryType::CYLINDER)
  {
  geom = this->scene->CreateCylinder();

diff --git a/src/rendering/MarkerManager.cc b/src/rendering/MarkerManager.cc
@@ -387,6 +387,8 @@ rendering::MarkerType MarkerManagerPrivate::MsgToType(
  return rendering::MarkerType::MT_BOX;
  case msgs::Marker::CAPSULE:
  return rendering::MarkerType::MT_CAPSULE;
+ case msgs::Marker::CONE:
+ return rendering::MarkerType::MT_CONE;
  case msgs::Marker::CYLINDER:
  return rendering::MarkerType::MT_CYLINDER;
  case msgs::Marker::LINE_STRIP:

diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc
@@ -24,6 +24,7 @@
 #include <sdf/Box.hh>
 #include <sdf/Capsule.hh>
 #include <sdf/Collision.hh>
+#include <sdf/Cone.hh>
 #include <sdf/Cylinder.hh>
 #include <sdf/Ellipsoid.hh>
 #include <sdf/Heightmap.hh>
@@ -667,6 +668,13 @@ rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom,
  capsule->SetLength(_geom.CapsuleShape()->Length());
  geom = capsule;
  }
+ else if (_geom.Type() == sdf::GeometryType::CONE)
+ {
+ geom = this->dataPtr->scene->CreateCone();
+ scale.X() = _geom.ConeShape()->Radius() * 2;
+ scale.Y() = scale.X();
+ scale.Z() = _geom.ConeShape()->Length();
+ }
  else if (_geom.Type() == sdf::GeometryType::CYLINDER)
  {
  geom = this->dataPtr->scene->CreateCylinder();

diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
@@ -53,6 +53,7 @@
 #include <gz/physics/RequestEngine.hh>
 
 #include <gz/physics/BoxShape.hh>
+#include <gz/physics/ConeShape.hh>
 #include <gz/physics/ContactProperties.hh>
 #include <gz/physics/CylinderShape.hh>
 #include <gz/physics/ForwardStep.hh>

diff --git a/src/systems/physics/Physics.hh b/src/systems/physics/Physics.hh
@@ -26,6 +26,7 @@
 // Features need to be defined ahead of entityCast
 #include <gz/physics/BoxShape.hh>
 #include <gz/physics/CapsuleShape.hh>
+#include <gz/physics/ConeShape.hh>
 #include <gz/physics/CylinderShape.hh>
 #include <gz/physics/EllipsoidShape.hh>
 #include <gz/physics/ForwardStep.hh>