Skip to content

Commit

Permalink
Remove EachNew calls from sensor PreUpdates (#1281)
Browse files Browse the repository at this point in the history 
Signed-off-by: Louise Poubel <louise@openrobotics.org>
  • Loading branch information
@chapulina
chapulina authored Jan 18, 2022
1 parent 6f58c91 commit 650b746
Show file tree
Hide file tree
Showing 11 changed files with 211 additions and 83 deletions.
4 changes: 4 additions & 0 deletions Migration.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,10 @@ Deprecated code produces compile-time warnings. These warning serve as
notification to users that their code should be upgraded. The next major
release will remove the deprecated code.

## Ignition Gazebo 3.12.0 to 3.X.X

* Some sensors will only have the `SensorTopic` component after the 1st iteration.

## Ignition Gazebo 2.x to 3.x

* Use ign-rendering3, ign-sensors3 and ign-gui3.
Expand Down
46 changes: 33 additions & 13 deletions src/systems/air_pressure/AirPressure.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@

#include <string>
#include <unordered_map>
#include <unordered_set>
#include <utility>

#include <ignition/plugin/Register.hh>
Expand Down Expand Up @@ -49,30 +50,35 @@ using namespace systems;
/// \brief Private AirPressure data class.
class ignition::gazebo::systems::AirPressurePrivate
{
/// \brief A map of air pressure entity to its vertical reference
/// \brief A map of air pressure entity to its sensor
public: std::unordered_map<Entity,
std::unique_ptr<sensors::AirPressureSensor>> entitySensorMap;

/// \brief Ign-sensors sensor factory for creating sensors
public: sensors::SensorFactory sensorFactory;

/// \brief Keep list of sensors that were created during the previous
/// `PostUpdate`, so that components can be created during the next
/// `PreUpdate`.
public: std::unordered_set<Entity> newSensors;

/// True if the rendering component is initialized
public: bool initialized = false;

/// \brief Create sensor
/// \param[in] _ecm Mutable reference to ECM.
/// \param[in] _ecm Immutable reference to ECM.
/// \param[in] _entity Entity of the IMU
/// \param[in] _airPressure AirPressureSensor component.
/// \param[in] _parent Parent entity component.
public: void AddAirPressure(
EntityComponentManager &_ecm,
const EntityComponentManager &_ecm,
const Entity _entity,
const components::AirPressureSensor *_airPressure,
const components::ParentEntity *_parent);

/// \brief Create air pressure sensor
/// \param[in] _ecm Mutable reference to ECM.
public: void CreateAirPressureEntities(EntityComponentManager &_ecm);
/// \param[in] _ecm Immutable reference to ECM.
public: void CreateSensors(const EntityComponentManager &_ecm);

/// \brief Update air pressure sensor data based on physics data
/// \param[in] _ecm Immutable reference to ECM.
Expand All @@ -98,7 +104,21 @@ void AirPressure::PreUpdate(const UpdateInfo &/*_info*/,
EntityComponentManager &_ecm)
{
IGN_PROFILE("AirPressure::PreUpdate");
this->dataPtr->CreateAirPressureEntities(_ecm);

// Create components
for (auto entity : this->dataPtr->newSensors)
{
auto it = this->dataPtr->entitySensorMap.find(entity);
if (it == this->dataPtr->entitySensorMap.end())
{
ignerr << "Entity [" << entity
<< "] isn't in sensor map, this shouldn't happen." << std::endl;
continue;
}
// Set topic
_ecm.CreateComponent(entity, components::SensorTopic(it->second->Topic()));
}
this->dataPtr->newSensors.clear();
}

//////////////////////////////////////////////////
Expand All @@ -116,6 +136,8 @@ void AirPressure::PostUpdate(const UpdateInfo &_info,
<< "s]. System may not work properly." << std::endl;
}

this->dataPtr->CreateSensors(_ecm);

if (!_info.paused)
{
this->dataPtr->UpdateAirPressures(_ecm);
Expand All @@ -134,7 +156,7 @@ void AirPressure::PostUpdate(const UpdateInfo &_info,

//////////////////////////////////////////////////
void AirPressurePrivate::AddAirPressure(
EntityComponentManager &_ecm,
const EntityComponentManager &_ecm,
const Entity _entity,
const components::AirPressureSensor *_airPressure,
const components::ParentEntity *_parent)
Expand Down Expand Up @@ -171,15 +193,13 @@ void AirPressurePrivate::AddAirPressure(
math::Pose3d sensorWorldPose = worldPose(_entity, _ecm);
sensor->SetPose(sensorWorldPose);

// Set topic
_ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic()));

this->entitySensorMap.insert(
std::make_pair(_entity, std::move(sensor)));
this->newSensors.insert(_entity);
}

//////////////////////////////////////////////////
void AirPressurePrivate::CreateAirPressureEntities(EntityComponentManager &_ecm)
void AirPressurePrivate::CreateSensors(const EntityComponentManager &_ecm)
{
IGN_PROFILE("AirPressurePrivate::CreateAirPressureEntities");
if (!this->initialized)
Expand All @@ -190,7 +210,7 @@ void AirPressurePrivate::CreateAirPressureEntities(EntityComponentManager &_ecm)
const components::AirPressureSensor *_airPressure,
const components::ParentEntity *_parent)->bool
{
AddAirPressure(_ecm, _entity, _airPressure, _parent);
this->AddAirPressure(_ecm, _entity, _airPressure, _parent);
return true;
});
this->initialized = true;
Expand All @@ -203,7 +223,7 @@ void AirPressurePrivate::CreateAirPressureEntities(EntityComponentManager &_ecm)
const components::AirPressureSensor *_airPressure,
const components::ParentEntity *_parent)->bool
{
AddAirPressure(_ecm, _entity, _airPressure, _parent);
this->AddAirPressure(_ecm, _entity, _airPressure, _parent);
return true;
});
}
Expand Down
46 changes: 33 additions & 13 deletions src/systems/altimeter/Altimeter.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@

