Fix Crash On Negative Value

src/Box.cc

@@ -20,6 +20,7 @@
 #include <gz/math/Material.hh>
 #include <gz/math/MassMatrix3.hh>
 #include <gz/math/Inertial.hh>
+#include "sdf/Console.hh"
 #include "sdf/Box.hh"
 #include "sdf/parser.hh"
 #include "Utils.hh"
@@ -75,10 +76,19 @@ Errors Box::Load(ElementPtr _sdf)
     if (!pair.second)
     {
       errors.push_back({ErrorCode::ELEMENT_INVALID,
-          "Invalid <size> data for a <box> geometry. "
-          "Using a size of 1, 1, 1 "});
+          "Invalid <size> data for a <box> geometry."});
+    }
+    else
+    {
+      if (pair.first.X() <= 0 || pair.first.Y() <= 0 ||
+          pair.first.Z() <= 0)
+      {
+        sdfwarn << "Value of <size> is negative. "
+            << "Using default value of 1, 1, 1.\n";
+        pair.first = gz::math::Vector3d::One;
+      }
+      this->dataPtr->box.SetSize(pair.first);
     }
-    this->dataPtr->box.SetSize(pair.first);
   }
   else
   {

src/Box_TEST.cc

@@ -138,6 +138,22 @@ TEST(DOMBox, Load)
   EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code());
   EXPECT_NE(std::string::npos, errors[0].Message().find("missing a <size>"));
   EXPECT_NE(nullptr, box.Element());
+
+  // Define <size> element description
+  sdf::ElementPtr sizeDesc(new sdf::Element());
+  sizeDesc->SetName("size");
+  sizeDesc->AddValue("vector3", "1 1 1", true, "size vector");
+  sdf->AddElementDescription(sizeDesc);
+
+  // Now add the element and assign a negative value
+  sdf::ElementPtr sizeElem = sdf->AddElement("size");
+  sizeElem->Set<gz::math::Vector3d>(gz::math::Vector3d(-1, -1, -1));
+
+  // Load and check behavior
+  errors = box.Load(sdf);
+  ASSERT_EQ(0u, errors.size());
+  EXPECT_NE(nullptr, box.Element());
+  EXPECT_EQ(gz::math::Vector3d::One, box.Size());  // Defaulted to 1,1,1
 }
 
 /////////////////////////////////////////////////
@@ -170,15 +186,14 @@ TEST(DOMBox, CalculateInertial)
   box.SetSize(gz::math::Vector3d(l, w, h));
 
   double expectedMass = box.Shape().Volume() * density;
-  double ixx = (1.0/12.0) * expectedMass * (w*w + h*h);
-  double iyy = (1.0/12.0) * expectedMass * (l*l + h*h);
-  double izz = (1.0/12.0) * expectedMass * (l*l + w*w);
+  double ixx = (1.0 / 12.0) * expectedMass * (w * w + h * h);
+  double iyy = (1.0 / 12.0) * expectedMass * (l * l + h * h);
+  double izz = (1.0 / 12.0) * expectedMass * (l * l + w * w);
 
   gz::math::MassMatrix3d expectedMassMat(
-    expectedMass,
-    gz::math::Vector3d(ixx, iyy, izz),
-    gz::math::Vector3d::Zero
-  );
+      expectedMass,
+      gz::math::Vector3d(ixx, iyy, izz),
+      gz::math::Vector3d::Zero);
 
   gz::math::Inertiald expectedInertial;
   expectedInertial.SetMassMatrix(expectedMassMat);
@@ -189,9 +204,9 @@ TEST(DOMBox, CalculateInertial)
   ASSERT_NE(std::nullopt, boxInertial);
   EXPECT_EQ(expectedInertial, *boxInertial);
   EXPECT_EQ(expectedInertial.MassMatrix().DiagonalMoments(),
-    boxInertial->MassMatrix().DiagonalMoments());
+            boxInertial->MassMatrix().DiagonalMoments());
   EXPECT_EQ(expectedInertial.MassMatrix().Mass(),
-    boxInertial->MassMatrix().Mass());
+            boxInertial->MassMatrix().Mass());
   EXPECT_EQ(expectedInertial.Pose(), boxInertial->Pose());
 }
 
