From 77406ae9bbf06939650ff86a365a83277bfb5b8e Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 18 Jun 2020 11:14:11 -0700 Subject: [PATCH] Properly add new models into the scenegraph (#212) * Properly add new models into the scenegraph Signed-off-by: Nate Koenig * Changelog and bump patch number Signed-off-by: Nate Koenig * Added test Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- CMakeLists.txt | 2 +- Changelog.md | 5 ++ .../scene_broadcaster/SceneBroadcaster.cc | 7 +- test/integration/scene_broadcaster_system.cc | 71 +++++++++++++++++++ 4 files changed, 83 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ff64af92c4..a0b3fc90f5 100644 --- 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 index 84977bb292..2dd2a48176 100644 --- 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 index 6e1f1a243a..cc1aa5194e 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -693,13 +693,18 @@ void SceneBroadcasterPrivate::SceneGraphAddEntities( std::lock_guard 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 index 356c89dfc9..22f47cc028 100644 --- 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"( + + + + + + 1.0 + + + +)"; + + 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) {