From 650d174fafc1b14e5426a46bd0c01399b2a3f3aa Mon Sep 17 00:00:00 2001
From: Nate Koenig <natekoenig@gmail.com>
Date: Tue, 9 Apr 2019 16:21:16 -0700
Subject: [PATCH] Updating

---
 examples/factory.ign                    |  9 ++++++---
 include/ignition/launch/Plugin.hh       |  4 +++-
 plugins/gazebo_factory/GazeboFactory.cc | 24 ++++++++++++++++++++++--
 plugins/gazebo_factory/GazeboFactory.hh |  2 +-
 plugins/gazebo_gui/GazeboGui.cc         |  6 ++++--
 plugins/gazebo_gui/GazeboGui.hh         |  2 +-
 plugins/gazebo_server/GazeboServer.cc   |  3 ++-
 plugins/gazebo_server/GazeboServer.hh   |  2 +-
 plugins/joy_to_twist/JoyToTwist.cc      |  4 +++-
 plugins/joy_to_twist/JoyToTwist.hh      |  2 +-
 plugins/joystick/Joystick.cc            |  6 ++++--
 plugins/joystick/Joystick.hh            |  2 +-
 src/Manager.cc                          |  8 +++++---
 13 files changed, 54 insertions(+), 20 deletions(-)

diff --git a/examples/factory.ign b/examples/factory.ign
index 7a8b6653..6eb8fbf3 100644
--- a/examples/factory.ign
+++ b/examples/factory.ign
@@ -1,10 +1,13 @@
 <?xml version="1.0"?>
 <ignition version='1.0'>
   <plugin name="ignition::launch::GazeboFactory"
-          filename="libignition-launch0-gazebo-factory.so">
-    <name>x2</name>
+    filename="libignition-launch0-gazebo-factory.so"
+    keepAlive="false">
+
+    <name>x3</name>
+    <allow_renaming>true</allow_renaming>
     <uri>https://fuel.ignitionrobotics.org/1.0/openrobotics/models/X2 UGV/1</uri>
-    <pose>0 2 0.063494 0 0 0</pose>
+    <pose>5 2 0.063494 0 0 0</pose>
 
     <plugin entity_name="x2"
             entity_type="model"
diff --git a/include/ignition/launch/Plugin.hh b/include/ignition/launch/Plugin.hh
index 18730ad8..a3b66e97 100644
--- a/include/ignition/launch/Plugin.hh
+++ b/include/ignition/launch/Plugin.hh
@@ -32,7 +32,9 @@ namespace ignition
     {
       /// \brief Load function that each launch plugin must implement.
       /// \param[in] _elem Pointer to the XML for this plugin.
-      public: virtual void Load(const tinyxml2::XMLElement *_elem) = 0;
+      /// \return True to keep the plugin alive. Return false to have the
+      /// plugin unloaded immediately.
+      public: virtual bool Load(const tinyxml2::XMLElement *_elem) = 0;
     };
 
     /// \brief Pointer to a launch plugin.
diff --git a/plugins/gazebo_factory/GazeboFactory.cc b/plugins/gazebo_factory/GazeboFactory.cc
index d2c75053..6b9e9eb8 100644
--- a/plugins/gazebo_factory/GazeboFactory.cc
+++ b/plugins/gazebo_factory/GazeboFactory.cc
@@ -30,13 +30,13 @@ GazeboFactory::GazeboFactory()
 }
 
 /////////////////////////////////////////////////
-void GazeboFactory::Load(const tinyxml2::XMLElement *_elem)
+bool GazeboFactory::Load(const tinyxml2::XMLElement *_elem)
 {
   const tinyxml2::XMLElement *elem;
 
   msgs::EntityFactory req;
 
-  // Get the world file
+  // Get the uri
   elem = _elem->FirstChildElement("uri");
   if (elem)
   {
@@ -45,6 +45,24 @@ void GazeboFactory::Load(const tinyxml2::XMLElement *_elem)
     req.set_sdf(sdf);
   }
 
+  // Get the name
+  elem = _elem->FirstChildElement("name");
+  if (elem)
+    req.set_name(elem->GetText());
+
+  // Get the pose
+  elem = _elem->FirstChildElement("pose");
+  if (elem)
+  {
+    ignition::math::Pose3d pose;
+    std::stringstream stream;
+    stream << elem->GetText();
+    stream >> pose;
+    std::cout << "Pose[" << pose << "]\n";
+    msgs::Set(req.mutable_pose(), pose);
+  }
+
+
   unsigned int timeout = 2000;
   msgs::Boolean rep;
   bool result;
@@ -74,4 +92,6 @@ void GazeboFactory::Load(const tinyxml2::XMLElement *_elem)
   }
 
   igndbg << "Loaded GazeboFactory plugin.\n";
