add test to check presence of warning

Signed-off-by: Ian Chen <ichen@openrobotics.org>

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)
 {