Changed to callback based sensor removal

include/ignition/gazebo/rendering/RenderUtil.hh

@@ -110,6 +110,12 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
         std::string(const gazebo::Entity &, const sdf::Sensor &,
         const std::string &)> _createSensorCb = {});
 
+    /// \brief Set the callback function for removing the sensors
+    /// \param[in] _removeSensorCb Callback function for removing the sensors
+    /// The callback function arg is the sensor entity to remove
+    public : void SetRemoveSensorCb(
+        std::function<void(const gazebo::Entity &)> _removeSensorCb);
+
     /// \brief Get the scene manager
     /// Returns reference to the scene manager.
     public: class SceneManager &SceneManager();

src/rendering/RenderUtil.cc

@@ -169,12 +169,19 @@ class ignition::gazebo::RenderUtilPrivate
   //// \brief Flag to indicate whether to create sensors
   public: bool enableSensors = false;
 
+  /// \brief A set containing all the entities with attached rendering sensors
+  public: std::unordered_set<Entity> sensorEntities;
+
   /// \brief Callback function for creating sensors.
   /// The function args are: entity id, sensor sdf, and parent name.
   /// The function returns the id of the rendering sensor created.
   public: std::function<std::string(const gazebo::Entity &, const sdf::Sensor &,
       const std::string &)> createSensorCb;
 
+  /// \brief Callback function for removing sensors.
+  /// The function arg is the entity id
+  public: std::function<void(const gazebo::Entity &)> removeSensorCb;
+
   /// \brief Entity currently being selected.
   public: rendering::NodePtr selectedEntity{nullptr};
 
@@ -281,7 +288,6 @@ void RenderUtil::Update()
   }
 
   // remove existing entities
-  // \todo(anyone) Remove sensors
   {
     IGN_PROFILE("RenderUtil::Update Remove");
     for (auto &entity : removeEntities)
@@ -293,6 +299,13 @@ void RenderUtil::Update()
         this->dataPtr->selectedEntity.reset();
       }
       this->dataPtr->sceneManager.RemoveEntity(entity.first);
+      // delete associated sensor, if existing
+      auto sensorEntityIt = this->dataPtr->sensorEntities.find(entity.first);
+      if (sensorEntityIt != this->dataPtr->sensorEntities.end())
+      {
+        this->dataPtr->removeSensorCb(entity.first);
+        this->dataPtr->sensorEntities.erase(sensorEntityIt);
+      }
     }
   }
 
@@ -442,6 +455,7 @@ void RenderUtilPrivate::CreateRenderingEntities(
     }
     this->newSensors.push_back(
         std::make_tuple(_entity, std::move(sdfDataCopy), _parent));
+    this->sensorEntities.insert(_entity);
   };
 
   const std::string cameraSuffix{"/image"};
@@ -1028,6 +1042,13 @@ void RenderUtil::SetEnableSensors(bool _enable,
   this->dataPtr->createSensorCb = std::move(_createSensorCb);
 }
 
+/////////////////////////////////////////////////
+void RenderUtil::SetRemoveSensorCb(
+    std::function<void(const gazebo::Entity &)> _removeSensorCb)
+{
+  this->dataPtr->removeSensorCb = std::move(_removeSensorCb);
+}
+
 /////////////////////////////////////////////////
 SceneManager &RenderUtil::SceneManager()
 {

src/systems/sensors/Sensors.cc

@@ -153,14 +153,6 @@ class ignition::gazebo::systems::SensorsPrivate
 
   /// \brief Stop the rendering thread
   public: void Stop();
-
-  /// \brief Helper function to remove a sensor from the maps
-  /// \return True if the sensor was removed
-  public: bool RemoveSensor(const Entity &_entity);
-
-  /// \brief Checks if any Sensor Entity has been removed and performs
-  /// cleanup of the corresponding sensor and map entries.
-  public: void SensorCleanup(const EntityComponentManager &_ecm);
 };
 
 //////////////////////////////////////////////////
@@ -301,45 +293,15 @@ void SensorsPrivate::Stop()
 }
 
 //////////////////////////////////////////////////
-bool SensorsPrivate::RemoveSensor(const Entity &_entity)
+void Sensors::RemoveSensor(const Entity &_entity)
 {
-  auto idIter = this->entityToIdMap.find(_entity);
-  if (idIter != this->entityToIdMap.end())
+  auto idIter = this->dataPtr->entityToIdMap.find(_entity);
+  if (idIter != this->dataPtr->entityToIdMap.end())
   {
-    this->sensorIds.erase(idIter->second);
-    this->sensorManager.Remove(idIter->second);
-    this->entityToIdMap.erase(idIter);
+    this->dataPtr->sensorIds.erase(idIter->second);
+    this->dataPtr->sensorManager.Remove(idIter->second);
+    this->dataPtr->entityToIdMap.erase(idIter);
   }
-  return true;
-}
-
-//////////////////////////////////////////////////
-void SensorsPrivate::SensorCleanup(const EntityComponentManager &_ecm)
-{
-  _ecm.EachRemoved<components::Camera>(
-      [&](const Entity &_entity,
-          const components::Camera *)->bool
-      {
-        return this->RemoveSensor(_entity);
-      });
-  _ecm.EachRemoved<components::DepthCamera>(
-      [&](const Entity &_entity,
-          const components::DepthCamera *)->bool
-      {
-        return this->RemoveSensor(_entity);
-      });
-  _ecm.EachRemoved<components::GpuLidar>(
-      [&](const Entity &_entity,
-          const components::GpuLidar *)->bool
-      {
-        return this->RemoveSensor(_entity);
-      });
-  _ecm.EachRemoved<components::RgbdCamera>(
-      [&](const Entity &_entity,
-          const components::RgbdCamera *)->bool
-      {
-        return this->RemoveSensor(_entity);
-      });
 }
 
 //////////////////////////////////////////////////
@@ -368,6 +330,8 @@ void Sensors::Configure(const Entity &/*_id*/,
   this->dataPtr->renderUtil.SetEnableSensors(true,
       std::bind(&Sensors::CreateSensor, this,
       std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+  this->dataPtr->renderUtil.SetRemoveSensorCb(
+      std::bind(&Sensors::RemoveSensor, this, std::placeholders::_1));
 
   this->dataPtr->stopConn = _eventMgr.Connect<events::Stop>(
       std::bind(&SensorsPrivate::Stop, this->dataPtr.get()));
@@ -405,8 +369,6 @@ void Sensors::PostUpdate(const UpdateInfo &_info,
   if (this->dataPtr->running && this->dataPtr->initialized)
   {
     this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm);
-    // Performs cleanup for removed sensors
-    this->dataPtr->SensorCleanup(_ecm);
 
     auto time = math::durationToSecNsec(_info.simTime);
     auto t = common::Time(time.first, time.second);

src/systems/sensors/Sensors.hh

@@ -63,10 +63,14 @@ namespace systems
     /// \param[in] _sdf SDF description of the sensor
     /// \param[in] _parentName Name of parent that the sensor is attached to
     /// \return Sensor name
-    private : std::string CreateSensor(const Entity& _entity,
+    private : std::string CreateSensor(const Entity &_entity,
                                        const sdf::Sensor &_sdf,
                                        const std::string &_parentName);
 
+    /// \brief Removes a rendering sensor
+    /// \param[in] _entity Entity of the sensor
+    private : void RemoveSensor(const Entity &_entity);
+
     /// \brief Private data pointer.
     private: std::unique_ptr<SensorsPrivate> dataPtr;
   };