System inspector GUI widget (#1404)

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

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

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);
+}
+