@@ -219,14 +234,14 @@ TEST(DOMBox, ToElementErrorOutput)
   sdf::testing::RedirectConsoleStream redir(
       sdf::Console::Instance()->GetMsgStream(), &buffer);
 
-  #ifdef _WIN32
-    sdf::Console::Instance()->SetQuiet(false);
-    sdf::testing::ScopeExit revertSetQuiet(
+#ifdef _WIN32
+  sdf::Console::Instance()->SetQuiet(false);
+  sdf::testing::ScopeExit revertSetQuiet(
       []
       {
         sdf::Console::Instance()->SetQuiet(true);
       });
-  #endif
+#endif
 
   sdf::Box box;
   sdf::Errors errors;

src/Capsule.cc

@@ -20,6 +20,7 @@
 #include <gz/math/Material.hh>
 #include <gz/math/Inertial.hh>
 #include "sdf/Capsule.hh"
+#include "sdf/Console.hh"
 #include "sdf/parser.hh"
 #include "Utils.hh"
 
@@ -78,7 +79,16 @@ Errors Capsule::Load(ElementPtr _sdf)
          << this->dataPtr->capsule.Radius() << ".";
       errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()});
     }
-    this->dataPtr->capsule.SetRadius(pair.first);
+    else
+    {
+      if (pair.first <= 0)
+      {
+        sdfwarn << "Value of <radius> is negative. "
+            << "Using default value of 0.5.\n";
+        pair.first = 0.5;
+      }
+      this->dataPtr->capsule.SetRadius(pair.first);
+    }
   }
 
   {
@@ -93,7 +103,16 @@ Errors Capsule::Load(ElementPtr _sdf)
          << this->dataPtr->capsule.Length() << ".";
       errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()});
     }
-    this->dataPtr->capsule.SetLength(pair.first);
+    else
+    {
+      if (pair.first <= 0)
+      {
+        sdfwarn << "Value of <length> is negative. "
+            << "Using default value of 1.\n";
+        pair.first = 1.0;
+      }
+      this->dataPtr->capsule.SetLength(pair.first);
+    }
   }
 
   return errors;

src/Capsule_TEST.cc

@@ -156,11 +156,13 @@ TEST(DOMCapsule, Load)
       << errors[1].Message();
   EXPECT_NE(nullptr, capsule.Element());
 
-  // Add a radius element
+  // Add <radius> element description
   sdf::ElementPtr radiusDesc(new sdf::Element());
   radiusDesc->SetName("radius");
   radiusDesc->AddValue("double", "1.0", true, "radius");
   sdf->AddElementDescription(radiusDesc);
+
+  // Add radius element
   sdf::ElementPtr radiusElem = sdf->AddElement("radius");
   radiusElem->Set<double>(2.0);
 
@@ -171,6 +173,29 @@ TEST(DOMCapsule, Load)
   EXPECT_EQ(sdf::ErrorCode::ELEMENT_INVALID, errors[0].Code());
   EXPECT_NE(std::string::npos, errors[0].Message().find("Invalid <length>"))
       << errors[0].Message();
+
+  // Add <length> element description
+  sdf::ElementPtr lengthDesc(new sdf::Element());
+  lengthDesc->SetName("length");
+  lengthDesc->AddValue("double", "1.0", true, "length");
+  sdf->AddElementDescription(lengthDesc);
+
+  // Add length element and test negative radius
+  sdf::ElementPtr lengthElem = sdf->AddElement("length");
+  lengthElem->Set<double>(3.0);
+  radiusElem->Set<double>(-1.0);
+  errors = capsule.Load(sdf);
+  ASSERT_EQ(0u, errors.size());
+  EXPECT_NE(nullptr, capsule.Element());
+  EXPECT_DOUBLE_EQ(0.5, capsule.Radius());
+
+  // Test negative length
+  radiusElem->Set<double>(1.0);
+  lengthElem->Set<double>(-1.0);
+  errors = capsule.Load(sdf);
+  ASSERT_EQ(0u, errors.size());
+  EXPECT_NE(nullptr, capsule.Element());
+  EXPECT_DOUBLE_EQ(1.0, capsule.Length());
 }
 
 /////////////////////////////////////////////////

