support both wrapped and unwrapped elements

gazebosim · Feb 7, 2018 · 3c47f4d · 3c47f4d
1 parent 35cc01c
commit 3c47f4d
Show file tree

Hide file tree

Showing 4 changed files with 80 additions and 14 deletions.
diff --git a/gazebo/physics/World.cc b/gazebo/physics/World.cc
@@ -2206,12 +2206,8 @@ void World::SetState(const WorldState &_state)
       {
         boost::mutex::scoped_lock lock(this->dataPtr->factoryDeleteMutex);
 
-        // FIXME: We need to publish a light factory message to update the client,
-        // see issue #2288
         LightPtr light = this->LoadLight(elem, this->dataPtr->rootElement);
         light->Init();
-        // msgs::Light lightMsg = msgs::LightFromSDF(elem);
-        // this->dataPtr->lightFactoryPub->Publish(lightMsg);
       }
       catch(...)
       {

diff --git a/gazebo/physics/WorldState.cc b/gazebo/physics/WorldState.cc
@@ -355,7 +355,46 @@ const std::vector<std::string> &WorldState::Insertions() const
 /////////////////////////////////////////////////
 void WorldState::SetInsertions(const std::vector<std::string> &_insertions)
 {
-  this->insertions = _insertions;
+  sdf::SDFPtr root(new sdf::SDF);
+  sdf::initFile("root.sdf", root);
+
+  // Unwrap insertions from <sdf>
+  for (const auto &insertion : _insertions)
+  {
+    root->Root()->ClearElements();
+    // <sdf>
+    if (sdf::readString(insertion, root))
+    {
+      // <model>
+      if (root->Root()->HasElement("model"))
+      {
+        this->insertions.push_back(
+            root->Root()->GetElement("model")->ToString(""));
+      }
+      // <light>
+      else if (root->Root()->HasElement("light"))
+      {
+        this->insertions.push_back(
+            root->Root()->GetElement("light")->ToString(""));
+      }
+      // <actor>
+      else if (root->Root()->HasElement("actor"))
+      {
+        this->insertions.push_back(
+            root->Root()->GetElement("actor")->ToString(""));
+      }
+      else
+      {
+        gzwarn << "Unsupported insertion [" << insertion << "]" << std::endl;
+        continue;
+      }
+    }
+    // Otherwise copy as-is without validating
+    else
+    {
+      this->insertions.push_back(insertion);
+    }
+  }
 }
 
 /////////////////////////////////////////////////

diff --git a/gazebo/physics/WorldState.hh b/gazebo/physics/WorldState.hh
@@ -140,13 +140,15 @@ namespace gazebo
 
       /// \brief Get the vector of SDF insertions.
       /// \return A vector of SDF blocks. Each block contains the SDF of the
-      /// model to be spawned in the simulation.
+      /// model to be spawned in the simulation, without the <sdf> tag wrapping
+      /// it.
       public: const std::vector<std::string> &Insertions() const;
 
       /// \brief Set a new vector of SDF insertions.
       /// \param[in] _insertions Vector containing SDF blocks. Each block should
-      /// contain the SDF of the new models spawned in the current simulation
-      /// frame.
+      /// contain the SDF of the new model, light or actor spawned in the
+      /// current simulation frame. The block could either come wrapped in an
+      /// <sdf> tag or not.
       public: void SetInsertions(const std::vector<std::string> &_insertions);
 
       /// \brief Get the vector of SDF deletions.

diff --git a/test/integration/physics.cc b/test/integration/physics.cc
@@ -730,14 +730,41 @@ TEST_F(PhysicsTest, StateInsertion)
               << "</link>"
               << "</model>";
 
+  std::stringstream newModelSDFStr;
+  newModelSDFStr << "<sdf version='" << SDF_VERSION << "'>"
+                 << "<model name ='" << newModelName << "_SDF'>"
+                 << "<link name ='link'>"
+                 << "  <collision name ='collision'>"
+                 << "    <geometry>"
+                 << "      <box><size>2 2 2</size></box>"
+                 << "    </geometry>"
+                 << "  </collision>"
+                 << "  <visual name ='visual'>"
+                 << "    <geometry>"
+                 << "      <box><size>2 2 2</size></box>"
+                 << "    </geometry>"
+                 << "  </visual>"
+                 << "</link>"
+                 << "</model>"
+                 << "</sdf>";
+
   std::stringstream newLightStr;
   newLightStr << "<light name ='" << newLightName << "' type='spot'>"
               << "<diffuse>0 1 0 1</diffuse>"
               << "</light>";
 
+  std::stringstream newLightSDFStr;
+  newLightSDFStr << "<sdf version='" << SDF_VERSION << "'>"
+                 << "<light name ='" << newLightName << "_SDF' type='point'>"
+                 << "<diffuse>0 1 0 1</diffuse>"
+                 << "</light>"
+                 << "</sdf>";
+
   std::vector<std::string> insertions;
   insertions.push_back(newModelStr.str());
+  insertions.push_back(newModelSDFStr.str());
   insertions.push_back(newLightStr.str());
+  insertions.push_back(newLightSDFStr.str());
 
   worldState.SetInsertions(insertions);
 
@@ -748,15 +775,17 @@ TEST_F(PhysicsTest, StateInsertion)
   world->Step(1);
 
   // Check entities were inserted
-  EXPECT_EQ(2u, world->GetModelCount());
-  EXPECT_EQ(2u, world->LightCount());
-  EXPECT_FALSE(world->GetModel(newModelName) == NULL);
-  EXPECT_FALSE(world->Light(newLightName) == NULL);
+  EXPECT_EQ(3u, world->GetModelCount());
+  EXPECT_EQ(3u, world->LightCount());
+  EXPECT_NE(nullptr, world->GetModel(newModelName));
+  EXPECT_NE(nullptr, world->GetModel(newModelName + "_SDF"));
+  EXPECT_NE(nullptr, world->Light(newLightName));
+  EXPECT_NE(nullptr, world->Light(newLightName + "_SDF"));
 
   // New world state
   physics::WorldState newWorldState(world);
-  EXPECT_EQ(2u, newWorldState.GetModelStateCount());
-  EXPECT_EQ(2u, newWorldState.LightStateCount());
+  EXPECT_EQ(3u, newWorldState.GetModelStateCount());
+  EXPECT_EQ(3u, newWorldState.LightStateCount());
 }
 
 //////////////////////////////////////////////////