From 30f8b9e9ff0c204e4eb32e4dbdb82b0d6b2391a7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= <ahcorde@gmail.com>
Date: Tue, 15 Mar 2022 16:32:23 +0100
Subject: [PATCH 1/5] Allow to turn on/off lights (#1343)

Signed-off-by: ahcorde <ahcorde@gmail.com>
Co-authored-by: Louise Poubel <louise@openrobotics.org>
---
 CMakeLists.txt                                |  2 +-
 src/Conversions.cc                            | 27 ++++++++++++++
 .../component_inspector/ComponentInspector.cc | 29 ++++++++++++++-
 .../component_inspector/ComponentInspector.hh |  3 +-
 .../ComponentInspector.qml                    |  6 ++-
 src/gui/plugins/component_inspector/Light.qml | 37 ++++++++++++++++++-
 src/rendering/RenderUtil.cc                   | 32 ++++++++++++++--
 src/systems/user_commands/UserCommands.cc     | 21 +++++++++++
 test/integration/user_commands.cc             |  8 ++++
 9 files changed, 154 insertions(+), 11 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 150f789e88..9631ab9fc3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -47,7 +47,7 @@ set(CMAKE_POLICY_DEFAULT_CMP0077 NEW)
 # as protobuf could be find transitively by any dependency
 set(protobuf_MODULE_COMPATIBLE TRUE)
 
-ign_find_package(sdformat11 REQUIRED VERSION 11.2.1)
+ign_find_package(sdformat11 REQUIRED VERSION 11.4.0)
 set(SDF_VER ${sdformat11_VERSION_MAJOR})
 
 #--------------------------------------
diff --git a/src/Conversions.cc b/src/Conversions.cc
index a71ef0aafb..aafd9213f7 100644
--- a/src/Conversions.cc
+++ b/src/Conversions.cc
@@ -568,6 +568,14 @@ msgs::Light ignition::gazebo::convert(const sdf::Light &_in)
   out.set_spot_inner_angle(_in.SpotInnerAngle().Radian());
   out.set_spot_outer_angle(_in.SpotOuterAngle().Radian());
   out.set_spot_falloff(_in.SpotFalloff());
+
+  // todo(ahcorde) Use the field is_light_off in light.proto from
+  // Garden on.
+  auto header = out.mutable_header()->add_data();
+  header->set_key("isLightOn");
+  std::string *value = header->add_value();
+  *value = std::to_string(_in.LightOn());
+
   if (_in.Type() == sdf::LightType::POINT)
     out.set_type(msgs::Light_LightType_POINT);
   else if (_in.Type() == sdf::LightType::SPOT)
@@ -597,6 +605,25 @@ sdf::Light ignition::gazebo::convert(const msgs::Light &_in)
   out.SetSpotInnerAngle(math::Angle(_in.spot_inner_angle()));
   out.SetSpotOuterAngle(math::Angle(_in.spot_outer_angle()));
   out.SetSpotFalloff(_in.spot_falloff());
+
+  // todo(ahcorde) Use the field is_light_off in light.proto from
+  // Garden on.
+  bool isLightOn = true;
+  for (int i = 0; i < _in.header().data_size(); ++i)
+  {
+    for (int j = 0;
+        j < _in.header().data(i).value_size(); ++j)
+    {
+      if (_in.header().data(i).key() ==
+          "isLightOn")
+      {
+        isLightOn = ignition::math::parseInt(
+          _in.header().data(i).value(0));
+      }
+    }
+  }
+  out.SetLightOn(isLightOn);
+
   if (_in.type() == msgs::Light_LightType_POINT)
     out.SetType(sdf::LightType::POINT);
   else if (_in.type() == msgs::Light_LightType_SPOT)
diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc
index f2c31ec06f..f16712ce73 100644
--- a/src/gui/plugins/component_inspector/ComponentInspector.cc
+++ b/src/gui/plugins/component_inspector/ComponentInspector.cc
@@ -153,6 +153,21 @@ void ignition::gazebo::setData(QStandardItem *_item, const msgs::Light &_data)
     lightType = 2;
   }
 
+  bool isLightOn = true;
+  for (int i = 0; i < _data.header().data_size(); ++i)
+  {
+    for (int j = 0;
+        j < _data.header().data(i).value_size(); ++j)
+    {
+      if (_data.header().data(i).key() ==
+          "isLightOn")
+      {
+        isLightOn = ignition::math::parseInt(
+          _data.header().data(i).value(0));
+      }
+    }
+  }
+
   _item->setData(QString("Light"),
       ComponentsModel::RoleNames().key("dataType"));
   _item->setData(QList({
@@ -176,7 +191,8 @@ void ignition::gazebo::setData(QStandardItem *_item, const msgs::Light &_data)
     QVariant(_data.spot_outer_angle()),
     QVariant(_data.spot_falloff()),
     QVariant(_data.intensity()),
-    QVariant(lightType)
+    QVariant(lightType),
+    QVariant(isLightOn)
   }), ComponentsModel::RoleNames().key("data"));
 }
 
