Fix density handling in Collision::ToElement

A density element was written unconditionally, even if it was not originally set by the user. This checks the optional density value before setting via ToElement(). Signed-off-by: Steve Peters <[email protected]>
gazebosim · Jan 24, 2024 · 8d91b6a · 8d91b6a
1 parent b41b48b
commit 8d91b6a
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 2 deletions.
diff --git a/src/Collision.cc b/src/Collision.cc
@@ -351,6 +351,7 @@ sdf::ElementPtr Collision::Element() const
   return this->dataPtr->sdf;
 }
 
+/////////////////////////////////////////////////
 sdf::ElementPtr Collision::ToElement() const
 {
   sdf::Errors errors;
@@ -377,8 +378,11 @@ sdf::ElementPtr Collision::ToElement(sdf::Errors &_errors) const
   poseElem->Set<gz::math::Pose3d>(_errors, this->RawPose());
 
   // Set the density
-  sdf::ElementPtr densityElem = elem->GetElement("density", _errors);
-  densityElem->Set<double>(this->Density());
+  if (this->dataPtr->density.has_value())
+  {
+    sdf::ElementPtr densityElem = elem->GetElement("density", _errors);
+    densityElem->Set<double>(this->Density());
+  }
 
   // Set the geometry
   elem->InsertElement(this->dataPtr->geom.ToElement(_errors), true);

diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc
@@ -421,6 +421,8 @@ TEST(DOMCollision, ToElement)
 
   sdf::ElementPtr elem = collision.ToElement();
   ASSERT_NE(nullptr, elem);
+  // Expect no density element
+  EXPECT_FALSE(elem->HasElement("density"));
 
   sdf::Collision collision2;
   collision2.Load(elem);
@@ -435,6 +437,19 @@ TEST(DOMCollision, ToElement)
   ASSERT_NE(nullptr, surface2->Friction());
   ASSERT_NE(nullptr, surface2->Friction()->ODE());
   EXPECT_DOUBLE_EQ(1.23, surface2->Friction()->ODE()->Mu());
+
+  // Now set density in collision
+  const double kDensity = 1234.5;
+  collision.SetDensity(kDensity);
+  sdf::ElementPtr elemWithDensity = collision.ToElement();
+  ASSERT_NE(nullptr, elemWithDensity);
+  // Expect density element
+  ASSERT_TRUE(elemWithDensity->HasElement("density"));
+  EXPECT_DOUBLE_EQ(kDensity, elemWithDensity->Get<double>("density"));
+
+  sdf::Collision collision3;
+  collision3.Load(elem);
+  EXPECT_DOUBLE_EQ(kDensity, collision.Density());
 }
 
 /////////////////////////////////////////////////