#include <string>
#include <unordered_map>
#include <unordered_set>
#include <utility>

#include <ignition/common/Profiler.hh>
Expand Down Expand Up @@ -51,30 +52,35 @@ using namespace systems;
/// \brief Private Altimeter data class.
class ignition::gazebo::systems::AltimeterPrivate
{
/// \brief A map of altimeter entity to its vertical reference
/// \brief A map of altimeter entity to its sensor
public: std::unordered_map<Entity,
std::unique_ptr<sensors::AltimeterSensor>> entitySensorMap;

/// \brief Ign-sensors sensor factory for creating sensors
public: sensors::SensorFactory sensorFactory;

/// \brief Keep list of sensors that were created during the previous
/// `PostUpdate`, so that components can be created during the next
/// `PreUpdate`.
public: std::unordered_set<Entity> newSensors;

/// True if the rendering component is initialized
public: bool initialized = false;

/// \brief Create sensor
/// \param[in] _ecm Mutable reference to ECM.
/// \param[in] _ecm Immutable reference to ECM.
/// \param[in] _entity Entity of the IMU
/// \param[in] _altimeter Altimeter component.
/// \param[in] _parent Parent entity component.
public: void AddAltimeter(
EntityComponentManager &_ecm,
const EntityComponentManager &_ecm,
const Entity _entity,
const components::Altimeter *_altimeter,
const components::ParentEntity *_parent);

/// \brief Create altimeter sensor
/// \param[in] _ecm Mutable reference to ECM.
public: void CreateAltimeterEntities(EntityComponentManager &_ecm);
/// \param[in] _ecm Immutable reference to ECM.
public: void CreateSensors(const EntityComponentManager &_ecm);

/// \brief Update altimeter sensor data based on physics data
/// \param[in] _ecm Immutable reference to ECM.
Expand All @@ -99,7 +105,21 @@ void Altimeter::PreUpdate(const UpdateInfo &/*_info*/,
EntityComponentManager &_ecm)
{
IGN_PROFILE("Altimeter::PreUpdate");
this->dataPtr->CreateAltimeterEntities(_ecm);

// Create components
for (auto entity : this->dataPtr->newSensors)
{
auto it = this->dataPtr->entitySensorMap.find(entity);
if (it == this->dataPtr->entitySensorMap.end())
{
ignerr << "Entity [" << entity
<< "] isn't in sensor map, this shouldn't happen." << std::endl;
continue;
}
// Set topic
_ecm.CreateComponent(entity, components::SensorTopic(it->second->Topic()));
}
this->dataPtr->newSensors.clear();
}

//////////////////////////////////////////////////
Expand All @@ -116,6 +136,8 @@ void Altimeter::PostUpdate(const UpdateInfo &_info,
<< "s]. System may not work properly." << std::endl;
}

this->dataPtr->CreateSensors(_ecm);

// Only update and publish if not paused.
if (!_info.paused)
{
Expand All @@ -135,7 +157,7 @@ void Altimeter::PostUpdate(const UpdateInfo &_info,

//////////////////////////////////////////////////
void AltimeterPrivate::AddAltimeter(
EntityComponentManager &_ecm,
const EntityComponentManager &_ecm,
const Entity _entity,
const components::Altimeter *_altimeter,
const components::ParentEntity *_parent)
Expand Down Expand Up @@ -173,15 +195,13 @@ void AltimeterPrivate::AddAltimeter(
sensor->SetVerticalReference(verticalReference);
sensor->SetPosition(verticalReference);

// Set topic
_ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic()));

this->entitySensorMap.insert(
std::make_pair(_entity, std::move(sensor)));
this->newSensors.insert(_entity);
}

//////////////////////////////////////////////////
void AltimeterPrivate::CreateAltimeterEntities(EntityComponentManager &_ecm)
void AltimeterPrivate::CreateSensors(const EntityComponentManager &_ecm)
{
IGN_PROFILE("Altimeter::CreateAltimeterEntities");
if (!this->initialized)
Expand All @@ -192,7 +212,7 @@ void AltimeterPrivate::CreateAltimeterEntities(EntityComponentManager &_ecm)
const components::Altimeter *_altimeter,
const components::ParentEntity *_parent)->bool
{
AddAltimeter(_ecm, _entity, _altimeter, _parent);
this->AddAltimeter(_ecm, _entity, _altimeter, _parent);
return true;
});
this->initialized = true;
Expand All @@ -205,7 +225,7 @@ void AltimeterPrivate::CreateAltimeterEntities(EntityComponentManager &_ecm)
const components::Altimeter *_altimeter,
const components::ParentEntity *_parent)->bool
{
AddAltimeter(_ecm, _entity, _altimeter, _parent);
this->AddAltimeter(_ecm, _entity, _altimeter, _parent);
return true;
});
}
Expand Down
52 changes: 36 additions & 16 deletions src/systems/imu/Imu.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,9 +17,10 @@