@@ -989,7 +1005,8 @@ void ComponentInspector::OnLight(
   double _attRange, double _attLinear, double _attConstant,
   double _attQuadratic, bool _castShadows, double _directionX,
   double _directionY, double _directionZ, double _innerAngle,
-  double _outerAngle, double _falloff, double _intensity, int _type)
+  double _outerAngle, double _falloff, double _intensity, int _type,
+  bool _isLightOn)
 {
   std::function<void(const ignition::msgs::Boolean &, const bool)> cb =
       [](const ignition::msgs::Boolean &/*_rep*/, const bool _result)
@@ -999,6 +1016,14 @@ void ComponentInspector::OnLight(
   };
 
   ignition::msgs::Light req;
+
+  // todo(ahcorde) Use the field is_light_off in light.proto from
+  // Garden on.
+  auto header = req.mutable_header()->add_data();
+  header->set_key("isLightOn");
+  std::string *value = header->add_value();
+  *value = std::to_string(_isLightOn);
+
   req.set_name(this->dataPtr->entityName);
   req.set_id(this->dataPtr->entity);
   ignition::msgs::Set(req.mutable_diffuse(),
diff --git a/src/gui/plugins/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh
index 9229a24188..0590aa31c7 100644
--- a/src/gui/plugins/component_inspector/ComponentInspector.hh
+++ b/src/gui/plugins/component_inspector/ComponentInspector.hh
@@ -252,6 +252,7 @@ namespace gazebo
     /// \param[in] _falloff Falloff of the spotlight
     /// \param[in] _intensity Intensity of the light
     /// \param[in] _type light type
+    /// \param[in] _isLightOn is light on
     public: Q_INVOKABLE void OnLight(
       double _rSpecular, double _gSpecular, double _bSpecular,
       double _aSpecular, double _rDiffuse, double _gDiffuse,
@@ -259,7 +260,7 @@ namespace gazebo
       double _attLinear, double _attConstant, double _attQuadratic,
       bool _castShadows, double _directionX, double _directionY,
       double _directionZ, double _innerAngle, double _outerAngle,
-      double _falloff, double _intensity, int _type);
+      double _falloff, double _intensity, int _type, bool _isLightOn);
 
     /// \brief Callback in Qt thread when physics' properties change.
     /// \param[in] _stepSize step size
diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml
index 545e46d538..c9f68bd1e4 100644
--- a/src/gui/plugins/component_inspector/ComponentInspector.qml
+++ b/src/gui/plugins/component_inspector/ComponentInspector.qml
@@ -101,12 +101,14 @@ Rectangle {
                    _rDiffuse, _gDiffuse, _bDiffuse, _aDiffuse,
                    _attRange, _attLinear, _attConstant, _attQuadratic,
                    _castShadows, _directionX, _directionY, _directionZ,
-                   _innerAngle, _outerAngle, _falloff, _intensity, _type) {
+                   _innerAngle, _outerAngle, _falloff, _intensity, _type,
+                   _isLightOn) {
     ComponentInspector.OnLight(_rSpecular, _gSpecular, _bSpecular, _aSpecular,
                                _rDiffuse, _gDiffuse, _bDiffuse, _aDiffuse,
                                _attRange, _attLinear, _attConstant, _attQuadratic,
                                _castShadows, _directionX, _directionY, _directionZ,
-                               _innerAngle, _outerAngle, _falloff, _intensity, _type)
+                               _innerAngle, _outerAngle, _falloff, _intensity, _type,
+                               _isLightOn)
   }
 
   /*
diff --git a/src/gui/plugins/component_inspector/Light.qml b/src/gui/plugins/component_inspector/Light.qml
index 98cf143e82..f051feb2b3 100644
--- a/src/gui/plugins/component_inspector/Light.qml
+++ b/src/gui/plugins/component_inspector/Light.qml
@@ -99,6 +99,9 @@ Rectangle {
   // Loaded item for intensity
   property var intensityItem: {}
 
+  // Loaded item for isLightOn
+  property var isLightOnItem: {}
+
   // Send new light data to C++
   function sendLight() {
     // TODO(anyone) There's a loss of precision when these values get to C++
@@ -123,7 +126,8 @@ Rectangle {
       outerAngleItem.value,
       falloffItem.value,
       intensityItem.value,
-      model.data[20]
+      model.data[20],
+      isLightOnItem.checked
     );
   }
 
@@ -285,6 +289,37 @@ Rectangle {
         id: grid
         width: parent.width
 
+        RowLayout {
+          Rectangle {
+            color: "transparent"
+            height: 40
+            Layout.preferredWidth: isOnText.width + indentation*3
+
+            Text {
+              id : isOnText
+              text: ' Turn on/off'
+              leftPadding: 5
+              color: Material.theme == Material.Light ? "#444444" : "#bbbbbb"
+              font.pointSize: 12
+              anchors.centerIn: parent
+            }
+          }
+          Item {
+            Layout.fillWidth: true
+            height: 40
+
+            Loader {
+              id: isOnLoader
+              anchors.fill: parent
+              property double numberValue: model.data[21]
+              sourceComponent: ignSwitch
+              onLoaded: {
+                isLightOnItem = isOnLoader.item
+              }
+            }
+          }
+        }
+
         RowLayout {
           Rectangle {
             color: "transparent"
diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc
index 35c347cd33..15e7bdfd43 100644
--- a/src/rendering/RenderUtil.cc
+++ b/src/rendering/RenderUtil.cc
@@ -2131,11 +2131,35 @@ void RenderUtilPrivate::UpdateLights(
     auto l = std::dynamic_pointer_cast<rendering::Light>(node);
     if (l)
     {
-      if (!ignition::math::equal(
-          l->Intensity(),
-          static_cast<double>(light.second.intensity())))
+      // todo(ahcorde) Use the field is_light_off in light.proto from
+      // Garden on.
+      bool isLightOn = true;
+      for (int i = 0; i < light.second.header().data_size(); ++i)
+      {
+        for (int j = 0;
+            j < light.second.header().data(i).value_size(); ++j)
+        {
+          if (light.second.header().data(i).key() ==
+              "isLightOn")
+          {
+            isLightOn = ignition::math::parseInt(
+              light.second.header().data(i).value(0));
+          }
+        }
+      }
+
+      if (isLightOn)
+      {
+        if (!ignition::math::equal(
+            l->Intensity(),
+            static_cast<double>(light.second.intensity())))
+        {
+          l->SetIntensity(light.second.intensity());
+        }
+      }
+      else
       {
-        l->SetIntensity(light.second.intensity());
+        l->SetIntensity(0);
       }
       if (light.second.has_diffuse())
       {
diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc
index 079245773c..1bd29a7d96 100644
--- a/src/systems/user_commands/UserCommands.cc
+++ b/src/systems/user_commands/UserCommands.cc
@@ -159,7 +159,28 @@ class LightCommand : public UserCommandBase
   public: std::function<bool(const msgs::Light &, const msgs::Light &)>
           lightEql { [](const msgs::Light &_a, const msgs::Light &_b)
             {
+              // todo(ahcorde) Use the field is_light_off in light.proto from
+              // Garden on.
+              auto getIsLightOn = [](const msgs::Light &_light) -> bool
+              {
+                bool isLightOn = true;
+                for (int i = 0; i < _light.header().data_size(); ++i)
+                {
+                  for (int j = 0;
+                      j < _light.header().data(i).value_size(); ++j)
+                  {
+                    if (_light.header().data(i).key() ==
+                        "isLightOn")
+                    {
+                      isLightOn = ignition::math::parseInt(
+                        _light.header().data(i).value(0));
+                    }
+                  }
+                }
+                return isLightOn;
+               };
              return
+                getIsLightOn(_a) == getIsLightOn(_b) &&
                 _a.type() == _b.type() &&
                 _a.name() == _b.name() &&
                 math::equal(
diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc
index 4011cfaccf..390f4a4a2e 100644
--- a/test/integration/user_commands.cc
+++ b/test/integration/user_commands.cc
@@ -760,6 +760,14 @@ TEST_F(UserCommandsTest, IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Light))
   req.set_attenuation_constant(0.6f);
   req.set_attenuation_quadratic(0.001f);
   req.set_cast_shadows(true);
+
+  // todo(ahcorde) Use the field is_light_off in light.proto from
+  // Garden on.
+  auto header = req.mutable_header()->add_data();
+  header->set_key("isLightOn");
+  std::string *value = header->add_value();
+  *value = std::to_string(true);
+
   EXPECT_TRUE(node.Request(service, req, timeout, res, result));
   EXPECT_TRUE(result);
   EXPECT_TRUE(res.data());

From 1231d49de10166335aa4872dbc34b7c1522d7fc4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= <ahcorde@gmail.com>
Date: Fri, 18 Mar 2022 19:45:55 +0100
Subject: [PATCH 2/5] Toggle Light visuals (#1387)

Signed-off-by: ahcorde <ahcorde@gmail.com>
---
 examples/worlds/lights.sdf                    |  1 +
 src/Conversions.cc                            | 41 +++++++++++++++---
 .../component_inspector/ComponentInspector.cc | 43 +++++++++++++++----
 .../component_inspector/ComponentInspector.hh |  4 +-
 .../ComponentInspector.qml                    |  4 +-
 src/gui/plugins/component_inspector/Light.qml | 37 +++++++++++++++-
 src/rendering/RenderUtil.cc                   | 24 +++++++++++
 src/rendering/SceneManager.cc                 |  3 ++
 src/systems/user_commands/UserCommands.cc     | 22 ++++++++++
 9 files changed, 160 insertions(+), 19 deletions(-)

diff --git a/examples/worlds/lights.sdf b/examples/worlds/lights.sdf
index f1910f0ac7..61d2d92fd1 100644
--- a/examples/worlds/lights.sdf
+++ b/examples/worlds/lights.sdf
@@ -52,6 +52,7 @@
         <quadratic>0.01</quadratic>
       </attenuation>
       <cast_shadows>false</cast_shadows>
+      <visualize>false</visualize>
     </light>
 
     <light type="spot" name="spot">
diff --git a/src/Conversions.cc b/src/Conversions.cc
index aafd9213f7..fa72327900 100644
--- a/src/Conversions.cc
+++ b/src/Conversions.cc
@@ -569,12 +569,23 @@ msgs::Light ignition::gazebo::convert(const sdf::Light &_in)
   out.set_spot_outer_angle(_in.SpotOuterAngle().Radian());
   out.set_spot_falloff(_in.SpotFalloff());
 
-  // todo(ahcorde) Use the field is_light_off in light.proto from
-  // Garden on.
-  auto header = out.mutable_header()->add_data();
-  header->set_key("isLightOn");
-  std::string *value = header->add_value();
-  *value = std::to_string(_in.LightOn());
+  {
+    // todo(ahcorde) Use the field is_light_off in light.proto from
+    // Garden on.
+    auto header = out.mutable_header()->add_data();
+    header->set_key("isLightOn");
+    std::string *value = header->add_value();
+    *value = std::to_string(_in.LightOn());
+  }
+
+  {
+    // todo(ahcorde) Use the field visualize_visual in light.proto from
+    // Garden on.
+    auto header = out.mutable_header()->add_data();
+    header->set_key("visualizeVisual");
+    std::string *value = header->add_value();
+    *value = std::to_string(_in.Visualize());
+  }
 
   if (_in.Type() == sdf::LightType::POINT)
     out.set_type(msgs::Light_LightType_POINT);
@@ -606,6 +617,24 @@ sdf::Light ignition::gazebo::convert(const msgs::Light &_in)
   out.SetSpotOuterAngle(math::Angle(_in.spot_outer_angle()));
   out.SetSpotFalloff(_in.spot_falloff());
 
+  // todo(ahcorde) Use the field is_light_off in light.proto from
+  // Garden on.
+  bool visualizeVisual = true;
+  for (int i = 0; i < _in.header().data_size(); ++i)
+  {
+    for (int j = 0;
+        j < _in.header().data(i).value_size(); ++j)
+    {
+      if (_in.header().data(i).key() ==
+          "visualizeVisual")
+      {
+        visualizeVisual = ignition::math::parseInt(
+          _in.header().data(i).value(0));
+      }
+    }
+  }
+  out.SetVisualize(visualizeVisual);
+
   // todo(ahcorde) Use the field is_light_off in light.proto from
   // Garden on.
   bool isLightOn = true;
diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc
index f16712ce73..4deed41829 100644
--- a/src/gui/plugins/component_inspector/ComponentInspector.cc
+++ b/src/gui/plugins/component_inspector/ComponentInspector.cc
@@ -153,6 +153,21 @@ void ignition::gazebo::setData(QStandardItem *_item, const msgs::Light &_data)
     lightType = 2;
   }
 
+  bool visualizeVisual = true;
+  for (int i = 0; i < _data.header().data_size(); ++i)
+  {
+    for (int j = 0;
+        j < _data.header().data(i).value_size(); ++j)
+    {
+      if (_data.header().data(i).key() ==
+          "visualizeVisual")
+      {
+        visualizeVisual = ignition::math::parseInt(
+          _data.header().data(i).value(0));
+      }
+    }
+  }
+
   bool isLightOn = true;
   for (int i = 0; i < _data.header().data_size(); ++i)
   {
@@ -192,7 +207,8 @@ void ignition::gazebo::setData(QStandardItem *_item, const msgs::Light &_data)
     QVariant(_data.spot_falloff()),
     QVariant(_data.intensity()),
     QVariant(lightType),
-    QVariant(isLightOn)
+    QVariant(isLightOn),
+    QVariant(visualizeVisual)
   }), ComponentsModel::RoleNames().key("data"));
 }
 
@@ -1006,7 +1022,7 @@ void ComponentInspector::OnLight(
   double _attQuadratic, bool _castShadows, double _directionX,
   double _directionY, double _directionZ, double _innerAngle,
   double _outerAngle, double _falloff, double _intensity, int _type,
-  bool _isLightOn)
+  bool _isLightOn, bool _visualizeVisual)
 {
   std::function<void(const ignition::msgs::Boolean &, const bool)> cb =
       [](const ignition::msgs::Boolean &/*_rep*/, const bool _result)
