From 1b2905080b85614c443ae6a13fd3bf3cc59e232c Mon Sep 17 00:00:00 2001
From: Ian Chen <ichen@osrfoundation.org>
Date: Wed, 29 Apr 2020 17:27:05 -0700
Subject: [PATCH] Add disable_physics_time param to breadcrumb plugin to auto
 disable them

Signed-off-by: Ian Chen <ichen@osrfoundation.org>
---
 .../costar_husky_sensor_config_2/launch/spawner.rb           | 1 +
 .../explorer_x1_sensor_config_2/launch/spawner.rb            | 1 +
 subt_ign/launch/cave_circuit.ign                             | 2 ++
 subt_ign/launch/cloudsim_sim.ign                             | 2 ++
 subt_ign/src/CommsBrokerPlugin.cc                            | 5 ++++-
 5 files changed, 10 insertions(+), 1 deletion(-)

diff --git a/submitted_models/costar_husky_sensor_config_2/launch/spawner.rb b/submitted_models/costar_husky_sensor_config_2/launch/spawner.rb
index 892fb354..eaddafa3 100644
--- a/submitted_models/costar_husky_sensor_config_2/launch/spawner.rb
+++ b/submitted_models/costar_husky_sensor_config_2/launch/spawner.rb
@@ -56,6 +56,7 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw)
         name="ignition::gazebo::systems::Breadcrumbs">
         <topic>/model/#{_name}/breadcrumb/deploy</topic>
         <max_deployments>12</max_deployments>"
+        <disable_physics_time>3.0</disable_physics_time>
         <breadcrumb>"
           <sdf version="1.6">
             <model name="#{_name}__breadcrumb__">
diff --git a/submitted_models/explorer_x1_sensor_config_2/launch/spawner.rb b/submitted_models/explorer_x1_sensor_config_2/launch/spawner.rb
index 97457937..7499cc64 100644
--- a/submitted_models/explorer_x1_sensor_config_2/launch/spawner.rb
+++ b/submitted_models/explorer_x1_sensor_config_2/launch/spawner.rb
@@ -56,6 +56,7 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw)
         name="ignition::gazebo::systems::Breadcrumbs">
         <topic>/model/#{_name}/breadcrumb/deploy</topic>
         <max_deployments>12</max_deployments>"
+        <disable_physics_time>3.0</disable_physics_time>
         <breadcrumb>"
           <sdf version="1.6">
             <model name="#{_name}__breadcrumb__">
diff --git a/subt_ign/launch/cave_circuit.ign b/subt_ign/launch/cave_circuit.ign
index 2b57d491..1c11e669 100644
--- a/subt_ign/launch/cave_circuit.ign
+++ b/subt_ign/launch/cave_circuit.ign
@@ -527,6 +527,7 @@
     "      name=\"ignition::gazebo::systems::Breadcrumbs\">\n"\
     "      <topic>/model/#{_name}/breadcrumb/deploy</topic>\n"\
     "      <max_deployments>#{max_breadcrumbs}</max_deployments>"\
+    "      <disable_physics_time>3.0</disable_physics_time>"\
     "      <breadcrumb>\n"\
     "        <sdf version=\"1.6\">\n"\
     "          <model name=\"#{_name}__breadcrumb__\">\n"\
@@ -627,6 +628,7 @@
     "      name=\"ignition::gazebo::systems::Breadcrumbs\">\n"\
     "      <topic>/model/#{_name}/breadcrumb/deploy</topic>\n"\
     "      <max_deployments>#{max_breadcrumbs}</max_deployments>"\
+    "      <disable_physics_time>3.0</disable_physics_time>"\
     "      <breadcrumb>\n"\
     "        <sdf version=\"1.6\">\n"\
     "          <model name=\"#{_name}__breadcrumb__\">\n"\
diff --git a/subt_ign/launch/cloudsim_sim.ign b/subt_ign/launch/cloudsim_sim.ign
index a359afef..bd74a377 100644
--- a/subt_ign/launch/cloudsim_sim.ign
+++ b/subt_ign/launch/cloudsim_sim.ign
@@ -583,6 +583,7 @@
     "      name=\"ignition::gazebo::systems::Breadcrumbs\">\n"\
     "      <topic>/model/#{_name}/breadcrumb/deploy</topic>\n"\
     "      <max_deployments>#{max_breadcrumbs}</max_deployments>"\
+    "      <disable_physics_time>3.0</disable_physics_time>"\
     "      <breadcrumb>\n"\
     "        <sdf version=\"1.6\">\n"\
     "          <model name=\"#{_name}__breadcrumb__\">\n"\
@@ -662,6 +663,7 @@
     "      name=\"ignition::gazebo::systems::Breadcrumbs\">\n"\
     "      <topic>/model/#{_name}/breadcrumb/deploy</topic>\n"\
     "      <max_deployments>#{max_breadcrumbs}</max_deployments>"\
+    "      <disable_physics_time>3.0</disable_physics_time>"\
     "      <breadcrumb>\n"\
     "        <sdf version=\"1.6\">\n"\
     "          <model name=\"#{_name}__breadcrumb__\">\n"\
diff --git a/subt_ign/src/CommsBrokerPlugin.cc b/subt_ign/src/CommsBrokerPlugin.cc
index ea280025..0a6d045e 100644
--- a/subt_ign/src/CommsBrokerPlugin.cc
+++ b/subt_ign/src/CommsBrokerPlugin.cc
@@ -19,6 +19,7 @@
 
 #include <functional>
 #include <mutex>
+#include <regex>
 
 #include <ignition/common/Console.hh>
 #include <ignition/common/Util.hh>
@@ -310,7 +311,9 @@ void CommsBrokerPlugin::UpdateIfNewBreadcrumbs()
   for (const auto& [name, pose] : this->poses)
   {
     // New breadcrumb found.
-    if (name.find("__breadcrumb__") != std::string::npos &&
+    // A static model is spawned when the breadcrumb is made static
+    if (std::regex_match(name,
+        std::regex(".*__breadcrumb___(\\d+)__static__")) &&
         this->breadcrumbs.find(name) == this->breadcrumbs.end())
     {
       this->breadcrumbs[name] = pose;