Backport: Adding cone primitives (gazebosim#2404)

Signed-off-by: Benjamin Perseghetti <[email protected]>
rudislabs · Jun 26, 2024 · 26d425b · 26d425b
1 parent 04a5e0d
commit 26d425b
Show file tree

Hide file tree

Showing 15 changed files with 173 additions and 10 deletions.
diff --git a/examples/worlds/shapes.sdf b/examples/worlds/shapes.sdf
@@ -1,12 +1,13 @@
 <?xml version="1.0" ?>
-<!--
-
-Try moving a model:
-
-    gz service -s /world/shapes/set_pose --reqtype gz.msgs.Pose --reptype gz.msgs.Boolean --timeout 300 --req 'name: "box", position: {z: 5.0}'
-
--->
-<sdf version="1.6">
+<sdf version="1.11">
+  <!--
+    Try moving a model using the command in the following CDATA block::
+  -->
+  <![CDATA[
+    gz service -s /world/shapes/set_pose \
+               --reqtype gz.msgs.Pose --reptype gz.msgs.Boolean \
+               --timeout 300 --req 'name: "box", position: {z: 5.0}'
+  ]]>
   <world name="shapes">
     <scene>
       <ambient>1.0 1.0 1.0</ambient>
@@ -240,5 +241,36 @@ Try moving a model:
         </visual>
       </link>
     </model>
+
+    <model name="cone">
+      <pose>0 4.5 0.5 0 0 0</pose>
+      <link name="cone_link">
+        <inertial auto="true">
+          <density>1</density>
+        </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>1 0.47 0 1</ambient>
+            <diffuse>1 0.47 0 1</diffuse>
+            <specular>1 0.47 0 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
   </world>
 </sdf>
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);

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>