@@ -1016,13 +1032,22 @@ void ComponentInspector::OnLight(
   };
 
   ignition::msgs::Light req;
-
-  // todo(ahcorde) Use the field is_light_off in light.proto from
-  // Garden on.
-  auto header = req.mutable_header()->add_data();
-  header->set_key("isLightOn");
-  std::string *value = header->add_value();
-  *value = std::to_string(_isLightOn);
+  {
+    // todo(ahcorde) Use the field is_light_off in light.proto from
+    // Garden on.
+    auto header = req.mutable_header()->add_data();
+    header->set_key("isLightOn");
+    std::string *value = header->add_value();
+    *value = std::to_string(_isLightOn);
+  }
+  {
+    // todo(ahcorde) Use the field visualize_visual in light.proto from
+    // Garden on.
+    auto header = req.mutable_header()->add_data();
+    header->set_key("visualizeVisual");
+    std::string *value = header->add_value();
+    *value = std::to_string(_visualizeVisual);
+  }
 
   req.set_name(this->dataPtr->entityName);
   req.set_id(this->dataPtr->entity);
diff --git a/src/gui/plugins/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh
index 0590aa31c7..54a6659a70 100644
--- a/src/gui/plugins/component_inspector/ComponentInspector.hh
+++ b/src/gui/plugins/component_inspector/ComponentInspector.hh
@@ -253,6 +253,7 @@ namespace gazebo
     /// \param[in] _intensity Intensity of the light
     /// \param[in] _type light type
     /// \param[in] _isLightOn is light on
+    /// \param[in] _visualizeVisual is visual enabled
     public: Q_INVOKABLE void OnLight(
       double _rSpecular, double _gSpecular, double _bSpecular,
       double _aSpecular, double _rDiffuse, double _gDiffuse,
@@ -260,7 +261,8 @@ namespace gazebo
       double _attLinear, double _attConstant, double _attQuadratic,
       bool _castShadows, double _directionX, double _directionY,
       double _directionZ, double _innerAngle, double _outerAngle,
-      double _falloff, double _intensity, int _type, bool _isLightOn);
+      double _falloff, double _intensity, int _type, bool _isLightOn,
+      bool _visualizeVisual);
 
     /// \brief Callback in Qt thread when physics' properties change.
     /// \param[in] _stepSize step size
diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml
index c9f68bd1e4..9e94025b16 100644
--- a/src/gui/plugins/component_inspector/ComponentInspector.qml
+++ b/src/gui/plugins/component_inspector/ComponentInspector.qml
@@ -102,13 +102,13 @@ Rectangle {
                    _attRange, _attLinear, _attConstant, _attQuadratic,
                    _castShadows, _directionX, _directionY, _directionZ,
                    _innerAngle, _outerAngle, _falloff, _intensity, _type,
-                   _isLightOn) {
+                   _isLightOn, _visualizeVisual) {
     ComponentInspector.OnLight(_rSpecular, _gSpecular, _bSpecular, _aSpecular,
                                _rDiffuse, _gDiffuse, _bDiffuse, _aDiffuse,
                                _attRange, _attLinear, _attConstant, _attQuadratic,
                                _castShadows, _directionX, _directionY, _directionZ,
                                _innerAngle, _outerAngle, _falloff, _intensity, _type,
-                               _isLightOn)
+                               _isLightOn, _visualizeVisual)
   }
 
   /*
diff --git a/src/gui/plugins/component_inspector/Light.qml b/src/gui/plugins/component_inspector/Light.qml
index f051feb2b3..b32da765a3 100644
--- a/src/gui/plugins/component_inspector/Light.qml
+++ b/src/gui/plugins/component_inspector/Light.qml
@@ -102,6 +102,9 @@ Rectangle {
   // Loaded item for isLightOn
   property var isLightOnItem: {}
 
+  // Loaded item for visualizeVisuals
+  property var visualizeVisualItem: {}
+
   // Send new light data to C++
   function sendLight() {
     // TODO(anyone) There's a loss of precision when these values get to C++
@@ -127,7 +130,8 @@ Rectangle {
       falloffItem.value,
       intensityItem.value,
       model.data[20],
-      isLightOnItem.checked
+      isLightOnItem.checked,
+      visualizeVisualItem.checked
     );
   }
 
@@ -289,6 +293,37 @@ Rectangle {
         id: grid
         width: parent.width
 
+        RowLayout {
+          Rectangle {
+            color: "transparent"
+            height: 40
+            Layout.preferredWidth: visualizeVisualText.width + indentation*3
+
+            Text {
+              id : visualizeVisualText
+              text: ' View gizmo'
+              leftPadding: 5
+              color: Material.theme == Material.Light ? "#444444" : "#bbbbbb"
+              font.pointSize: 12
+              anchors.centerIn: parent
+            }
+          }
+          Item {
+            Layout.fillWidth: true
+            height: 40
+
+            Loader {
+              id: visualizeVisualLoader
+              anchors.fill: parent
+              property double numberValue: model.data[22]
+              sourceComponent: ignSwitch
+              onLoaded: {
+                visualizeVisualItem = visualizeVisualLoader.item
+              }
+            }
+          }
+        }
+
         RowLayout {
           Rectangle {
             color: "transparent"
diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc
index 15e7bdfd43..4954674668 100644
--- a/src/rendering/RenderUtil.cc
+++ b/src/rendering/RenderUtil.cc
@@ -279,6 +279,7 @@ class ignition::gazebo::RenderUtilPrivate
           lightEql { [](const sdf::Light &_a, const sdf::Light &_b)
             {
              return
+                _a.Visualize() == _b.Visualize() &&
                 _a.Type() == _b.Type() &&
                 _a.Name() == _b.Name() &&
                 _a.Diffuse() == _b.Diffuse() &&
@@ -2131,6 +2132,29 @@ void RenderUtilPrivate::UpdateLights(
     auto l = std::dynamic_pointer_cast<rendering::Light>(node);
     if (l)
     {
+      // todo(ahcorde) Use the field visualize_visual in light.proto from
+      // Garden on.
+      bool visualizeVisual = true;
+      for (int i = 0; i < light.second.header().data_size(); ++i)
+      {
+        for (int j = 0;
+            j < light.second.header().data(i).value_size(); ++j)
+        {
+          if (light.second.header().data(i).key() ==
+              "visualizeVisual")
+          {
+            visualizeVisual = ignition::math::parseInt(
+              light.second.header().data(i).value(0));
+          }
+        }
+      }
+
+      rendering::VisualPtr lightVisual =
+          this->sceneManager.VisualById(
+            this->matchLightWithVisuals[light.first]);
+      if (lightVisual)
+        lightVisual->SetVisible(visualizeVisual);
+
       // todo(ahcorde) Use the field is_light_off in light.proto from
       // Garden on.
       bool isLightOn = true;
diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc
index fe334bad2a..79893983f2 100644
--- a/src/rendering/SceneManager.cc
+++ b/src/rendering/SceneManager.cc
@@ -1066,6 +1066,9 @@ rendering::VisualPtr SceneManager::CreateLightVisual(Entity _id,
     lightVisual->SetInnerAngle(_light.SpotInnerAngle().Radian());
     lightVisual->SetOuterAngle(_light.SpotOuterAngle().Radian());
   }
+
+  lightVisual->SetVisible(_light.Visualize());
+
   rendering::VisualPtr lightVis = std::dynamic_pointer_cast<rendering::Visual>(
     lightVisual);
   lightVis->SetUserData("gazebo-entity", static_cast<int>(_id));
diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc
index 1bd29a7d96..2b56e369c9 100644
--- a/src/systems/user_commands/UserCommands.cc
+++ b/src/systems/user_commands/UserCommands.cc
@@ -159,6 +159,27 @@ class LightCommand : public UserCommandBase
   public: std::function<bool(const msgs::Light &, const msgs::Light &)>
           lightEql { [](const msgs::Light &_a, const msgs::Light &_b)
             {
+              // todo(ahcorde) Use the field is_light_off in light.proto from
+              // Garden on.
+              auto getVisualizeVisual = [](const msgs::Light &_light) -> bool
+              {
+                bool visualizeVisual = true;
+                for (int i = 0; i < _light.header().data_size(); ++i)
+                {
+                  for (int j = 0;
+                      j < _light.header().data(i).value_size(); ++j)
+                  {
+                    if (_light.header().data(i).key() ==
+                        "visualizeVisual")
+                    {
+                      visualizeVisual = ignition::math::parseInt(
+                        _light.header().data(i).value(0));
+                    }
+                  }
+                }
+                return visualizeVisual;
+              };
+
               // todo(ahcorde) Use the field is_light_off in light.proto from
               // Garden on.
               auto getIsLightOn = [](const msgs::Light &_light) -> bool
@@ -180,6 +201,7 @@ class LightCommand : public UserCommandBase
                 return isLightOn;
                };
              return
+                getVisualizeVisual(_a) == getVisualizeVisual(_b) &&
                 getIsLightOn(_a) == getIsLightOn(_b) &&
                 _a.type() == _b.type() &&
                 _a.name() == _b.name() &&

From 6feba087f4e241ab6d84e57a60dc1b10b1d7b587 Mon Sep 17 00:00:00 2001
From: Louise Poubel <louise@openrobotics.org>
Date: Wed, 23 Mar 2022 10:08:32 -0700
Subject: [PATCH 3/5] Component inspector: refactor Pose3d C++ code into a
 separate class (#1400)

Signed-off-by: Louise Poubel <louise@openrobotics.org>
---
 .../component_inspector/CMakeLists.txt        |  8 +-
 .../component_inspector/ComponentInspector.cc | 92 +++++++----------
 .../component_inspector/ComponentInspector.hh | 33 ++++---
 src/gui/plugins/component_inspector/Pose3d.cc | 64 ++++++++++++
 src/gui/plugins/component_inspector/Pose3d.hh | 98 +++++++++++++++++++
 .../plugins/component_inspector/Pose3d.qml    |  2 +-
 src/gui/plugins/component_inspector/Types.hh  | 42 ++++++++
 7 files changed, 261 insertions(+), 78 deletions(-)
 create mode 100644 src/gui/plugins/component_inspector/Pose3d.cc
 create mode 100644 src/gui/plugins/component_inspector/Pose3d.hh
 create mode 100644 src/gui/plugins/component_inspector/Types.hh

diff --git a/src/gui/plugins/component_inspector/CMakeLists.txt b/src/gui/plugins/component_inspector/CMakeLists.txt
index 367278b24d..d4bc795abc 100644
--- a/src/gui/plugins/component_inspector/CMakeLists.txt
+++ b/src/gui/plugins/component_inspector/CMakeLists.txt
@@ -1,4 +1,8 @@
 gz_add_gui_plugin(ComponentInspector
-  SOURCES ComponentInspector.cc
-  QT_HEADERS ComponentInspector.hh
+  SOURCES
+    ComponentInspector.cc
+    Pose3d.cc
+  QT_HEADERS
+    ComponentInspector.hh
+    Pose3d.hh
 )
diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc
index c6b06f37b9..8350d79f77 100644
--- a/src/gui/plugins/component_inspector/ComponentInspector.cc
+++ b/src/gui/plugins/component_inspector/ComponentInspector.cc
@@ -22,7 +22,6 @@
 #include <ignition/gui/Application.hh>
 #include <ignition/gui/MainWindow.hh>
 #include <ignition/plugin/Register.hh>
-#include <ignition/transport/Node.hh>
 
 #include "ignition/gazebo/components/Actor.hh"
 #include "ignition/gazebo/components/AngularAcceleration.hh"
@@ -50,8 +49,6 @@
 #include "ignition/gazebo/components/Performer.hh"
 #include "ignition/gazebo/components/PerformerAffinity.hh"
 #include "ignition/gazebo/components/Physics.hh"
-#include "ignition/gazebo/components/Pose.hh"
-#include "ignition/gazebo/components/PoseCmd.hh"
 #include "ignition/gazebo/components/SelfCollide.hh"
 #include "ignition/gazebo/components/Sensor.hh"
 #include "ignition/gazebo/components/SourceFilePath.hh"
@@ -66,6 +63,7 @@
 #include "ignition/gazebo/gui/GuiEvents.hh"
 
 #include "ComponentInspector.hh"
+#include "Pose3d.hh"
 
 namespace ignition::gazebo
 {
@@ -97,31 +95,19 @@ namespace ignition::gazebo
 
     /// \brief Transport node for making command requests
     public: transport::Node node;
+
+    /// \brief A map of component types to the function used to update it.
+    public: std::map<ComponentTypeId, inspector::UpdateViewCb>
+        updateViewCbs;
+
+    /// \brief Handles all components displayed as a 3D pose.
+    public: std::unique_ptr<inspector::Pose3d> pose3d;
   };
 }
 
 using namespace ignition;
 using namespace gazebo;
 
-//////////////////////////////////////////////////
-template<>
-void ignition::gazebo::setData(QStandardItem *_item, const math::Pose3d &_data)
-{
-  if (nullptr == _item)
-    return;
-
-  _item->setData(QString("Pose3d"),
-      ComponentsModel::RoleNames().key("dataType"));
-  _item->setData(QList({
-    QVariant(_data.Pos().X()),
-    QVariant(_data.Pos().Y()),
-    QVariant(_data.Pos().Z()),
-    QVariant(_data.Rot().Roll()),
-    QVariant(_data.Rot().Pitch()),
-    QVariant(_data.Rot().Yaw())
-  }), ComponentsModel::RoleNames().key("data"));
-}
-
 //////////////////////////////////////////////////
 template<>
 void ignition::gazebo::setData(QStandardItem *_item,
@@ -340,6 +326,9 @@ void ComponentInspector::LoadConfig(const tinyxml2::XMLElement *)
   // Connect model
   this->Context()->setContextProperty(
       "ComponentsModel", &this->dataPtr->componentsModel);
+
+  // Type-specific handlers
+  this->dataPtr->pose3d = std::make_unique<inspector::Pose3d>(this);
 }
 
 //////////////////////////////////////////////////
@@ -588,12 +577,6 @@ void ComponentInspector::Update(const UpdateInfo &,
       if (comp)
         setData(item, comp->Data());
     }
-    else if (typeId == components::Pose::typeId)
-    {
-      auto comp = _ecm.Component<components::Pose>(this->dataPtr->entity);
-      if (comp)
-        setData(item, comp->Data());
-    }
     else if (typeId == components::Static::typeId)
     {
       auto comp = _ecm.Component<components::Static>(this->dataPtr->entity);
@@ -694,18 +677,10 @@ void ComponentInspector::Update(const UpdateInfo &,
         setUnit(item, "m/s");
       }
     }
-    else if (typeId == components::WorldPose::typeId)
+    else if (this->dataPtr->updateViewCbs.find(typeId) !=
+          this->dataPtr->updateViewCbs.end())
     {
-      auto comp = _ecm.Component<components::WorldPose>(this->dataPtr->entity);
-      if (comp)
-        setData(item, comp->Data());
-    }
-    else if (typeId == components::WorldPoseCmd::typeId)
-    {
-      auto comp = _ecm.Component<components::WorldPoseCmd>(
-          this->dataPtr->entity);
-      if (comp)
-        setData(item, comp->Data());
+      this->dataPtr->updateViewCbs[typeId](_ecm, item);
     }
   }
 
@@ -723,6 +698,13 @@ void ComponentInspector::Update(const UpdateInfo &,
   }
 }
 
+/////////////////////////////////////////////////
+void ComponentInspector::AddUpdateViewCb(ComponentTypeId _id,
+    inspector::UpdateViewCb _cb)
+{
+  this->dataPtr->updateViewCbs[_id] = _cb;
+}
+
 /////////////////////////////////////////////////
 bool ComponentInspector::eventFilter(QObject *_obj, QEvent *_event)
 {
@@ -812,26 +794,6 @@ void ComponentInspector::SetPaused(bool _paused)
   this->PausedChanged();
 }
 
-/////////////////////////////////////////////////
-void ComponentInspector::OnPose(double _x, double _y, double _z, double _roll,
-    double _pitch, double _yaw)
-{
-  std::function<void(const ignition::msgs::Boolean &, const bool)> cb =
-      [](const ignition::msgs::Boolean &/*_rep*/, const bool _result)
-  {
-    if (!_result)
-        ignerr << "Error setting pose" << std::endl;
-  };
-
-  ignition::msgs::Pose req;
-  req.set_id(this->dataPtr->entity);
-  msgs::Set(req.mutable_position(), math::Vector3d(_x, _y, _z));
-  msgs::Set(req.mutable_orientation(), math::Quaterniond(_roll, _pitch, _yaw));
-  auto poseCmdService = "/world/" + this->dataPtr->worldName
-      + "/set_pose";
-  this->dataPtr->node.Request(poseCmdService, req, cb);
-}
-
 /////////////////////////////////////////////////
 void ComponentInspector::OnPhysics(double _stepSize, double _realTimeFactor)
 {
@@ -862,6 +824,18 @@ bool ComponentInspector::NestedModel() const
   return this->dataPtr->nestedModel;
 }
 
