Always publish if flag is passed to Model::SetScale (#3116)

Currently Model::SetScale does nothing if the
new scale is approximately equal to the current
scale, even if the _publish flag is set to true.
This change ensures that the scale is always published
if the flag is set.

* Add test for new Model::SetScale behavior

Expect new ~/model/info messages to be passed for
each call to SetScale if publish flag is set, even
if the scale value is not different.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>

gazebo/physics/Model.cc

@@ -1390,17 +1390,17 @@ void Model::SetState(const ModelState &_state)
 void Model::SetScale(const ignition::math::Vector3d &_scale,
       const bool _publish)
 {
-  if (this->scale == _scale)
-    return;
-
-  this->scale = _scale;
-
-  Base_V::iterator iter;
-  for (iter = this->children.begin(); iter != this->children.end(); ++iter)
+  if (this->scale != _scale)
   {
-    if (*iter && (*iter)->HasType(LINK))
+    this->scale = _scale;
+
+    Base_V::iterator iter;
+    for (iter = this->children.begin(); iter != this->children.end(); ++iter)
     {
-      boost::static_pointer_cast<Link>(*iter)->SetScale(_scale);
+      if (*iter && (*iter)->HasType(LINK))
+      {
+        boost::static_pointer_cast<Link>(*iter)->SetScale(_scale);
+      }
     }
   }
 

test/integration/model.cc

@@ -152,6 +152,23 @@ TEST_F(ModelTest, SetScale)
     receivedVisualMsg.geometry().sphere().radius());
 
   g_modelMsgs.clear();
+
+  // Call SetScale again with same value and expect another message
+  model->SetScale(scale, true);
+
+  // publish msg
+  sleep = 0;
+  receivedMsgs = false;
+  while (!receivedMsgs && sleep++ < 100)
+  {
+    world->Step(1);
+    common::Time::MSleep(100);
+    {
+      std::lock_guard<std::mutex> lock(g_mutex);
+      receivedMsgs = !g_modelMsgs.empty();
+    }
+  }
+  EXPECT_FALSE(g_modelMsgs.empty());
 }