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

Fixed sensor plugins #1104

Merged
merged 6 commits into from
Nov 5, 2021
Merged

Fixed sensor plugins #1104

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
5e881bf
Fixed sensor plugins
ahcorde Oct 13, 2021
468943e
Merge branch 'ign-gazebo3' into ahcorde/fix/sensor_plugins
ahcorde Oct 25, 2021
0e3ef23
Fixed compilation
ahcorde Oct 25, 2021
7f8a341
Merge branch 'ahcorde/fix/sensor_plugins' of https://github.com/ignit…
ahcorde Oct 25, 2021
f56dd5a
Fix comment
ahcorde Nov 3, 2021
df54565
Merge branch 'ign-gazebo3' into ahcorde/fix/sensor_plugins
ahcorde Nov 3, 2021
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
125 changes: 82 additions & 43 deletions src/systems/air_pressure/AirPressure.cc
View file
Open in desktop
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 IMUs
chapulina marked this conversation as resolved.
Show resolved Hide resolved
_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
Open in desktop
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