Allow renaming breadcrumb models if there is a name conflict

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)
 

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::steady_clock::duration>(
       std::chrono::duration<double>(period));
 
+  this->allowRenaming =
+      _sdf->Get<bool>("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<std::string> modelNames;
+        _ecm.Each<components::Name, components::Model>(
+            [&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;

src/systems/breadcrumbs/Breadcrumbs.hh

@@ -71,6 +71,10 @@ namespace systems
   /// `<performer_volume>`: Geometry that represents the bounding volume of
   /// the performer. Only `<geometry><box>` is supported currently. When this
   /// parameter is present, the deployed models will be performers.
+  /// `<allow_renaming>`: 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.
   /// `<breadcrumb>`: 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<sdf::Geometry> performerGeometry;
 

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<msgs::Empty>("/model/vehicle_blue/breadcrumbs/B1/deploy");
+  auto noRenameDeploy =
+      node.Advertise<msgs::Empty>("/no_rename_deploy");
+  auto renameDeploy =
+      node.Advertise<msgs::Empty>("/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"));
+}

test/worlds/breadcrumbs.sdf

@@ -655,6 +655,58 @@
         </sdf>
        </breadcrumb>
       </plugin>
+
+      <!-- Two Breadcrumbs containing a breadcrumb model that is already spawned (B1).-->
+      <plugin filename="libignition-gazebo-breadcrumbs-system.so" name="ignition::gazebo::systems::Breadcrumbs">
+       <max_deployments>3</max_deployments>
+       <allow_renaming>false</allow_renaming>
+       <topic>/no_rename_deploy</topic>
+       <breadcrumb>
+         <sdf version="1.6">
+          <model name="B1">
+            <pose>-10 0 0 0 0 0</pose>
+            <link name='body'>
+              <inertial>
+                <mass>0.6</mass>
+                <inertia>
+                  <ixx>0.017</ixx>
+                  <ixy>0</ixy>
+                  <ixz>0</ixz>
+                  <iyy>0.017</iyy>
+                  <iyz>0</iyz>
+                  <izz>0.009</izz>
+                </inertia>
+              </inertial>
+            </link>
+          </model>
+        </sdf>
+       </breadcrumb>
+      </plugin>
+      <plugin filename="libignition-gazebo-breadcrumbs-system.so" name="ignition::gazebo::systems::Breadcrumbs">
+        <max_deployments>3</max_deployments>
+        <allow_renaming>true</allow_renaming>
+        <topic>/rename_deploy</topic>
+        <breadcrumb>
+          <sdf version="1.6">
+            <model name="B1">
+              <pose>-20 0 0 0 0 0</pose>
+              <link name='body'>
+                <inertial>
+                  <mass>0.6</mass>
+                  <inertia>
+                    <ixx>0.017</ixx>
+                    <ixy>0</ixy>
+                    <ixz>0</ixz>
+                    <iyy>0.017</iyy>
+                    <iyz>0</iyz>
+                    <izz>0.009</izz>
+                  </inertia>
+                </inertial>
+              </link>
+            </model>
+          </sdf>
+        </breadcrumb>
+      </plugin>
     </model>
     <plugin name="ignition::gazebo" filename="dummy">
       <performer name="perf1">