Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Refactor common sensor setup interactions #1471

Closed
wants to merge 3 commits into from
Closed

Refactor common sensor setup interactions #1471

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
6ac2822
Split boilerplate sensor implementation
mjcarroll May 3, 2022
15c7999
Move header to common location
mjcarroll May 3, 2022
f0bbe58
Update AirPressure to use common functionality
mjcarroll May 3, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 6 additions & 0 deletions src/systems/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,12 @@ function(gz_add_system system_name)
ignition-plugin${IGN_PLUGIN_VER}::register
)

target_include_directories(${system_target}
PRIVATE
${PROJECT_SOURCE_DIR}/src/systems/common
)


if(gz_add_system_PRIVATE_INCLUDE_DIRS)
target_include_directories(${system_target}
PRIVATE
Expand Down
150 changes: 23 additions & 127 deletions src/systems/air_pressure/AirPressure.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
*/

#include "AirPressure.hh"
#include "gz_sensor/GzSensorImpl.hh"

#include <ignition/msgs/air_pressure_sensor.pb.h>

Expand All @@ -32,7 +33,6 @@

#include <ignition/math/Helpers.hh>

#include <ignition/sensors/SensorFactory.hh>
#include <ignition/sensors/AirPressureSensor.hh>

#include "ignition/gazebo/components/AirPressureSensor.hh"
Expand All @@ -49,45 +49,25 @@ using namespace systems;

/// \brief Private AirPressure data class.
class ignition::gazebo::systems::AirPressurePrivate
: public GzSensorImpl<sensors::AirPressureSensor, components::AirPressureSensor>
{
/// \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 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(
public: void AddSensor(
const EntityComponentManager &_ecm,
const Entity _entity,
const components::AirPressureSensor *_airPressure,
const components::ParentEntity *_parent);

/// \brief Create air pressure sensor
/// \param[in] _ecm Immutable reference to ECM.
public: void CreateSensors(const EntityComponentManager &_ecm);
const components::ParentEntity *_parent) override;

/// \brief Update air pressure sensor data based on physics data
/// \param[in] _ecm Immutable reference to ECM.
public: void UpdateAirPressures(const EntityComponentManager &_ecm);
public: void CreateComponents(
EntityComponentManager &_ecm,
const Entity _entity,
const sensors::AirPressureSensor *_sensor) override;

/// \brief Remove air pressure sensors if their entities have been removed
/// from simulation.
/// \param[in] _ecm Immutable reference to ECM.
public: void RemoveAirPressureEntities(const EntityComponentManager &_ecm);
public: void Update(const EntityComponentManager &_ecm) override;
};

//////////////////////////////////////////////////
Expand All @@ -100,60 +80,31 @@ AirPressure::AirPressure() :
AirPressure::~AirPressure() = default;

//////////////////////////////////////////////////
void AirPressure::PreUpdate(const UpdateInfo &/*_info*/,
void AirPressure::PreUpdate(const UpdateInfo &_info,
EntityComponentManager &_ecm)
{
IGN_PROFILE("AirPressure::PreUpdate");

// 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();
this->dataPtr->PreUpdate(_info, _ecm);
}

//////////////////////////////////////////////////
void AirPressure::PostUpdate(const UpdateInfo &_info,
const EntityComponentManager &_ecm)
{
// Only update and publish if not paused.
IGN_PROFILE("AirPressure::PostUpdate");

// \TODO(anyone) Support rewind
if (_info.dt < std::chrono::steady_clock::duration::zero())
{
ignwarn << "Detected jump back in time ["
<< std::chrono::duration_cast<std::chrono::seconds>(_info.dt).count()
<< "s]. System may not work properly." << std::endl;
}

this->dataPtr->CreateSensors(_ecm);

if (!_info.paused)
{
this->dataPtr->UpdateAirPressures(_ecm);

for (auto &it : this->dataPtr->entitySensorMap)
{
// Update measurement time
it.second->Update(_info.simTime, false);
}
}
this->dataPtr->PostUpdate(_info, _ecm);
}

this->dataPtr->RemoveAirPressureEntities(_ecm);
//////////////////////////////////////////////////
void AirPressurePrivate::CreateComponents(
EntityComponentManager &_ecm,
const Entity _entity,
const sensors::AirPressureSensor *_sensor)
{
// Set topic
_ecm.CreateComponent(_entity, components::SensorTopic(_sensor->Topic()));
}

//////////////////////////////////////////////////
void AirPressurePrivate::AddAirPressure(
void AirPressurePrivate::AddSensor(
const EntityComponentManager &_ecm,
const Entity _entity,
const components::AirPressureSensor *_airPressure,
Expand Down Expand Up @@ -197,40 +148,8 @@ void AirPressurePrivate::AddAirPressure(
}

//////////////////////////////////////////////////
void AirPressurePrivate::CreateSensors(const EntityComponentManager &_ecm)
{
IGN_PROFILE("AirPressurePrivate::CreateAirPressureEntities");
if (!this->initialized)
{
// Create air pressure sensors
_ecm.Each<components::AirPressureSensor, components::ParentEntity>(
[&](const Entity &_entity,
const components::AirPressureSensor *_airPressure,
const components::ParentEntity *_parent)->bool
{
this->AddAirPressure(_ecm, _entity, _airPressure, _parent);
return true;
});
this->initialized = true;
}
else
{
// Create air pressure sensors
_ecm.EachNew<components::AirPressureSensor, components::ParentEntity>(
[&](const Entity &_entity,
const components::AirPressureSensor *_airPressure,
const components::ParentEntity *_parent)->bool
{
this->AddAirPressure(_ecm, _entity, _airPressure, _parent);
return true;
});
}
}

//////////////////////////////////////////////////
void AirPressurePrivate::UpdateAirPressures(const EntityComponentManager &_ecm)
void AirPressurePrivate::Update(const EntityComponentManager &_ecm)
{
IGN_PROFILE("AirPressurePrivate::UpdateAirPressures");
_ecm.Each<components::AirPressureSensor, components::WorldPose>(
[&](const Entity &_entity,
const components::AirPressureSensor *,
Expand All @@ -252,29 +171,6 @@ void AirPressurePrivate::UpdateAirPressures(const EntityComponentManager &_ecm)
});
}

//////////////////////////////////////////////////
void AirPressurePrivate::RemoveAirPressureEntities(
const EntityComponentManager &_ecm)
{
IGN_PROFILE("AirPressurePrivate::RemoveAirPressureEntities");
_ecm.EachRemoved<components::AirPressureSensor>(
[&](const Entity &_entity,
const components::AirPressureSensor *)->bool
{
auto sensorId = this->entitySensorMap.find(_entity);
if (sensorId == this->entitySensorMap.end())
{
ignerr << "Internal error, missing air pressure sensor for entity ["
<< _entity << "]" << std::endl;
return true;
}

this->entitySensorMap.erase(sensorId);

return true;
});
}

IGNITION_ADD_PLUGIN(AirPressure, System,
AirPressure::ISystemPreUpdate,
AirPressure::ISystemPostUpdate
Expand Down
Loading