System inspector GUI widget (#1404)

Signed-off-by: Louise Poubel <[email protected]>

Co-authored-by: Nate Koenig <[email protected]>

Author: chapulina

include/ignition/gazebo/Conversions.hh

@@ -49,6 +49,7 @@
 #include <sdf/Material.hh>
 #include <sdf/Noise.hh>
 #include <sdf/ParticleEmitter.hh>
+#include <sdf/Plugin.hh>
 #include <sdf/Physics.hh>
 #include <sdf/Scene.hh>
 #include <sdf/Sensor.hh>
@@ -709,6 +710,38 @@ namespace ignition
     /// \return Particle emitter message.
     template<>
     sdf::ParticleEmitter convert(const msgs::ParticleEmitter &_in);
+
+    /// \brief Generic conversion from an SDF element to another type.
+    /// \param[in] _in SDF element.
+    /// \return Conversion result.
+    /// \tparam Out Output type.
+    template<class Out>
+    Out convert(const sdf::Element &/*_in*/)
+    {
+      Out::ConversionNotImplemented;
+    }
+
+    /// \brief Specialized conversion from an SDF element to a plugin message.
+    /// \param[in] _in SDF element.
+    /// \return Plugin message.
+    template<>
+    msgs::Plugin convert(const sdf::Element &_in);
+
+    /// \brief Generic conversion from an SDF plugin to another type.
+    /// \param[in] _in SDF plugin.
+    /// \return Conversion result.
+    /// \tparam Out Output type.
+    template<class Out>
+    Out convert(const sdf::Plugin &/*_in*/)
+    {
+      Out::ConversionNotImplemented;
+    }
+
+    /// \brief Specialized conversion from an SDF plugin to a plugin message.
+    /// \param[in] _in SDF plugin.
+    /// \return Plugin message.
+    template<>
+    msgs::Plugin convert(const sdf::Plugin &_in);
     }
   }
 }

include/ignition/gazebo/components/SystemPluginInfo.hh

