Skip to content

Commit 26d425b

Browse files
bperseghettibperseghetti
authored
Backport: Adding cone primitives (gazebosim#2404)
Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>
1 parent 04a5e0d commit 26d425b

File tree

15 files changed

+173
-10
lines changed

15 files changed

+173
-10
lines changed

‎examples/worlds/shapes.sdf

Lines changed: 40 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,13 @@
11
<?xml version="1.0" ?>
2-
<!--
3-
4-
Try moving a model:
5-
6-
gz service -s /world/shapes/set_pose --reqtype gz.msgs.Pose --reptype gz.msgs.Boolean --timeout 300 --req 'name: "box", position: {z: 5.0}'
7-
8-
-->
9-
<sdf version="1.6">
2+
<sdf version="1.11">
3+
<!--
4+
Try moving a model using the command in the following CDATA block::
5+
-->
6+
<![CDATA[
7+
gz service -s /world/shapes/set_pose \
8+
--reqtype gz.msgs.Pose --reptype gz.msgs.Boolean \
9+
--timeout 300 --req 'name: "box", position: {z: 5.0}'
10+
]]>
1011
<world name="shapes">
1112
<scene>
1213
<ambient>1.0 1.0 1.0</ambient>
@@ -240,5 +241,36 @@ Try moving a model:
240241
</visual>
241242
</link>
242243
</model>
244+
245+
<model name="cone">
246+
<pose>0 4.5 0.5 0 0 0</pose>
247+
<link name="cone_link">
248+
<inertial auto="true">
249+
<density>1</density>
250+
</inertial>
251+
<collision name="cone_collision">
252+
<geometry>
253+
<cone>
254+
<radius>0.5</radius>
255+
<length>1.0</length>
256+
</cone>
257+
</geometry>
258+
</collision>
259+
260+
<visual name="cone_visual">
261+
<geometry>
262+
<cone>
263+
<radius>0.5</radius>
264+
<length>1.0</length>
265+
</cone>
266+
</geometry>
267+
<material>
268+
<ambient>1 0.47 0 1</ambient>
269+
<diffuse>1 0.47 0 1</diffuse>
270+
<specular>1 0.47 0 1</specular>
271+
</material>
272+
</visual>
273+
</link>
274+
</model>
243275
</world>
244276
</sdf>

‎include/gz/sim/Primitives.hh

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,7 @@ namespace gz
3535
{
3636
kBox,
3737
kCapsule,
38+
kCone,
3839
kCylinder,
3940
kEllipsoid,
4041
kSphere,
@@ -67,8 +68,8 @@ namespace gz
6768
/// \brief Return an SDF string of one of the available primitive shape or
6869
/// light types.
6970
/// \param[in] _typeName Type name of the of shape or light to retrieve.
70-
/// Must be one of: box, sphere, cylinder, capsule, ellipsoid, directional,
71-
/// point, or spot.
71+
/// Must be one of: box, sphere, cylinder, cone, capsule, ellipsoid,
72+
/// directional, point, or spot.
7273
/// \return String containing SDF description of primitive shape or light.
7374
/// Empty string if the _typeName is invalid.
7475
std::string GZ_SIM_VISIBLE

‎src/Conversions.cc

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
#include <gz/msgs/axis_aligned_box.pb.h>
2121
#include <gz/msgs/boxgeom.pb.h>
2222
#include <gz/msgs/capsulegeom.pb.h>
23+
#include <gz/msgs/conegeom.pb.h>
2324
#include <gz/msgs/cylindergeom.pb.h>
2425
#include <gz/msgs/ellipsoidgeom.pb.h>
2526
#include <gz/msgs/entity.pb.h>
@@ -53,6 +54,7 @@
5354
#include <sdf/Box.hh>
5455
#include <sdf/Camera.hh>
5556
#include <sdf/Capsule.hh>
57+
#include <sdf/Cone.hh>
5658
#include <sdf/Cylinder.hh>
5759
#include <sdf/Ellipsoid.hh>
5860
#include <sdf/Geometry.hh>
@@ -176,6 +178,12 @@ msgs::Geometry gz::sim::convert(const sdf::Geometry &_in)
176178
out.mutable_capsule()->set_radius(_in.CapsuleShape()->Radius());
177179
out.mutable_capsule()->set_length(_in.CapsuleShape()->Length());
178180
}
181+
else if (_in.Type() == sdf::GeometryType::CONE && _in.ConeShape())
182+
{
183+
out.set_type(msgs::Geometry::CONE);
184+
out.mutable_cone()->set_radius(_in.ConeShape()->Radius());
185+
out.mutable_cone()->set_length(_in.ConeShape()->Length());
186+
}
179187
else if (_in.Type() == sdf::GeometryType::CYLINDER && _in.CylinderShape())
180188
{
181189
out.set_type(msgs::Geometry::CYLINDER);
@@ -293,6 +301,16 @@ sdf::Geometry gz::sim::convert(const msgs::Geometry &_in)
293301

294302
out.SetCapsuleShape(capsuleShape);
295303
}
304+
else if (_in.type() == msgs::Geometry::CONE && _in.has_cone())
305+
{
306+
out.SetType(sdf::GeometryType::CONE);
307+
308+
sdf::Cone coneShape;
309+
coneShape.SetRadius(_in.cone().radius());
310+
coneShape.SetLength(_in.cone().length());
311+
312+
out.SetConeShape(coneShape);
313+
}
296314
else if (_in.type() == msgs::Geometry::CYLINDER && _in.has_cylinder())
297315
{
298316
out.SetType(sdf::GeometryType::CYLINDER);

‎src/Primitives.cc

Lines changed: 48 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -104,6 +104,49 @@ constexpr const char * kSphereSdf = R"(<?xml version="1.0"?>
104104
</sdf>
105105
)";
106106

107+
/////////////////////////////////////////////////
108+
constexpr const char * kConeSdf = R"(<?xml version="1.0"?>
109+
<sdf version="1.9">
110+
<model name="cone">
111+
<pose>0 0 0.5 0 0 0</pose>
112+
<link name="cone_link">
113+
<inertial>
114+
<inertia>
115+
<ixx>0.075</ixx>
116+
<ixy>0</ixy>
117+
<ixz>0</ixz>
118+
<iyy>0.075</iyy>
119+
<iyz>0</iyz>
120+
<izz>0.075</izz>
121+
</inertia>
122+
<mass>1.0</mass>
123+
</inertial>
124+
<collision name="cone_collision">
125+
<geometry>
126+
<cone>
127+
<radius>0.5</radius>
128+
<length>1.0</length>
129+
</cone>
130+
</geometry>
131+
</collision>
132+
<visual name="cone_visual">
133+
<geometry>
134+
<cone>
135+
<radius>0.5</radius>
136+
<length>1.0</length>
137+
</cone>
138+
</geometry>
139+
<material>
140+
<ambient>0.3 0.3 0.3 1</ambient>
141+
<diffuse>0.7 0.7 0.7 1</diffuse>
142+
<specular>1 1 1 1</specular>
143+
</material>
144+
</visual>
145+
</link>
146+
</model>
147+
</sdf>
148+
)";
149+
107150
/////////////////////////////////////////////////
108151
constexpr const char * kCylinderSdf = R"(<?xml version="1.0"?>
109152
<sdf version="1.9">
@@ -301,6 +344,8 @@ std::string gz::sim::getPrimitiveShape(const PrimitiveShape &_type)
301344
return kBoxSdf;
302345
case PrimitiveShape::kSphere:
303346
return kSphereSdf;
347+
case PrimitiveShape::kCone:
348+
return kConeSdf;
304349
case PrimitiveShape::kCylinder:
305350
return kCylinderSdf;
306351
case PrimitiveShape::kCapsule:
@@ -339,6 +384,8 @@ std::string gz::sim::getPrimitive(const std::string &_typeName)
339384
return getPrimitiveShape(PrimitiveShape::kSphere);
340385
else if (type == "cylinder")
341386
return getPrimitiveShape(PrimitiveShape::kCylinder);
387+
else if (type == "cone")
388+
return getPrimitiveShape(PrimitiveShape::kCone);
342389
else if (type == "capsule")
343390
return getPrimitiveShape(PrimitiveShape::kCapsule);
344391
else if (type == "ellipsoid")
@@ -354,6 +401,7 @@ std::string gz::sim::getPrimitive(const std::string &_typeName)
354401
gzwarn << "The valid options are:\n";
355402
gzwarn << " - box\n";
356403
gzwarn << " - sphere\n";
404+
gzwarn << " - cone\n";
357405
gzwarn << " - cylinder\n";
358406
gzwarn << " - capsule\n";
359407
gzwarn << " - ellipsoid\n";