+
+  return false;
 }
diff --git a/plugins/gazebo_factory/GazeboFactory.hh b/plugins/gazebo_factory/GazeboFactory.hh
index 42f030b3..342aac98 100644
--- a/plugins/gazebo_factory/GazeboFactory.hh
+++ b/plugins/gazebo_factory/GazeboFactory.hh
@@ -53,7 +53,7 @@ namespace ignition
       public: virtual ~GazeboFactory() = default;
 
       // Documentation inherited.
-      public: virtual void Load(
+      public: virtual bool Load(
                   const tinyxml2::XMLElement *_elem) override final;
 
       private: transport::Node node;
diff --git a/plugins/gazebo_gui/GazeboGui.cc b/plugins/gazebo_gui/GazeboGui.cc
index 2bcea47b..c0c78b35 100644
--- a/plugins/gazebo_gui/GazeboGui.cc
+++ b/plugins/gazebo_gui/GazeboGui.cc
@@ -37,7 +37,7 @@ GazeboGui::~GazeboGui()
 }
 
 /////////////////////////////////////////////////
-void GazeboGui::Load(const tinyxml2::XMLElement *_elem)
+bool GazeboGui::Load(const tinyxml2::XMLElement *_elem)
 {
   int argc;
   char **argv = nullptr;
@@ -51,7 +51,7 @@ void GazeboGui::Load(const tinyxml2::XMLElement *_elem)
   if (!app->LoadConfig(configPath))
   {
     ignerr << "Unable to load GazeboGui config file[" << configPath << "]\n";
-    return;
+    return false;
   }
 
   // Customize window
@@ -116,4 +116,6 @@ void GazeboGui::Load(const tinyxml2::XMLElement *_elem)
   // This blocks until the window is closed or we receive a SIGINT
   this->app->exec();
   this->app.reset();
+
+  return false;
 }
diff --git a/plugins/gazebo_gui/GazeboGui.hh b/plugins/gazebo_gui/GazeboGui.hh
index c7538f7a..024a1697 100644
--- a/plugins/gazebo_gui/GazeboGui.hh
+++ b/plugins/gazebo_gui/GazeboGui.hh
@@ -46,7 +46,7 @@ namespace ignition
       public: virtual ~GazeboGui();
 
       // Documentation inherited.
-      public: virtual void Load(
+      public: virtual bool Load(
                   const tinyxml2::XMLElement *_elem) override final;
 
       /// \brief Run the GUI
diff --git a/plugins/gazebo_server/GazeboServer.cc b/plugins/gazebo_server/GazeboServer.cc
index c97bcefc..da535aae 100644
--- a/plugins/gazebo_server/GazeboServer.cc
+++ b/plugins/gazebo_server/GazeboServer.cc
@@ -57,7 +57,7 @@ GazeboServer::GazeboServer()
 }
 
 /////////////////////////////////////////////////
-void GazeboServer::Load(const tinyxml2::XMLElement *_elem)
+bool GazeboServer::Load(const tinyxml2::XMLElement *_elem)
 {
   gazebo::ServerConfig serverConfig;
   const tinyxml2::XMLElement *elem;
@@ -144,4 +144,5 @@ void GazeboServer::Load(const tinyxml2::XMLElement *_elem)
   this->server->Run(false, 0, !run);
 
   igndbg << "Loaded GazeboServer plugin.\n";
+  return true;
 }
diff --git a/plugins/gazebo_server/GazeboServer.hh b/plugins/gazebo_server/GazeboServer.hh
index ec1ee412..aa1d40f6 100644
--- a/plugins/gazebo_server/GazeboServer.hh
+++ b/plugins/gazebo_server/GazeboServer.hh
@@ -53,7 +53,7 @@ namespace ignition
       public: virtual ~GazeboServer() = default;
 
       // Documentation inherited.
-      public: virtual void Load(
+      public: virtual bool Load(
                   const tinyxml2::XMLElement *_elem) override final;
 
       /// \brief Private data pointer
diff --git a/plugins/joy_to_twist/JoyToTwist.cc b/plugins/joy_to_twist/JoyToTwist.cc
index 7cab65e0..f971a010 100644
--- a/plugins/joy_to_twist/JoyToTwist.cc
+++ b/plugins/joy_to_twist/JoyToTwist.cc
@@ -57,7 +57,7 @@ JoyToTwist::~JoyToTwist()
 }
 
 /////////////////////////////////////////////////
