From 5e881bff10aeb777612514b6751a231b3374ec40 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 13 Oct 2021 23:45:54 +0200 Subject: [PATCH 1/3] Fixed sensor plugins Signed-off-by: ahcorde --- src/systems/air_pressure/AirPressure.cc | 125 +++++++++++------- src/systems/altimeter/Altimeter.cc | 127 +++++++++++------- src/systems/logical_camera/LogicalCamera.cc | 122 ++++++++++++------ src/systems/magnetometer/Magnetometer.cc | 136 +++++++++++++------- 4 files changed, 335 insertions(+), 175 deletions(-) diff --git a/src/systems/air_pressure/AirPressure.cc b/src/systems/air_pressure/AirPressure.cc index 26f8eb1d81..518988d22b 100644 --- a/src/systems/air_pressure/AirPressure.cc +++ b/src/systems/air_pressure/AirPressure.cc @@ -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); @@ -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( - [&](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 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 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( - _parent->Data())->Data(); - sensor->SetParent(parentName); + // set sensor parent + std::string parentName = _ecm.Component( + _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( + [&](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 + _ecm.EachNew( + [&](const Entity &_entity, + const components::AirPressureSensor *_airPressure, + const components::ParentEntity *_parent)->bool + { + AddAirPressure(_ecm, _entity, _airPressure, _parent); + return true; + }); + } } ////////////////////////////////////////////////// diff --git a/src/systems/altimeter/Altimeter.cc b/src/systems/altimeter/Altimeter.cc index 68c492624c..4cc12f12bc 100644 --- a/src/systems/altimeter/Altimeter.cc +++ b/src/systems/altimeter/Altimeter.cc @@ -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); @@ -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( - [&](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 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 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( - _parent->Data())->Data(); - sensor->SetParent(parentName); + // set sensor parent + std::string parentName = _ecm.Component( + _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( + [&](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( + [&](const Entity &_entity, + const components::Altimeter *_altimeter, + const components::ParentEntity *_parent)->bool + { + AddAltimeter(_ecm, _entity, _altimeter, _parent); + return true; + }); + } } ////////////////////////////////////////////////// diff --git a/src/systems/logical_camera/LogicalCamera.cc b/src/systems/logical_camera/LogicalCamera.cc index 100f21b6a4..a6ea3eb134 100644 --- a/src/systems/logical_camera/LogicalCamera.cc +++ b/src/systems/logical_camera/LogicalCamera.cc @@ -59,6 +59,20 @@ class ignition::gazebo::systems::LogicalCameraPrivate /// \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] _logicalCamera LogicalCamera component. + /// \param[in] _parent Parent entity component. + public: void AddLogicalCamera( + EntityComponentManager &_ecm, + const Entity _entity, + const components::LogicalCamera *_logicalCamera, + const components::ParentEntity *_parent); + /// \brief Create logicalCamera sensor /// \param[in] _ecm Mutable reference to ECM. public: void CreateLogicalCameraEntities(EntityComponentManager &_ecm); @@ -121,55 +135,81 @@ void LogicalCamera::PostUpdate(const UpdateInfo &_info, this->dataPtr->RemoveLogicalCameraEntities(_ecm); } +////////////////////////////////////////////////// +void AddLogicalCamera( + EntityComponentManager &_ecm, + const Entity _entity, + const components::LogicalCamera *_logicalCamera, + const components::ParentEntity *_parent) +{ + // create sensor + std::string sensorScopedName = + removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); + auto data = _logicalCamera->Data()->Clone(); + data->GetAttribute("name")->Set(sensorScopedName); + // check topic + if (!data->HasElement("topic")) + { + std::string topic = scopedName(_entity, _ecm) + "/logical_camera"; + data->GetElement("topic")->Set(topic); + } + std::unique_ptr sensor = + this->sensorFactory.CreateSensor< + sensors::LogicalCameraSensor>(data); + if (nullptr == sensor) + { + ignerr << "Failed to create sensor [" << sensorScopedName << "]" + << std::endl; + return; + } + + // set sensor parent + std::string parentName = _ecm.Component( + _parent->Data())->Data(); + sensor->SetParent(parentName); + + // set sensor world pose + 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))); +} + ////////////////////////////////////////////////// void LogicalCameraPrivate::CreateLogicalCameraEntities( EntityComponentManager &_ecm) { IGN_PROFILE("LogicalCameraPrivate::CreateLogicalCameraEntities"); - // Create logicalCameras - _ecm.EachNew( - [&](const Entity &_entity, - const components::LogicalCamera *_logicalCamera, - const components::ParentEntity *_parent)->bool - { - // create sensor - std::string sensorScopedName = - removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); - auto data = _logicalCamera->Data()->Clone(); - data->GetAttribute("name")->Set(sensorScopedName); - // check topic - if (!data->HasElement("topic")) - { - std::string topic = scopedName(_entity, _ecm) + "/logical_camera"; - data->GetElement("topic")->Set(topic); - } - std::unique_ptr sensor = - this->sensorFactory.CreateSensor< - sensors::LogicalCameraSensor>(data); - if (nullptr == sensor) + if (!this->initialized) + { + // Create logicalCameras + _ecm.Each( + [&](const Entity &_entity, + const components::LogicalCamera *_logicalCamera, + const components::ParentEntity *_parent)->bool { - ignerr << "Failed to create sensor [" << sensorScopedName << "]" - << std::endl; + AddLogicalCamera(_ecm, _entity, _logicalCamera, _parent); return true; - } - - // set sensor parent - std::string parentName = _ecm.Component( - _parent->Data())->Data(); - sensor->SetParent(parentName); - - // set sensor world pose - math::Pose3d sensorWorldPose = worldPose(_entity, _ecm); - sensor->SetPose(sensorWorldPose); - - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); + }); + this->initialized = true; - this->entitySensorMap.insert( - std::make_pair(_entity, std::move(sensor))); - - return true; - }); + } + else + { + // Create logicalCameras + _ecm.EachNew( + [&](const Entity &_entity, + const components::LogicalCamera *_logicalCamera, + const components::ParentEntity *_parent)->bool + { + AddLogicalCamera(_ecm, _entity, _logicalCamera, _parent); + return true; + }); + } } ////////////////////////////////////////////////// diff --git a/src/systems/magnetometer/Magnetometer.cc b/src/systems/magnetometer/Magnetometer.cc index 9fba17afe1..ade0e3dd08 100644 --- a/src/systems/magnetometer/Magnetometer.cc +++ b/src/systems/magnetometer/Magnetometer.cc @@ -56,6 +56,22 @@ class ignition::gazebo::systems::MagnetometerPrivate /// \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] _magnetometer Magnetometer component. + /// \param[in] _worldField MagneticField component. + /// \param[in] _parent Parent entity component. + public: void AddMagnetometer( + EntityComponentManager &_ecm, + const Entity _entity, + const components::Magnetometer *_magnetometer, + const components::MagneticField *_worldField, + const components::ParentEntity *_parent); + /// \brief Create magnetometer sensor /// \param[in] _ecm Mutable reference to ECM. public: void CreateMagnetometerEntities(EntityComponentManager &_ecm); @@ -118,6 +134,57 @@ void Magnetometer::PostUpdate(const UpdateInfo &_info, this->dataPtr->RemoveMagnetometerEntities(_ecm); } +////////////////////////////////////////////////// +void MagnetometerPrivate::AddMagnetometer( + EntityComponentManager &_ecm, + const Entity _entity, + const components::Magnetometer *_magnetometer, + const components::MagneticField *_worldField, + const components::ParentEntity *_parent) +{ + // create sensor + std::string sensorScopedName = + removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); + sdf::Sensor data = _magnetometer->Data(); + data.SetName(sensorScopedName); + // check topic + if (data.Topic().empty()) + { + std::string topic = scopedName(_entity, _ecm) + "/magnetometer"; + data.SetTopic(topic); + } + std::unique_ptr sensor = + this->sensorFactory.CreateSensor< + sensors::MagnetometerSensor>(data); + if (nullptr == sensor) + { + ignerr << "Failed to create sensor [" << sensorScopedName << "]" + << std::endl; + return; + } + + // set sensor parent + std::string parentName = _ecm.Component( + _parent->Data())->Data(); + sensor->SetParent(parentName); + + // set world magnetic field. Assume uniform in world and does not + // change throughout simulation + sensor->SetWorldMagneticField(_worldField->Data()); + + // 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 + math::Pose3d p = worldPose(_entity, _ecm); + sensor->SetWorldPose(p); + + // Set topic + _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); + + this->entitySensorMap.insert( + std::make_pair(_entity, std::move(sensor))); +} + ////////////////////////////////////////////////// void MagnetometerPrivate::CreateMagnetometerEntities( EntityComponentManager &_ecm) @@ -138,56 +205,31 @@ void MagnetometerPrivate::CreateMagnetometerEntities( return; } - // Create magnetometers - _ecm.EachNew( - [&](const Entity &_entity, - const components::Magnetometer *_magnetometer, - const components::ParentEntity *_parent)->bool - { - // create sensor - std::string sensorScopedName = - removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); - sdf::Sensor data = _magnetometer->Data(); - data.SetName(sensorScopedName); - // check topic - if (data.Topic().empty()) + if (!this->initialized) + { + // Create magnetometers + _ecm.Each( + [&](const Entity &_entity, + const components::Magnetometer *_magnetometer, + const components::ParentEntity *_parent)->bool { - std::string topic = scopedName(_entity, _ecm) + "/magnetometer"; - data.SetTopic(topic); - } - std::unique_ptr sensor = - this->sensorFactory.CreateSensor< - sensors::MagnetometerSensor>(data); - if (nullptr == sensor) + AddMagnetometer(_ecm, _entity, _magnetometer, worldField, _parent); + return true; + }); + this->initialized = true; + } + else + { + // Create magnetometers + _ecm.EachNew( + [&](const Entity &_entity, + const components::Magnetometer *_magnetometer, + const components::ParentEntity *_parent)->bool { - ignerr << "Failed to create sensor [" << sensorScopedName << "]" - << std::endl; + AddMagnetometer(_ecm, _entity, _magnetometer, worldField, _parent); return true; - } - - // set sensor parent - std::string parentName = _ecm.Component( - _parent->Data())->Data(); - sensor->SetParent(parentName); - - // set world magnetic field. Assume uniform in world and does not - // change throughout simulation - sensor->SetWorldMagneticField(worldField->Data()); - - // 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 - math::Pose3d p = worldPose(_entity, _ecm); - sensor->SetWorldPose(p); - - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); - - this->entitySensorMap.insert( - std::make_pair(_entity, std::move(sensor))); - - return true; - }); + }); + } } ////////////////////////////////////////////////// From 0e3ef239a768cf2f60c09ef2b7da9246dc4168db Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 25 Oct 2021 11:38:05 +0200 Subject: [PATCH 2/3] Fixed compilation Signed-off-by: ahcorde --- src/systems/logical_camera/LogicalCamera.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/logical_camera/LogicalCamera.cc b/src/systems/logical_camera/LogicalCamera.cc index a6ea3eb134..3b53fb4c0c 100644 --- a/src/systems/logical_camera/LogicalCamera.cc +++ b/src/systems/logical_camera/LogicalCamera.cc @@ -136,7 +136,7 @@ void LogicalCamera::PostUpdate(const UpdateInfo &_info, } ////////////////////////////////////////////////// -void AddLogicalCamera( +void LogicalCameraPrivate::AddLogicalCamera( EntityComponentManager &_ecm, const Entity _entity, const components::LogicalCamera *_logicalCamera, From f56dd5ad2c0c383819c514b59bffdc684e8c0fbf Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 3 Nov 2021 09:56:43 +0100 Subject: [PATCH 3/3] Fix comment Signed-off-by: ahcorde --- src/systems/air_pressure/AirPressure.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/air_pressure/AirPressure.cc b/src/systems/air_pressure/AirPressure.cc index 518988d22b..cff6ea8858 100644 --- a/src/systems/air_pressure/AirPressure.cc +++ b/src/systems/air_pressure/AirPressure.cc @@ -197,7 +197,7 @@ void AirPressurePrivate::CreateAirPressureEntities(EntityComponentManager &_ecm) } else { - // Create IMUs + // Create air pressure sensors _ecm.EachNew( [&](const Entity &_entity, const components::AirPressureSensor *_airPressure,