From 8a6552e589935e5830bec5eeafdeff8e65359c46 Mon Sep 17 00:00:00 2001
From: Ian Chen <ichen@osrfoundation.org>
Date: Thu, 11 Nov 2021 17:02:34 -0800
Subject: [PATCH] update cameras list on sensor removal

Signed-off-by: Ian Chen <ichen@osrfoundation.org>
---
 src/systems/sensors/Sensors.cc | 12 +++++++++++-
 1 file changed, 11 insertions(+), 1 deletion(-)

diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc
index 9f5b0f434b..a2739240ca 100644
--- a/src/systems/sensors/Sensors.cc
+++ b/src/systems/sensors/Sensors.cc
@@ -91,7 +91,6 @@ class ignition::gazebo::systems::SensorsPrivate
   /// \brief Keep track of cameras, in case we need to handle stereo cameras.
   /// Key: Camera's parent scoped name
   /// Value: Pointer to camera
-  // TODO(anyone) Remove element when sensor is deleted
   public: std::map<std::string, sensors::CameraSensor *> cameras;
 
   /// \brief Maps gazebo entity to its matching sensor ID
@@ -361,6 +360,17 @@ void Sensors::RemoveSensor(const Entity &_entity)
         this->dataPtr->activeSensors.erase(activeSensorIt);
       }
     }
+
+    // update cameras list
+    for (auto &it : this->dataPtr->cameras)
+    {
+      if (it.second->Id() == idIter->second)
+      {
+        this->dataPtr->cameras.erase(it.first);
+        break;
+      }
+    }
+
     this->dataPtr->sensorIds.erase(idIter->second);
     this->dataPtr->sensorManager.Remove(idIter->second);
     this->dataPtr->entityToIdMap.erase(idIter);