‎src/gui/plugins/component_inspector_editor/ComponentInspectorEditor.qml

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -373,6 +373,14 @@ Rectangle {
373373
}
374374
}
375375

376+
MenuItem {
377+
id: coneLink
378+
text: "Cone"
379+
onClicked: {
380+
ComponentInspectorEditor.OnAddEntity("cone", "link");
381+
}
382+
}
383+
376384
MenuItem {
377385
id: cylinderLink
378386
text: "Cylinder"

‎src/gui/plugins/component_inspector_editor/ModelEditor.cc

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@
3232
#include <sdf/Box.hh>
3333
#include <sdf/Ellipsoid.hh>
3434
#include <sdf/Capsule.hh>
35+
#include <sdf/Cone.hh>
3536
#include <sdf/Cylinder.hh>
3637
#include <sdf/Mesh.hh>
3738
#include <sdf/Sphere.hh>
@@ -330,6 +331,14 @@ std::optional<sdf::Geometry> ModelEditorPrivate::CreateGeom(
330331
geom.SetSphereShape(shape);
331332
geom.SetType(sdf::GeometryType::SPHERE);
332333
}
334+
else if (_eta.geomOrLightType == "cone")
335+
{
336+
sdf::Cone shape;
337+
shape.SetRadius(size.X() * 0.5);
338+
shape.SetLength(size.Z());
339+
geom.SetConeShape(shape);
340+
geom.SetType(sdf::GeometryType::CONE);
341+
}
333342
else if (_eta.geomOrLightType == "cylinder")
334343
{
335344
sdf::Cylinder shape;

‎src/gui/plugins/entity_tree/EntityTree.qml

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -202,6 +202,15 @@ Rectangle {
202202
}
203203
}
204204

205+
MenuItem
206+
{
207+
id: cone
208+
text: "Cone"
209+
onClicked: {
210+
EntityTree.OnInsertEntity("cone")
211+
}
212+
}
213+
205214
MenuItem
206215
{
207216
id: cylinder

‎src/gui/plugins/shapes/Shapes.qml

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -67,6 +67,23 @@ ToolBar {
6767
Shapes.OnMode("sphere")
6868
}
6969
}
70+
ToolButton {
71+
id: cone
72+
ToolTip.text: "Cone"
73+
ToolTip.visible: hovered
74+
ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval
75+
contentItem: Image {
76+
fillMode: Image.Pad
77+
horizontalAlignment: Image.AlignHCenter
78+
verticalAlignment: Image.AlignVCenter
79+
source: "cone.png"
80+
sourceSize.width: 24;
81+
sourceSize.height: 24;
82+
}
83+
onClicked: {
84+
Shapes.OnMode("cone")
85+
}
86+
}
7087
ToolButton {
7188
id: cylinder
7289
ToolTip.text: "Cylinder"

‎src/gui/plugins/shapes/Shapes.qrc

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
<file>Shapes.qml</file>
44
<file>box.png</file>
55
<file>sphere.png</file>
6+
<file>cone.png</file>
67
<file>cylinder.png</file>
78
<file>capsule.png</file>
89
<file>ellipsoid.png</file>

‎src/gui/plugins/shapes/cone.png

5.47 KB
Loading

‎src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -63,6 +63,7 @@
6363
#include <gz/transport/Node.hh>
6464

6565
#include <sdf/Capsule.hh>
66+
#include <sdf/Cone.hh>
6667
#include <sdf/Ellipsoid.hh>
6768
#include <sdf/Joint.hh>
6869
#include <sdf/Heightmap.hh>
@@ -1191,6 +1192,13 @@ rendering::GeometryPtr VisualizationCapabilitiesPrivate::CreateGeometry(
11911192
capsule->SetLength(_geom.CapsuleShape()->Length());
11921193
geom = capsule;
11931194
}
1195+
else if (_geom.Type() == sdf::GeometryType::CONE)
1196+
{
1197+
geom = this->scene->CreateCone();
1198+
scale.X() = _geom.ConeShape()->Radius() * 2;
1199+
scale.Y() = scale.X();
1200+
scale.Z() = _geom.ConeShape()->Length();
1201+
}
11941202
else if (_geom.Type() == sdf::GeometryType::CYLINDER)
11951203
{
11961204
geom = this->scene->CreateCylinder();

‎src/rendering/MarkerManager.cc

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -387,6 +387,8 @@ rendering::MarkerType MarkerManagerPrivate::MsgToType(
387387
return rendering::MarkerType::MT_BOX;
388388
case msgs::Marker::CAPSULE:
389389
return rendering::MarkerType::MT_CAPSULE;
390+
case msgs::Marker::CONE:
391+
return rendering::MarkerType::MT_CONE;
390392
case msgs::Marker::CYLINDER:
391393
return rendering::MarkerType::MT_CYLINDER;
392394
case msgs::Marker::LINE_STRIP:

‎src/rendering/SceneManager.cc

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@
2424
#include <sdf/Box.hh>
2525
#include <sdf/Capsule.hh>
2626
#include <sdf/Collision.hh>
27+
#include <sdf/Cone.hh>
2728
#include <sdf/Cylinder.hh>
2829
#include <sdf/Ellipsoid.hh>
2930
#include <sdf/Heightmap.hh>
@@ -667,6 +668,13 @@ rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom,
667668
capsule->SetLength(_geom.CapsuleShape()->Length());
668669
geom = capsule;
669670
}
671+
else if (_geom.Type() == sdf::GeometryType::CONE)
672+
{
673+
geom = this->dataPtr->scene->CreateCone();
674+
scale.X() = _geom.ConeShape()->Radius() * 2;
675+
scale.Y() = scale.X();
676+
scale.Z() = _geom.ConeShape()->Length();
677+
}
670678
else if (_geom.Type() == sdf::GeometryType::CYLINDER)
671679
{
672680
geom = this->dataPtr->scene->CreateCylinder();

‎src/systems/physics/Physics.cc

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -53,6 +53,7 @@
5353
#include <gz/physics/RequestEngine.hh>
5454

5555
#include <gz/physics/BoxShape.hh>
56+
#include <gz/physics/ConeShape.hh>
5657
#include <gz/physics/ContactProperties.hh>
5758
#include <gz/physics/CylinderShape.hh>
5859
#include <gz/physics/ForwardStep.hh>

‎src/systems/physics/Physics.hh

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@
2626
// Features need to be defined ahead of entityCast
2727
#include <gz/physics/BoxShape.hh>
2828
#include <gz/physics/CapsuleShape.hh>
29+
#include <gz/physics/ConeShape.hh>
2930
#include <gz/physics/CylinderShape.hh>
3031
#include <gz/physics/EllipsoidShape.hh>
3132
#include <gz/physics/ForwardStep.hh>

0 commit comments

Comments
 (0)