use new LoadPlugin function

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

Author: iche033

src/SimulationRunner.cc

@@ -237,12 +237,6 @@ SimulationRunner::SimulationRunner(const sdf::World *_world,
 
   ignmsg << "Serving world SDF generation service on [" << opts.NameSpace()
          << "/" << genWorldSdfService << "]" << std::endl;
-
-  std::string entitySystemService{"entity/system/add"};
-  this->node->Advertise(entitySystemService,
-      &SimulationRunner::EntitySystemAddService, this);
-  ignmsg << "Serving entity system service on [" << opts.NameSpace()
-         << "/" << entitySystemService << "]" << std::endl;
 }
 
 //////////////////////////////////////////////////
@@ -814,7 +808,7 @@ void SimulationRunner::Step(const UpdateInfo &_info)
   this->ProcessRecreateEntitiesRemove();
 
   // handle systems that need to be added
-  this->ProcessEntitySystems();
+  this->systemMgr->ProcessPendingEntitySystems();
 
   // Update all the systems.
   this->UpdateSystems();
@@ -1446,53 +1440,6 @@ bool SimulationRunner::GenerateWorldSdf(const msgs::SdfGeneratorConfig &_req,
   return false;
 }
 
-//////////////////////////////////////////////////
-bool SimulationRunner::EntitySystemAddService(const msgs::EntityPlugin_V &_req,
-                                              msgs::Boolean &_res)
-{
-  std::lock_guard<std::mutex> lock(this->systemsMsgMutex);
-  this->systemsToAdd.push_back(_req);
-  _res.set_data(true);
-  return true;
-}
-
-//////////////////////////////////////////////////
-void SimulationRunner::ProcessEntitySystems()
-{
-  std::lock_guard<std::mutex> lock(this->systemsMsgMutex);
-  for (auto &req : this->systemsToAdd)
-  {
-    Entity entity = req.entity().id();
-
-    for (auto &pluginMsg : req.plugins())
-    {
-      std::string fname = pluginMsg.filename();
-      std::string name = pluginMsg.name();
-      std::string innerxml = pluginMsg.innerxml();
-
-      // Use the SDF parser to read all the inner xml.
-      sdf::ElementPtr pluginSDF(new sdf::Element);
-      sdf::initFile("plugin.sdf", pluginSDF);
-      std::stringstream tmp;
-      tmp << "<sdf version='" << SDF_VERSION << "'>";
-      tmp << "<plugin name='" << name << "' filename='" << fname << "'>";
-      tmp << innerxml;
-      tmp << "</plugin></sdf>";
-
-      if (sdf::readString(tmp.str(), pluginSDF))
-      {
-        this->LoadPlugin(entity, fname, name, pluginSDF);
-      }
-      else
-      {
-        ignerr << "Error reading Plugin SDF. Unable to parse Innerxml:\n"
-               << innerxml << std::endl;
-      }
-    }
-  }
-  this->systemsToAdd.clear();
-}
-
 
 //////////////////////////////////////////////////
 void SimulationRunner::SetFuelUriMap(

src/SimulationRunner.hh

@@ -17,7 +17,6 @@
 #ifndef IGNITION_GAZEBO_SIMULATIONRUNNER_HH_
 #define IGNITION_GAZEBO_SIMULATIONRUNNER_HH_
 
-#include <ignition/msgs/entity_plugin_v.pb.h>
 #include <ignition/msgs/gui.pb.h>
 #include <ignition/msgs/log_playback_control.pb.h>
 #include <ignition/msgs/sdf_generator_config.pb.h>
@@ -317,15 +316,6 @@ namespace ignition
       /// \return True if successful.
       private: bool GuiInfoService(ignition::msgs::GUI &_res);
 
-      /// \brief Callback for entity add system service.
-      /// \param[in] _req Request message containing the entity id and plugins
-      /// to add to that entity
-      /// \param[out] _res Response containing a boolean value indicating if
-      /// service request is received or not
-      /// \return True if request received.
-      private: bool EntitySystemAddService(const msgs::EntityPlugin_V &_req,
-                                           msgs::Boolean &_res);
-
       /// \brief Calculate real time factor and populate currentInfo.
       private: void UpdateCurrentInfo();
 
@@ -387,9 +377,6 @@ namespace ignition
       /// See the newWorldControlState variable below.
       private: void ProcessNewWorldControlState();
 
-      /// \brief Process system messages and add systems to entities
-      private: void ProcessEntitySystems();
-
       /// \brief This is used to indicate that a stop event has been received.
       private: std::atomic<bool> stopReceived{false};
 
@@ -540,12 +527,6 @@ namespace ignition
       /// at the appropriate time.
       private: std::unique_ptr<msgs::WorldControlState> newWorldControlState;
 
-      /// \brief A list of entity systems to add
-      private: std::vector<msgs::EntityPlugin_V> systemsToAdd;
-
-      /// \brief Mutex to protect systemsToAdd list
-      private: std::mutex systemsMsgMutex;
-
       friend class LevelManager;
     };
     }

