Skip to content

Commit

Permalink
Fixed sensor plugins (gazebosim#1104)
Browse files Browse the repository at this point in the history 
* Fixed sensor plugins

Signed-off-by: ahcorde <ahcorde@gmail.com>

* Fixed compilation

Signed-off-by: ahcorde <ahcorde@gmail.com>

* Fix comment

Signed-off-by: ahcorde <ahcorde@gmail.com>
Signed-off-by: William Lew <WilliamMilesLew@gmail.com>
  • Loading branch information
@ahcorde @WilliamLewww
ahcorde authored and WilliamLewww committed Dec 7, 2021
1 parent 15f5b63 commit e3b76f1
Show file tree
Hide file tree
Showing 4 changed files with 335 additions and 175 deletions.
125 changes: 82 additions & 43 deletions src/systems/air_pressure/AirPressure.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,20 @@ class ignition::gazebo::systems::AirPressurePrivate
/// \brief Ign-sensors sensor factory for creating sensors
public: sensors::SensorFactory sensorFactory;

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

/// \brief Create sensor
/// \param[in] _ecm Mutable 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 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);
Expand Down Expand Up @@ -119,55 +133,80 @@ void AirPressure::PostUpdate(const UpdateInfo &_info,
}

//////////////////////////////////////////////////
void AirPressurePrivate::CreateAirPressureEntities(EntityComponentManager &_ecm)
void AirPressurePrivate::AddAirPressure(
EntityComponentManager &_ecm,
const Entity _entity,
const components::AirPressureSensor *_airPressure,
const components::ParentEntity *_parent)
{
IGN_PROFILE("AirPressurePrivate::CreateAirPressureEntities");
// Create air pressure sensors
_ecm.EachNew<components::AirPressureSensor, components::ParentEntity>(
[&](const Entity &_entity,
const components::AirPressureSensor *_airPressure,
const components::ParentEntity *_parent)->bool
{
// create sensor
std::string sensorScopedName =
removeParentScope(scopedName(_entity, _ecm, "::", false), "::");
sdf::Sensor data = _airPressure->Data();
data.SetName(sensorScopedName);
// check topic
if (data.Topic().empty())
{
std::string topic = scopedName(_entity, _ecm) + "/air_pressure";
data.SetTopic(topic);
}
std::unique_ptr<sensors::AirPressureSensor> sensor =
this->sensorFactory.CreateSensor<
sensors::AirPressureSensor>(data);
if (nullptr == sensor)
{
ignerr << "Failed to create sensor [" << sensorScopedName << "]"
<< std::endl;
return true;
}
// create sensor
std::string sensorScopedName =
removeParentScope(scopedName(_entity, _ecm, "::", false), "::");
sdf::Sensor data = _airPressure->Data();
data.SetName(sensorScopedName);
// check topic
if (data.Topic().empty())
{
std::string topic = scopedName(_entity, _ecm) + "/air_pressure";
data.SetTopic(topic);
}
std::unique_ptr<sensors::AirPressureSensor> sensor =
this->sensorFactory.CreateSensor<
sensors::AirPressureSensor>(data);
if (nullptr == sensor)
{
ignerr << "Failed to create sensor [" << sensorScopedName << "]"
<< std::endl;
return;
}

// set sensor parent
std::string parentName = _ecm.Component<components::Name>(
_parent->Data())->Data();
sensor->SetParent(parentName);
// set sensor parent
std::string parentName = _ecm.Component<components::Name>(
_parent->Data())->Data();
sensor->SetParent(parentName);

// The WorldPose component was just created and so it's empty
// We'll compute the world pose manually here
// set sensor world pose
math::Pose3d sensorWorldPose = worldPose(_entity, _ecm);
sensor->SetPose(sensorWorldPose);
// The WorldPose component was just created and so it's empty
// We'll compute the world pose manually here
// set sensor world pose
math::Pose3d sensorWorldPose = worldPose(_entity, _ecm);
sensor->SetPose(sensorWorldPose);

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

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

return true;
});
//////////////////////////////////////////////////
void AirPressurePrivate::CreateAirPressureEntities(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
{
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
{
AddAirPressure(_ecm, _entity, _airPressure, _parent);
return true;
});
}
}

//////////////////////////////////////////////////
Expand Down
127 changes: 83 additions & 44 deletions src/systems/altimeter/Altimeter.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,20 @@ class ignition::gazebo::systems::AltimeterPrivate
/// \brief Ign-sensors sensor factory for creating sensors
public: sensors::SensorFactory sensorFactory;

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

/// \brief Create sensor
/// \param[in] _ecm Mutable 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 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);
Expand Down Expand Up @@ -120,56 +134,81 @@ void Altimeter::PostUpdate(const UpdateInfo &_info,
}

//////////////////////////////////////////////////
void AltimeterPrivate::CreateAltimeterEntities(EntityComponentManager &_ecm)
void AltimeterPrivate::AddAltimeter(
EntityComponentManager &_ecm,
const Entity _entity,
const components::Altimeter *_altimeter,
const components::ParentEntity *_parent)
{
IGN_PROFILE("Altimeter::CreateAltimeterEntities");
// Create altimeters
_ecm.EachNew<components::Altimeter, components::ParentEntity>(
[&](const Entity &_entity,
const components::Altimeter *_altimeter,
const components::ParentEntity *_parent)->bool
{
// create sensor
std::string sensorScopedName =
removeParentScope(scopedName(_entity, _ecm, "::", false), "::");
sdf::Sensor data = _altimeter->Data();
data.SetName(sensorScopedName);
// check topic
if (data.Topic().empty())
{
std::string topic = scopedName(_entity, _ecm) + "/altimeter";
data.SetTopic(topic);
}
std::unique_ptr<sensors::AltimeterSensor> sensor =
this->sensorFactory.CreateSensor<
sensors::AltimeterSensor>(data);
if (nullptr == sensor)
{
ignerr << "Failed to create sensor [" << sensorScopedName << "]"
<< std::endl;
return true;
}
// create sensor
std::string sensorScopedName =
removeParentScope(scopedName(_entity, _ecm, "::", false), "::");
sdf::Sensor data = _altimeter->Data();
data.SetName(sensorScopedName);
// check topic
if (data.Topic().empty())
{
std::string topic = scopedName(_entity, _ecm) + "/altimeter";
data.SetTopic(topic);
}
std::unique_ptr<sensors::AltimeterSensor> sensor =
this->sensorFactory.CreateSensor<
sensors::AltimeterSensor>(data);
if (nullptr == sensor)
{
ignerr << "Failed to create sensor [" << sensorScopedName << "]"
<< std::endl;
return;
}

// set sensor parent
std::string parentName = _ecm.Component<components::Name>(
_parent->Data())->Data();
sensor->SetParent(parentName);
// set sensor parent
std::string parentName = _ecm.Component<components::Name>(
_parent->Data())->Data();
sensor->SetParent(parentName);

// Get initial pose of sensor and set the reference z pos
// The WorldPose component was just created and so it's empty
// We'll compute the world pose manually here
double verticalReference = worldPose(_entity, _ecm).Pos().Z();
sensor->SetVerticalReference(verticalReference);
sensor->SetPosition(verticalReference);
// Get initial pose of sensor and set the reference z pos
// The WorldPose component was just created and so it's empty
// We'll compute the world pose manually here
double verticalReference = worldPose(_entity, _ecm).Pos().Z();
sensor->SetVerticalReference(verticalReference);
sensor->SetPosition(verticalReference);

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

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

return true;
});
//////////////////////////////////////////////////
void AltimeterPrivate::CreateAltimeterEntities(EntityComponentManager &_ecm)
{
IGN_PROFILE("Altimeter::CreateAltimeterEntities");
if (!this->initialized)
{
// Create altimeters
_ecm.Each<components::Altimeter, components::ParentEntity>(
[&](const Entity &_entity,
const components::Altimeter *_altimeter,
const components::ParentEntity *_parent)->bool
{
AddAltimeter(_ecm, _entity, _altimeter, _parent);
return true;
});
this->initialized = true;
}
else
{
// Create altimeters
_ecm.EachNew<components::Altimeter, components::ParentEntity>(
[&](const Entity &_entity,
const components::Altimeter *_altimeter,
const components::ParentEntity *_parent)->bool
{
AddAltimeter(_ecm, _entity, _altimeter, _parent);
return true;
});
}
}

//////////////////////////////////////////////////
Expand Down
Loading

0 comments on commit e3b76f1

Please sign in to comment.