@@ -0,0 +1,47 @@
+/*
+ * 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_COMPONENTS_SYSTEMINFO_HH_
+#define IGNITION_GAZEBO_COMPONENTS_SYSTEMINFO_HH_
+
+#include <ignition/msgs/plugin_v.pb.h>
+#include <ignition/gazebo/components/Factory.hh>
+#include <ignition/gazebo/components/Component.hh>
+#include <ignition/gazebo/components/Serialization.hh>
+#include <ignition/gazebo/config.hh>
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace components
+{
+  /// \brief This component holds information about all the system plugins that
+  /// are attached to an entity. The content of each system is populated the
+  /// moment the plugin is instantiated and isn't modified throughout
+  /// simulation.
+  using SystemPluginInfo = Component<msgs::Plugin_V, class SystemPluginInfoTag,
+      serializers::MsgSerializer>;
+  IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.SystemPluginInfo",
+      SystemPluginInfo)
+}
+}
+}
+}
+
+#endif

src/Conversions.cc

@@ -711,28 +711,13 @@ msgs::GUI ignition::gazebo::convert(const sdf::Gui &_in)
   out.set_fullscreen(_in.Fullscreen());
 
   // Set gui plugins
-  auto elem = _in.Element();
-  if (elem && elem->HasElement("plugin"))
+  for (uint64_t i = 0; i < _in.PluginCount(); ++i)
   {
-    auto pluginElem = elem->GetElement("plugin");
-    while (pluginElem)
-    {
-      auto pluginMsg = out.add_plugin();
-      pluginMsg->set_name(pluginElem->Get<std::string>("name"));
-      pluginMsg->set_filename(pluginElem->Get<std::string>("filename"));
-
-      std::stringstream ss;
-      for (auto innerElem = pluginElem->GetFirstElement();
-          innerElem; innerElem = innerElem->GetNextElement(""))
-      {
-        ss << innerElem->ToString("");
-      }
-      pluginMsg->set_innerxml(ss.str());
-      pluginElem = pluginElem->GetNextElement("plugin");
-    }
+    auto pluginMsg = out.add_plugin();
+    pluginMsg->CopyFrom(convert<msgs::Plugin>(*_in.PluginByIndex(i)));
   }
 
-  if (elem->HasElement("camera"))
+  if (_in.Element()->HasElement("camera"))
   {
     ignwarn << "<gui><camera> can't be converted yet" << std::endl;
   }
@@ -1759,3 +1744,51 @@ sdf::ParticleEmitter ignition::gazebo::convert(const msgs::ParticleEmitter &_in)
 
   return out;
 }
+
+//////////////////////////////////////////////////
+template<>
+IGNITION_GAZEBO_VISIBLE
+msgs::Plugin ignition::gazebo::convert(const sdf::Element &_in)
+{
+  msgs::Plugin result;
+
+  if (_in.GetName() != "plugin")
+  {
+    ignerr << "Tried to convert SDF [" << _in.GetName()
+           << "] into [plugin]" << std::endl;
+    return result;
+  }
+
+  result.set_name(_in.Get<std::string>("name"));
+  result.set_filename(_in.Get<std::string>("filename"));
+
+  std::stringstream ss;
+  for (auto innerElem = _in.GetFirstElement(); innerElem;
+      innerElem = innerElem->GetNextElement(""))
+  {
+    ss << innerElem->ToString("");
+  }
+  result.set_innerxml(ss.str());
+
+  return result;
+}
+
+//////////////////////////////////////////////////
+template<>
+IGNITION_GAZEBO_VISIBLE
+msgs::Plugin ignition::gazebo::convert(const sdf::Plugin &_in)
+{
+  msgs::Plugin result;
+
+  result.set_name(_in.Name());
+  result.set_filename(_in.Filename());
+
+  std::stringstream ss;
+  for (auto content : _in.Contents())
+  {
+    ss << content->ToString("");
+  }
+  result.set_innerxml(ss.str());
+
+  return result;
+}

src/Conversions_TEST.cc

@@ -24,6 +24,7 @@
 #include <sdf/Box.hh>
 #include <sdf/Capsule.hh>
 #include <sdf/Cylinder.hh>
+#include <sdf/Element.hh>
 #include <sdf/Ellipsoid.hh>
 #include <sdf/Gui.hh>
 #include <sdf/Heightmap.hh>
@@ -1060,3 +1061,51 @@ TEST(Conversions, ParticleEmitter)
   EXPECT_EQ(emitter2.RawPose(), emitter.RawPose());
   EXPECT_FLOAT_EQ(emitter2.ScatterRatio(), emitter.ScatterRatio());
 }
+
+/////////////////////////////////////////////////
+TEST(Conversions, PluginElement)
+{
+  sdf::Root root;
+  root.LoadSdfString("<?xml version='1.0'?><sdf version='1.6'>"
+      "<world name='default'>"
+      "  <plugin filename='plum' name='peach'>"
+      "    <avocado>0.5</avocado>"
+      "  </plugin>"
+      "</world></sdf>");
+
+  auto world = root.WorldByIndex(0);
+  ASSERT_NE(nullptr, world);
+
+  auto worldElem = world->Element();
+  ASSERT_NE(nullptr, worldElem);
+
+  auto pluginElem = worldElem->GetElement("plugin");
+  ASSERT_NE(nullptr, pluginElem);
+
+  auto pluginMsg = convert<msgs::Plugin>(*(pluginElem.get()));
+  EXPECT_EQ("plum", pluginMsg.filename());
+  EXPECT_EQ("peach", pluginMsg.name());
+
+  EXPECT_NE(pluginMsg.innerxml().find("<avocado>0.5</avocado>"),
+      std::string::npos);
+}
+
+/////////////////////////////////////////////////
+TEST(Conversions, Plugin)
+{
+  sdf::Plugin pluginSdf;
+  pluginSdf.SetName("peach");
+  pluginSdf.SetFilename("plum");
+
+  auto content = std::make_shared<sdf::Element>();
+  content->SetName("avocado");
+  content->AddValue("double", "0.5", false, "");
+  pluginSdf.InsertContent(content);
+
+  auto pluginMsg = convert<msgs::Plugin>(pluginSdf);
+  EXPECT_EQ("plum", pluginMsg.filename());
+  EXPECT_EQ("peach", pluginMsg.name());
+
+  EXPECT_NE(pluginMsg.innerxml().find("<avocado>0.5</avocado>"),
+      std::string::npos) << pluginMsg.innerxml();
+}

src/SystemManager.cc

@@ -15,6 +15,8 @@
  *
 */
 
+#include "ignition/gazebo/components/SystemPluginInfo.hh"
+#include "ignition/gazebo/Conversions.hh"
 #include "SystemManager.hh"
 
 using namespace ignition;