+/////////////////////////////////////////////////
+const std::string &ComponentInspector::WorldName() const
+{
+  return this->dataPtr->worldName;
+}
+
+/////////////////////////////////////////////////
+transport::Node &ComponentInspector::TransportNode()
+{
+  return this->dataPtr->node;
+}
+
 // Register this plugin
 IGNITION_ADD_PLUGIN(ignition::gazebo::ComponentInspector,
                     ignition::gui::Plugin)
diff --git a/src/gui/plugins/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh
index 4b41e15371..194893a9ca 100644
--- a/src/gui/plugins/component_inspector/ComponentInspector.hh
+++ b/src/gui/plugins/component_inspector/ComponentInspector.hh
@@ -21,8 +21,8 @@
 #include <map>
 #include <memory>
 #include <string>
-#include <ignition/math/Pose3.hh>
 #include <ignition/math/Vector3.hh>
+#include <ignition/transport/Node.hh>
 
 #include <ignition/gazebo/components/Component.hh>
 #include <ignition/gazebo/gui/GuiSystem.hh>
@@ -30,6 +30,8 @@
 
 #include "ignition/gazebo/components/Physics.hh"
 
+#include "Types.hh"
+
 Q_DECLARE_METATYPE(ignition::gazebo::ComponentTypeId)
 
 namespace ignition
@@ -65,12 +67,6 @@ namespace gazebo
   template<>
   void setData(QStandardItem *_item, const std::string &_data);
 
-  /// \brief Specialized to set pose data.
-  /// \param[in] _item Item whose data will be set.
-  /// \param[in] _data Data to set.
-  template<>
-  void setData(QStandardItem *_item, const math::Pose3d &_data);
-
   /// \brief Specialized to set vector data.
   /// \param[in] _item Item whose data will be set.
   /// \param[in] _data Data to set.
@@ -205,15 +201,12 @@ namespace gazebo
     // Documentation inherited
     public: void Update(const UpdateInfo &, EntityComponentManager &) override;
 
-    /// \brief Callback in Qt thread when pose changes.
-    /// \param[in] _x X
-    /// \param[in] _y Y
-    /// \param[in] _z Z
-    /// \param[in] _roll Roll
-    /// \param[in] _pitch Pitch
-    /// \param[in] _yaw Yaw
-    public: Q_INVOKABLE void OnPose(double _x, double _y, double _z,
-        double _roll, double _pitch, double _yaw);
+    /// \brief Add a callback that's called whenever there are updates from the
+    /// ECM to the view, for a given component type.
+    /// \param[in] _id The component type id
+    /// \param[in] _cb Function that's called when there are updates.
+    public: void AddUpdateViewCb(ComponentTypeId _id,
+                inspector::UpdateViewCb _cb);
 
     /// \brief Callback in Qt thread when physics' properties change.
     /// \param[in] _stepSize step size
@@ -275,6 +268,14 @@ namespace gazebo
     /// \brief Notify that paused has changed.
     signals: void PausedChanged();
 
+    /// \brief Name of world entity
+    /// \return World name
+    public: const std::string &WorldName() const;
+
+    /// \brief Node for communication
+    /// \return Transport node
+    public: transport::Node &TransportNode();
+
     /// \internal
     /// \brief Pointer to private data.
     private: std::unique_ptr<ComponentInspectorPrivate> dataPtr;
