Multi spawn

CMakeLists.txt

@@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.5.1 FATAL_ERROR)
 #============================================================================
 # Initialize the project
 #============================================================================
-project(ignition-launch1 VERSION 1.4.2)
+project(ignition-launch1 VERSION 1.5.0)
 
 #============================================================================
 # Find ignition-cmake
@@ -77,7 +77,7 @@ ign_find_package (Qt5
 
 #--------------------------------------
 # Find ignition-gazebo
-ign_find_package(ignition-gazebo2 REQUIRED VERSION 2.17
+ign_find_package(ignition-gazebo2 REQUIRED VERSION 2.18
   PRIVATE
     COMPONENTS gui
 )

Changelog.md

@@ -1,5 +1,10 @@
 ## Ignition Launch 1.x
 
+### Ignition Launch 1.5.0 (2020-05-20)
+
+1. Added support for spawning multiple entities in the same simulation step.
+    * [Pull Request 30](https://github.com/ignitionrobotics/ign-launch/pull/30)
+
 ### Ignition Launch 1.4.2 (2020-05-18)
 
 1.  Use the new GUI API of ign-gazebo. This adds support for saving worlds to SDFormat from the GUI.

examples/factory.ign

@@ -2,27 +2,29 @@
 <ignition version='1.0'>
   <plugin name="ignition::launch::GazeboFactory"
     filename="libignition-launch-gazebo-factory.so">
-    <name>x2</name>
-    <allow_renaming>true</allow_renaming>
-    <pose>1 2 0.5 0 0 0</pose>
+    <spawn>
+      <name>x2</name>
+      <allow_renaming>true</allow_renaming>
+      <pose>1 2 0.5 0 0 0</pose>
 
-    <sdf version='1.6'>
-      <include>
-        <uri>https://fuel.ignitionrobotics.org/1.0/openrobotics/models/X2 UGV/1</uri>
-        <plugin filename="libignition-gazebo-diff-drive-system.so"
-                name="ignition::gazebo::systems::DiffDrive">
-          <left_joint>front_left_wheel_joint</left_joint>
-          <left_joint>rear_left_wheel_joint</left_joint>
-          <right_joint>front_right_wheel_joint</right_joint>
-          <right_joint>rear_right_wheel_joint</right_joint>
-          <wheel_separation>0.33559</wheel_separation>
-          <wheel_radius>0.098</wheel_radius>
-        </plugin>
+      <sdf version='1.6'>
+        <include>
+          <uri>https://fuel.ignitionrobotics.org/1.0/openrobotics/models/X2 UGV/1</uri>
+          <plugin filename="libignition-gazebo-diff-drive-system.so"
+                  name="ignition::gazebo::systems::DiffDrive">
+            <left_joint>front_left_wheel_joint</left_joint>
+            <left_joint>rear_left_wheel_joint</left_joint>
+            <right_joint>front_right_wheel_joint</right_joint>
+            <right_joint>rear_right_wheel_joint</right_joint>
+            <wheel_separation>0.33559</wheel_separation>
+            <wheel_radius>0.098</wheel_radius>
+          </plugin>
 
-        <!-- Publish robot state information -->
-        <plugin filename="libignition-gazebo-state-publisher-system.so"
-                name="ignition::gazebo::systems::StatePublisher"></plugin>
-      </include>
-    </sdf>
+          <!-- Publish robot state information -->
+          <plugin filename="libignition-gazebo-state-publisher-system.so"
+                  name="ignition::gazebo::systems::StatePublisher"></plugin>
+        </include>
+      </sdf>
+    </spawn>
   </plugin>
 </ignition>

examples/multi_factory.ign

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<ignition version='1.0'>
+  <plugin name="ignition::launch::GazeboFactory"
+    filename="libignition-launch-gazebo-factory.so">
+    <spawn>
+      <name>x2</name>
+      <allow_renaming>true</allow_renaming>
+      <pose>1 2 0.5 0 0 0</pose>
+
+      <sdf version='1.6'>
+        <include>
+          <uri>https://fuel.ignitionrobotics.org/1.0/openrobotics/models/X3 UAV Config 1</uri>
+        </include>
+      </sdf>
+    </spawn>
+    <spawn>
+      <name>x1</name>
+      <allow_renaming>true</allow_renaming>
+      <pose>4 2 0.5 0 0 0</pose>
+
+      <sdf version='1.6'>
+        <include>
+          <uri>https://fuel.ignitionrobotics.org/1.0/openrobotics/models/X1 Config 1</uri>
+        </include>
+      </sdf>
+    </spawn>
+  </plugin>
+</ignition>

plugins/gazebo_factory/GazeboFactory.cc

@@ -32,52 +32,16 @@ GazeboFactory::GazeboFactory()
 }
 
 /////////////////////////////////////////////////
-bool GazeboFactory::Load(const tinyxml2::XMLElement *_elem)
+void GazeboFactory::ProcessSpawn(const tinyxml2::XMLElement *_elem)
 {
   const tinyxml2::XMLElement *elem;
 
-  msgs::EntityFactory req;
-
-  // Set the sdf field, if an SDF string has been specified.
-  tinyxml2::XMLPrinter printer;
-  elem = _elem->FirstChildElement("sdf");
-  if (elem)
-  {
-    elem->Accept(&printer);
-    req.set_sdf(printer.CStr());
-  }
-
-  // Get <name>
-  elem = _elem->FirstChildElement("name");
-  if (elem)
-    req.set_name(elem->GetText());
-
-  // Get <is_performer>, by default we assume a spawned model is a performer
-  bool isPerformer = true;
-  elem = _elem->FirstChildElement("is_performer");
-  if (elem)
-  {
-    std::string str = elem->GetText();
-    isPerformer = str == "1" || common::lowercase(str) == "true";
-  }
-
-  // Get <allow_renaming>
-  elem = _elem->FirstChildElement("allow_renaming");
-  if (elem)
-  {
-    std::string str = elem->GetText();
-    req.set_allow_renaming(str == "1" || common::lowercase(str) == "true");
-  }
-
-  // Get the pose
-  elem = _elem->FirstChildElement("pose");
-  if (elem)
+  // Get the sdf field, otherwise return.
+  const tinyxml2::XMLElement *sdfElem = _elem->FirstChildElement("sdf");
+  if (!sdfElem)
   {
-    ignition::math::Pose3d pose;
-    std::stringstream stream;
-    stream << elem->GetText();
-    stream >> pose;
-    msgs::Set(req.mutable_pose(), pose);
+    // Return early if there is no <sdf> element.
+    return;
   }
 
   // Get a user-defined world name
@@ -112,7 +76,7 @@ bool GazeboFactory::Load(const tinyxml2::XMLElement *_elem)
     {
       ignerr << "No simulation worlds were found. Unable to run the factory. "
         << "Is Gazebo running?\n";
-      return false;
+      return;
     }
 
     // Warning if multiple worlds were found.
@@ -125,56 +89,125 @@ bool GazeboFactory::Load(const tinyxml2::XMLElement *_elem)
     worldName = *worlds.begin();
   }
 
-  unsigned int timeout = 2000;
-  msgs::Boolean rep;
-  bool result;
+  msgs::EntityFactory *req = this->worldFactoryMsgs[worldName].add_data();
 
-  std::string topic = "/world/";
-  topic += worldName + "/create";
+  tinyxml2::XMLPrinter printer;
+  sdfElem->Accept(&printer);
+  req->set_sdf(printer.CStr());
 
-  // Send the request.
-  bool executed = this->node.Request(topic, req, timeout, rep, result);
+  // Get <name>
+  elem = _elem->FirstChildElement("name");
+  if (elem)
+    req->set_name(elem->GetText());
 
-  if (executed && result && rep.data())
+  // Get <is_performer>, by default we assume a spawned model is a performer
+  bool isPerformer = true;
+  elem = _elem->FirstChildElement("is_performer");
+  if (elem)
   {
-    igndbg << "Factory service call succeeded.\n";
-    if (isPerformer)
-    {
-      IGN_SLEEP_S(2);
-      topic = std::string("/world/") + worldName + "/level/set_performer";
-      msgs::StringMsg performerReq;
-      performerReq.set_data(req.name());
-      // \todo(anyone) Setting the size to 2,2,2 is a hack. Gazebo should
-      // calculate the bounding box based on the model information.
-      executed = this->node.Request(topic, performerReq, timeout, rep, result);
-
-      // msgs::Performer performerReq;
-      // performerReq.set_name(req.name());
-      // // \todo(anyone) Setting the size to 2,2,2 is a hack. Gazebo should
-      // // calculate the bounding box based on the model information.
-      //  msgs::Set(performerReq.mutable_geometry()->mutable_box()->mutable_size(), math::Vector3d(2, 2, 2));
-      // executed = this->node.Request(topic, performerReq, timeout, rep, result);
-    }
+    std::string str = elem->GetText();
+    isPerformer = str == "1" || common::lowercase(str) == "true";
   }
-  else
+  if (isPerformer)
+    this->worldPerformers[worldName].push_back(req->name());
+
+  // Get <allow_renaming>
+  elem = _elem->FirstChildElement("allow_renaming");
+  if (elem)
+  {
+    std::string str = elem->GetText();
+    req->set_allow_renaming(str == "1" || common::lowercase(str) == "true");
+  }
+
+  // Get the pose
+  elem = _elem->FirstChildElement("pose");
+  if (elem)
+  {
+    ignition::math::Pose3d pose;
+    std::stringstream stream;
+    stream << elem->GetText();
+    stream >> pose;
+    msgs::Set(req->mutable_pose(), pose);
+  }
+}
+
+/////////////////////////////////////////////////
+bool GazeboFactory::Load(const tinyxml2::XMLElement *_elem)
+{
+  const tinyxml2::XMLElement *elem;
+
+  // Process each <spawn> ... </spawn> element.
+  elem = _elem->FirstChildElement("spawn");
+  while (elem)
   {
-    if (executed)
+    this->ProcessSpawn(elem);
+    elem = elem->NextSiblingElement("spawn");
+  }
+
+  // Process the case where there is no <spawn> .. </spawn> entity, and only
+  // one "spawn" request is embedded in the plugin.
+  this->ProcessSpawn(_elem);
+
+  for (const auto &msg : this->worldFactoryMsgs)
+  {
+    unsigned int timeout = 2000;
+    msgs::Boolean rep;
+    bool result;
+
+    std::string topic = "/world/";
+    topic += msg.first + "/create_multiple";
+
+    // Send the request.
+    bool executed = this->node.Request(topic, msg.second, timeout, rep, result);
+
+    if (executed && result && rep.data())
     {
-      if (result && !rep.data())
-      {
-        ignerr << "Factory service call completed, but returned a false value."
-          << "You may have an invalid request. Check the configuration.\n";
-      }
-      else
+      igndbg << "Factory service call succeeded.\n";
+      if (!this->worldPerformers[msg.first].empty())
       {
-        ignerr << "Factory service call failed.\n";
+        IGN_SLEEP_S(2);
+        topic = std::string("/world/") + msg.first + "/level/set_performer";
+
+        for (const auto &perf : this->worldPerformers[msg.first])
+        {
+          msgs::StringMsg performerReq;
+          performerReq.set_data(perf);
+          // \todo(anyone) Setting the size to 2,2,2 is a hack. Gazebo should
+          // calculate the bounding box based on the model information.
+          executed = this->node.Request(
+              topic, performerReq, timeout, rep, result);
+
+          // msgs::Performer performerReq;
+          // performerReq.set_name(req.name());
+          // // \todo(anyone) Setting the size to 2,2,2 is a hack. Gazebo should
+          // // calculate the bounding box based on the model information.
+          //  msgs::Set(performerReq.mutable_geometry()->mutable_box()->mutable_size(), math::Vector3d(2, 2, 2));
+          // executed = this->node.Request(topic, performerReq, timeout, rep, result);
+        }
       }
     }
     else
     {
-      ignerr << "Factory service call timed out.\n";
+      if (executed)
+      {
+        if (result && !rep.data())
+        {
+          ignerr << "Factory service call completed, but returned a false value."
+            << "You may have an invalid request. Check the configuration.\n";
+        }
+        else
+        {
+          ignerr << "Factory service call failed.\n";
+        }
+      }
+      else
+      {
+        ignerr << "Factory service call timed out.\n";
+      }
     }
   }
 
+
+
   return false;
 }