src/Cone.cc

@@ -23,6 +23,7 @@
 #include <gz/math/Inertial.hh>
 #include <gz/math/Cone.hh>
 #include "sdf/Cone.hh"
+#include "sdf/Console.hh"
 #include "sdf/parser.hh"
 #include "Utils.hh"
 
@@ -81,7 +82,16 @@ Errors Cone::Load(ElementPtr _sdf)
          << this->dataPtr->cone.Radius() << ".";
       errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()});
     }
-    this->dataPtr->cone.SetRadius(pair.first);
+    else
+    {
+      if (pair.first <= 0)
+      {
+        sdfwarn << "Value of <radius> is negative. "
+            << "Using default value of 0.5.\n";
+        pair.first = 0.5;
+      }
+      this->dataPtr->cone.SetRadius(pair.first);
+    }
   }
 
   {
@@ -96,7 +106,16 @@ Errors Cone::Load(ElementPtr _sdf)
          << this->dataPtr->cone.Length() << ".";
       errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()});
     }
-    this->dataPtr->cone.SetLength(pair.first);
+    else
+    {
+      if (pair.first <= 0)
+      {
+        sdfwarn << "Value of <length> is negative. "
+            << "Using default of 1.\n";
+        pair.first = 1.0;
+      }
+      this->dataPtr->cone.SetLength(pair.first);
+    }
   }
 
   return errors;

src/Cone_TEST.cc

@@ -156,11 +156,13 @@ TEST(DOMCone, Load)
       << errors[1].Message();
   EXPECT_NE(nullptr, cone.Element());
 
-  // Add a radius element
+  // Add <radius> element description
   sdf::ElementPtr radiusDesc(new sdf::Element());
   radiusDesc->SetName("radius");
   radiusDesc->AddValue("double", "1.0", true, "radius");
   sdf->AddElementDescription(radiusDesc);
+
+  // Add a radius element
   sdf::ElementPtr radiusElem = sdf->AddElement("radius");
   radiusElem->Set<double>(2.0);
 
@@ -171,6 +173,29 @@ TEST(DOMCone, Load)
   EXPECT_EQ(sdf::ErrorCode::ELEMENT_INVALID, errors[0].Code());
   EXPECT_NE(std::string::npos, errors[0].Message().find("Invalid <length>"))
       << errors[0].Message();
+
+  // Now add <length> element description
+  sdf::ElementPtr lengthDesc(new sdf::Element());
+  lengthDesc->SetName("length");
+  lengthDesc->AddValue("double", "1.0", true, "length");
+  sdf->AddElementDescription(lengthDesc);
+
+  // Add length element and test negative radius
+  sdf::ElementPtr lengthElem = sdf->AddElement("length");
+  lengthElem->Set<double>(3.0);
+  radiusElem->Set<double>(-1.0);
+  errors = cone.Load(sdf);
+  ASSERT_EQ(0u, errors.size());
+  EXPECT_NE(nullptr, cone.Element());
+  EXPECT_DOUBLE_EQ(0.5, cone.Radius());  // default fallback
+
+  // Test negative length
+  radiusElem->Set<double>(1.0);
+  lengthElem->Set<double>(-1.0);
+  errors = cone.Load(sdf);
+  ASSERT_EQ(0u, errors.size());
+  EXPECT_NE(nullptr, cone.Element());
+  EXPECT_DOUBLE_EQ(1.0, cone.Length());  // default fallback
 }
 
 /////////////////////////////////////////////////

src/Cylinder.cc

@@ -18,6 +18,7 @@
 #include <utility>
 
 #include <gz/math/Inertial.hh>
+#include "sdf/Console.hh"
 #include "sdf/Cylinder.hh"
 #include "sdf/parser.hh"
 #include "Utils.hh"
@@ -77,7 +78,16 @@ Errors Cylinder::Load(ElementPtr _sdf)
          << this->dataPtr->cylinder.Radius() << ".";
       errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()});
     }