src/SystemManager.cc

@@ -30,6 +30,12 @@ SystemManager::SystemManager(const SystemLoaderPtr &_systemLoader,
     entityCompMgr(_entityCompMgr),
     eventMgr(_eventMgr)
 {
+  this->node = std::make_unique<transport::Node>();
+  std::string entitySystemService{"/entity/system/add"};
+  this->node->Advertise(entitySystemService,
+      &SystemManager::EntitySystemAddService, this);
+  ignmsg << "Serving entity system service on ["
+         << "/" << entitySystemService << "]" << std::endl;
 }
 
 //////////////////////////////////////////////////
@@ -195,3 +201,35 @@ std::vector<SystemInternal> SystemManager::TotalByEntity(Entity _entity)
       std::back_inserter(result), checkEntity);
   return result;
 }
+
+//////////////////////////////////////////////////
+bool SystemManager::EntitySystemAddService(const msgs::EntityPlugin_V &_req,
+                                           msgs::Boolean &_res)
+{
+  std::lock_guard<std::mutex> lock(this->systemsMsgMutex);
+  this->systemsToAdd.push_back(_req);
+  _res.set_data(true);
+  return true;
+}
+
+//////////////////////////////////////////////////
+void SystemManager::ProcessPendingEntitySystems()
+{
+  std::lock_guard<std::mutex> lock(this->systemsMsgMutex);
+  for (auto &req : this->systemsToAdd)
+  {
+    Entity entity = req.entity().id();
+
+    for (auto &pluginMsg : req.plugins())
+    {
+      std::string fname = pluginMsg.filename();
+      std::string name = pluginMsg.name();
+      std::string innerxml = pluginMsg.innerxml();
+      sdf::Plugin pluginSDF(fname, name, innerxml);
+      this->LoadPlugin(entity, pluginSDF);
+    }
+  }
+  this->systemsToAdd.clear();
+}
+
+

src/SystemManager.hh

@@ -17,11 +17,14 @@
 #ifndef IGNITION_GAZEBO_SYSTEMMANAGER_HH_
 #define IGNITION_GAZEBO_SYSTEMMANAGER_HH_
 
+#include <ignition/msgs/entity_plugin_v.pb.h>
+
 #include <memory>
 #include <string>
 #include <vector>
 
 #include <sdf/Plugin.hh>
+#include <ignition/transport/Node.hh>
 
 #include "ignition/gazebo/config.hh"
 #include "ignition/gazebo/EntityComponentManager.hh"
@@ -110,6 +113,9 @@ namespace ignition
       /// \return Vector of systems.
       public: std::vector<SystemInternal> TotalByEntity(Entity _entity);
 
+      /// \brief Process system messages and add systems to entities
+      public: void ProcessPendingEntitySystems();
+
       /// \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.
@@ -125,6 +131,15 @@ namespace ignition
       private: void AddSystemImpl(SystemInternal _system,
                                   const sdf::Plugin &_sdf);
 
+      /// \brief Callback for entity add system service.
+      /// \param[in] _req Request message containing the entity id and plugins
+      /// to add to that entity
+      /// \param[out] _res Response containing a boolean value indicating if
+      /// service request is received or not
+      /// \return True if request received.
+      private: bool EntitySystemAddService(const msgs::EntityPlugin_V &_req,
+                                           msgs::Boolean &_res);
+
       /// \brief All the systems.
       private: std::vector<SystemInternal> systems;
 
@@ -157,6 +172,15 @@ namespace ignition
 
       /// \brief Pointer to associated event manager
       private: EventManager *eventMgr;
+
+      /// \brief A list of entity systems to add
+      private: std::vector<msgs::EntityPlugin_V> systemsToAdd;
+
+      /// \brief Mutex to protect systemsToAdd list
+      private: std::mutex systemsMsgMutex;
+
+      /// \brief Node for communication.
+      private: std::unique_ptr<transport::Node> node{nullptr};
     };
     }
   }  // namespace gazebo

test/integration/entity_system.cc

@@ -128,7 +128,7 @@ class EntitySystemTest : public InternalFixture<::testing::TestWithParam<int>>
     msgs::Boolean res;
     bool result;
     unsigned int timeout = 5000;
-    std::string service{"/world/diff_drive/entity/system/add"};
+    std::string service{"/entity/system/add"};
 
     EXPECT_TRUE(node.Request(service, req, timeout, res, result));
     EXPECT_TRUE(result);