diff --git a/Changelog.md b/Changelog.md index 2f301c0a95..7d8ff73e48 100644 --- a/Changelog.md +++ b/Changelog.md @@ -2,6 +2,9 @@ ### Ignition Gazebo 2.XX.XX (20XX-XX-XX) +1. Allow renaming breadcrumb models if there is a name conflict + * [Pull Request 155](https://github.com/ignitionrobotics/ign-gazebo/pull/155) + 1. Add TriggeredPublisher system * [Pull Request 139](https://github.com/ignitionrobotics/ign-gazebo/pull/139) diff --git a/src/systems/breadcrumbs/Breadcrumbs.cc b/src/systems/breadcrumbs/Breadcrumbs.cc index 1c1e8fe657..26449732c7 100644 --- a/src/systems/breadcrumbs/Breadcrumbs.cc +++ b/src/systems/breadcrumbs/Breadcrumbs.cc @@ -31,6 +31,7 @@ #include "ignition/gazebo/components/DetachableJoint.hh" #include "ignition/gazebo/components/Geometry.hh" #include "ignition/gazebo/components/Link.hh" +#include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/Performer.hh" @@ -66,6 +67,9 @@ void Breadcrumbs::Configure(const Entity &_entity, std::chrono::duration_cast( std::chrono::duration(period)); + this->allowRenaming = + _sdf->Get("allow_renaming", this->allowRenaming).first; + this->model = Model(_entity); if (!_sdf->HasElement("breadcrumb")) @@ -161,8 +165,42 @@ void Breadcrumbs::PreUpdate(const ignition::gazebo::UpdateInfo &_info, this->numDeployments < this->maxDeployments) { sdf::Model modelToSpawn = *this->modelRoot.ModelByIndex(0); - modelToSpawn.SetName(modelToSpawn.Name() + "_" + - std::to_string(this->numDeployments)); + std::string desiredName = + modelToSpawn.Name() + "_" + std::to_string(this->numDeployments); + + std::vector modelNames; + _ecm.Each( + [&modelNames](const Entity &, const components::Name *_name, + const components::Model *) + { + modelNames.push_back(_name->Data()); + return true; + }); + + // Check if there's a model with the same name. + auto it = std::find(modelNames.begin(), modelNames.end(), desiredName); + if (it != modelNames.end()) + { + if (!this->allowRenaming) + { + ignwarn << "Entity named [" << desiredName + << "] already exists and " + << "[allow_renaming] is false. Entity not spawned." + << std::endl; + return; + } + + std::string newName = desiredName; + int counter = 0; + while (std::find(modelNames.begin(), modelNames.end(), newName) != + modelNames.end()) + { + newName = desiredName + "_" + std::to_string(++counter); + } + desiredName = newName; + } + + modelToSpawn.SetName(desiredName); modelToSpawn.SetPose(poseComp->Data() * modelToSpawn.Pose()); ignmsg << "Deploying " << modelToSpawn.Name() << " at " << modelToSpawn.Pose() << std::endl; diff --git a/src/systems/breadcrumbs/Breadcrumbs.hh b/src/systems/breadcrumbs/Breadcrumbs.hh index 2184f2dd5f..7402d8763a 100644 --- a/src/systems/breadcrumbs/Breadcrumbs.hh +++ b/src/systems/breadcrumbs/Breadcrumbs.hh @@ -71,6 +71,10 @@ namespace systems /// ``: Geometry that represents the bounding volume of /// the performer. Only `` is supported currently. When this /// parameter is present, the deployed models will be performers. + /// ``: If true, the deployed model will be renamed if another + /// model with the same name already exists in the world. If false and there + /// is another model with the same name, the breadcrumb will not be deployed. + /// Defaults to false. /// ``: This is the model used as a template for deploying /// breadcrumbs. class IGNITION_GAZEBO_VISIBLE Breadcrumbs @@ -128,6 +132,10 @@ namespace systems /// \brief Whether the deployed models will be performers private: bool isPerformer{false}; + /// \brief Whether the deployed model should be renamed if a model with the + /// same name already exists + private: bool allowRenaming{false}; + /// \brief Bounding volume of the performer private: std::optional performerGeometry; diff --git a/test/integration/breadcrumbs.cc b/test/integration/breadcrumbs.cc index ffc95f2310..179e2834f8 100644 --- a/test/integration/breadcrumbs.cc +++ b/test/integration/breadcrumbs.cc @@ -529,3 +529,48 @@ TEST_F(BreadcrumbsTest, DeployDisablePhysics) server.AddSystem(testSystem.systemPtr); server.Run(true, iterTestStart + 2001, false); } + +///////////////////////////////////////////////// +// The test verifies that if allow_renaming is true, the Breadcrumb system +// renames spawned models if a model with the same name exists. +TEST_F(BreadcrumbsTest, AllowRenaming) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/breadcrumbs.sdf"; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + using namespace std::chrono_literals; + server.SetUpdatePeriod(1ns); + + transport::Node node; + auto deployB1 = + node.Advertise("/model/vehicle_blue/breadcrumbs/B1/deploy"); + auto noRenameDeploy = + node.Advertise("/no_rename_deploy"); + auto renameDeploy = + node.Advertise("/rename_deploy"); + + server.Run(true, 1, false); + deployB1.Publish(msgs::Empty()); + server.Run(true, 100, false); + EXPECT_TRUE(server.HasEntity("B1_0")); + + // Deploying via "/no_rename_deploy" will try to spawn B1_0, but since the + // model already exists, the spawn should fail. + auto curEntityCount = server.EntityCount().value(); + noRenameDeploy.Publish(msgs::Empty()); + server.Run(true, 100, false); + EXPECT_EQ(curEntityCount, server.EntityCount().value()); + + // Deploying via "/rename_deploy" will try to spawn B1_0, but since the + // model already exists, it will spawn B1_0_1 instead. + renameDeploy.Publish(msgs::Empty()); + server.Run(true, 100, false); + EXPECT_TRUE(server.HasEntity("B1_0_1")); +} diff --git a/test/worlds/breadcrumbs.sdf b/test/worlds/breadcrumbs.sdf index 333fcb53fb..2d0351c4cb 100644 --- a/test/worlds/breadcrumbs.sdf +++ b/test/worlds/breadcrumbs.sdf @@ -655,6 +655,58 @@ + + + + 3 + false + /no_rename_deploy + + + + -10 0 0 0 0 0 + + + 0.6 + + 0.017 + 0 + 0 + 0.017 + 0 + 0.009 + + + + + + + + + 3 + true + /rename_deploy + + + + -20 0 0 0 0 0 + + + 0.6 + + 0.017 + 0 + 0 + 0.017 + 0 + 0.009 + + + + + + +