#include "Imu.hh"

#include <string>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <string>

#include <ignition/plugin/Register.hh>

Expand Down Expand Up @@ -56,6 +57,11 @@ class ignition::gazebo::systems::ImuPrivate
/// \brief Ign-sensors sensor factory for creating sensors
public: sensors::SensorFactory sensorFactory;

/// \brief Keep list of sensors that were created during the previous
/// `PostUpdate`, so that components can be created during the next
/// `PreUpdate`.
public: std::unordered_set<Entity> newSensors;

/// \brief Keep track of world ID, which is equivalent to the scene's
/// root visual.
/// Defaults to zero, which is considered invalid by Ignition Gazebo.
Expand All @@ -64,21 +70,21 @@ class ignition::gazebo::systems::ImuPrivate
/// True if the rendering component is initialized
public: bool initialized = false;

/// \brief Create IMU sensor
/// \param[in] _ecm Mutable reference to ECM.
public: void CreateImuEntities(EntityComponentManager &_ecm);
/// \brief Create IMU sensors in ign-sensors
/// \param[in] _ecm Immutable reference to ECM.
public: void CreateSensors(const EntityComponentManager &_ecm);

/// \brief Update IMU sensor data based on physics data
/// \param[in] _ecm Immutable reference to ECM.
public: void Update(const EntityComponentManager &_ecm);

