From 119dc8d221563e3727e3b4e5d2855365a4ead26c Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Thu, 7 Oct 2021 15:24:52 -0700
Subject: [PATCH] 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   | 18 +++++++++---------
 test/integration/model.cc | 17 +++++++++++++++++
 2 files changed, 26 insertions(+), 9 deletions(-)

diff --git a/gazebo/physics/Model.cc b/gazebo/physics/Model.cc
index a561344d02..a8a30c24fb 100644
--- a/gazebo/physics/Model.cc
+++ b/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);
+      }
     }
   }
 
diff --git a/test/integration/model.cc b/test/integration/model.cc
index 0d51370f62..331b1b4d55 100644
--- a/test/integration/model.cc
+++ b/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());
 }