-void JoyToTwist::Load(const tinyxml2::XMLElement *_elem)
+bool JoyToTwist::Load(const tinyxml2::XMLElement *_elem)
 {
   const tinyxml2::XMLElement *elem;
 
@@ -109,6 +109,8 @@ void JoyToTwist::Load(const tinyxml2::XMLElement *_elem)
     << "  output_topic: " << this->outputTopic << std::endl;
   this->running = true;
   this->node.Subscribe(this->inputTopic, &JoyToTwist::OnJoy, this);
+
+  return true;
 }
 
 //////////////////////////////////////////////////
diff --git a/plugins/joy_to_twist/JoyToTwist.hh b/plugins/joy_to_twist/JoyToTwist.hh
index af57d5e7..a3693c2d 100644
--- a/plugins/joy_to_twist/JoyToTwist.hh
+++ b/plugins/joy_to_twist/JoyToTwist.hh
@@ -80,7 +80,7 @@ namespace ignition
       public: virtual ~JoyToTwist();
 
       // Documentation inherited
-      public: virtual void Load(
+      public: virtual bool Load(
                   const tinyxml2::XMLElement *_elem) override final;
 
       /// \brief Run the plugin
diff --git a/plugins/joystick/Joystick.cc b/plugins/joystick/Joystick.cc
index def72f4b..4e1ddd03 100644
--- a/plugins/joystick/Joystick.cc
+++ b/plugins/joystick/Joystick.cc
@@ -47,7 +47,7 @@ Joystick::~Joystick()
 }
 
 /////////////////////////////////////////////////
-void Joystick::Load(const tinyxml2::XMLElement *_elem)
+bool Joystick::Load(const tinyxml2::XMLElement *_elem)
 {
   // Get the name of the joystick device.
   std::string deviceFilename = "/dev/input/js0";
@@ -161,7 +161,7 @@ void Joystick::Load(const tinyxml2::XMLElement *_elem)
   {
     std::cerr << "Unable to open joystick at [" << deviceFilename
           << "]. The joystick will not work.\n";
-    return;
+    return false;
   }
 
   this->unscaledDeadzone = 32767.0f * deadzone;
@@ -179,6 +179,8 @@ void Joystick::Load(const tinyxml2::XMLElement *_elem)
     << "  dead_zone: " << deadzone << std::endl
     << "  rate: " << intervalRate << std::endl
     << "  accumulation_rate: " << accumulationRate  << std::endl;
+
+  return true;
 }
 
 //////////////////////////////////////////////////
diff --git a/plugins/joystick/Joystick.hh b/plugins/joystick/Joystick.hh
index a91f50d6..68f8ad1e 100644
--- a/plugins/joystick/Joystick.hh
+++ b/plugins/joystick/Joystick.hh
@@ -58,7 +58,7 @@ namespace ignition
       public: virtual ~Joystick();
 
       // Documentation inherited
-      public: virtual void Load(
+      public: virtual bool Load(
                   const tinyxml2::XMLElement *_elem) override final;
 
       /// \brief Run the plugin
diff --git a/src/Manager.cc b/src/Manager.cc
index 2e5b8fcd..2d93cfa7 100644
--- a/src/Manager.cc
+++ b/src/Manager.cc
@@ -38,6 +38,7 @@
 #include "Manager.hh"
 
 using namespace ignition::launch;
+using namespace std::chrono_literals;
 
 /// \brief A class to encapsulate an executable (program) to run.
 class Executable
@@ -233,7 +234,8 @@ bool Manager::RunConfig(const std::string &_config)
   while (this->dataPtr->running && (!this->dataPtr->executables.empty() ||
                                     !this->dataPtr->plugins.empty()))
   {
-    this->dataPtr->runCondition.wait(lock);
+    std::cout << "Plugins[" << this->dataPtr->plugins.size() << "]\n";
+    this->dataPtr->runCondition.wait_for(lock, 1000ms);
   }
   this->dataPtr->running = false;
 
@@ -647,8 +649,8 @@ void ManagerPrivate::LoadPlugin(const tinyxml2::XMLElement *_elem)
     << "] File[" << file << "]" << std::endl;
 
   PluginPtr plugin = loader.Instantiate(name);
-  plugin->QueryInterface<Plugin>()->Load(_elem);
-  this->plugins.insert(plugin);
+  if (plugin->QueryInterface<Plugin>()->Load(_elem))
+    this->plugins.insert(plugin);
 }
 
 //////////////////////////////////////////////////