/// \brief Create sensor
/// \param[in] _ecm Mutable reference to ECM.
/// \param[in] _ecm Immutable reference to ECM.
/// \param[in] _entity Entity of the IMU
/// \param[in] _imu IMU component.
/// \param[in] _parent Parent entity component.
public: void addIMU(
EntityComponentManager &_ecm,
public: void AddSensor(
const EntityComponentManager &_ecm,
const Entity _entity,
const components::Imu *_imu,
const components::ParentEntity *_parent);
Expand All @@ -102,7 +108,21 @@ void Imu::PreUpdate(const UpdateInfo &/*_info*/,
EntityComponentManager &_ecm)
{
IGN_PROFILE("Imu::PreUpdate");
this->dataPtr->CreateImuEntities(_ecm);

// Create components
for (auto entity : this->dataPtr->newSensors)
{
auto it = this->dataPtr->entitySensorMap.find(entity);
if (it == this->dataPtr->entitySensorMap.end())
{
ignerr << "Entity [" << entity
<< "] isn't in sensor map, this shouldn't happen." << std::endl;
continue;
}
// Set topic
_ecm.CreateComponent(entity, components::SensorTopic(it->second->Topic()));
}
this->dataPtr->newSensors.clear();
}

//////////////////////////////////////////////////
Expand All @@ -119,6 +139,8 @@ void Imu::PostUpdate(const UpdateInfo &_info,
<< "s]. System may not work properly." << std::endl;
}

this->dataPtr->CreateSensors(_ecm);

// Only update and publish if not paused.
if (!_info.paused)
{
Expand All @@ -137,8 +159,8 @@ void Imu::PostUpdate(const UpdateInfo &_info,
}

//////////////////////////////////////////////////
void ImuPrivate::addIMU(
EntityComponentManager &_ecm,
void ImuPrivate::AddSensor(
const EntityComponentManager &_ecm,
const Entity _entity,
const components::Imu *_imu,
const components::ParentEntity *_parent)
Expand Down Expand Up @@ -186,15 +208,13 @@ void ImuPrivate::addIMU(
math::Pose3d p = worldPose(_entity, _ecm);
sensor->SetOrientationReference(p.Rot());

// Set topic
_ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic()));

this->entitySensorMap.insert(
std::make_pair(_entity, std::move(sensor)));
this->newSensors.insert(_entity);
}

//////////////////////////////////////////////////
void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm)
void ImuPrivate::CreateSensors(const EntityComponentManager &_ecm)
{
IGN_PROFILE("ImuPrivate::CreateImuEntities");
// Get World Entity
Expand All @@ -214,7 +234,7 @@ void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm)
const components::Imu *_imu,
const components::ParentEntity *_parent)->bool
{
addIMU(_ecm, _entity, _imu, _parent);
this->AddSensor(_ecm, _entity, _imu, _parent);
return true;
});
this->initialized = true;
Expand All @@ -227,7 +247,7 @@ void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm)
const components::Imu *_imu,
const components::ParentEntity *_parent)->bool
{
addIMU(_ecm, _entity, _imu, _parent);
this->AddSensor(_ecm, _entity, _imu, _parent);
return true;
});
}
Expand Down
Loading

0 comments on commit 650b746

Please sign in to comment.