-    this->dataPtr->cylinder.SetRadius(pair.first);
+    else
+    {
+      if (pair.first <= 0)
+      {
+        sdfwarn << "Value of <radius> is negative. "
+            << "Using default value of 0.5.\n";
+        pair.first = 0.5;
+      }
+      this->dataPtr->cylinder.SetRadius(pair.first);
+    }
   }
 
   {
@@ -92,7 +102,16 @@ Errors Cylinder::Load(ElementPtr _sdf)
          << this->dataPtr->cylinder.Length() << ".";
       errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()});
     }
-    this->dataPtr->cylinder.SetLength(pair.first);
+    else
+    {
+      if (pair.first <= 0)
+      {
+        sdfwarn << "Value of <length> is negative. "
+            << "Using default value of 1.\n";
+        pair.first = 1.0;
+      }
+      this->dataPtr->cylinder.SetLength(pair.first);
+    }
   }
 
   return errors;

src/Cylinder_TEST.cc

@@ -152,11 +152,13 @@ TEST(DOMCylinder, Load)
       << errors[1].Message();
   EXPECT_NE(nullptr, cylinder.Element());
 
-  // Add a radius element
+  // Add <radius> element description
   sdf::ElementPtr radiusDesc(new sdf::Element());
   radiusDesc->SetName("radius");
   radiusDesc->AddValue("double", "1.0", true, "radius");
   sdf->AddElementDescription(radiusDesc);
+
+  // Add radius element
   sdf::ElementPtr radiusElem = sdf->AddElement("radius");
   radiusElem->Set<double>(2.0);
 
@@ -167,6 +169,29 @@ TEST(DOMCylinder, Load)
   EXPECT_EQ(sdf::ErrorCode::ELEMENT_INVALID, errors[0].Code());
   EXPECT_NE(std::string::npos, errors[0].Message().find("Invalid <length>"))
       << errors[0].Message();
+
+  // Now add <length> element description
+  sdf::ElementPtr lengthDesc(new sdf::Element());
+  lengthDesc->SetName("length");
+  lengthDesc->AddValue("double", "1.0", true, "length");
+  sdf->AddElementDescription(lengthDesc);
+
+  // Add length element and test negative radius
+  sdf::ElementPtr lengthElem = sdf->AddElement("length");
+  lengthElem->Set<double>(3.0);
+  radiusElem->Set<double>(-1.0);
+  errors = cylinder.Load(sdf);
+  ASSERT_EQ(0u, errors.size());
+  EXPECT_NE(nullptr, cylinder.Element());
+  EXPECT_DOUBLE_EQ(0.5, cylinder.Radius());  // Default to 0.5
+
+  // Test negative length
+  radiusElem->Set<double>(1.0);
+  lengthElem->Set<double>(-1.0);
+  errors = cylinder.Load(sdf);
+  ASSERT_EQ(0u, errors.size());
+  EXPECT_NE(nullptr, cylinder.Element());
+  EXPECT_DOUBLE_EQ(1.0, cylinder.Length());  // Default to 1.0
 }
 
 /////////////////////////////////////////////////

src/Ellipsoid.cc

@@ -19,6 +19,7 @@
 
 #include <gz/math/Inertial.hh>
 #include <gz/math/Material.hh>
+#include "sdf/Console.hh"
 #include "sdf/Ellipsoid.hh"
 #include "sdf/parser.hh"
 #include "Utils.hh"
@@ -78,7 +79,16 @@ Errors Ellipsoid::Load(ElementPtr _sdf)
           "Invalid <radii> data for a <ellipsoid> geometry. "
           "Using a radii of 1, 1, 1 "});
     }
-    this->dataPtr->ellipsoid.SetRadii(pair.first);
+    else
+    {
+      if (pair.first.X() <= 0 || pair.first.Y() <= 0 || pair.first.Z() <= 0)
+      {
+        sdfwarn << "Value of <radii> is negative. "
+            << "Using default value of 1, 1, 1.\n";
+        pair.first = gz::math::Vector3d::One;
+      }
+      this->dataPtr->ellipsoid.SetRadii(pair.first);
+    }
   }
   else
   {