From 7750a411eb0135da1b8bc06c8d19e4e08ef83190 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <luca@openrobotics.org>
Date: Fri, 3 Apr 2020 17:51:21 +0800
Subject: [PATCH] Cleanup activeSensor structure to avoid segfaults

---
 src/systems/sensors/Sensors.cc | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc
index c3653e1fa8..3689998f7e 100644
--- a/src/systems/sensors/Sensors.cc
+++ b/src/systems/sensors/Sensors.cc
@@ -298,9 +298,24 @@ void Sensors::RemoveSensor(const Entity &_entity)
   auto idIter = this->dataPtr->entityToIdMap.find(_entity);
   if (idIter != this->dataPtr->entityToIdMap.end())
   {
+    // Remove from active sensors as well
+    // Locking mutex to make sure the vector is not being changed while
+    // the rendering thread is iterating over it
+    {
+      std::unique_lock<std::mutex> lock(this->dataPtr->sensorMaskMutex);
+      sensors::Sensor *s = this->dataPtr->sensorManager.Sensor(idIter->second);
+      auto rs = dynamic_cast<sensors::RenderingSensor *>(s);
+      auto activeSensorIt = std::find(this->dataPtr->activeSensors.begin(),
+          this->dataPtr->activeSensors.end(), rs);
+      if (activeSensorIt != this->dataPtr->activeSensors.end())
+      {
+        this->dataPtr->activeSensors.erase(activeSensorIt);
+      }
+    }
     this->dataPtr->sensorIds.erase(idIter->second);
     this->dataPtr->sensorManager.Remove(idIter->second);
     this->dataPtr->entityToIdMap.erase(idIter);
+
   }
 }