diff --git a/src/gui/plugins/component_inspector/Pose3d.cc b/src/gui/plugins/component_inspector/Pose3d.cc
new file mode 100644
index 0000000000..e3a9446202
--- /dev/null
+++ b/src/gui/plugins/component_inspector/Pose3d.cc
@@ -0,0 +1,64 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include <string>
+
+#include <ignition/msgs/boolean.pb.h>
+#include <ignition/msgs/pose.pb.h>
+
+#include "Pose3d.hh"
+
+using namespace ignition;
+using namespace gazebo;
+using namespace inspector;
+
+/////////////////////////////////////////////////
+Pose3d::Pose3d(ComponentInspector *_inspector)
+{
+  _inspector->Context()->setContextProperty("Pose3dImpl", this);
+  this->inspector = _inspector;
+
+  this->inspector->AddUpdateViewCb(components::Pose::typeId,
+      std::bind(&Pose3d::UpdateView<components::Pose>, this,
+      std::placeholders::_1, std::placeholders::_2));
+  this->inspector->AddUpdateViewCb(components::WorldPose::typeId,
+      std::bind(&Pose3d::UpdateView<components::WorldPose>, this,
+      std::placeholders::_1, std::placeholders::_2));
+  this->inspector->AddUpdateViewCb(components::WorldPoseCmd::typeId,
+      std::bind(&Pose3d::UpdateView<components::WorldPoseCmd>, this,
+      std::placeholders::_1, std::placeholders::_2));
+}
+
+/////////////////////////////////////////////////
+void Pose3d::OnPose(double _x, double _y, double _z, double _roll,
+    double _pitch, double _yaw)
+{
+  std::function<void(const msgs::Boolean &, const bool)> cb =
+      [](const msgs::Boolean &, const bool _result)
+  {
+    if (!_result)
+        ignerr << "Error setting pose" << std::endl;
+  };
+
+  msgs::Pose req;
+  req.set_id(this->inspector->GetEntity());
+  msgs::Set(req.mutable_position(), math::Vector3d(_x, _y, _z));
+  msgs::Set(req.mutable_orientation(), math::Quaterniond(_roll, _pitch, _yaw));
+  std::string poseCmdService("/world/" + this->inspector->WorldName()
+      + "/set_pose");
+  this->inspector->TransportNode().Request(poseCmdService, req, cb);
+}
diff --git a/src/gui/plugins/component_inspector/Pose3d.hh b/src/gui/plugins/component_inspector/Pose3d.hh
new file mode 100644
index 0000000000..890cac2590
--- /dev/null
+++ b/src/gui/plugins/component_inspector/Pose3d.hh
@@ -0,0 +1,98 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef IGNITION_GAZEBO_GUI_COMPONENTINSPECTOR_POSE3D_HH_
+#define IGNITION_GAZEBO_GUI_COMPONENTINSPECTOR_POSE3D_HH_
+
+#include <ignition/math/Pose3.hh>
+
+#include "ignition/gazebo/components/Pose.hh"
+#include "ignition/gazebo/components/PoseCmd.hh"
+#include "ignition/gazebo/EntityComponentManager.hh"
+
+#include "ComponentInspector.hh"
+#include "Types.hh"
+
+#include <QObject>
+#include <QStandardItem>
+
+namespace ignition
+{
+namespace gazebo
+{
+class ComponentInspector;
+namespace inspector
+{
+  /// \brief Handles components that are displayed as a 3D pose:
+  /// * `components::Pose`
+  /// * `components::WorldPose`
+  /// * `components::WorldPoseCmd`
+  class Pose3d : public QObject
+  {
+    Q_OBJECT
+
+    /// \brief Constructor
+    /// \param[in] _inspector The component inspector.
+    public: explicit Pose3d(ComponentInspector *_inspector);
+
+    /// \brief Callback when there are ECM updates.
+    /// \param[in] _ecm Immutable reference to the ECM.
+    /// \param[in] _item Item to update.
+    /// \tparam ComponentType Type of component being updated.
+    public:
+    template<typename ComponentType>
+    void UpdateView(const EntityComponentManager &_ecm,
+        QStandardItem *_item)
+    {
+      if (nullptr == _item)
+        return;
+
+      auto comp = _ecm.Component<ComponentType>(this->inspector->GetEntity());
+      if (nullptr == comp)
+        return;
+
+      auto pose = comp->Data();
+
+      _item->setData(QString("Pose3d"),
+          ComponentsModel::RoleNames().key("dataType"));
+      _item->setData(QList({
+        QVariant(pose.Pos().X()),
+        QVariant(pose.Pos().Y()),
+        QVariant(pose.Pos().Z()),
+        QVariant(pose.Rot().Roll()),
+        QVariant(pose.Rot().Pitch()),
+        QVariant(pose.Rot().Yaw())
+      }), ComponentsModel::RoleNames().key("data"));
+    }
+
+    /// \brief Callback in Qt thread when pose changes.
+    /// \param[in] _x X
+    /// \param[in] _y Y
+    /// \param[in] _z Z
+    /// \param[in] _roll Roll
+    /// \param[in] _pitch Pitch
+    /// \param[in] _yaw Yaw
+    public: Q_INVOKABLE void OnPose(double _x, double _y, double _z,
+        double _roll, double _pitch, double _yaw);
+
+    /// \brief Pointer to the component inspector. This is used to add
+    /// callbacks.
+    private: ComponentInspector *inspector{nullptr};
+  };
+}
+}
+}
+#endif
diff --git a/src/gui/plugins/component_inspector/Pose3d.qml b/src/gui/plugins/component_inspector/Pose3d.qml
index 962acb770e..3574a13b34 100644
--- a/src/gui/plugins/component_inspector/Pose3d.qml
+++ b/src/gui/plugins/component_inspector/Pose3d.qml
@@ -65,7 +65,7 @@ Rectangle {
   // Send new pose to C++
   function sendPose() {
     // TODO(anyone) There's a loss of precision when these values get to C++
-    componentInspector.onPose(
+    Pose3dImpl.OnPose(
       xItem.value,
       yItem.value,
       zItem.value,
diff --git a/src/gui/plugins/component_inspector/Types.hh b/src/gui/plugins/component_inspector/Types.hh
new file mode 100644
index 0000000000..5e0b682027
--- /dev/null
+++ b/src/gui/plugins/component_inspector/Types.hh
@@ -0,0 +1,42 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef IGNITION_GAZEBO_GUI_COMPONENTINSPECTOR_TYPES_HH_
+#define IGNITION_GAZEBO_GUI_COMPONENTINSPECTOR_TYPES_HH_
+
+#include <QStandardItem>
+
+#include <ignition/gazebo/Types.hh>
+
+namespace ignition
+{
+namespace gazebo
+{
+namespace inspector
+{
+  /// \brief Function definition that a component can use
+  /// to update its UI elements based on changes from the ECM.
+  ///   * _ecm Immutable reference to the ECM
+  ///   * _item Item to be updated
+  /// \sa ComponentInspector::AddUpdateViewCb
+  using UpdateViewCb = std::function<void(const EntityComponentManager &_ecm,
+      QStandardItem *_item)>;
+}
+}
+}
+#endif
+

From 1fa99f4e9177a6cdaa9eff73fa29d3567868be24 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= <ahcorde@gmail.com>
Date: Thu, 24 Mar 2022 17:37:56 +0100
Subject: [PATCH 4/5] Added user command to set multiple entities (#1394)

Signed-off-by: ahcorde <ahcorde@gmail.com>
Signed-off-by: Louise Poubel <louise@openrobotics.org>

Co-authored-by: Louise Poubel <louise@openrobotics.org>
---
 src/systems/user_commands/UserCommands.cc | 165 +++++++++++++++++-----
 src/systems/user_commands/UserCommands.hh |  16 +++
 test/integration/user_commands.cc         |  75 ++++++++++
 3 files changed, 220 insertions(+), 36 deletions(-)

diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc
index 0280a0c078..65d72ec60f 100644
--- a/src/systems/user_commands/UserCommands.cc
+++ b/src/systems/user_commands/UserCommands.cc
@@ -21,6 +21,7 @@
 #include <ignition/msgs/boolean.pb.h>
 #include <ignition/msgs/entity_factory.pb.h>
 #include <ignition/msgs/pose.pb.h>
+#include <ignition/msgs/pose_v.pb.h>
 #include <ignition/msgs/physics.pb.h>
 
 #include <string>
@@ -138,17 +139,19 @@ class PoseCommand : public UserCommandBase
 
   // Documentation inherited
   public: bool Execute() final;
+};
+
+/// \brief Command to update an entity's pose transform.
+class PoseVectorCommand : public UserCommandBase
+{
+  /// \brief Constructor
+  /// \param[in] _msg pose_v message.
+  /// \param[in] _iface Pointer to user commands interface.
+  public: PoseVectorCommand(msgs::Pose_V *_msg,
+      std::shared_ptr<UserCommandsInterface> &_iface);
 
-  /// \brief Pose3d equality comparison function.
-  public: std::function<bool(const math::Pose3d &, const math::Pose3d &)>
-          pose3Eql { [](const math::Pose3d &_a, const math::Pose3d &_b)
-                     {
-                       return _a.Pos().Equal(_b.Pos(), 1e-6) &&
-                         math::equal(_a.Rot().X(), _b.Rot().X(), 1e-6) &&
-                         math::equal(_a.Rot().Y(), _b.Rot().Y(), 1e-6) &&
-                         math::equal(_a.Rot().Z(), _b.Rot().Z(), 1e-6) &&
-                         math::equal(_a.Rot().W(), _b.Rot().W(), 1e-6);
-                     }};
+  // Documentation inherited
+  public: bool Execute() final;
 };
 
 /// \brief Command to modify the physics parameters of a simulation.
@@ -202,6 +205,13 @@ class ignition::gazebo::systems::UserCommandsPrivate
   /// \return True if successful.
   public: bool PoseService(const msgs::Pose &_req, msgs::Boolean &_res);
 
+  /// \brief Callback for pose_v service
+  /// \param[in] _req Request containing pose update of several entities.
+  /// \param[out] _res True if message successfully received and queued.
+  /// It does not mean that the entity will be successfully moved.
+  /// \return True if successful.
+  public: bool PoseVectorService(const msgs::Pose_V &_req, msgs::Boolean &_res);
+
   /// \brief Callback for physics service
   /// \param[in] _req Request containing updates to the physics parameters.
   /// \param[in] _res True if message successfully received and queued.
@@ -222,6 +232,26 @@ class ignition::gazebo::systems::UserCommandsPrivate
   public: std::mutex pendingMutex;
 };
 
+/// \brief Pose3d equality comparison function.
+/// \param[in] _a A pose to compare
+/// \param[in] _b Another pose to compare
+bool pose3Eql(const math::Pose3d &_a, const math::Pose3d &_b)
+{
+  return _a.Pos().Equal(_b.Pos(), 1e-6) &&
+    math::equal(_a.Rot().X(), _b.Rot().X(), 1e-6) &&
+    math::equal(_a.Rot().Y(), _b.Rot().Y(), 1e-6) &&
+    math::equal(_a.Rot().Z(), _b.Rot().Z(), 1e-6) &&
+    math::equal(_a.Rot().W(), _b.Rot().W(), 1e-6);
+}
+
+/// \brief Update pose for a specific pose message
+/// \param[in] _req Message containing new pose
+/// \param[in] _iface Pointer to user commands interface.
+/// \return True if successful.
+bool updatePose(
+  const msgs::Pose &_req,
+  std::shared_ptr<UserCommandsInterface> _iface);
+
 //////////////////////////////////////////////////
 UserCommands::UserCommands() : System(),
     dataPtr(std::make_unique<UserCommandsPrivate>())
@@ -273,6 +303,14 @@ void UserCommands::Configure(const Entity &_entity,
 
   ignmsg << "Pose service on [" << poseService << "]" << std::endl;
 
+  // Pose vector service
+  std::string poseVectorService{
+    "/world/" + worldName + "/set_pose_vector"};
+  this->dataPtr->node.Advertise(poseVectorService,
+      &UserCommandsPrivate::PoseVectorService, this->dataPtr.get());
+
+  ignmsg << "Pose service on [" << poseVectorService << "]" << std::endl;
+
   // Physics service
   std::string physicsService{"/world/" + worldName + "/set_physics"};
   this->dataPtr->node.Advertise(physicsService,
@@ -390,6 +428,25 @@ bool UserCommandsPrivate::PoseService(const msgs::Pose &_req,
   return true;
 }
 
+//////////////////////////////////////////////////
+bool UserCommandsPrivate::PoseVectorService(const msgs::Pose_V &_req,
+    msgs::Boolean &_res)
+{
+  // Create command and push it to queue
+  auto msg = _req.New();
+  msg->CopyFrom(_req);
+  auto cmd = std::make_unique<PoseVectorCommand>(msg, this->iface);
+
+  // Push to pending
+  {
+    std::lock_guard<std::mutex> lock(this->pendingMutex);
+    this->pendingCmds.push_back(std::move(cmd));
+  }
+
+  _res.set_data(true);
+  return true;
+}
+
 //////////////////////////////////////////////////
 bool UserCommandsPrivate::PhysicsService(const msgs::Physics &_req,
     msgs::Boolean &_res)
@@ -679,60 +736,96 @@ bool RemoveCommand::Execute()
   return true;
 }
 
-//////////////////////////////////////////////////
-PoseCommand::PoseCommand(msgs::Pose *_msg,
-    std::shared_ptr<UserCommandsInterface> &_iface)
-    : UserCommandBase(_msg, _iface)
-{
-}
 
 //////////////////////////////////////////////////
-bool PoseCommand::Execute()
+bool updatePose(
+  const msgs::Pose &_poseMsg,
+  std::shared_ptr<UserCommandsInterface> _iface)
 {
-  auto poseMsg = dynamic_cast<const msgs::Pose *>(this->msg);
-  if (nullptr == poseMsg)
-  {
-    ignerr << "Internal error, null create message" << std::endl;
-    return false;
-  }
-
   // Check the name of the entity being spawned
-  std::string entityName = poseMsg->name();
+  std::string entityName = _poseMsg.name();
   Entity entity = kNullEntity;
   // TODO(anyone) Update pose message to use Entity, with default ID null
-  if (poseMsg->id() != kNullEntity && poseMsg->id() != 0)
+  if (_poseMsg.id() != kNullEntity && _poseMsg.id() != 0)
   {
-    entity = poseMsg->id();
+    entity = _poseMsg.id();
   }
   else if (!entityName.empty())
   {
-    entity = this->iface->ecm->EntityByComponents(components::Name(entityName),
-      components::ParentEntity(this->iface->worldEntity));
+    entity = _iface->ecm->EntityByComponents(components::Name(entityName),
+      components::ParentEntity(_iface->worldEntity));
   }
 
-  if (!this->iface->ecm->HasEntity(entity))
+  if (!_iface->ecm->HasEntity(entity))
   {
-    ignerr << "Unable to update the pose for entity id:[" << poseMsg->id()
+    ignerr << "Unable to update the pose for entity id:[" << _poseMsg.id()
            << "], name[" << entityName << "]" << std::endl;
     return false;
   }
 
   auto poseCmdComp =
-    this->iface->ecm->Component<components::WorldPoseCmd>(entity);
+    _iface->ecm->Component<components::WorldPoseCmd>(entity);
   if (!poseCmdComp)
   {
-    this->iface->ecm->CreateComponent(
-        entity, components::WorldPoseCmd(msgs::Convert(*poseMsg)));
+    _iface->ecm->CreateComponent(
+        entity, components::WorldPoseCmd(msgs::Convert(_poseMsg)));
   }
   else
   {
     /// \todo(anyone) Moving an object is not captured in a log file.
-    auto state = poseCmdComp->SetData(msgs::Convert(*poseMsg), this->pose3Eql) ?
+    auto state = poseCmdComp->SetData(msgs::Convert(_poseMsg), pose3Eql) ?
         ComponentState::OneTimeChange :
         ComponentState::NoChange;
-    this->iface->ecm->SetChanged(entity, components::WorldPoseCmd::typeId,
+    _iface->ecm->SetChanged(entity, components::WorldPoseCmd::typeId,
         state);
   }
+  return true;
+}
+
+//////////////////////////////////////////////////
+PoseCommand::PoseCommand(msgs::Pose *_msg,
+    std::shared_ptr<UserCommandsInterface> &_iface)
+    : UserCommandBase(_msg, _iface)
+{
+}
+
+//////////////////////////////////////////////////
+bool PoseCommand::Execute()
+{
+  auto poseMsg = dynamic_cast<const msgs::Pose *>(this->msg);
+  if (nullptr == poseMsg)
+  {
+    ignerr << "Internal error, null create message" << std::endl;
+    return false;
+  }
+
+  return updatePose(*poseMsg, this->iface);
+}
+
+//////////////////////////////////////////////////
+PoseVectorCommand::PoseVectorCommand(msgs::Pose_V *_msg,
+    std::shared_ptr<UserCommandsInterface> &_iface)
+    : UserCommandBase(_msg, _iface)
+{
+}
+
+//////////////////////////////////////////////////
+bool PoseVectorCommand::Execute()
+{
+  auto poseVectorMsg = dynamic_cast<const msgs::Pose_V *>(this->msg);
+  if (nullptr == poseVectorMsg)
+  {
+    ignerr << "Internal error, null create message" << std::endl;
+    return false;
+  }
+
+  for (int i = 0; i < poseVectorMsg->pose_size(); i++)
+  {
+    if (!updatePose(poseVectorMsg->pose(i), this->iface))
+    {
+      return false;
+    }
+  }
 
   return true;
 }
diff --git a/src/systems/user_commands/UserCommands.hh b/src/systems/user_commands/UserCommands.hh
index 3db285aad6..95af084c96 100644
--- a/src/systems/user_commands/UserCommands.hh
+++ b/src/systems/user_commands/UserCommands.hh
@@ -53,6 +53,22 @@ namespace systems
   /// * **Request type*: ignition.msgs.EntityFactory_V
   /// * **Response type*: ignition.msgs.Boolean
   ///
+  /// # Set entity pose
+  ///
+  /// This service set the pose of entities
+  ///
+  /// * **Service**: `/world/<world name>/set_pose`
+  /// * **Request type*: ignition.msgs.Pose
+  /// * **Response type*: ignition.msgs.Boolean
+  ///
+  /// # Set multiple entity poses
+  ///
+  /// This service set the pose of multiple entities
+  ///
+  /// * **Service**: `/world/<world name>/set_pose_vector`
+  /// * **Request type*: ignition.msgs.Pose_V
+  /// * **Response type*: ignition.msgs.Boolean
+  ///
   /// Try some examples described on examples/worlds/empty.sdf
   class UserCommands:
     public System,
diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc
index d06ce9a2cb..63dee21695 100644
--- a/test/integration/user_commands.cc
+++ b/test/integration/user_commands.cc
@@ -669,6 +669,81 @@ TEST_F(UserCommandsTest, Pose)
   EXPECT_NEAR(500.0, poseComp->Data().Pos().Y(), 0.2);
 }
 
+
+/////////////////////////////////////////////////
+TEST_F(UserCommandsTest, PoseVector)
+{
+  // Start server
+  ServerConfig serverConfig;
+  const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
+    "/test/worlds/shapes.sdf";
+  serverConfig.SetSdfFile(sdfFile);
+
+  Server server(serverConfig);
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+
+  // Create a system just to get the ECM
+  EntityComponentManager *ecm{nullptr};
+  test::Relay testSystem;
+  testSystem.OnPreUpdate([&](const gazebo::UpdateInfo &,
+                             gazebo::EntityComponentManager &_ecm)
+      {
+        ecm = &_ecm;
+      });
+
+  server.AddSystem(testSystem.systemPtr);
+
+  // Run server and check we have the ECM
+  EXPECT_EQ(nullptr, ecm);
+  server.Run(true, 1, false);
+  EXPECT_NE(nullptr, ecm);
+
+  // Entity move by name
+  msgs::Pose_V req;
+
+  auto poseBoxMsg = req.add_pose();
+  poseBoxMsg->set_name("box");
+  poseBoxMsg->mutable_position()->set_y(123.0);
+
+  auto poseSphereMsg = req.add_pose();
+  poseSphereMsg->set_name("sphere");
+  poseSphereMsg->mutable_position()->set_y(456.0);
+
+  msgs::Boolean res;
+  bool result;
+  unsigned int timeout = 5000;
+  std::string service{"/world/default/set_pose_vector"};
+
+  transport::Node node;
+  EXPECT_TRUE(node.Request(service, req, timeout, res, result));
+  EXPECT_TRUE(result);
+  EXPECT_TRUE(res.data());
+
+  // Box entity
+  auto boxEntity = ecm->EntityByComponents(components::Name("box"));
+  EXPECT_NE(kNullEntity, boxEntity);
+
+  // Check entity has not been moved yet
+  auto poseComp = ecm->Component<components::Pose>(boxEntity);
+  ASSERT_NE(nullptr, poseComp);
+  EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 1), poseComp->Data());
+
+  // Run an iteration and check it was moved
+  server.Run(true, 1, false);
+
+  poseComp = ecm->Component<components::Pose>(boxEntity);
+  ASSERT_NE(nullptr, poseComp);
+  EXPECT_NEAR(123.0, poseComp->Data().Pos().Y(), 0.2);
+
+  auto sphereEntity = ecm->EntityByComponents(components::Name("sphere"));
+  EXPECT_NE(kNullEntity, sphereEntity);
+
+  poseComp = ecm->Component<components::Pose>(sphereEntity);
+  ASSERT_NE(nullptr, poseComp);
+  EXPECT_NEAR(456, poseComp->Data().Pos().Y(), 0.2);
+}
+
 /////////////////////////////////////////////////
 TEST_F(UserCommandsTest, Physics)
 {

From dad34de0cf132e73ccd95dad6f9e33aa7531ac9c Mon Sep 17 00:00:00 2001
From: Louise Poubel <louise@openrobotics.org>
Date: Thu, 24 Mar 2022 14:34:20 -0700
Subject: [PATCH 5/5] Disable tests that are expected to fail on Windows
 (#1408)

Signed-off-by: Louise Poubel <louise@openrobotics.org>
---
 test/integration/buoyancy.cc      | 2 +-
 test/integration/user_commands.cc | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/test/integration/buoyancy.cc b/test/integration/buoyancy.cc
index 065e145677..0a5cbf72f6 100644
--- a/test/integration/buoyancy.cc
+++ b/test/integration/buoyancy.cc
@@ -200,7 +200,7 @@ TEST_F(BuoyancyTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(Movement))
 }
 
 /////////////////////////////////////////////////
-TEST_F(BuoyancyTest, OffsetAndRotation)
+TEST_F(BuoyancyTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(OffsetAndRotation))
 {
   TestFixture fixture(common::joinPaths(std::string(PROJECT_SOURCE_PATH),
     "test", "worlds", "center_of_volume.sdf"));
diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc
index fb6d28501e..81528beb64 100644
--- a/test/integration/user_commands.cc
+++ b/test/integration/user_commands.cc
@@ -694,7 +694,7 @@ TEST_F(UserCommandsTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(Pose))
 
 
 /////////////////////////////////////////////////
-TEST_F(UserCommandsTest, PoseVector)
+TEST_F(UserCommandsTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(PoseVector))
 {
   // Start server
   ServerConfig serverConfig;