@@ -120,6 +122,29 @@ void SystemManager::AddSystemImpl(
       SystemInternal _system,
       std::shared_ptr<const sdf::Element> _sdf)
 {
+  // Add component
+  if (this->entityCompMgr && kNullEntity != _system.parentEntity)
+  {
+    msgs::Plugin_V systemInfoMsg;
+    auto systemInfoComp =
+        this->entityCompMgr->Component<components::SystemPluginInfo>(
+        _system.parentEntity);
+    if (systemInfoComp)
+    {
+      systemInfoMsg = systemInfoComp->Data();
+    }
+    if (_sdf)
+    {
+      auto pluginMsg = systemInfoMsg.add_plugins();
+      pluginMsg->CopyFrom(convert<msgs::Plugin>(*_sdf.get()));
+    }
+
+    this->entityCompMgr->SetComponentData<components::SystemPluginInfo>(
+        _system.parentEntity, systemInfoMsg);
+    this->entityCompMgr->SetChanged(_system.parentEntity,
+        components::SystemPluginInfo::typeId);
+  }
+
   // Configure the system, if necessary
   if (_system.configure && this->entityCompMgr && this->eventMgr)
   {
@@ -160,16 +185,15 @@ const std::vector<ISystemPostUpdate *>& SystemManager::SystemsPostUpdate()
 //////////////////////////////////////////////////
 std::vector<SystemInternal> SystemManager::TotalByEntity(Entity _entity)
 {
+  auto checkEntity = [&](const SystemInternal &_system)
+      {
+        return _system.parentEntity == _entity;
+      };
+
   std::vector<SystemInternal> result;
-  for (auto system : this->systems)
-  {
-    if (system.parentEntity == _entity)
-      result.push_back(system);
-  }
-  for (auto system : this->pendingSystems)
-  {
-    if (system.parentEntity == _entity)
-      result.push_back(system);
-  }
+  std::copy_if(this->systems.begin(), this->systems.end(),
+      std::back_inserter(result), checkEntity);
+  std::copy_if(this->pendingSystems.begin(), this->pendingSystems.end(),
+      std::back_inserter(result), checkEntity);
   return result;
 }

src/SystemManager.hh

@@ -112,13 +112,20 @@ namespace ignition
       /// \return Vector of systems.
       public: std::vector<SystemInternal> TotalByEntity(Entity _entity);
 
+      /// \brief Implementation for AddSystem functions that takes an SDF
+      /// element. This calls the AddSystemImpl that accepts an SDF Plugin.
+      /// \param[in] _system Generic representation of a system.
+      /// \param[in] _sdf SDF element.
+      private: void AddSystemImpl(SystemInternal _system,
+                                  std::shared_ptr<const sdf::Element> _sdf);
+
       /// \brief Implementation for AddSystem functions. This only adds systems
       /// to a queue, the actual addition is performed by `AddSystemToRunner` at
       /// the appropriate time.
       /// \param[in] _system Generic representation of a system.
       /// \param[in] _sdf SDF received from AddSystem.
       private: void AddSystemImpl(SystemInternal _system,
-                                  std::shared_ptr<const sdf::Element> _sdf);
+                                  const sdf::Plugin &_sdf);
 
       /// \brief All the systems.
       private: std::vector<SystemInternal> systems;

src/SystemManager_TEST.cc

@@ -21,6 +21,7 @@
 #include "ignition/gazebo/System.hh"
 #include "ignition/gazebo/SystemLoader.hh"
 #include "ignition/gazebo/Types.hh"
+#include "ignition/gazebo/components/SystemPluginInfo.hh"
 #include "ignition/gazebo/test_config.hh"  // NOLINT(build/include)
 
 #include "SystemManager.hh"
@@ -202,3 +203,60 @@ TEST(SystemManager, AddSystemEcm)
   EXPECT_EQ(1u, systemMgr.SystemsPostUpdate().size());
 }
 
+/////////////////////////////////////////////////
+TEST(SystemManager, AddSystemWithInfo)
+{
+  auto loader = std::make_shared<SystemLoader>();
+
+  EntityComponentManager ecm;
+  auto entity = ecm.CreateEntity();
+  EXPECT_NE(kNullEntity, entity);
+
+  auto eventManager = EventManager();
+
+  SystemManager systemMgr(loader, &ecm, &eventManager);
+
+  // No element, no SystemPluginInfo component
+  auto configSystem = std::make_shared<SystemWithConfigure>();
+  systemMgr.AddSystem(configSystem, entity, nullptr);
+
+  // Element becomes SystemPluginInfo component
+  auto pluginElem = std::make_shared<sdf::Element>();
+  sdf::initFile("plugin.sdf", pluginElem);
+  sdf::readString("<?xml version='1.0'?><sdf version='1.6'>"
+      "  <plugin filename='plum' name='peach'>"
+      "    <avocado>0.5</avocado>"
+      "  </plugin>"
+      "</sdf>", pluginElem);
+
+  auto updateSystem = std::make_shared<SystemWithUpdates>();
+  systemMgr.AddSystem(updateSystem, entity, pluginElem);
+
+  int entityCount{0};
+  ecm.Each<components::SystemPluginInfo>(
+      [&](const Entity &_entity,
+          const components::SystemPluginInfo *_systemInfoComp) -> bool
+      {
+        EXPECT_EQ(entity, _entity);
+
+        EXPECT_NE(nullptr, _systemInfoComp);
+        if (nullptr == _systemInfoComp)
+          return true;
+
+        auto pluginsMsg = _systemInfoComp->Data();
+        EXPECT_EQ(1, pluginsMsg.plugins().size());
+        if (1u != pluginsMsg.plugins().size())
+          return true;
+
+        auto pluginMsg = pluginsMsg.plugins(0);
+        EXPECT_EQ("plum", pluginMsg.filename());
+        EXPECT_EQ("peach", pluginMsg.name());
+        EXPECT_NE(pluginMsg.innerxml().find("<avocado>0.5</avocado>"),
+            std::string::npos);
+
+        entityCount++;
+        return true;
+      });
+  EXPECT_EQ(1, entityCount);
+}
+