From 9d43d634f678c4c714bed1dbc51477fd69bde943 Mon Sep 17 00:00:00 2001 From: TigerWuu Date: Thu, 3 Apr 2025 00:11:37 +0800 Subject: [PATCH 1/5] FIX: airspeed calculation by tranmitting wind information into airspeed sensor Signed-off-by: TigerWuu --- src/systems/air_speed/AirSpeed.cc | 36 +++++++++++++++++++++++++++++-- src/systems/air_speed/AirSpeed.hh | 7 ++++++ 2 files changed, 41 insertions(+), 2 deletions(-) diff --git a/src/systems/air_speed/AirSpeed.cc b/src/systems/air_speed/AirSpeed.cc index 39904b9575..b56c64ad12 100644 --- a/src/systems/air_speed/AirSpeed.cc +++ b/src/systems/air_speed/AirSpeed.cc @@ -39,6 +39,9 @@ #include "gz/sim/components/Name.hh" #include "gz/sim/components/ParentEntity.hh" #include "gz/sim/components/Pose.hh" +#include "gz/sim/components/Wind.hh" +#include "gz/sim/components/WindMode.hh" +#include "gz/sim/components/LinearVelocitySeed.hh" #include "gz/sim/components/Sensor.hh" #include "gz/sim/EntityComponentManager.hh" #include "gz/sim/Util.hh" @@ -67,6 +70,12 @@ class gz::sim::systems::AirSpeedPrivate public: Entity entity; + /// \brief Wind entity on which this sytem operates. + public: Entity windEntity; + + /// \param[in] _ecm Mutable reference to the EntityComponentManager. + public: void Load(EntityComponentManager &_ecm); + /// \brief Create sensor /// \param[in] _ecm Immutable reference to ECM. /// \param[in] _entity Entity of the IMU @@ -101,12 +110,20 @@ AirSpeed::AirSpeed() : ////////////////////////////////////////////////// AirSpeed::~AirSpeed() = default; +////////////////////////////////////////////////// +void AirSpeed::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &) +{ + this->dataPtr->Load(_ecm); +} + ////////////////////////////////////////////////// void AirSpeed::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) { GZ_PROFILE("AirSpeed::PreUpdate"); - // Create components for (auto entity : this->dataPtr->newSensors) { @@ -137,7 +154,6 @@ void AirSpeed::PostUpdate(const UpdateInfo &_info, << std::chrono::duration(_info.dt).count() << "s]. System may not work properly." << std::endl; } - this->dataPtr->CreateSensors(_ecm); if (!_info.paused) @@ -172,6 +188,13 @@ void AirSpeed::PostUpdate(const UpdateInfo &_info, this->dataPtr->RemoveAirSpeedEntities(_ecm); } +////////////////////////////////////////////////// +void AirSpeedPrivate::Load( + EntityComponentManager &_ecm) +{ + this->windEntity = _ecm.EntityByComponents(components::Wind()); +} + ////////////////////////////////////////////////// void AirSpeedPrivate::AddAirSpeed( const EntityComponentManager &_ecm, @@ -265,6 +288,14 @@ void AirSpeedPrivate::UpdateAirSpeeds(const EntityComponentManager &_ecm) math::Vector3d sensorRelativeVel = relativeVel(_entity, _ecm); it->second->SetVelocity(sensorRelativeVel); + + // update wind velocity + auto windVelSeed = + _ecm.Component(this->windEntity); + math::Vector3d windVel{windVelSeed->Data().X(), + windVelSeed->Data().Y(), + windVelSeed->Data().Z()}; + it->second->SetWindVelocity(windVel); } else { @@ -300,6 +331,7 @@ void AirSpeedPrivate::RemoveAirSpeedEntities( } GZ_ADD_PLUGIN(AirSpeed, System, + AirSpeed::ISystemConfigure, AirSpeed::ISystemPreUpdate, AirSpeed::ISystemPostUpdate ) diff --git a/src/systems/air_speed/AirSpeed.hh b/src/systems/air_speed/AirSpeed.hh index 4c60d3b48f..da0ef1c13d 100644 --- a/src/systems/air_speed/AirSpeed.hh +++ b/src/systems/air_speed/AirSpeed.hh @@ -37,6 +37,7 @@ namespace systems /// readings over gz transport class AirSpeed: public System, + public ISystemConfigure, public ISystemPreUpdate, public ISystemPostUpdate { @@ -46,6 +47,12 @@ namespace systems /// \brief Destructor public: ~AirSpeed() override; + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) final; + /// Documentation inherited public: void PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) final; From b801a07b375220c1ca75dffaa503a89de4dcb186 Mon Sep 17 00:00:00 2001 From: TigerWuu Date: Thu, 3 Apr 2025 07:42:20 +0800 Subject: [PATCH 2/5] MOD: restore unrelated modification Signed-off-by: TigerWuu --- src/systems/air_speed/AirSpeed.cc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/systems/air_speed/AirSpeed.cc b/src/systems/air_speed/AirSpeed.cc index b56c64ad12..3b3c369ab8 100644 --- a/src/systems/air_speed/AirSpeed.cc +++ b/src/systems/air_speed/AirSpeed.cc @@ -124,6 +124,7 @@ void AirSpeed::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) { GZ_PROFILE("AirSpeed::PreUpdate"); + // Create components for (auto entity : this->dataPtr->newSensors) { @@ -154,6 +155,7 @@ void AirSpeed::PostUpdate(const UpdateInfo &_info, << std::chrono::duration(_info.dt).count() << "s]. System may not work properly." << std::endl; } + this->dataPtr->CreateSensors(_ecm); if (!_info.paused) From efe2262a8673fb51aeda03ee2b3a01f7304112cd Mon Sep 17 00:00:00 2001 From: Ching-Hsiang Wu Date: Wed, 2 Jul 2025 13:08:59 +0800 Subject: [PATCH 3/5] Update src/systems/air_speed/AirSpeed.cc Co-authored-by: Ian Chen Signed-off-by: Ching-Hsiang Wu --- src/systems/air_speed/AirSpeed.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/systems/air_speed/AirSpeed.cc b/src/systems/air_speed/AirSpeed.cc index 3b3c369ab8..523d7baab2 100644 --- a/src/systems/air_speed/AirSpeed.cc +++ b/src/systems/air_speed/AirSpeed.cc @@ -155,7 +155,6 @@ void AirSpeed::PostUpdate(const UpdateInfo &_info, << std::chrono::duration(_info.dt).count() << "s]. System may not work properly." << std::endl; } - this->dataPtr->CreateSensors(_ecm); if (!_info.paused) From f74137f6fa223391bebb477bee9075e24882ad62 Mon Sep 17 00:00:00 2001 From: Ching-Hsiang Wu Date: Wed, 2 Jul 2025 13:09:15 +0800 Subject: [PATCH 4/5] Update src/systems/air_speed/AirSpeed.cc Co-authored-by: Ian Chen Signed-off-by: Ching-Hsiang Wu --- src/systems/air_speed/AirSpeed.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/air_speed/AirSpeed.cc b/src/systems/air_speed/AirSpeed.cc index 523d7baab2..b1eb19f4df 100644 --- a/src/systems/air_speed/AirSpeed.cc +++ b/src/systems/air_speed/AirSpeed.cc @@ -70,7 +70,7 @@ class gz::sim::systems::AirSpeedPrivate public: Entity entity; - /// \brief Wind entity on which this sytem operates. + /// \brief Wind entity on which this system operates. public: Entity windEntity; /// \param[in] _ecm Mutable reference to the EntityComponentManager. From 7bfd1bbab49f3907d7a65cf59b98e7c38a16ffb6 Mon Sep 17 00:00:00 2001 From: Ching-Hsiang Wu Date: Thu, 3 Jul 2025 01:36:36 +0800 Subject: [PATCH 5/5] Update src/systems/air_speed/AirSpeed.cc Co-authored-by: Ian Chen Signed-off-by: Ching-Hsiang Wu --- src/systems/air_speed/AirSpeed.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systems/air_speed/AirSpeed.cc b/src/systems/air_speed/AirSpeed.cc index b1eb19f4df..ac5b2cf6bb 100644 --- a/src/systems/air_speed/AirSpeed.cc +++ b/src/systems/air_speed/AirSpeed.cc @@ -111,8 +111,8 @@ AirSpeed::AirSpeed() : AirSpeed::~AirSpeed() = default; ////////////////////////////////////////////////// -void AirSpeed::Configure(const Entity &_entity, - const std::shared_ptr &_sdf, +void AirSpeed::Configure(const Entity &, + const std::shared_ptr &, EntityComponentManager &_ecm, EventManager &) {