From 80faf4402b5c0a5e1de0afe884e3425996112671 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Mon, 17 Jun 2024 23:12:36 -0400 Subject: [PATCH] Backport: Adding cone primitives. (#2410) Signed-off-by: Benjamin Perseghetti --- examples/worlds/shapes.sdf | 48 +++++++++++++++--- include/gz/sim/Primitives.hh | 5 +- src/Conversions.cc | 18 +++++++ src/Primitives.cc | 48 ++++++++++++++++++ .../ComponentInspectorEditor.qml | 8 +++ .../component_inspector_editor/ModelEditor.cc | 9 ++++ src/gui/plugins/entity_tree/EntityTree.qml | 9 ++++ src/gui/plugins/shapes/Shapes.qml | 17 +++++++ src/gui/plugins/shapes/Shapes.qrc | 1 + src/gui/plugins/shapes/cone.png | Bin 0 -> 5605 bytes .../VisualizationCapabilities.cc | 8 +++ src/rendering/MarkerManager.cc | 2 + src/rendering/SceneManager.cc | 8 +++ src/systems/physics/Physics.cc | 1 + src/systems/physics/Physics.hh | 1 + 15 files changed, 173 insertions(+), 10 deletions(-) create mode 100644 src/gui/plugins/shapes/cone.png diff --git a/examples/worlds/shapes.sdf b/examples/worlds/shapes.sdf index 1d75661b49c..3a0e0f5dc8a 100644 --- a/examples/worlds/shapes.sdf +++ b/examples/worlds/shapes.sdf @@ -1,12 +1,13 @@ - - + + + 1.0 1.0 1.0 @@ -240,5 +241,36 @@ Try moving a model: + + + 0 4.5 0.5 0 0 0 + + + 1 + + + + + 0.5 + 1.0 + + + + + + + + 0.5 + 1.0 + + + + 1 0.47 0 1 + 1 0.47 0 1 + 1 0.47 0 1 + + + + diff --git a/include/gz/sim/Primitives.hh b/include/gz/sim/Primitives.hh index 84bc8344f55..b90b9e75537 100644 --- 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 index fbe75871418..e10e0d1184b 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -53,6 +54,7 @@ #include #include #include +#include #include #include #include @@ -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 index 4fa93cea544..fcb7f65942d 100644 --- a/src/Primitives.cc +++ b/src/Primitives.cc @@ -104,6 +104,49 @@ constexpr const char * kSphereSdf = R"( )"; +///////////////////////////////////////////////// +constexpr const char * kConeSdf = R"( + + + 0 0 0.5 0 0 0 + + + + 0.075 + 0 + 0 + 0.075 + 0 + 0.075 + + 1.0 + + + + + 0.5 + 1.0 + + + + + + + 0.5 + 1.0 + + + + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 1 1 1 1 + + + + + +)"; + ///////////////////////////////////////////////// constexpr const char * kCylinderSdf = R"( @@ -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 index f1e47ccd819..c21f8787f1f 100644 --- 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 index a838054b800..2f32916f668 100644 --- a/src/gui/plugins/component_inspector_editor/ModelEditor.cc +++ b/src/gui/plugins/component_inspector_editor/ModelEditor.cc @@ -32,6 +32,7 @@ #include #include #include +#include #include #include #include @@ -330,6 +331,14 @@ std::optional 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 index 8b6db131237..1d65b9e7be4 100644 --- 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 index 781eaca28f9..fb80c1431f3 100644 --- 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 index 754d997c5d8..00fddab57c5 100644 --- a/src/gui/plugins/shapes/Shapes.qrc +++ b/src/gui/plugins/shapes/Shapes.qrc @@ -3,6 +3,7 @@ Shapes.qml box.png sphere.png + cone.png cylinder.png capsule.png ellipsoid.png diff --git a/src/gui/plugins/shapes/cone.png b/src/gui/plugins/shapes/cone.png new file mode 100644 index 0000000000000000000000000000000000000000..53f26d831c7b5f129ab2c47739781ebcfcb9dcee GIT binary patch literal 5605 zcmV zaB^>EX>4U6ba`-PAZ2)IW&i+q+O3*vn%g>$g#U9Dvjp)@E{E6WnH|jX=LIO)?sO+P z-nv-&{Ng#Xyng!L@$Px|=gZf8UIy|X7o^v_{pouffBD|Org2|ZRGxtxuaEENW!?4p zQ{I2~o@H~-M$Qf`B;u3qn|f|Vk2tXt7_XNlUYWm<~`5#KYj7e54XJMTlZRv)p@H< zH*8_zX*ttb2D4nDFY_GyUtd2CwD*t@S?>Oc73*>maF!v+$-kThAa*}IiW=bO_3ID7 zGDzSo33K5F+ttqyJ=!<7yiuS%Cq~}Gxj4^z07Asvk;PaK0#~v=+GMZnT(*`t7Vv4T z+<6mR2Lc%NB`d+NEDUV9tRXM_<)8hMmaM>Eq+Kf{bO%{y4wpiMb&)4>^pLQM$O)+0hm$D0w+DF@paD65LV4%m|fM8716QEDBw&hh=E{O*2QgK*u9bahMOVp|1ECr6FDQ${eK{51iBxP z`;OZWs7=xLA$fQqY?6i&)XS58oYY+5&vpI&z16wr=ImMe=zGq^M^pE5BYBHn+BSOc zGx#-g_dCb#Sz`%iHZ$pA;|f)#kwH;sPS*j#*gfRB8Gixqn7wDaR`xUXzV>Ct zE|q0i=XZ}1>OAWnKt~j&&udK%lLa|$sdKrVt*846CTKZuYo!{Eerh8*7+Wq614f7p zQ?sswbHF*ZIaqEfEzRqKl2m%fTHD}efO;(>lX4Sw32?*pw1$%bSjkHKuBSBQ_bCf@ zkTG6YoZmTN{zjV8TF%VL?ZHXm#LmhITVM?suX4E+vk|_?X@gMoHf#5Ok5~sMUpuw7 za#hHss+9#XM)chV=tRg$BSn0Zm>Bt#reA$5s7c)6M1E7^vMUoTN-v0IxoaU|k^JaZ z<69SWY6UmKt;5ce{EZzaw{&Btgmj}TdOc6>rbQXDBruU*!<#8QHn^wBtZRT1j36l{ zF|`9eZ;*t+nM;_X+;sE{uC991F)Om&-GrPQOe6=mlRi@IDAKZ(l?o|CCRj>J*6Vk* z{Ig;sRiPE*dOKY=$PGD`Y6}vztB?{Yx;uiXCQB>a?$US+=7cxPLx|8&mtHtA+1iI` zbCSp`cuRKoX=CVNs|4)d5GjfdDL+2m&F2@bMU*Rl%FKt#P!BjS^sVXKEpoe29X<+S zjL^!m*ogcRFF)pWzlTC)xe$C5I9x3bKKWEWcvJ!f#s-FQ5$lJdZ%@KDpGORbCqa0U z4o?vDWkoDT6wT4K0kQy70>MpnrCLl9hDO*(auR>es&-FSJw1Tz6?8bLA-O@U`&x=p z!_`eMqxmiXn}e{(4hRdrDmn*W6&;3C(U}>It=D@(q2fIuOf-4DW>hlfUWJTnA5~0; zhNZt$F>mK@RV3Dwxmkqn#3*e-MhOc-p=txwP&i4Rq80>N!b-^!;K}`#7k-!V{aqeN zEh!I%G>1K{;|?-vG?&nO&WQC&L{2M;tSV<&G4X&-tw*MbHbnb+XGJmN@ltB3H^jF~ z+CkEqKLkb4i-Ec!cHiA(a8s>_lomc2Z1_G|sgsIg7-QCz6_A@}pq0U4{I?oJgP9FKFPPv zEsWSk5`kzDAWHKsV9J9YMQf);m>>*REaFGS(&$mco7N)IK}PXsnRGtFYi{Pv40p;> z++T>XzY-(3bVxOdH-C3NzZf5gWyQg8pnrnl@@f`m49HKJ|X^>jeYl*gq zp-zGF1~$b?jT=VX>cv0w=+g%M)IUG$!wDZ#-M9E)>Vbd#2)~E|!3zTwtPR&;&uK$@ zcD;#KOXKWnG*5R71A^FulXKEcG4AYu{IvbuVf^mvvo zm+xWd${y1aK21ydVp_7?y89g`F8F4g2*oOxI|498zy|k6HQDfyyY_08lLr4HA~a(I zHK)(gtjvi9XQFkcs^38EcA;?5S28tTGFVjz5rHg9ab!b;tXha5T*}33XG^LRCWEmP zO1udnY&iHU`T#HZgT92O&M?T-P#IXuLsu%)70jd|JtYDT@WoAqrc@A{@MZ$XqEc;K zZ+LE*K-fN%;^3o70Y1`xkSRe|67sT5;i0}cKab@IAC|+k^wo1D@f_~06Abw$jUgR%2anKVgoc zz<)QIF|I3B+pUW$_Mcki*!vgzXYK=vyptn@FLyEhXlg;U09F!+BqLUrm->DY1$-R- zL6XS@5E{#xxQL3yx2+_b6KS4}gV3=1(bCPNoLKWdW$#VCJ`6SETC;F2v{*-?4=TV6z z3@``LqB``GH~at-|BA7DF$akks%9llGe_YMb3PS5&XqryQ?D0uwET=yed?(`yI)4i zkJRv=whp4GHn|z3pC5h2J&qJ~X$41&j?9Z*JG0S|RP-Fc$yLA0=m3Rh1mlihTBu zNFxP|G*ZA;m#m%7bmMF}B?bK>Z@*-%x!VBl{HLrvb3S>iQ~oA%jU13PMJPiSB>l_PnLj1{ z4=Mkd@L%%XsBvS>RDGp3WfZqoIo*Pz_IBX7f0U23-U>oMS~NX*~{zzm{4-^~qD|#={SLmfzm{)t~ZQj8^CQr8- z%Ss<@oPFaM(lv9WIB1=aX9Po9Qa|P4l9}02j^m1~2T7A1Qt1;!YpM9BRXPNi^(_qf1j)0-$Um}AG{@bpwUX9zw|VIc}#k1-S$iv5>k+;YFpavl(s{3$ajhQ zC7V$WhjCg{+J0iLjwcz>IR^B)GfaRkW=WDf;L$)w~CK2rAlGqtFW3}9sdral9tGxHfhX13>gBMqVqVNvB>Tow`_`$ZwV zca~C+CZpz~2z?Bp48E&7j2ojhz%;6fc~sS=*Kn(E}*ZC^S_%JV&_^s&2VXo^b&LauSgQZXp(Qk@QUG;bN#mp73y`N^M-a`FeO# zhDx{;bocj1U9WfZ7}!e{rp87;CNs=t7wEwQI7}NTF_m3T2s?ctJy<$-NjsP|c}>te z-(*GipS0s{|H7QF#4#VtnMuVKxBG^VYjb`9NTHOSD$(mHOBL({Jl0C3rQqgyk;O6q zzqkH10D8O}Yq^>5O7aJ}hF;mr zqv-RD&D84^>|>*@v5*r5DU7!Ec?M35yC@4J~h0X|m~5&ZC`G&kpI? z^%HAx`Zuvy_3VqtuO9AU5g~FYOhI?V45EKtpL^>At0wzY8OhEUPN^bzwy5Bh_SQ@T3U=3Y50CP{C-T4<;I)B%1izH{SshOcjYr@f(nc z4<<+eXD=VlZC}gUjvihhrhVW_U;|H!$U?nSfRbJ9oD6|Qaxe3XC82sXV~B0fnS&~ zDis?~f0;rU)&$l9BBt~M?sC1K19zD4M4%_rmuWl$Y>4lPDLpdz)#BZ8m3Oo>DhnOpT{3 z2G;W+B%xEV8oY0$uRMF50xpRk7Nx$aS2YR|fm`fRY!6B3K&z6K=Jb9Th}%sBc9^M! zFEZi;@R8`jXw&XR6l}~#2M;TNQH@>^SZ@)CdvyeU4GL=YZaoLy2R;!K4a}}yT-OL} zGg4E|dM~5KwaEj$IWG)lCES|;n4q8thU36zl1;s)tc%hs6Cww8uu%;+rD|KwgE76d zEemY4qHsHOKuUlLJ{*9;+ra1IJl!VJFFic4RrdSUE6f0++%Yx4=L9aaqHv=k@Vhd_ z5KX;r0bh0;|ILm(t_av5ufsB|jcXG?#4u-DdLRXXo!Tj;YHtExiGNih_&#G{CnNB0 z%~Yb%k{ncOu1e{_#g>U^+pJ3q{gS}R6zwuK2sVtUB)Y=yDX1u&3TFg1TamCaoFq19 z`USyxe-b%9G30e zm|#tiztwvImeB}bqS_vRIxTQ%Y8npx-Aq>FUB1h+!128POCAJQzPli2H2rNn$grY_ z7C??kZqvlmo`yiz4{%E0^0d97eSTifnhky z1FBuV_e0>axTFkARFq@TZk6CE?0&C17j-JZpwr+(`}O5nlLyZ-1jiux=@PJ+>yLrr zvZeiX8U-#HQlWl7_Ac@v3_xA_HF+@163qtjKMP!tIGU395Z~{*G8$wKEGW82a^0y3 zh`KOKN$4Joi+zC$7NE<)`IjDLslC@D2eRR(#b2WAr$zqP!KWS!DgrMvDdEloYmx$K zHGenCvjCJ!GLuu#<1gxpP=jwG5qe<4HQ@p>atN!yis;2Tg_2#2fFtpu)I4@0rz}s? zP}EI7)8Q)*Y`Q9b1)19bmPMjvN4y;gDfG*L!`cze%w0YH61hS`q&RsP`;i0C3N~cb^bwQx>cI2eMqDEj5*kWu%-G1mn=(7fV)l506ox9Ss?%9oY zfVrcvh3|p&^#dF@I{%G7?V?iYcOEcz?BJCar{xFUnEzIytw_8G?E^o5?BLhgC*K1j zf&Qvp6HyN+`nE9k6=Ln_#1S*7$%GzT@Fevr53H^qU~Kk%$N$jg-#y%Lux6x|K-PYc z$u7VLjBCL&++j03j7?t>P2ipbuTLRR{vS>L=>R^>G*z|V5OSr1S0 ze5ZdO47=I-LfQFm*Z&@SiobvEweCUMVv+v #include +#include #include #include #include @@ -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 index 50b761cecbb..04b1bef0e14 100644 --- 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 index d94e6aa8b79..c1aefe89184 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -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 index 1f6defbf449..1d7ca23c157 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -53,6 +53,7 @@ #include #include +#include #include #include #include diff --git a/src/systems/physics/Physics.hh b/src/systems/physics/Physics.hh index cec62d341c4..256ee5ac754 100644 --- 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 #include +#include #include #include #include