From d9e897b8aa11e61b5412d7c549692528dfb4da8a Mon Sep 17 00:00:00 2001
From: Ian Chen <ichen@openrobotics.org>
Date: Fri, 20 Dec 2024 21:45:26 +0000
Subject: [PATCH] add test to check presence of warning

Signed-off-by: Ian Chen <ichen@openrobotics.org>
---
 src/Link_TEST.cc | 47 +++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 47 insertions(+)

diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc
index a2c346c2d..977aab5d0 100644
--- a/src/Link_TEST.cc
+++ b/src/Link_TEST.cc
@@ -724,6 +724,53 @@ TEST(DOMLink, ResolveAutoInertialsWithMultipleCollisions)
     link->Inertial().MassMatrix().DiagonalMoments());
 }
 
+/////////////////////////////////////////////////
+TEST(DOMLink, ResolveAutoInertialsWithDefaultDensity)
+{
+  std::string sdf = "<?xml version=\"1.0\"?>"
+  "<sdf version=\"1.11\">"
+  "  <model name='model_default_density'>"
+  "   <pose>0 0 1.0 0 0 0</pose>"
+  "   <link name='compound_link'>"
+  "     <inertial auto='true' />"
+  "     <collision name='box_collision'>"
+  "      <pose>0 0 -0.5 0 0 0</pose>"
+  "      <geometry>"
+  "        <box>"
+  "          <size>1 1 1</size>"
+  "        </box>"
+  "        </geometry>"
+  "      </collision>"
+  "    </link>"
+  "   </model>"
+  "  </sdf>";
+
+  sdf::Root root;
+  sdf::ParserConfig sdfParserConfig;
+  // Set enforcement policy to store warnings as error.
+  // Because density is missing so we expect that a warning is generated about
+  // using the default density when resolving auto inertia.
+  sdfParserConfig.SetWarningsPolicy(sdf::EnforcementPolicy::ERR);
+  sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig);
+  EXPECT_FALSE(errors.empty());
+  EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code());
+
+  const sdf::Model *model = root.Model();
+  const sdf::Link *link = model->LinkByIndex(0);
+
+  root.ResolveAutoInertials(errors, sdfParserConfig);
+  EXPECT_FALSE(errors.empty());
+  EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code());
+
+  // The collision should use the default density value of 1000
+  // Mass of cube(volume * density)
+  double expectedMass = 1.0 * 1000.0;
+
+  EXPECT_DOUBLE_EQ(expectedMass, link->Inertial().MassMatrix().Mass());
+  EXPECT_EQ(gz::math::Vector3d(166.667, 166.667, 166.667),
+    link->Inertial().MassMatrix().DiagonalMoments());
+}
+
 /////////////////////////////////////////////////
 TEST(DOMLink, InertialValuesGivenWithAutoSetToTrue)
 {