Properly add new models into the scenegraph (#212)

* Properly add new models into the scenegraph Signed-off-by: Nate Koenig <[email protected]> * Changelog and bump patch number Signed-off-by: Nate Koenig <[email protected]> * Added test Signed-off-by: Nate Koenig <[email protected]> Co-authored-by: Nate Koenig <[email protected]>
gazebosim · Jun 18, 2020 · 77406ae · 77406ae
1 parent e42ad43
commit 77406ae
Show file tree

Hide file tree

Showing 4 changed files with 83 additions and 2 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.5.1 FATAL_ERROR)
 #============================================================================
 # Initialize the project
 #============================================================================
-project(ignition-gazebo2 VERSION 2.20.0)
+project(ignition-gazebo2 VERSION 2.20.1)
 
 #============================================================================
 # Find ignition-cmake

diff --git a/Changelog.md b/Changelog.md
@@ -1,6 +1,11 @@
 ## Ignition Gazebo 2.x
 
 
+### Ignition Gazebo 2.20.1 (2020-06-18)
+
+1. Properly add new models into the scenegraph. With this fix, when a model is spawned it will be added into the graph and resulting calls to the `scene/info` service will return a correct `msgs::Scene`.
+    * [Pull Request 212](https://github.com/ignitionrobotics/ign-gazebo/pull/212)
+
 ### Ignition Gazebo 2.20.0 (2020-06-09)
 
 1. Updated battery model to stop battery drain when there is no joint

diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc
@@ -693,13 +693,18 @@ void SceneBroadcasterPrivate::SceneGraphAddEntities(
     std::lock_guard<std::mutex> lock(this->graphMutex);
     for (const auto &[id, vert] : newGraph.Vertices())
     {
+      // Add the vertex only if it's not already in the graph
       if (!this->sceneGraph.VertexFromId(id).Valid())
         this->sceneGraph.AddVertex(vert.get().Name(), vert.get().Data(), id);
     }
     for (const auto &[id, edge] : newGraph.Edges())
     {
-      if (!this->sceneGraph.EdgeFromId(id).Valid())
+      // Add the edge only if it's not already in the graph
+      if (!this->sceneGraph.EdgeFromVertices(edge.get().Vertices().first,
+            edge.get().Vertices().second).Valid())
+      {
         this->sceneGraph.AddEdge(edge.get().Vertices(), edge.get().Data());
+      }
     }
   }
 

diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc
@@ -277,6 +277,77 @@ TEST_P(SceneBroadcasterTest, DeletedTopic)
       }));
 }
 
+/////////////////////////////////////////////////
+/// Test whether the scene is updated when a model is spawned.
+TEST_P(SceneBroadcasterTest, SpawnedModel)
+{
+  // Start server
+  ignition::gazebo::ServerConfig serverConfig;
+  serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) +
+                          "/test/worlds/shapes.sdf");
+
+  gazebo::Server server(serverConfig);
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+
+  const std::size_t initEntityCount = 16;
+  EXPECT_EQ(initEntityCount, *server.EntityCount());
+
+  server.Run(true, 1, false);
+
+  transport::Node node;
+
+  // Spawn a model
+  {
+    auto modelStr = R"(
+<?xml version="1.0" ?>
+<sdf version='1.6'>
+  <model name='spawned_model'>
+    <link name='link'>
+      <visual name='visual'>
+        <geometry><sphere><radius>1.0</radius></sphere></geometry>
+      </visual>
+    </link>
+  </model>
+</sdf>)";
+
+    msgs::EntityFactory req;
+    msgs::Boolean res;
+    bool result;
+    unsigned int timeout = 5000;
+    req.set_sdf(modelStr);
+    EXPECT_TRUE(node.Request("/world/default/create",
+          req, timeout, res, result));
+    EXPECT_TRUE(result);
+    EXPECT_TRUE(res.data());
+  }
+
+  // Iterate once so that the model can get spawned.
+  server.Run(true, 1, false);
+
+
+  // Check that the model is in the scene/infor response
+  {
+    ignition::msgs::Empty req;
+    ignition::msgs::Scene rep;
+    bool result;
+    unsigned int timeout = 2000;
+    EXPECT_TRUE(node.Request("/world/default/scene/info", req, timeout,
+          rep, result));
+    EXPECT_TRUE(result);
+
+    bool found = false;
+    for (int i = 0; i < rep.model_size(); ++i)
+    {
+      found = rep.model(i).name() == "spawned_model";
+      if (found)
+        break;
+    }
+    EXPECT_TRUE(found);
+  }
+  EXPECT_EQ(initEntityCount + 3, *server.EntityCount());
+}
+
 /////////////////////////////////////////////////
 TEST_P(SceneBroadcasterTest, State)
 {